dami_nrf24l01_rxbase_v1
//
// 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] = {109, -4000, 1 , 2 , 3, 6, 7 }; // the two values to be sent to the master
bool newData = false;
char dataReceived[32]; // this must match dataToSend in the TX
union u_tag {
byte b[4];
int i[2];
long l;
double d;
} rf_data[8];
byte cmdCode = 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( &dataReceived, sizeof(dataReceived) );
updateReplyData();
newData = 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:
byte tmp = Serial.parseInt();
if (Serial.read() == '\n') {
cmdCode = tmp;
}
}
}
//============
void showData() {
int i;
if (newData == true) {
// manage received radio data
for(i=0; i < 8; i++) {
rf_data[i].b[0] = dataReceived[i*4 + 0];
rf_data[i].b[1] = dataReceived[i*4 + 1];
rf_data[i].b[2] = dataReceived[i*4 + 2];
rf_data[i].b[3] = dataReceived[i*4 + 3];
}
// select the output for data
switch(cmdCode) {
case 0:
showData4ProcessingMap();
break;
case 1:
showData4ProcessingMap();
break;
case 10:
showData4SerialMonitor();
break;
}
// reset flag
newData = false;
}
}
void showData4SerialMonitor() {
Serial.print("D0 ");
Serial.print(rf_data[0].b[0]); Serial.print(" "); // prgIdx
Serial.print(rf_data[0].b[1]); Serial.print(" "); // cmd
Serial.print(rf_data[0].b[2]); Serial.print(" "); // prgCmd[prgIdx].intCmd
Serial.print(rf_data[0].b[3]); Serial.print(" "); // internalCmdProcessStatus
Serial.print(rf_data[1].b[0]); Serial.print(" "); // prgCmd[prgIdx].extCmd
Serial.print(rf_data[1].b[1]); Serial.print(" "); // bodyMoveProcessStatus
Serial.print(rf_data[1].b[2]); Serial.print(" "); // slaveCmdRequestId
Serial.print(rf_data[1].b[3]); Serial.print(" "); // slaveCmdWaitingId
Serial.print(rf_data[2].i[0]); Serial.print(" "); // bodyFrontLaserRead
Serial.print(rf_data[2].i[1]); Serial.print(" "); // bodyFrontLaserDistance
Serial.print(rf_data[3].i[0]); Serial.print(" "); // bodySonarDistance[0]
Serial.print(rf_data[3].i[1]); Serial.print(" "); // bodySonarDistance[1]
Serial.print(rf_data[4].d); Serial.print(" "); // bodyThetaRadians * 180 / PI
Serial.print(rf_data[5].d); Serial.print(" "); // bodyPosX
Serial.print(rf_data[6].d); Serial.print(" "); // bodyPosY
Serial.print(rf_data[7].b[0]); Serial.print(" "); // loopTime
//Serial.print(dataReceived);
Serial.print(" ackPayload sent ");
Serial.print(ackData[0]);
Serial.print(", ");
Serial.println(ackData[1]);
}
void showData4ProcessingMap() {
float bodyThetaRadians;
Serial.print(0); Serial.print(" ");
Serial.print(rf_data[5].d); Serial.print(" "); // bodyPosX
Serial.print(rf_data[6].d); Serial.print(" "); // bodyPosY
Serial.print(rf_data[4].d); Serial.print(" "); // bodyThetaRadians * 180 / PI
bodyThetaRadians = rf_data[4].d / 180 * PI;
Serial.print(getPointXpos(rf_data[2].i[1], 0, bodyThetaRadians)); Serial.print("\t"); // bodyFrontLaserDistance X
Serial.print(getPointYpos(rf_data[2].i[1], 0, bodyThetaRadians)); Serial.print("\t"); // bodyFrontLaserDistance Y
Serial.print(getPointXpos(rf_data[3].i[0], 90, bodyThetaRadians)); Serial.print("\t"); // bodySonarDistance[0] X
Serial.print(getPointYpos(rf_data[3].i[0], 90, bodyThetaRadians)); Serial.print("\t"); // bodySonarDistance[0] Y
Serial.print(getPointXpos(rf_data[3].i[1], -90, bodyThetaRadians)); Serial.print("\t"); // bodySonarDistance[1] X
Serial.print(getPointYpos(rf_data[3].i[1], -90, bodyThetaRadians)); Serial.print("\t"); // bodySonarDistance[1] Y
Serial.print(rf_data[2].i[1]); Serial.print(" "); // bodyFrontLaserDistance
Serial.print(rf_data[3].i[0]); Serial.print(" "); // bodySonarDistance[0]
Serial.print(rf_data[3].i[1]); Serial.print(" "); // bodySonarDistance[1]
Serial.print(rf_data[0].b[0]); Serial.print(" "); // prgIdx
Serial.print(rf_data[0].b[1]); Serial.print(" "); // cmd
Serial.print(rf_data[0].b[2]); Serial.print(" "); // prgCmd[prgIdx].intCmd
Serial.print(rf_data[0].b[3]); Serial.print(" "); // internalCmdProcessStatus
Serial.print(rf_data[1].b[0]); Serial.print(" "); // prgCmd[prgIdx].extCmd
Serial.print(rf_data[1].b[1]); 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);
}
//
// TODO: updateReplyData()
//
void updateReplyData() {
ackData[0] -= 1;
ackData[1] -= 1;
if (ackData[0] < 100) {
ackData[0] = 109;
}
if (ackData[1] < -4009) {
ackData[1] = -4000;
}
radio.writeAckPayload(1, &ackData, sizeof(ackData)); // load the payload for the next time
}