master-sensor-mod2-v11
/*
*
* 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 SIDE_OFFSET 60
#define FRONT_OFFSET 130
//#define REAR_OFFSET 60
#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_LEFT_THRESHOLD 100
#define WALL_RIGHT_THRESHOLD 100
#define WALL_FRONT_THRESHOLD 200
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] = {-1, -1, -1, -1, -1, -1, -1, -1}; // 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 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, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
NewPing(7, 7, 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;
int bodyFrontLaserDistanceQuick;
int bodySonarDistance[SONAR_NUM];
float batteryVoltage;
Kalman batteryKalman(0.125,32,1023,0);
Kalman bodyFrontLaserKalman(0.125,32,1023,1000);
Kalman bodyFrontLaserKalmanQuick(0.125,0.25,1023,1000);
Kalman bodySonarKalman0(0.125,8,1023,100);
Kalman bodySonarKalman1(0.125,8,1023,100);
// EOF: KALMAN
#define LED_PIN 8
bool internalLedBlinkState = false;
/*
// BOF: PATH TRACK
struct cell {
unsigned char x;
unsigned char y;
unsigned char theta;
unsigned char wall;
uint8_t weight;
};
struct cell current_cell;
struct cell track[156];
// EOF: PATH TRACK
*/
#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 BODY_MOVE_CMDS_MAX 7
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;
} bodyMoveCmdType;
/* bodyMoveCmdType prgCmd[BODY_MOVE_CMDS_MAX] = {
{0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}
};*/
/*
bodyMoveCmdType prgCmd[BODY_MOVE_CMDS_MAX] = {
{0, 0, 100, 100, 3, -360, 100, 50, 1000}, {0, 0, 2, 0, 2, 250, 250, 50, 1500}, {0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 0}
};
*/
/*
bodyMoveCmdType prgCmd[BODY_MOVE_CMDS_MAX] = {
{0, 0, 2, 0, 2, 150, 250, 50, 1500}, {0, 0, 3, 0, 3, 30, 250, 50, 1500}, {0, 0, 2, 0, 2, 100, 250, 50, 1500},
{0, 0, 2, 0, 2, -100, 250, 50, 1500}, {0, 0, 3, 0, 3, -30, 250, 50, 1500}, {0, 0, 2, 0, 2, -150, 250, 50, 1500}, {0, 0, 253, 253, 0, 0, 0, 0, 0}
};
bodyMoveCmdType prgCmd[BODY_MOVE_CMDS_MAX] = {
{0, 0, 2, 0, 2, 250, 250, 50, 1500}, {0, 0, 253, 253, 0, 0, 0, 0, 0}
};
*/
bodyMoveCmdType prgCmd[BODY_MOVE_CMDS_MAX] = {
{0, 0, 2, 0, 2, 250, 250, 50, 1500}, {0, 0, 3, 0, 3, 30, 250, 50, 1500}, {0, 0, 2, 0, 2, 100, 250, 50, 1500},
{0, 0, 2, 0, 2, -100, 250, 50, 1500}, {0, 0, 3, 0, 3, -30, 250, 50, 1500}, {0, 0, 2, 0, 2, -250, 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 bodyMoveCommandCurrentGroup = 0;
byte bodyMoveCommandGroup = 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
} bodyMoveLogType;
bodyMoveLogType 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];
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_LEFT_THRESHOLD) || (bodySonarDistance[1] - SIDE_OFFSET > WALL_RIGHT_THRESHOLD)
) {
getSonarDistance();
getFrontLaserDistance();
// 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;
bodyMoveProcess();
processInternalOnlyCommands();
processInternalSlaveCommands();
readSensors();
// detect obstacles moving front
if(prgCmd[prgIdx].extCmd == 2 && !frontObstacleDetectedStatus) {
if(bodyFrontLaserDistance - FRONT_OFFSET < WALL_FRONT_THRESHOLD) {
frontObstacleDetectedStatus = 1;
//Serial.print("SENDSTOP"); Serial.println("\t");
}
}
//
//
//
//currentMillis = millis();
//if (currentMillis - prevMillis >= txIntervalMillis) {
sendRadioLoopData();
//}
//showData();
//
//
//
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 thetaIndex = 0;
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("");
if(readPositionDistance[i] > maxDistance) {
maxDistance = readPositionDistance[i];
thetaIndex = i;
}
}
Serial.print(readPosition[thetaIndex]); Serial.println("\t");
////Serial.print(readPositionDistance[thetaIndex]); Serial.println("\t");
//theta = 90 - readPosition[thetaIndex];
theta = readPosition[thetaIndex] - 90;
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();
}
// dismiss reaction to a front obstacle (not check is setted)
frontObstacleDetectedStatus = 0;
// set new commands
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 bodyMoveProcess() {
static unsigned long odometryTimeStart = millis() + 250;
// do in every loop
bodyMoveCheck();
if(bodyMoveProcessStatus) {
// do only during move commands
// TODO: need to handle moviment interruptions
//if(millis() >= odometryTimeStart) {
//odometryTimeStart = millis() + 50;
odometry();
//}
bodyMoveLoopCounter++;
}
}
void processInternalOnlyCommands() {
static boolean internalCmdStepStatus;
// 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;
internalCmdStepStatus = false;
readPositionDistance[0] = bodySonarDistance[1];
readPositionDistance[READ_POSITION_MAX - 1] = bodySonarDistance[0];
// servo
frontServo.attach(frontServoPin);
delay(50);
}
if(internalCmdLoopCountNext == internalMoveLoopCounter) {
readPositionDistance[internalCmdStepCount+1] = bodyFrontLaserDistanceQuick;
internalCmdLoopCountNext = internalMoveLoopCounter + 10; // TODO HARDCODED
internalCmdStepCount++;
internalCmdStepStatus = false;
}
if(internalCmdStepCount == prgCmd[prgIdx].units) {
// move servo to front, detach and end
frontServo.write(90); // TODO: HARDCODED
delay(100);
frontServo.detach();
delay(50);
bodyMoveEnd();
internalCmdStepStatus = false;
bodyMoveDecide();
}
if(!internalCmdStepStatus) {
// move servo to some angle in position table
delay(100);
frontServo.write(readPosition[internalCmdStepCount]);
delay(200);
internalCmdStepStatus = true;
}
break;
//
// restart the program counter (keep program)
//
case 253: // HARDCODED
bodyMoveEnd();
break;
}
}
}
void processInternalSlaveCommands() {
// TODO: internal behaviors with slave active
if(internalCmdProcessStatus && slaveCmdWaitingId) {
// TODO: rotate scan
}
}
void bodyMoveCheck() {
//
// run on all loops
//
if(!bodyMoveProcessStatus && !internalCmdProcessStatus) {
idleLoopCounter++;
} else {
idleLoopCounter = 0;
}
// we are idle for some time, lets run
if(idleLoopCounter > 5) { // HARDCODED
// just go front
// TODO: a smarter behavior
prgCmd[0] = {0, bodyMoveCommandGroup, 0, 0, 2, 500, 0, 0, 0}; // HARDCODED commands
}
if(!bodyMoveProcessStatus || frontObstacleDetectedStatus == 1) { // TODO? ADD to if() internalCmdProcessStatus
//
// run on move first loop or on obstacle detected require stop
//
if(frontObstacleDetectedStatus == 1) {
//Serial.print("Obstacle 1"); Serial.println("\t");
//DEBUG store old command data
//tmpBodyMotorStepsTarget = i2c_data[0][2].l; // bodyMotorStepsTarget
//tmpBodyMotorLeftStepsToGo = i2c_data[0][3].l; // bodyMotorLeftStepsToGo
// log current command with current bodyMotorLeftStepsToGo
logCmd(prgCmd[prgIdx].extCmd, i2c_data[0][3].l);
// end sequence
resetPrgCmd();
// create command group
bodyMoveCommandGroup++;
prgCmd[0] = {0, bodyMoveCommandGroup, 150, 0, 1, 0, 0, 0, 0}; // HARDCODED commands
prgCmd[1] = {0, bodyMoveCommandGroup, 0, 0, 2, -50, 0, 0, 0}; // HARDCODED commands
prgCmd[2] = {0, bodyMoveCommandGroup, 0, 150, 0, 11, 0, 0, 0}; // HARDCODED commands 11=frontServoPositions(1-11)
prgIdx = 0;
// start stop command
frontObstacleDetectedStatus = 2;
}
// start a new move
bodyMoveStart();
// 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();
}
}
void detectSlaveCmdTerminated() {
// detect command end reports by slave
switch(i2c_data[0][0].b[0]) { // requestEventInfoType
case 1: // stop // is needed ????
case 2: // move, rotate
//Serial.print("DETECT END"); Serial.println("\t");
//Serial.print(i2c_data[0][0].b[0]); 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");
//Serial.print(slaveCmdWaitingId); Serial.println("\t");
frontObstacleDetectedStatus = 3;
}
slaveCmdTerminateId = slaveCmdWaitingId;
slaveCmdWaitingId = 0;
// TODO?: if is a move/rotate command
//Serial.print("RESET bodyMotorLeftStepsLast"); Serial.println("\t");
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");
bodyMoveEnd();
slaveCmdTerminateId = 0;
}
}
void bodyMoveStart() {
/*
Serial.print("START new command?"); Serial.print("\t");
Serial.print(prgIdx); 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].intCmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx].extCmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx].units); Serial.print("\t");
Serial.print(internalCmdProcessStatus); Serial.print("\t");
Serial.println("\t");
*/
if(prgCmd[prgIdx].cmd > 0) {
//Serial.print("New primary command"); Serial.println("\t");
// 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;
bodyMoveCommandCurrentGroup = 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 bodyMoveEnd() {
// TODO: wait for last wheel to stop (to get current total pulses)
logCmd(prgCmd[prgIdx].extCmd, prgCmd[prgIdx].units);
/*
Serial.print("END MOVE"); Serial.println("\t");
Serial.print("current"); 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].intCmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx].extCmd); Serial.println("\t");
//Serial.print(prgCmd[prgIdx].units); Serial.println("\t");
Serial.print("next"); Serial.print("\t");
Serial.print(prgCmd[prgIdx + 1].extId); Serial.print("\t");
Serial.print(prgCmd[prgIdx+1].group); Serial.print("\t");
Serial.print(prgCmd[prgIdx+1].cmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx+1].intCmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx+1].extCmd); Serial.println("\t");
//Serial.print(prgCmd[prgIdx+1].units); Serial.println("\t");
*/
// check if a restart program command
if(prgCmd[prgIdx].intCmd == 253) { // HARDCODED restart program internal command
//Serial.println("RESTART");
// restart command sequence (keep old commands)
prgIdx = 0;
cmd = 0;
bodyMoveCommandCurrentGroup = 0;
} else {
// advance command sequence
prgIdx++;
if((prgIdx == BODY_MOVE_CMDS_MAX) || (!prgCmd[prgIdx].intCmd && !prgCmd[prgIdx].extCmd)) {
// reset command sequence (clean old commands)
//Serial.println("RESET");
resetPrgCmd();
}
}
// 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 < BODY_MOVE_CMDS_MAX; i++){
prgCmd[i].cmd = 0;
prgCmd[i].intCmd = 0;
prgCmd[i].extCmd = 0;
// TODO: should I clean all the itens?
}
// DEBUG
//for(i = 0; i < BODY_MOVE_LOG_MAX; i++) {
// Serial.print(bodyMoveLog[i].cmd); Serial.print("\t");
// Serial.print(bodyMoveLog[i].units); Serial.println("\t");
//}
// reset control vars
prgIdx = 0;
//internalCmdProcessStatus = 0;
cmd = 0;
bodyMoveCommandCurrentGroup = 0;
}
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
//leftStepsTarget = abs((signed long)i2c_data[0][2].l); // bodyMotorStepsTarget
bodyMotorLeftStepsToGo = (signed long)i2c_data[0][3].l; // bodyMotorLeftStepsToGo
//bodyMotorLeftStepsToGo = abs((signed long)i2c_data[0][3].l); // bodyMotorLeftStepsToGo
if(leftStepsTarget > 0) targetPositive = true;
if(bodyMotorLeftStepsToGo > 0) toGoPositive = true;
if(targetPositive && !toGoPositive) {
bodyMotorLeftSteps = leftStepsTarget - abs(bodyMotorLeftStepsToGo);
} else {
bodyMotorLeftSteps = leftStepsTarget - bodyMotorLeftStepsToGo;
}
//bodyMotorLeftSteps = leftStepsTarget - bodyMotorLeftStepsToGo;
//Serial.print(bodyMotorLeftStepsLast); Serial.print("\t");
////distanceSteps = bodyMotorStepsPerUnit * (bodyMotorLeftSteps - bodyMotorLeftStepsLast);
distanceSteps = bodyMotorLeftSteps - bodyMotorLeftStepsLast;
bodyMotorLeftStepsLast = bodyMotorLeftSteps;
distanceUnits = stepsToUnits(distanceSteps);
/*
Serial.print(leftStepsTarget); Serial.print("\t");
Serial.print(bodyMotorLeftStepsToGo); Serial.print("\t");
Serial.print(bodyMotorLeftSteps); Serial.print("\t");
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));
//Serial.print(bodyThetaRadians); Serial.print("\t");
if(bodyThetaRadians > PI2)
bodyThetaRadians -= PI2;
else if(bodyThetaRadians < -PI2)
bodyThetaRadians += PI2;
break;
}
//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[]
getSonarDistance();
// front distance update
// data stored on bodyFrontLaserDistance
getFrontLaserDistance();
}
// BOF: SparkFun VL53L1X 4M Laser Distance Sensor
void getFrontLaserDistance() {
// global: int bodyFrontLaserRead
// global: kalman bodyFrontLaserKalman
// global: int bodyFrontLaserDistance
// global: int bodyFrontLaserDistanceQuick
if(distanceSensor.newDataReady()) {
//bodyFrontLaserRead = distanceSensor.getDistance();
bodyFrontLaserRead = distanceSensor.getDistance() + FRONT_LASER_FRONT_OFFSET;
bodyFrontLaserDistance = bodyFrontLaserKalman.getFilteredValue(bodyFrontLaserRead); // kalman
bodyFrontLaserDistanceQuick = bodyFrontLaserKalmanQuick.getFilteredValue(bodyFrontLaserRead); // kalman
}
}
// EOF: SparkFun VL53L1X 4M Laser Distance Sensor
// BOF HC-SR04
void getSonarDistance() {
// global: kalman bodySonarKalman10, bodySonarKalman1
// global: int bodySonarRead[], bodySonarDistance[]
static byte bodySonarIndex = 0;
bodySonarRead[bodySonarIndex] = bodySonar[bodySonarIndex].ping_cm() * 10 + SIDE_SONAR_SIDE_OFFSET;
if(bodySonarRead[bodySonarIndex] == 0) {
bodySonarRead[bodySonarIndex] = 200;
}
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 sendRadioLoopData() {
rfSendData[0].b[0] = prgIdx;
rfSendData[0].b[1] = cmd;
rfSendData[0].b[2] = prgCmd[prgIdx].intCmd;
rfSendData[0].b[3] = internalCmdProcessStatus;
rfSendData[1].b[0] = prgCmd[prgIdx].extCmd;
rfSendData[1].b[1] = bodyMoveProcessStatus;
rfSendData[1].b[2] = slaveCmdRequestId;
rfSendData[1].b[3] = slaveCmdWaitingId;
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].b[0] = loopTime;
sendRadio();
}
void sendRadio() {
//void sendRadio(byte data[]) {
bool rslt;
/*
Serial.print(radio.getPayloadSize()); Serial.print("\t");
Serial.print(rfSendData[0].b[0]); Serial.print("\t");
Serial.print(rfSendData[0].b[1]); Serial.print("\t");
Serial.print(rfSendData[0].b[2]); Serial.print("\t");
Serial.print(rfSendData[0].b[3]); Serial.print("\t");
Serial.print(rfSendData[1].b[0]); Serial.print("\t");
Serial.print(rfSendData[1].b[1]); Serial.print("\t");
Serial.print(rfSendData[1].b[2]); Serial.print("\t");
Serial.print(rfSendData[1].b[3]); Serial.print("\t");
Serial.print(rfSendData[2].i[0]); Serial.print("\t");
Serial.print(rfSendData[2].i[1]); Serial.print("\t");
Serial.print(rfSendData[3].i[0]); Serial.print("\t");
Serial.print(rfSendData[3].i[1]); Serial.print("\t");
Serial.print(rfSendData[4].d); Serial.print("\t");
Serial.print(rfSendData[5].d); Serial.print("\t");
Serial.print(rfSendData[6].d); Serial.print("\t");
Serial.print(rfSendData[7].l); Serial.print("\t");
*/
//rslt = radio.write( &dataToSend, sizeof(dataToSend) );
rslt = radio.write(rfSendData, 32);
// Always use sizeof() as it gives the size as the number of bytes.
// For example if dataToSend was an int sizeof() would correctly return 2
if (rslt) {
if ( radio.isAckPayloadAvailable() ) {
radio.read(&rfAckData, sizeof(rfAckData));
rfSendDataStatus = 2;
} else {
rfSendDataStatus = 1;
//Serial.println(" Acknowledge but no data ");
}
updateMessage();
} else {
rfSendDataStatus = 1;
//Serial.println(" Tx failed");
}
//prevMillis = millis();
}
/*
void showData() {
if (rfSendDataStatus == 2) {
Serial.print(" Acknowledge data ");
Serial.print(rfAckData[1]);
Serial.print(", ");
Serial.print(rfAckData[2]);
Serial.print(", ");
Serial.print(rfAckData[3]);
Serial.print(", ");
Serial.print(rfAckData[4]);
Serial.print(", ");
Serial.print(rfAckData[5]);
Serial.print(", ");
Serial.print(rfAckData[6]);
Serial.print(", ");
Serial.println(rfAckData[7]);
Serial.println();
//rfSendDataStatus = false;
}
}
*/
void updateMessage() {
/*
// so you can see that new data is being sent
txNum += 1;
if (txNum > '9') {
txNum = '0';
}
dataToSend[8] = txNum;
*/
}
// EOF: nRF24L01
void distanceSensorsAdaptativePeriod( byte n ) {
int i;
for(i=0; i < n; i++) {
delay(30);
getFrontLaserDistance();
getSonarDistance();
/*
Serial.print(bodyFrontLaserDistance); Serial.print("\t");
Serial.print(bodySonarDistance[0]); Serial.print("\t");
Serial.print(bodySonarDistance[1]); Serial.println("\t");
*/
}
}
void sendSerialLoopData1() {
int i;
Serial.print(loopTime); Serial.print("\t");
//Serial.print(rfSendDataStatus); Serial.print("\t");
//Serial.print(rfAckData[1]); Serial.print("\t");
//Serial.print(bodyLaserRead); 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);
}
//
//
//
//void sendSerialLoopProcessingData() {
void sendSerialLoopData() {
int i;
Serial.print(0); Serial.print("\t");
Serial.print(bodyPosX); Serial.print("\t");
Serial.print(bodyPosY); Serial.print("\t ");
Serial.print(bodyThetaRadians * 180 / PI, 6); Serial.print("\t");
//Serial.print(bodyFrontLaserDistance); Serial.print("\t");
Serial.print(getPointXpos(bodyFrontLaserDistance, 0)); Serial.print("\t");
Serial.print(getPointYpos(bodyFrontLaserDistance, 0)); Serial.print("\t");
//Serial.print(bodySonarDistance[0]); Serial.print("\t");
Serial.print(getPointXpos(bodySonarDistance[0], 90)); Serial.print("\t");
Serial.print(getPointYpos(bodySonarDistance[0], 90)); Serial.print("\t");
//Serial.print(bodySonarDistance[1]); Serial.print("\t");
Serial.print(getPointXpos(bodySonarDistance[1], -90)); Serial.print("\t");
Serial.print(getPointYpos(bodySonarDistance[1], -90)); Serial.print("\t");
// not required
Serial.print(prgIdx); Serial.print("\t");
Serial.print(prgCmd[prgIdx].intCmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx].extCmd); Serial.print("\t");
Serial.print(prgCmd[prgIdx].units); Serial.print("\t");
Serial.print(batteryVoltage); Serial.print("\t");
Serial.print(bodyFrontLaserDistance); Serial.print("\t");
Serial.print(bodySonarDistance[0]); Serial.print("\t");
Serial.print(bodySonarDistance[1]); Serial.print("\t");
Serial.print(loopTime); Serial.print("\t");
Serial.println("\t");
}