//
// DAMI nRF24L01 RX base station
//
// fixed station that collect real time data from robot
// using the nRF24L01 2.4GHz transciver
//
// it provides output to display on serial monitor
// or to use in a processing3 sketch for map display
//
// based on SimpleRxAckPayload - the slave or the receiver
// http://forum.arduino.cc/index.php?topic=421081
//
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#define CE_PIN 9
#define CSN_PIN 10
const byte thisSlaveAddress[5] = {'R','x','A','A','A'};
RF24 radio(CE_PIN, CSN_PIN);
int ackData[8] = {0, 0, 0 , 0 , 0, 0, 0 }; // the values to be sent to the master
bool newRadioData = false;
char radioDataReceivedBuffer[32]; // this must match dataToSend in the TX
union u_tag {
byte b[4];
int i[2];
long l;
double d;
} rData[2][8]; // 0=loopData, 1=frontScanData
boolean rDataNew[2] = {false, false};
int stationInternalCommand = 0;
int stationSerialDataDisplayCode = 0;
int remoteCommand = 0;
//==============
void setup() {
Serial.begin(115200);
//Serial.println("DAMI nRF24L01 RX base station starting...");
radio.begin();
radio.setDataRate( RF24_250KBPS );
radio.openReadingPipe(1, thisSlaveAddress);
radio.enableAckPayload();
radio.writeAckPayload(1, &ackData, sizeof(ackData)); // pre-load data
radio.startListening();
}
//==========
void loop() {
// get remote robot data
getRadioData();
// get serial data
getSerialData();
// show remote robot data
showData();
}
//============
void getRadioData() {
if ( radio.available() ) {
radio.read( &radioDataReceivedBuffer, sizeof(radioDataReceivedBuffer) );
updateReplyData();
newRadioData = true;
}
}
//============
void getSerialData() {
// if there's any serial available, read it:
while (Serial.available() > 0) {
// look for the next valid integer in the incoming serial stream:
int tmp1 = Serial.parseInt();
if (Serial.read() == '\n') {
stationInternalCommand = tmp1;
switch(stationInternalCommand) {
case 0:
case 1:
case 2:
stationSerialDataDisplayCode = tmp1;
break;
case 11:
remoteCommand = 11;
break;
case 12:
remoteCommand = 2;
break;
case 19:
remoteCommand = 254;
break;
}
}
}
}
//============
void showData() {
int i;
byte currentPage = 0;
if (newRadioData == true) {
// manage received radio data using first byte of 32 byte page as index
// before apply the page index check that index value is allowed
if(radioDataReceivedBuffer[0] >= 0 && radioDataReceivedBuffer[0] < 2) { // HARDCODED
currentPage = radioDataReceivedBuffer[0];
for(i=0; i < 8; i++) {
rData[currentPage][i].b[0] = radioDataReceivedBuffer[i*4 + 0];
rData[currentPage][i].b[1] = radioDataReceivedBuffer[i*4 + 1];
rData[currentPage][i].b[2] = radioDataReceivedBuffer[i*4 + 2];
rData[currentPage][i].b[3] = radioDataReceivedBuffer[i*4 + 3];
}
}
// bof debug
//Serial.print(stationInternalCommand); Serial.print(" ");
//Serial.print(remoteCommand); Serial.print(" ");
//Serial.println("");
//bof debug
// select the output for data
switch(stationSerialDataDisplayCode) {
case 0:
//showData4ProcessingMap();
break;
case 1:
showData4ProcessingMap();
break;
case 2:
showData4SerialMonitor();
break;
}
// reset flag
newRadioData = false;
// reset rdata page
if(currentPage > 0) {
for(i=0; i < 8; i++) {
rData[currentPage][i].b[0] = 0;
rData[currentPage][i].b[1] = 0;
rData[currentPage][i].b[2] = 0;
rData[currentPage][i].b[3] = 0;
}
}
}
}
void showData4SerialMonitor() {
Serial.print("D0 ");
Serial.print(rData[0][0].b[0]); Serial.print(" "); // data type
Serial.print(rData[0][0].b[1]); Serial.print(" "); // loopTime
Serial.print(rData[0][0].b[2]); Serial.print(" "); // prgIdx
Serial.print(rData[0][0].b[3]); Serial.print(" "); // cmd
Serial.print(rData[0][1].b[0]); Serial.print(" "); // prgCmd[prgIdx].intCmd
Serial.print(rData[0][1].b[1]); Serial.print(" "); // internalCmdProcessStatus
Serial.print(rData[0][1].b[2]); Serial.print(" "); // prgCmd[prgIdx].extCmd
Serial.print(rData[0][1].b[3]); Serial.print(" "); // bodyMoveProcessStatus
Serial.print(rData[0][2].i[0]); Serial.print(" "); // bodyFrontLaserDistanceQuick
Serial.print(rData[0][2].i[1]); Serial.print(" "); // bodyFrontLaserDistance
Serial.print(rData[0][3].i[0]); Serial.print(" "); // bodySonarDistance[0]
Serial.print(rData[0][3].i[1]); Serial.print(" "); // bodySonarDistance[1]
Serial.print(rData[0][4].d); Serial.print(" "); // bodyThetaRadians * 180 / PI
Serial.print(rData[0][5].d); Serial.print(" "); // bodyPosX
Serial.print(rData[0][6].d); Serial.print(" "); // bodyPosY
Serial.print(rData[0][7].i[0]); Serial.print(" "); // target (int)prgCmd[prgIdx].units
Serial.print(rData[0][7].i[1]); Serial.print(" "); // done (int)stepsToUnits(bodyMotorLeftSteps)
//
Serial.print("SSKD ");
Serial.print(rData[1][1].i[0]); Serial.print(" "); // 0
Serial.print(rData[1][1].i[1]); Serial.print(" "); // 15
Serial.print(rData[1][2].i[0]); Serial.print(" "); // 30
Serial.print(rData[1][2].i[1]); Serial.print(" "); // 45
Serial.print(rData[1][3].i[0]); Serial.print(" "); // 60
Serial.print(rData[1][3].i[1]); Serial.print(" "); // 75
Serial.print(rData[1][4].i[0]); Serial.print(" "); // 90
Serial.print(rData[1][4].i[1]); Serial.print(" "); // 105
Serial.print(rData[1][5].i[0]); Serial.print(" "); // 120
Serial.print(rData[1][5].i[1]); Serial.print(" "); // 135
Serial.print(rData[1][6].i[0]); Serial.print(" "); // 150
Serial.print(rData[1][6].i[1]); Serial.print(" "); // 165
Serial.print(rData[1][7].i[0]); Serial.print(" "); // 180
//
Serial.print("AKPL ");
Serial.print(ackData[0]);
Serial.print(", ");
Serial.print(ackData[1]);
Serial.print(", ");
Serial.print(ackData[2]);
Serial.print(", ");
Serial.print(ackData[3]);
Serial.print(", ");
Serial.println(ackData[4]);
}
void showData4ProcessingMap() {
Serial.print(0); Serial.print(" ");
Serial.print(rData[0][5].d); Serial.print(" "); // bodyPosX
Serial.print(rData[0][6].d); Serial.print(" "); // bodyPosY
Serial.print(rData[0][4].d); Serial.print(" "); // bodyThetaRadians * 180 / PI
Serial.print(rData[0][2].i[1]); Serial.print(" "); // bodyFrontLaserDistance
Serial.print(0); Serial.print(" ");
Serial.print(rData[0][3].i[0]); Serial.print(" "); // bodySonarDistance[0]
Serial.print(-90); Serial.print(" ");
Serial.print(rData[0][3].i[1]); Serial.print(" "); // bodySonarDistance[1]
Serial.print(90); Serial.print(" ");
Serial.print(rData[1][1].i[0]); Serial.print(" "); // 0
Serial.print(0); Serial.print(" ");
Serial.print(rData[1][1].i[1]); Serial.print(" "); // 15
Serial.print(15); Serial.print(" ");
Serial.print(rData[1][2].i[0]); Serial.print(" "); // 30
Serial.print(30); Serial.print(" ");
Serial.print(rData[1][2].i[1]); Serial.print(" "); // 45
Serial.print(45); Serial.print(" ");
Serial.print(rData[1][3].i[0]); Serial.print(" "); // 60
Serial.print(60); Serial.print(" ");
Serial.print(rData[1][3].i[1]); Serial.print(" "); // 75
Serial.print(75); Serial.print(" ");
Serial.print(rData[1][4].i[0]); Serial.print(" "); // 90
Serial.print(90); Serial.print(" ");
Serial.print(rData[1][4].i[1]); Serial.print(" "); // 105
Serial.print(105); Serial.print(" ");
Serial.print(rData[1][5].i[0]); Serial.print(" "); // 120
Serial.print(120); Serial.print(" ");
Serial.print(rData[1][5].i[1]); Serial.print(" "); // 135
Serial.print(135); Serial.print(" ");
Serial.print(rData[1][6].i[0]); Serial.print(" "); // 150
Serial.print(150); Serial.print(" ");
Serial.print(rData[1][6].i[1]); Serial.print(" "); // 165
Serial.print(165); Serial.print(" ");
Serial.print(rData[1][7].i[0]); Serial.print(" "); // 180
Serial.print(180); Serial.print(" ");
Serial.print(rData[1][0].b[1]); Serial.print(" "); // decision
Serial.print(rData[0][0].b[1]); Serial.print(" "); // loopTime
Serial.print(rData[0][0].b[2]); Serial.print(" "); // prgIdx
Serial.print(rData[0][0].b[3]); Serial.print(" "); // cmd
Serial.print(rData[0][1].b[0]); Serial.print(" "); // prgCmd[prgIdx].intCmd
Serial.print(rData[0][1].b[1]); Serial.print(" "); // internalCmdProcessStatus
Serial.print(rData[0][1].b[2]); Serial.print(" "); // prgCmd[prgIdx].extCmd
Serial.print(rData[0][1].b[3]); Serial.print(" "); // bodyMoveProcessStatus
Serial.println("");
}
//================
/*
// coordinates based on polar coordinates and pose
// point distante, point angle and robot orientation
float getPointXpos(int d, float a, float t) {
float b = t + (float)(a / 180.0) * PI;
return d * cos(b);
}
float getPointYpos(int d, float a, float t) {
float b = t + (float)(a / 180.0) * PI;
return d * sin(b);
}
*/
//================
void updateReplyData() {
// the values to be sent to the master
// this is a global variable for debuging
//int ackData[8] = {0, 0, 0 , 0 , 0, 0, 0 };
//Serial.print("updateReplyData ");
//Serial.println(remoteCommand);
switch(remoteCommand) {
case 11:
case 254:
ackData[1] = remoteCommand;
break;
case 2:
ackData[1] = 2;
ackData[2] = 0;
ackData[3] = 2;
ackData[4] = 500;
break;
}
// send command
radio.writeAckPayload(1, &ackData, sizeof(ackData)); // load the payload for the next time
// command sent
if(remoteCommand) {
// reset vars
remoteCommand = 0;
// required because ackData is a
// global variable for debuging
ackData[1] = 0;
ackData[2] = 0;
ackData[3] = 0;
ackData[4] = 0;
}
}