/*
*
* DAMI-M3
*
* master-sensor-mod2-vx
*
* LIBRARIES
* Wire (Arduino I2C Master)
* Servo
* SparkFun VL53L1X 4M Laser Distance Sensor
* newPing
* kalman simple (local)
* SPI
* RF24
* nRF24L01
*
*/
/*
O rascunho usa 15626 bytes (50%) do espaço de armazenamento do programa. O máximo é 30720 bytes.
Variáveis globais usam 1400 bytes (68%) de memória dinâmica
*/
#define IDLE_LOOP_COUNTER_MAX 5
#define SIDE_OFFSET 60
//#define FRONT_OFFSET 130
//#define REAR_OFFSET 60
#define FRONT_LASER_MAX_DISTANCE 3000
//#define FRONT_LASER_FRONT_OFFSET 120;
//#define FRONT_LASER_SIDE_OFFSET 0;
//#define SIDE_SONAR_FRONT_OFFSET 85;
//#define SIDE_SONAR_SIDE_OFFSET 60;
#define WALL_SIDE_THRESHOLD 100
#define WALL_SIDE_ROTATE_THRESHOLD 170
#define WALL_FRONT_THRESHOLD 200
#define WALL_FRONT_ALLEY_THRESHOLD 500
union u_tag {
byte b[4];
int i[2];
double d;
long l;
};
// BOF: nRF240L01
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#define CE_PIN 9
#define CSN_PIN 10
const byte slaveAddress[5] = {'R','x','A','A','A'};
RF24 radio(CE_PIN, CSN_PIN); // Create a Radio
//char dataToSend[10] = "Message 0";
//char txNum = '0';
int rfAckData[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // to hold the two values coming from the slave
byte rfSendDataStatus = 0;
union u_tag rfSendData[8];
// EOF: nRF240L01
#include <Servo.h>
Servo frontServo;
#define READ_POSITION_MAX 13
byte readPosition[READ_POSITION_MAX] = {0, 15, 30, 45, 60, 75, 90, 105, 120, 135, 150, 165, 180};
int readPositionDistance[READ_POSITION_MAX] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
#define frontServoPin 5
#define outputForPlotter false
#define serialSpeed 115200
// BOF: BATTERY VOLTAGE
// BAT 12V, R1=1.8K, R2=1.2K AoutMAx=883 # A0 * (12.00 / 883.00)
#define BATERY_VOLTAGE 12.00
#define BATERY_MAX_OUT 883
float batteryRead;
// EOF: BATTERY VOLTAGE
#include <Wire.h>
// arduino slaves i2c address array
byte i2cSlave[1] = {8};
// I2C byte conversion
union u_tag i2c_data[1][16];
// BOF: SparkFun VL53L1X 4M Laser Distance Sensor
#include "SparkFun_VL53L1X_Arduino_Library.h"
VL53L1X distanceSensor;
int bodyFrontLaserRead;
// EOF: SparkFun VL53L1X 4M Laser Distance Sensor
// BOF HC-SR04
#include <NewPing.h>
#define SONAR_NUM 2 // Number of sensors.
#define SONAR_MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_WAIT_TIME = 33; // should be more than 29ms
NewPing bodySonar[SONAR_NUM] = { // Sensor object array.
NewPing(4, 4, SONAR_MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
NewPing(7, 7, SONAR_MAX_DISTANCE),
};
int bodySonarRead[SONAR_NUM];
byte bodySonarIndex = 0; // Holds the current sonar for loop
// EOF HC-SR04
// BOF: KALMAN
#include "Kalman.h";
// kalman
int bodyFrontLaserDistance;
boolean bodyFrontLaserDistanceUpdate = true;
int bodyFrontLaserDistanceQuick;
int bodySonarDistance[SONAR_NUM];
float batteryVoltage;
Kalman batteryKalman(0.125,32,1023,0);
//Kalman bodyFrontLaserKalman(0.125,32,1023,1000);
Kalman bodyFrontLaserKalman(0.125,0.25,1023,1000);
//Kalman bodyFrontLaserKalmanQuick(0.125,0.25,1023,1000);
Kalman bodySonarKalman0(1,0.25,1023,100);
Kalman bodySonarKalman1(1,0.25,1023,100);
// EOF: KALMAN
#define LED_PIN 8
bool internalLedBlinkState = false;
// TODO redefine with internal defines
#ifndef PI2
#define PI2 6.28318530718
#endif
#define BODY_WHEEL_RADIUS 52
#define BODY_WHEEL_DISTANCE 120
#define BODY_WHEEL_RESOLUTION 4096 // 4096 / 2
#define BODY_MOTOR_ACCELERATION 25 // steps/sec
#define BODY_MOTOR_SPEEDBASE 250
#define BODY_MOTOR_SPEEDTOP 1000
float bodyMotorStepsPerUnit = 0;
//signed long bodyMotorUnitsTarget;
signed long bodyMotorStepsTarget;
signed long bodyMotorLeftStepsToGo;
signed long bodyMotorLeftSteps;
signed long bodyMotorLeftStepsLast;
//signed long bodyMotorRightStepsToGo;
// odometry
double bodyThetaRadians = 0;
double bodyPosX = 0;
double bodyPosY = 0;
byte slaveCmdRequestId = 0;
byte slaveCmdWaitingId = 0;
byte slaveCmdReceivedId = 0;
byte slaveCmdTerminateId = 0;
byte cmd;
// commands sequence
#define TMP_PRG_OFFSET 2
#define PRG_CMD_MAX 10 // 10 = 8 cmd + 2 reserved positions
typedef struct {
byte extId;
byte group;
byte cmd;
byte intCmd;
byte extCmd; // 1=stop, 2=line, 3=rotate
signed long units; // distance in mm/cm (depends on resolution), angles on degrees
int minSpeed;
int acceleration;
int maxSpeed;
} cmd_t;
/*
cmd_t prgCmd[PRG_CMD_MAX] = {
{0, 0, 2, 0, 2, 100, 250, 50, 1500}, {0, 0, 3, 0, 3, 90, 250, 50, 1500}, {0, 0, 2, 0, 2, -100, 250, 50, 1500},
{0, 0, 2, 0, 3, -90, 250, 50, 1500}, {0, 0, 253, 253, 0, 0, 0, 0, 0}
};
*/
cmd_t prgCmd[PRG_CMD_MAX] = {
{0, 0, 2, 21, 2, 5000, 250, 50, 1500}, {0, 0, 3, 0, 3, 90, 250, 50, 1500}, {0, 0, 2, 0, 2, 250, 250, 50, 1500},
{0, 0, 2, 0, 3, 90, 250, 50, 1500}, {0, 0, 253, 253, 0, 0, 0, 0, 0}
};
byte prgIdx = 0;
byte bodyMoveProcessStatus = 0;
boolean internalCmdProcessStatus = false;
//byte bodyMoveInternalCommand = 0;
byte currentCommandGroupId = 0;
byte commandGroupId = 0;
//byte bodyMoveCommandLastGroup = 0;
byte internalCmdStepCount = 0;
unsigned long internalCmdLoopCountNext = 0;
unsigned int internalMoveLoopCounter = 0;
unsigned int bodyMoveLoopCounter = 0;
#define BODY_MOVE_LOG_MAX 24
typedef struct {
byte cmd; // external Command
signed long units; // distance in mm/cm (depends on resolution), angles on degrees
} moveLog_t;
moveLog_t bodyMoveLog[BODY_MOVE_LOG_MAX];
byte bodyMoveNextLog = 0;
boolean rotateScanRequest;
boolean rotateScanStatus;
byte frontObstacleDetectedStatus = 0;
byte loopTime;
unsigned long loopTimeStart;
byte idleLoopCounter = 0;
#define MAP_L0_MAX 10
#define MAP_L0_RES 500 // BASE MAP CELL in mm
#define MAP_L0_XOFFSET 5 // offset to 0,0
#define MAP_L0_YOFFSET 5 // offset to 0,0
#define MAP_L1_MAX 100 // (= MAP_L0_MAX * MAP_L0_MAX)
#define MAP_L1_RES 50 // SECUNDARY MAP CELL in mm (= MAP_L0_RES / (MAP_L1_MAX / MAP_L0_MAX))
#define MAP_L1_XOFFSET 50 // offset to 0,0
#define MAP_L1_YOFFSET 50
struct s_cellSummary {
byte hit;
byte miss;
};
struct s_cellSummary mapSummary[10][10];
//struct s_cellSummary cellSummary[10][10];
cmd_t receivedCmd;
//boolean ackReceivedCmd = false;
byte prgIdxPaused = 0;
signed long pausedUnits = 0;
byte pausedCmd = 0;
// reset function
void (* resetFunc) (void) = 0;
void setup() {
static unsigned long internalLedTimeStart = millis() + 250;
int i;
int j;
for(j=0; j < MAP_L0_MAX; j++)
for(i=0; i < MAP_L0_MAX; i++)
mapSummary[j][i] = {0, 0};
pinMode(LED_PIN, OUTPUT); // configure internal LED for output
Serial.begin(serialSpeed); // start serial for output
//while (!Serial);
Wire.begin(); // join i2c bus (address optional for master)
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
// set constant vars
bodyMotorStepsPerUnit = (float)((float)(BODY_WHEEL_RADIUS * PI2) / BODY_WHEEL_RESOLUTION);
// reset slave using i2c master writer
Wire.beginTransmission(i2cSlave[0]);
Wire.write(254); // cmd
Wire.write(slaveCmdRequestId); // cmdId
Wire.endTransmission();
// does not update cmdId
// BOF: nRF240L01
radio.begin();
radio.setDataRate( RF24_250KBPS );
radio.enableAckPayload();
radio.setRetries(3,5); // delay, count
radio.openWritingPipe(slaveAddress);
// EOF: nRF240L01
// init VL53L1X 4M Laser Distance Sensor
if (!distanceSensor.begin()) {
Serial.println("Sensor VL53L1X offline. Check wiring!");
while (1);
}
distanceSensorsAdaptativePeriod(45);
// wait some time for the slave restart
delay(1000);
// config basic slave vars using i2c master writer
Wire.beginTransmission(i2cSlave[0]);
Wire.write(90); // cmd
Wire.write(slaveCmdRequestId); // cmdId
Wire.write(BODY_WHEEL_RADIUS); Wire.write(BODY_WHEEL_DISTANCE);
i=BODY_WHEEL_RESOLUTION; Wire.write((byte*)&i, 2); // units, deg
i=BODY_MOTOR_ACCELERATION; Wire.write((byte*)&i, 2); // minSpeed
i=BODY_MOTOR_SPEEDBASE; Wire.write((byte*)&i, 2); // aceleration
i=BODY_MOTOR_SPEEDTOP; Wire.write((byte*)&i, 2); // maxSpeed
Wire.endTransmission();
// does not update cmdId
// get update from slave only
getSlavesUpdate();
// servo
frontServo.attach(frontServoPin);
//control the servo's direction and the position of the motor
delay(50);
frontServo.write(0);
delay(500);
frontServo.write(90);
delay(500);
frontServo.detach();
delay(50);
// extra delay for VL53L1X stability
delay(500);
/*
// wait for start & do some previous readings
//Serial.println("Wait for hands on sides!");
while((bodySonarDistance[0] < 1 ) || (bodySonarDistance[1] < 1) ||
(bodySonarDistance[0] - SIDE_OFFSET > WALL_SIDE_THRESHOLD) || (bodySonarDistance[1] - SIDE_OFFSET > WALL_SIDE_THRESHOLD)
) {
readSonarDistance();
readFrontLaserDistance();
// blink LED to indicate activity
if(millis() >= internalLedTimeStart) {
internalLedTimeStart = millis() + 250;
internalLedBlinkState = !internalLedBlinkState;
digitalWrite(LED_PIN, internalLedBlinkState);
}
}
*/
distanceSensorsAdaptativePeriod(30);
// lets start looping
loopTimeStart = millis();
}
void loop() {
static unsigned long internalLedTimeStart = millis() + 1000;
int i;
cmdProcess();
processInternalOnlyCommands();
processInternalSlaveCommands();
readSensors();
// detect obstacles moving front
if(prgCmd[prgIdx].extCmd == 2 && !frontObstacleDetectedStatus) {
// check front distance
//if(bodyFrontLaserDistance - FRONT_OFFSET < WALL_FRONT_THRESHOLD) {
i = getFrontLaserDistance(2); // bodyFrontLaserDistance HARDECODED distance mode
if(i > 0 && i < WALL_FRONT_THRESHOLD) {
frontObstacleDetectedStatus = 1;
//Serial.print("SENDSTOP"); Serial.println("\t");
}
// check front distance both sides 45 degrees
if(prgCmd[prgIdx].intCmd == 21 && !frontObstacleDetectedStatus) {
// check left
i = readPositionDistance[3]; // HARDECODED
if(i > 0 && i < WALL_FRONT_THRESHOLD) {
frontObstacleDetectedStatus = 1;
}
// check right
i = readPositionDistance[9]; // HARDECODED
if(i > 0 && i < WALL_FRONT_THRESHOLD) {
frontObstacleDetectedStatus = 1;
}
}
}
//currentMillis = millis();
//if (currentMillis - prevMillis >= txIntervalMillis) {
sendRadioLoopData();
//}
// processing incomming radio data
if (rfSendDataStatus == 2) {
// incomming remote data
// check for a cmd
if(rfAckData[0] == 0 && rfAckData[1]) { // HARDCODED: TODO: change == 0 to == 1
// there is a command to process
if(!receivedCmd.cmd) {
receivedCmd.cmd = rfAckData[1];
// process imediate commands
switch(receivedCmd.cmd) {
case 254: // reset
resetFunc();
break;
}
// the command will be processed on next loop
//ackReceivedCmd = true;
//} else {
//ackReceivedCmd = false;
}
}
}
loopTime = millis() - loopTimeStart;
loopTimeStart = millis();
sendSerialLoopData();
// update internal loop counter
if(internalCmdProcessStatus) internalMoveLoopCounter++;
// blink LED to indicate activity
if(millis() >= internalLedTimeStart) {
internalLedTimeStart = millis() + 1000;
internalLedBlinkState = !internalLedBlinkState;
digitalWrite(LED_PIN, internalLedBlinkState);
}
delay(15);
}
//
//
// BOF CINEMATIC CONTROL
//
//
void bodyMoveDecide() {
int i;
int maxDistance = 0;
byte maxDistanceIdx = 0;
byte emptyIndex[READ_POSITION_MAX];
byte emptyIndexIdx = 0;
boolean needToGoBackward = false;
//byte selectedIdx;
float theta;
int j;
float xpos, ypos;
int xidx, yidx;
float beta;
unsigned int memAddress;
// debug
//Serial.print("READINGS"); Serial.println("\t");
for(i = 0; i < READ_POSITION_MAX; i++) {
Serial.print(readPosition[i]); Serial.print("\t");
Serial.print(readPositionDistance[i]); Serial.println("\t");
beta = bodyThetaRadians + (float)(readPosition[i] / 180.0) * PI;
xpos = readPositionDistance[i] * cos(beta);
ypos = readPositionDistance[i] * sin(beta);
/*
Serial.print(xpos); Serial.print("\t");
Serial.print(ypos); Serial.print("\t");
*/
// low resolution
xidx = (int)(xpos/MAP_L0_RES) + MAP_L0_XOFFSET;
yidx = (int)(ypos/MAP_L0_RES) + MAP_L0_YOFFSET;
// hits
//mapSummary[(int)(xpos/MAP_L0_RES) + MAP_L0_XOFFSET][(int)(ypos/MAP_L0_RES) + MAP_L0_YOFFSET].hit++;
if( xidx >= 0 && xidx < MAP_L0_MAX && yidx >= 0 && yidx < MAP_L0_MAX) {
mapSummary[xidx][yidx].hit++;
}
// index on small map cell / just for memory
//xidx = (int)(fmod(xpos, MAP_L0_RES) * MAP_L1_K1 / MAP_L1_K2) + MAP_L0_XOFFSET;
//yidx = (int)(fmod(ypos, MAP_L0_RES) * MAP_L1_K1 / MAP_L1_K2) + MAP_L0_XOFFSET;
//Serial.print(xidx); Serial.print("\t");
//Serial.print(yidx); Serial.print("\t");
// middle resolution
xidx = (int)(xpos/MAP_L1_RES) + MAP_L1_XOFFSET;
yidx = (int)(ypos/MAP_L1_RES) + MAP_L1_YOFFSET;
//Serial.print(xidx); Serial.print("\t");
//Serial.print(yidx); Serial.print("\t");
memAddress = xidx * MAP_L1_MAX + yidx;
//Serial.print(memAddress); Serial.print("\t");
//Serial.print(memAddress); Serial.print("\t");
//if( xidx >= 0 && xidx < MAP_L1_MAX && yidx >= 0 && yidx < MAP_L1_MAX) {
//
//}
// miss
//Serial.print("miss\t");
for(j = 4000; j > 0; j -= MAP_L0_RES) { // HARDCODED: MAX SENSOR READ DISTANCE
if((j) < readPositionDistance[i]) {
// para cá da distancia
xpos = j * cos(beta);
ypos = j * sin(beta);
xidx = (int)(xpos/MAP_L0_RES) + MAP_L0_XOFFSET;
yidx = (int)(ypos/MAP_L0_RES) + MAP_L0_YOFFSET;
if( xidx >= 0 && xidx < MAP_L0_MAX && yidx >= 0 && yidx < MAP_L0_MAX) {
//mapSummary[(int)(xpos/MAP_L0_RES) + MAP_L0_XOFFSET][(int)(ypos/MAP_L0_RES) + MAP_L0_YOFFSET].miss++;
mapSummary[xidx][yidx].miss++;
}
xidx = (int)(xpos/MAP_L1_RES) + MAP_L1_XOFFSET;
yidx = (int)(ypos/MAP_L1_RES) + MAP_L1_YOFFSET;
//Serial.print(xidx); Serial.print("\t");
//Serial.print(yidx); Serial.print("\t");
memAddress = xidx * MAP_L1_MAX + yidx;
//Serial.print(memAddress); Serial.print("\t");
//if( xidx >= 0 && xidx < MAP_L1_MAX && yidx >= 0 && yidx < MAP_L1_MAX) {
//
//}
/*
Serial.print(j); Serial.print("\t");
Serial.print(xidx); Serial.print("\t");
Serial.print(yidx); Serial.print("\t");
Serial.print((int)(xpos/MAP_L0_RES) + MAP_L0_XOFFSET); Serial.print("\t");
Serial.print((int)(ypos/MAP_L0_RES) + MAP_L0_YOFFSET); Serial.print("\t");
Serial.print(xpos); Serial.print("\t");
Serial.print(ypos); Serial.print("\t");
*/
} else {
// para alem da distancia
}
}
//Serial.println("");
// update max distance found
if(readPositionDistance[i] > maxDistance) {
maxDistance = readPositionDistance[i];
maxDistanceIdx = i;
}
// update unknow distance array
if(readPositionDistance[i] == 0) {
emptyIndex[emptyIndexIdx] = i;
emptyIndexIdx++;
}
}
// check if we can rotate
// TODO: calculate the max allowed degrees to rotate in available space
boolean canRotateLeft = true;
boolean canRotateRight = true;
if(((bodySonarDistance[0] > 0) && (bodySonarDistance[0] < WALL_SIDE_ROTATE_THRESHOLD)) || ((bodySonarDistance[1] > 0) && (bodySonarDistance[1] < WALL_SIDE_THRESHOLD))) {
canRotateLeft = false;
}
if(((bodySonarDistance[1] > 0) && (bodySonarDistance[1] < WALL_SIDE_ROTATE_THRESHOLD)) || ((bodySonarDistance[0] > 0) && (bodySonarDistance[0] < WALL_SIDE_THRESHOLD))) {
canRotateRight = false;
}
// we have done a pre selection of best possible directions
// the ones we have more space lets do a final choose
if(!emptyIndexIdx) {
//
// he have no unknow distance readings
//
if(!canRotateLeft && !canRotateRight && readPositionDistance[maxDistanceIdx] < WALL_FRONT_ALLEY_THRESHOLD) {
// we are in a alley, and cant rotate.
needToGoBackward = true;
theta = 180;
//selectedIdx = READ_POSITION_MAX;
Serial.println("SELECT GoBackward ");
} else {
if(readPositionDistance[maxDistanceIdx] < WALL_FRONT_ALLEY_THRESHOLD) {
// we are in a alley and can rotate
if(canRotateLeft) {
// rotate 180 degrees left
theta = 180;
Serial.println("SELECT rotate 180");
} else {
// rotate 180 degrees right
theta = -180;
Serial.println("SELECT rotate -180");
}
//selectedIdx = READ_POSITION_MAX;
} else {
// be stupid, select the max distance
theta = readPosition[maxDistanceIdx] - 90;
//selectedIdx = maxDistanceIdx;
Serial.print("SELECT MAX ");
Serial.print(readPosition[maxDistanceIdx]); Serial.print("\t");
//Serial.print(readPositionDistance[maxDistanceIdx]); Serial.print("\t");
Serial.print(theta); Serial.println("\t");
}
}
} else {
// we have unknow distance readings
// must select one from the available
// be stupid, select the first
theta = readPosition[emptyIndex[0]] - 90;
//selectedIdx = emptyIndex[0];
Serial.print("SELECT empty ");
Serial.print(readPosition[emptyIndex[0]]); Serial.print("\t");
//Serial.print(readPositionDistance[0]); Serial.print("\t");
Serial.print(theta); Serial.println("\t");
}
/*
Serial.print("HIT MAP"); Serial.println("\t");
for(j=0; j < 10; j++) {
for(i=0; i < 10; i++) {
Serial.print(mapSummary[j][i].hit); Serial.print("\t");
}
Serial.println();
}
Serial.print("MISS MAP"); Serial.println("\t");
for(j=0; j < 10; j++) {
for(i=0; i < 10; i++) {
Serial.print(mapSummary[j][i].miss); Serial.print("\t");
}
Serial.println();
}
*/
sendRadioServoScanData(readPosition[(int)theta]);
// dismiss reaction to a front obstacle (not check is setted)
frontObstacleDetectedStatus = 0;
// set new commands
if(needToGoBackward) {
prgCmd[0] = {0, 0, 2, 0, 2, -500, 0, 0, 0}; // HARDCODED commands
} else {
prgCmd[0] = {0, 0, 3, 0, 3, theta, 0, 0, 0}; // HARDCODED commands
prgCmd[1] = {0, 0, 2, 0, 2, 500, 0, 0, 0}; // HARDCODED commands
}
}
void processInternalOnlyCommands() {
static boolean cmdStepStatus;
// internal behaviors with slave stopped
if(internalCmdProcessStatus && !slaveCmdWaitingId) { // ADD? && !slaveCmdTerminateId
switch(prgCmd[prgIdx].intCmd) {
//
// servo sweep behavior to get 180 degrees distances readings
//
case 150: // HARDCODED
// start process command 150 = servo scan
if(internalCmdLoopCountNext == 0) {
//Serial.print("Start sweep"); Serial.println("\t");
internalCmdLoopCountNext = internalMoveLoopCounter + 10; // TODO HARDCODED
internalCmdStepCount = 0;
cmdStepStatus = false;
readPositionDistance[0] = bodySonarDistance[1];
readPositionDistance[READ_POSITION_MAX - 1] = bodySonarDistance[0];
// servo
frontServo.attach(frontServoPin);
delay(50);
bodyFrontLaserDistanceUpdate = false;
}
if(internalCmdLoopCountNext == internalMoveLoopCounter) {
readPositionDistance[internalCmdStepCount+1] = getFrontLaserDistance(1); // TODO HARDCODED distance mode
internalCmdLoopCountNext = internalMoveLoopCounter + 10; // TODO HARDCODED
internalCmdStepCount++;
cmdStepStatus = false;
}
if(internalCmdStepCount == prgCmd[prgIdx].units) {
// move servo to front, detach and end
frontServo.write(90); // TODO: HARDCODED
delay(100);
frontServo.detach();
delay(50);
cmdEnd();
cmdStepStatus = false;
bodyFrontLaserDistanceUpdate = true;
bodyMoveDecide();
}
if(!cmdStepStatus) {
// move servo to some angle in position table
delay(100);
frontServo.write(readPosition[internalCmdStepCount]);
delay(200);
cmdStepStatus = true;
}
break;
//
case 251: // HARDCODED
// goto prgIdx command
case 252: // HARDCODED
// pause the program counter (keep executing pause)
case 253: // HARDCODED
// restart the program counter (keep program)
cmdEnd();
break;
}
}
}
void processInternalSlaveCommands() {
static boolean cmdStepStatus;
static unsigned long smallSweepNextTimer = 0;
static boolean cmdTimerStatus = false; // TODO?: rewrite to spare memory
if(internalCmdProcessStatus && slaveCmdWaitingId) {
// TODO: rotate scan
switch(prgCmd[prgIdx].intCmd) {
//
// servo sweep behavior to get 90 degrees wide distances readings
//
case 21: // HARDCODED
if(millis() > smallSweepNextTimer) {
smallSweepNextTimer = millis() + 10000; // TODO HARDCODED
cmdTimerStatus = true;
}
if(cmdTimerStatus) {
// start operation
if(internalCmdLoopCountNext == 0) {
//Serial.println("Start short sweep"); Serial.println("\t");
internalCmdLoopCountNext = internalMoveLoopCounter + 20; // TODO HARDCODED
internalCmdStepCount = 0;
cmdStepStatus = false;
// servo
frontServo.attach(frontServoPin);
delay(5);
bodyFrontLaserDistanceUpdate = false;
}
// do sensor readings
if(internalCmdLoopCountNext == internalMoveLoopCounter) {
readPositionDistance[ 3 + (internalCmdStepCount * 3)] = getFrontLaserDistance(1); // TODO HARDCODED start position, increment, distance mode
// update stall front laser distance
if(internalCmdStepCount == 1) {
bodyFrontLaserDistance = readPositionDistance[ 3 + (internalCmdStepCount * 3)];
}
// update control vars
internalCmdLoopCountNext = internalMoveLoopCounter + 20; // TODO HARDCODED
internalCmdStepCount++;
cmdStepStatus = false;
}
// end operation: move servo to front, detach and end
if(internalCmdStepCount == 3) {
frontServo.write(90); // TODO: HARDCODED
delay(100);
frontServo.detach();
delay(50);
internalCmdLoopCountNext = 0;
cmdTimerStatus = false;
bodyFrontLaserDistanceUpdate = true;
}
// move servo to some angle in position table
if(!cmdStepStatus) {
delay(5);
frontServo.write(readPosition[ 3 + (internalCmdStepCount * 3)]);
delay(50);
cmdStepStatus = true;
}
}
break;
}
}
}
void cmdProcess() {
//
// run on all loops
//
if(!bodyMoveProcessStatus && !internalCmdProcessStatus) {
idleLoopCounter++;
} else {
idleLoopCounter = 0;
}
// we are idle for some time, lets run
if(idleLoopCounter > IDLE_LOOP_COUNTER_MAX) {
// just go front
// TODO: a smarter behavior
prgCmd[0] = {0, commandGroupId, 2, 21, 2, 500, 0, 0, 0}; // HARDCODED commands
}
// process waiting received command
if(receivedCmd.cmd) {
switch(receivedCmd.cmd) {
case 11: //HARDCODED command
switch(prgCmd[prgIdx].extCmd) {
case 2: //HARDCODED command
case 3: //HARDCODED command
// log current command with current bodyMotorLeftStepsToGo
if(prgCmd[prgIdx].extCmd)
logCmd(prgCmd[prgIdx].extCmd, i2c_data[0][3].l);
// create command group
commandGroupId++;
// store addon program on prg temporary positions (last prgCmd positions)
prgCmd[PRG_CMD_MAX - TMP_PRG_OFFSET] = {0, commandGroupId, 11, 0, 1, 0, 0, 0, 0}; // HARDCODED commands
prgCmd[PRG_CMD_MAX - TMP_PRG_OFFSET + 1] = {0, commandGroupId, 0, 252, 0, 0, 0, 0, 0}; // HARDCODED commands
// store distance to go based on current bodyMotorLeftStepsToGo to use later
pausedUnits = stepsToUnits(i2c_data[0][3].l);
pausedCmd = cmd;
// change the program index
prgIdxPaused = prgIdx;
prgIdx = PRG_CMD_MAX - TMP_PRG_OFFSET;
break;
}
break;
case 2: //HARDCODED command
switch(prgCmd[prgIdx].intCmd) {
case 252:
if(prgIdx >= PRG_CMD_MAX - TMP_PRG_OFFSET) {
// executing temporary program
// load the new commands
prgCmd[PRG_CMD_MAX - TMP_PRG_OFFSET] = {0, prgCmd[prgIdxPaused].group, 0, 0, prgCmd[prgIdxPaused].extCmd, pausedUnits, 0, 0, 0};
prgCmd[PRG_CMD_MAX - TMP_PRG_OFFSET + 1] = {0, prgCmd[prgIdxPaused].group, 0, 251, 0, prgIdxPaused + 1, 0, 0, 0}; // HARDCODED commands
// reset tmp storage variables
prgIdxPaused = 0;
pausedUnits = 0;
// change the program index
prgIdx = PRG_CMD_MAX - TMP_PRG_OFFSET;
}
break;
}
break;
}
// reset control vars
receivedCmd.cmd = 0;
// TODO: should only be set if the command its ok
bodyMoveProcessStatus = 0;
}
if(!bodyMoveProcessStatus || frontObstacleDetectedStatus == 1) { // TODO? ADD to if() internalCmdProcessStatus
//
// run on move first loop or on obstacle detected require stop
//
if(frontObstacleDetectedStatus == 1) {
// log current command with current bodyMotorLeftStepsToGo
logCmd(prgCmd[prgIdx].extCmd, i2c_data[0][3].l);
// end sequence
resetPrgCmd();
// create command group
commandGroupId++;
prgCmd[0] = {0, commandGroupId, 150, 0, 1, 0, 0, 0, 0}; // HARDCODED commands
prgCmd[1] = {0, commandGroupId, 0, 0, 2, -50, 0, 0, 0}; // HARDCODED commands
prgCmd[2] = {0, commandGroupId, 0, 150, 0, 11, 0, 0, 0}; // HARDCODED commands 11=frontServoPositions(1-11)
prgIdx = 0;
// start stop command
frontObstacleDetectedStatus = 2;
}
// start a new move
cmdStart();
// get slave acknowledge and update slave data
getSlavesUpdate();
// detect commands received reports by slave
if(i2c_data[0][1].b[0] > 0) { // bodyMoveCmdRecId
// comand received
//Serial.print("DETECT REC"); Serial.println("\t");
slaveCmdReceivedId = i2c_data[0][1].b[0]; // TODO?
}
// detect command end reports by slave
detectSlaveCmdTerminated();
} else {
//
// run on all the others
//
// update slave data
getSlavesUpdate();
// detect command end reports by slave
detectSlaveCmdTerminated();
}
if(bodyMoveProcessStatus) {
odometry();
bodyMoveLoopCounter++;
}
}
void detectSlaveCmdTerminated() {
// detect command end reports by slave
//Serial.print(i2c_data[0][0].b[0]); Serial.println("\t");
switch(i2c_data[0][0].b[0]) { // requestEventInfoType
case 1: // stop // is needed ????
case 2: // move, rotate
//Serial.print("DETECT END"); Serial.println("\t");
if(i2c_data[0][1].b[1] == slaveCmdWaitingId) { // bodyMoveCmdEndId
if(rotateScanStatus) rotateScanStatus = false;
if(frontObstacleDetectedStatus == 2) {
//Serial.print("DETECT END Obstacle 2"); Serial.print("\t");
frontObstacleDetectedStatus = 3;
}
slaveCmdTerminateId = slaveCmdWaitingId;
slaveCmdWaitingId = 0;
// TODO?: if is a move/rotate command
bodyMotorLeftStepsLast = 0;
}
break;
}
// process the slave terminated commands
// TODO: if(slaveCmdTerminateId > 0 || internalCmdTerminateId > 0) {
if(slaveCmdTerminateId > 0) {
//Serial.print("END slave"); Serial.print("\t");
//Serial.print(slaveCmdTerminateId); Serial.println("\t");
cmdEnd();
slaveCmdTerminateId = 0;
}
}
void cmdStart() {
/*
Serial.print("START new command?"); Serial.print("\t");
Serial.print(prgIdx); Serial.print("\t");
Serial.println("\t");
*/
if(prgCmd[prgIdx].cmd > 0) {
// there is a external command?
if(prgCmd[prgIdx].extCmd > 0) {
//Serial.print("Primary slave"); Serial.println("\t");
slaveCmdRequestId++;
prgCmd[prgIdx].extId = slaveCmdRequestId;
sendSlaveMoveCmd(prgCmd[prgIdx].extId, prgCmd[prgIdx].extCmd, prgCmd[prgIdx].units, prgCmd[prgIdx].minSpeed, prgCmd[prgIdx].acceleration, prgCmd[prgIdx].maxSpeed);
bodyMoveProcessStatus = 1;
}
if(!internalCmdProcessStatus) {
//Serial.print("!internalCmdProcessStatus"); Serial.println("\t");
if(prgCmd[prgIdx].intCmd > 0) {
//Serial.print("Primary internal"); Serial.println("\t");
// update command general control vars
cmd = prgCmd[prgIdx].cmd;
currentCommandGroupId = prgCmd[prgIdx].group;
// set specific command vars
switch(prgCmd[prgIdx].cmd) {
case 100: // scan rotate
rotateScanRequest = true;
rotateScanStatus = false;
}
// reset some vars
internalCmdStepCount = 0;
internalCmdLoopCountNext = 0;
internalCmdProcessStatus = true;
}
}
} else {
// secondary group command
//Serial.print("New secundary command"); Serial.println("\t");
if(!internalCmdProcessStatus) {
//Serial.print("!internalCmdProcessStatus"); Serial.println("\t");
if(prgCmd[prgIdx].extCmd > 0) {
//Serial.print("Secondary slave"); Serial.println("\t");
slaveCmdRequestId++;
prgCmd[prgIdx].extId = slaveCmdRequestId;
sendSlaveMoveCmd(prgCmd[prgIdx].extId, prgCmd[prgIdx].extCmd, prgCmd[prgIdx].units, prgCmd[prgIdx].minSpeed, prgCmd[prgIdx].acceleration, prgCmd[prgIdx].maxSpeed);
// TODO?: where or bellow outside the if();
bodyMoveProcessStatus = 1;
}
if(prgCmd[prgIdx].intCmd > 0) {
//Serial.print("Secundary internal"); Serial.println("\t");
// set specific command vars
/* TODO: is required?
switch(prgCmd[prgIdx].cmd) {
case 100: // scan rotate
rotateScanRequest = true;
rotateScanStatus = false;
}
*/
// reset some vars
// reset some vars
internalCmdStepCount = 0;
internalCmdLoopCountNext = 0;
internalCmdProcessStatus = true;
}
}
}
}
void cmdEnd() {
boolean doPrgIdxUpdate = true;
// TODO: wait for last wheel to stop (to get current total pulses)
logCmd(prgCmd[prgIdx].extCmd, prgCmd[prgIdx].units);
// check what is the current internal command
switch(prgCmd[prgIdx].intCmd) {
case 251: // HARDCODED goto prgIdx command
/*
int f;
for(f=0; f<12; f++) {
Serial.print(prgCmd[f].cmd); Serial.print(" "); Serial.print(prgCmd[f].intCmd); Serial.print(" "); Serial.print(prgCmd[f].extCmd); Serial.print(" "); Serial.println(prgCmd[f].units);
}
*/
//Serial.print("GOTO "); Serial.println(prgCmd[prgIdx].units);
doPrgIdxUpdate = false;
// set next prgIdx
prgIdx = prgCmd[prgIdx].units;
// restore vars
currentCommandGroupId = prgCmd[prgIdx].group;
cmd = pausedCmd;
pausedCmd = 0;
// clean tmp prgCmd positions
resetTmpPrgCmd();
break;
case 252: // HARDCODED suspend program internal command
// pause advance in program index
delay(250);
// delay a bit and just keep execute pause
doPrgIdxUpdate = false;
//Serial.println("PAUSE");
break;
case 253: // HARDCODED restart program internal command
// restart command sequence (keep old commands)
doPrgIdxUpdate = false;
prgIdx = 0;
cmd = 0;
currentCommandGroupId = 0;
//Serial.println("RESTART");
break;
}
if(doPrgIdxUpdate) {
// advance command sequence
prgIdx++;
if((prgIdx == PRG_CMD_MAX) || (!prgCmd[prgIdx].intCmd && !prgCmd[prgIdx].extCmd)) {
// reset command sequence (clean old commands)
resetPrgCmd();
//Serial.println("RESET");
}
}
// allow new command processing
bodyMoveProcessStatus = 0;
bodyMoveLoopCounter = 0;
internalMoveLoopCounter = 0;
internalCmdProcessStatus = false;
}
void logCmd(byte cmd, unsigned int units) {
// temporary log
// TODO: save log somehere permanentely
// is stoped we have time to store more data in sdcard
if(cmd > 0) {
if(bodyMoveNextLog == BODY_MOVE_LOG_MAX) bodyMoveNextLog = 0;
bodyMoveLog[bodyMoveNextLog].cmd = cmd;
// TODO: assure that we receive the real units, not the planed
bodyMoveLog[bodyMoveNextLog].units = units;
bodyMoveNextLog++;
}
}
void resetPrgCmd() {
int i;
// clean sequence
for(i=0; i < PRG_CMD_MAX; i++){
prgCmd[i].cmd = 0;
prgCmd[i].intCmd = 0;
prgCmd[i].extCmd = 0;
// TODO: should I clean all the itens?
}
// reset control vars
prgIdx = 0;
//internalCmdProcessStatus = 0;
cmd = 0;
currentCommandGroupId = 0;
}
void resetTmpPrgCmd() {
int i;
// clean sequence
for(i=PRG_CMD_MAX - TMP_PRG_OFFSET; i < PRG_CMD_MAX; i++){
prgCmd[i].cmd = 0;
prgCmd[i].intCmd = 0;
prgCmd[i].extCmd = 0;
//prgCmd[i].units = 0;
// TODO: should I clean all the itens?
}
}
void odometry() {
signed long distanceSteps;
float distanceUnits;
signed long leftStepsTarget;
boolean targetPositive = false;
boolean toGoPositive = false;
// if bodyMoveProcessStatus
if(i2c_data[0][0].b[1] == 1) {
leftStepsTarget = (signed long)i2c_data[0][2].l; // bodyMotorStepsTarget
bodyMotorLeftStepsToGo = (signed long)i2c_data[0][3].l; // bodyMotorLeftStepsToGo
/*
Serial.print(leftStepsTarget); Serial.print("\t");
Serial.print(bodyMotorLeftStepsToGo); Serial.print("\t");
*/
if(leftStepsTarget > 0) targetPositive = true;
if(bodyMotorLeftStepsToGo > 0) toGoPositive = true;
if(targetPositive) {
if(toGoPositive) {
bodyMotorLeftSteps = leftStepsTarget - bodyMotorLeftStepsToGo;
} else {
bodyMotorLeftSteps = leftStepsTarget - abs(bodyMotorLeftStepsToGo);
}
} else {
if(toGoPositive) {
bodyMotorLeftSteps = -(abs(leftStepsTarget) - bodyMotorLeftStepsToGo);
} else {
bodyMotorLeftSteps = -(abs(leftStepsTarget) - abs(bodyMotorLeftStepsToGo));
}
}
distanceSteps = bodyMotorLeftSteps - bodyMotorLeftStepsLast;
/*
Serial.print(bodyMotorLeftStepsLast); Serial.print("\t");
Serial.print(bodyMotorLeftSteps); Serial.print("\t");
Serial.print(distanceSteps); Serial.print("\t");
*/
bodyMotorLeftStepsLast = bodyMotorLeftSteps;
distanceUnits = stepsToUnits(distanceSteps);
//Serial.print(distanceUnits); Serial.print("\t");
switch(i2c_data[0][0].b[2]) { // bodyMoveCmd.cmd
case 2:
bodyPosX = bodyPosX + distanceUnits * cos(bodyThetaRadians);
bodyPosY = bodyPosY + distanceUnits * sin(bodyThetaRadians);
break;
case 3:
bodyThetaRadians -= (float)(distanceUnits / (BODY_WHEEL_DISTANCE / 2));
if(bodyThetaRadians > PI2)
bodyThetaRadians -= PI2;
else if(bodyThetaRadians < -PI2)
bodyThetaRadians += PI2;
break;
}
/*
Serial.print(bodyThetaRadians * 180 / PI, 1); Serial.print("\t");
Serial.print(bodyPosX, 1); Serial.print("\t");
Serial.print(bodyPosY, 1); Serial.print("\t");
*/
//Serial.print(bodyThetaRadians); Serial.print("\t");
//Serial.println("\t");
}
}
/*
long unitsToSteps(float distance) {
return (signed long)(distance / bodyMotorStepsPerUnit);
}
*/
float stepsToUnits(signed long steps) {
return ((float)(steps * bodyMotorStepsPerUnit));
}
// EOF CINEMATIC CONTROL
//
//
// BOF SENSORS
//
//
void readSensors() {
// BOF: BATTERY VOLTAGE
batteryRead = analogRead(A3) * (BATERY_VOLTAGE / BATERY_MAX_OUT); //convert the value to a true voltage
batteryVoltage = batteryKalman.getFilteredValue(batteryRead); // kalman
// EOF: BATTERY VOLTAGE
// side distance update (one side per loop)
// data stored on bodySonarDistance[]
readSonarDistance();
// front distance update
// data stored on bodyFrontLaserDistance
readFrontLaserDistance();
}
// BOF: SparkFun VL53L1X 4M Laser Distance Sensor
int getFrontLaserDistance(byte mode) {
int r;
switch(mode) {
case 0: // HARDCODED
r = bodyFrontLaserRead;
break;
case 1: // HARDCODED
r = bodyFrontLaserDistanceQuick;
break;
case 2: // HARDCODED
default:
r = bodyFrontLaserDistance;
break;
}
return r;
}
void readFrontLaserDistance() {
// global: int bodyFrontLaserRead
// global: kalman bodyFrontLaserKalman
// global: int bodyFrontLaserDistance
// global: int bodyFrontLaserDistanceQuick
if(distanceSensor.newDataReady()) {
bodyFrontLaserRead = distanceSensor.getDistance();
// assure that return no more than max distance
if(bodyFrontLaserRead > FRONT_LASER_MAX_DISTANCE) {
bodyFrontLaserRead = 0;
bodyFrontLaserDistanceQuick = 0;
bodyFrontLaserDistance = 0;
} else {
// try to avoid front distance read polution
if(bodyFrontLaserDistanceUpdate) {
bodyFrontLaserDistance = bodyFrontLaserKalman.getFilteredValue(bodyFrontLaserRead); // kalman
bodyFrontLaserDistanceQuick = bodyFrontLaserDistance;
} else {
bodyFrontLaserDistanceQuick = bodyFrontLaserKalman.getFilteredValue(bodyFrontLaserRead); // kalman
}
/*
switch(prgCmd[prgIdx].intCmd) {
case 21: // HARDCODED
case 150: // HARDCODED
// front servo scan
// does not update all
if(bodyFrontLaserDistanceUpdate) {
bodyFrontLaserDistanceQuick = bodyFrontLaserKalman.getFilteredValue(bodyFrontLaserRead); // kalman
}
break;
default:
bodyFrontLaserDistance = bodyFrontLaserKalman.getFilteredValue(bodyFrontLaserRead); // kalman
bodyFrontLaserDistanceQuick = bodyFrontLaserDistance;
break;
}
*/
}
}
}
// EOF: SparkFun VL53L1X 4M Laser Distance Sensor
// BOF HC-SR04
void readSonarDistance() {
// global: kalman bodySonarKalman10, bodySonarKalman1
// global: int bodySonarRead[], bodySonarDistance[]
static byte bodySonarIndex = 0;
bodySonarRead[bodySonarIndex] = bodySonar[bodySonarIndex].ping_cm() * 10; // scaled to mm
//bodySonarRead[bodySonarIndex] = bodySonar[bodySonarIndex].ping_cm() * 10 + SIDE_SONAR_SIDE_OFFSET;
if(bodySonarRead[bodySonarIndex] == 0) {
bodySonarRead[bodySonarIndex] = SONAR_MAX_DISTANCE;
bodySonarDistance[bodySonarIndex] = 0;
}
switch(bodySonarIndex) {
case 0:
bodySonarDistance[bodySonarIndex] = bodySonarKalman0.getFilteredValue(bodySonarRead[bodySonarIndex]); // kalman
break;
case 1:
bodySonarDistance[bodySonarIndex] = bodySonarKalman1.getFilteredValue(bodySonarRead[bodySonarIndex]); // kalman
break;
}
bodySonarIndex++;
if(bodySonarIndex == SONAR_NUM) bodySonarIndex = 0;
}
// EOF HC-SR04
// EOF SENSORS
//
//
// BOF: I2C arduino communication
// I2C limit write to 32 bytes
//
//
void getSlavesUpdate() {
// global union u_tag i2c_data[][];
byte i2c_byte[32];
int i;
// master reader (first loop data request)
for(i=0; i < 32; i++) {
i2c_byte[i] = 0;
}
i = 0;
Wire.requestFrom((int)i2cSlave[0], 32); // request 6 bytes from slave device #8
while (Wire.available()) { // slave may send less than requested
char c = Wire.read(); // receive a byte as character
i2c_byte[i] = c;
i++;
}
for(i=0; i < 8; i++) {
i2c_data[0][i].b[0] = i2c_byte[i*4 + 0];
i2c_data[0][i].b[1] = i2c_byte[i*4 + 1];
i2c_data[0][i].b[2] = i2c_byte[i*4 + 2];
i2c_data[0][i].b[3] = i2c_byte[i*4 + 3];
}
}
void sendSlaveMoveCmd(byte id, byte cmd, int units, int minSpeed, int acceleration, int maxSpeed) {
// global: i2cSlave[0]
Wire.beginTransmission(i2cSlave[0]);
Wire.write(cmd); // cmd: rotate
Wire.write(id); // cmdId
Wire.write((byte*)&units, 2); // units, deg
Wire.write((byte*)&minSpeed, 2); // minSpeed
Wire.write((byte*)&acceleration, 2); // aceleration
Wire.write((byte*)&maxSpeed, 2); // maxSpeed
Wire.endTransmission();
slaveCmdWaitingId = id;
/*
Serial.print("sendSlaveMoveCmd"); Serial.print("\t");
Serial.print(cmd); Serial.print("\t");
Serial.print(id); Serial.print("\t");
Serial.print(units); Serial.print("\t");
Serial.print("-"); Serial.print("\t");
Serial.print(prgCmd[prgIdx].extId); Serial.print("\t");
Serial.print(prgCmd[prgIdx].group); Serial.print("\t");
Serial.print(prgCmd[prgIdx].cmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx].extCmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx].intCmd); Serial.println("\t");
*/
}
// EOF: I2C arduino communication
// BOF: nRF24L01
void sendRadioServoScanData(byte decision) {
rfSendData[0].b[0] = 1;
rfSendData[0].b[1] = decision;
rfSendData[0].b[2] = 0;
rfSendData[0].b[3] = 0;
rfSendData[1].i[0] = readPositionDistance[0];
rfSendData[1].i[1] = readPositionDistance[1];
rfSendData[2].i[0] = readPositionDistance[2];
rfSendData[2].i[1] = readPositionDistance[3];
rfSendData[3].i[0] = readPositionDistance[4];
rfSendData[3].i[1] = readPositionDistance[5];
rfSendData[4].i[0] = readPositionDistance[6];
rfSendData[4].i[1] = readPositionDistance[7];
rfSendData[5].i[0] = readPositionDistance[8];
rfSendData[5].i[1] = readPositionDistance[9];
rfSendData[6].i[0] = readPositionDistance[10];
rfSendData[6].i[1] = readPositionDistance[11];
rfSendData[7].i[0] = readPositionDistance[12];
rfSendData[7].i[1] = 0;
sendRadio();
}
void sendRadioLoopData() {
rfSendData[0].b[0] = 0;
rfSendData[0].b[1] = loopTime;
rfSendData[0].b[2] = prgIdx;
rfSendData[0].b[3] = cmd;
rfSendData[1].b[0] = prgCmd[prgIdx].intCmd;
rfSendData[1].b[1] = internalCmdProcessStatus;
rfSendData[1].b[2] = prgCmd[prgIdx].extCmd;
rfSendData[1].b[3] = bodyMoveProcessStatus;
rfSendData[2].i[0] = bodyFrontLaserRead;
rfSendData[2].i[1] = bodyFrontLaserDistance;
rfSendData[3].i[0] = bodySonarDistance[0];
rfSendData[3].i[1] = bodySonarDistance[1];
rfSendData[4].d = bodyThetaRadians * 180 / PI;
rfSendData[5].d = bodyPosX;
rfSendData[6].d = bodyPosY;
rfSendData[7].i[0] = (int)prgCmd[prgIdx].units;
rfSendData[7].i[1] = (int)stepsToUnits(bodyMotorLeftSteps);
sendRadio();
}
void sendRadio() {
bool rslt;
rslt = radio.write(rfSendData, 32);
if (rslt) {
if ( radio.isAckPayloadAvailable() ) {
radio.read(&rfAckData, sizeof(rfAckData));
rfSendDataStatus = 2;
} else {
rfSendDataStatus = 1;
}
} else {
rfSendDataStatus = 1;
}
}
// EOF: nRF24L01
void distanceSensorsAdaptativePeriod( byte n ) {
int i;
for(i=0; i < n; i++) {
delay(30);
readFrontLaserDistance();
readSonarDistance();
/*
Serial.print(bodyFrontLaserDistance); Serial.print("\t");
Serial.print(bodySonarDistance[0]); Serial.print("\t");
Serial.print(bodySonarDistance[1]); Serial.println("\t");
*/
}
}
void sendSerialLoopData() {
int i;
Serial.print(loopTime); Serial.print("\t");
/*
Serial.print(rfSendDataStatus); Serial.print("\t");
Serial.print(rfAckData[0]); Serial.print("\t");
Serial.print(rfAckData[1]); Serial.print("\t");
Serial.print(rfAckData[2]); Serial.print("\t");
Serial.print(rfAckData[3]); Serial.print("\t");
*/
Serial.print(bodyFrontLaserRead); Serial.print("\t");
Serial.print(bodyFrontLaserDistance); Serial.print("\t");
//Serial.print(bodyFrontLaserDistanceQuick); Serial.print("\t");
for(i=0; i < SONAR_NUM; i++) {
//Serial.print(bodySonarRead[i]); Serial.print("\t");
Serial.print(bodySonarDistance[i]); Serial.print("\t");
}
Serial.print(bodyThetaRadians * 180 / PI, 1); Serial.print("\t");
Serial.print(bodyPosX); Serial.print("\t");
Serial.print(bodyPosY); Serial.print("\t");
Serial.print(prgIdx); Serial.print("\t");
Serial.print(cmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx].intCmd); Serial.print("\t");
Serial.print(internalCmdProcessStatus); Serial.print("\t");
Serial.print(prgCmd[prgIdx].extCmd); Serial.print("\t");
Serial.print(bodyMoveProcessStatus); Serial.print("\t");
Serial.print(slaveCmdRequestId); Serial.print("\t");
Serial.print(slaveCmdReceivedId); Serial.print("\t");
Serial.print(slaveCmdWaitingId); Serial.print("\t");
Serial.print(slaveCmdTerminateId); Serial.print("\t");
Serial.print(bodyMoveLoopCounter); Serial.print("\t");
Serial.print(internalCmdLoopCountNext); Serial.print("\t");
Serial.print(internalCmdStepCount); Serial.print("\t");
//Serial.print(batteryRead); Serial.print("\t");
Serial.print(batteryVoltage); Serial.print("\t");
Serial.print(i2c_data[0][0].b[0]); Serial.print("\t"); // requestEventInfoType
Serial.print(i2c_data[0][0].b[1]); Serial.print("\t"); // bodyMoveProcessStatus
Serial.print(i2c_data[0][0].b[2]); Serial.print("\t"); // bodyMoveCmd.cmd
Serial.print(i2c_data[0][0].b[3]); Serial.print("\t"); // bodyMoveCmd.extId
Serial.print(i2c_data[0][1].b[0]); Serial.print("\t"); // bodyMoveCmdRecId
Serial.print(i2c_data[0][1].b[1]); Serial.print("\t"); // bodyMoveCmdEndId
//Serial.print(i2c_data[0][1].b[2]); Serial.print("\t"); // loopTime
Serial.print(i2c_data[0][1].b[3]); Serial.print("\t"); // loopTime
Serial.print((signed long)i2c_data[0][2].l); Serial.print("\t"); // bodyMotorStepsTarget
Serial.print((signed long)i2c_data[0][3].l); Serial.print("\t"); // bodyMotorLeftStepsToGo
Serial.print(i2c_data[0][4].d); Serial.print("\t"); // bodyMotorLeftSpeed
//Serial.print((signed long)i2c_data[0][5].l); Serial.print("\t"); // bodyMotorRightStepsToGo
//Serial.print(i2c_data[0][6].d); Serial.print("\t"); // bodyMotorRightSpeed
//Serial.print(i2c_data[0][7].l); Serial.print("\t"); // bodyMoveLoopCounter
Serial.println("\t");
}
/*
float getPointXpos(int d, float a) {
float b = bodyThetaRadians + (float)(a / 180.0) * PI;
return d * cos(b);
}
float getPointYpos(int d, float a) {
float b = bodyThetaRadians + (float)(a / 180.0) * PI;
return d * sin(b);
}
*/