James Hilder
V0.5 library for the Pi Swarm robot
Diff: communications.cpp
- Revision:
- 1:b067a08ff54e
- Parent:
- 0:9ffe8ebd1c40
- Child:
- 2:0a739218ab11
--- a/communications.cpp Fri Jan 31 22:59:25 2014 +0000
+++ b/communications.cpp Sun Feb 02 18:05:58 2014 +0000
@@ -1,121 +1,169 @@
/* University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
*
* (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
- *
+ *
* Version 0.4 January 2014
*
* Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
*
*/
-
- #include "communications.h"
- #include "piswarm.h"
- #include "main.h"
-
- DigitalOut actioning (LED1);
- DigitalOut errorled (LED2);
- DigitalOut tx (LED3);
- DigitalOut rx (LED4);
- Timeout tdma_timeout;
- char tdma_busy = 0;
- char waiting_message [64];
- char waiting_length = 0;
+
+// Important note: The communication stack is enabled by setting the "USE_COMMUNICATION_STACK" flag to 1
+// When being used, all received messages are decoded using the decodeMessage() function
+// See manual for more info on using the communication handler
+#include "communications.h"
+#include "piswarm.h"
+#include "main.h"
+
+struct swarm_member swarm[SWARM_SIZE]; // Array to hold received information about other swarm members
+DigitalOut actioning (LED1);
+DigitalOut errorled (LED2);
+DigitalOut tx (LED3);
+DigitalOut rx (LED4);
+Timeout tdma_timeout;
+char tdma_busy = 0;
+char waiting_message [64];
+char waiting_length = 0;
+int message_id = 0;
+
+void send_rf_message(char target, char command, char * data, char length)
+{
+ char message [4+length];
+ message[0]=piswarm.get_id();
+ message[1]=target;
+ message_id++;
+ message[2]=message_id % 256;
+ message[3]=command;
+ for(int i=0; i<length; i++) {
+ message[4+i]=data[i];
+ }
+ piswarm.send_rf_message(message,4+length);
+ if(RF_DEBUG==1)pc.printf("RF message sent");
+}
+
- const char * const requests_array[] = { "Req.Ack", "Req.LM", "Req.RM", "Req.But","Req.LEDCol","Req.LED","Req.Lgt","Req.Acc","Req.Gyro","Req.BIR","Req.RIR","Req.DIR","Req.LFS","Req.UpT","Req.???","Req.???"};
- const char * const commands_array[] = { "Cm.Stop","Cm.Fward","Cm.Bward","Cm.Left","Cm.Right","Cm.LMot","Cm.RMot","Cm.OLEDC","Cm.CLEDC","Cm.OLEDS","Cm.CLEDS","Cm.SetLED","Cm.Play","Cm.Sync","Cm.???","Cm.???"};
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Internal Functions
+// In general these functions should not be called by user code
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//Decode the received message, action it if it is valid and for me
-void decodeMessage(char sender, char target, char id, char command, char * data, char length){
+// Decode the received message header. Check it is min. 4 bytes long, that the sender and target are valid
+void processRadioData(char * data, char length)
+{
+ if(RF_USE_LEDS==1) {
+ errorled=0;
+ rx=1;
+ }
+ // Decompose the received message
+ if(length < 4) errormessage(0);
+ else {
+ // Establish the sender and target of the packet
+ char sender = data[0];
+ char target = data[1];
+ char id = data[2];
+ char command = data[3];
+ if(sender<32 || sender>63)errormessage(1);
+ else {
+ if(target<32 || target>63)errormessage(2);
+ else {
+ sender -= 32;
+ target -= 32;
+ decodeMessage(sender,target,id,command,data+4,length-4);
+ }
+ }
+ }
+ if(RF_USE_LEDS==1) rx=0;
+}
+
+//Decode the received message, action it if it is valid and for me [called in alpha433.cpp]
+void decodeMessage(char sender, char target, char id, char command, char * data, char length)
+{
char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0;
-
- if(target==0) broadcast_message = 1;
+
+ if(target==0) broadcast_message = 1;
is_response = 0 != (command & (1 << 7));
request_response = 0 != (command & (1 << 6));
is_user = 0 != (command & (1 << 5));
is_command = 0 != (command & (1 << 4));
function = command % 16;
-
+
if (RF_DEBUG==1) {
if(is_command == 1) pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, commands_array[function],length);
else pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, requests_array[function],length);
}
-
+
//Action the message only if I am a recipient
- if(target==0 || target==piswarm.get_id()){
+ if(target==0 || target==piswarm.get_id()) {
if(RF_USE_LEDS==1) actioning = 1;
- if(is_response == 1) {
- if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length);
- else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length);
- }else {
- if(is_command == 1) {
- if(RF_ALLOW_COMMANDS == 1){
- if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
- else handle_command(sender, broadcast_message, request_response, id, function, data, length);
- } else if (RF_DEBUG==1) pc.printf(" - Blocked\n");
- }
- else {
+ if(is_response == 1) {
+ if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length);
+ else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length);
+ } else {
+ if(is_command == 1) {
+ if(RF_ALLOW_COMMANDS == 1) {
+ if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
+ else handle_command(sender, broadcast_message, request_response, id, function, data, length);
+ } else if (RF_DEBUG==1) pc.printf(" - Blocked\n");
+ } else {
//A information request has no extra parameters
- if(length == 0){
- if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
- else handle_request(sender, broadcast_message, request_response, id, function);
- } else if (RF_DEBUG==1) pc.printf(" - Invalid\n");
- }
- }
- if(RF_USE_LEDS==1) actioning = 0;
+ if(length == 0) {
+ if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
+ else handle_request(sender, broadcast_message, request_response, id, function);
+ } else if (RF_DEBUG==1) pc.printf(" - Invalid\n");
+ }
+ }
+ if(RF_USE_LEDS==1) actioning = 0;
} else if (RF_DEBUG==1) pc.printf(" - Ignored\n");
}
//Send a response message
-void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){
+void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
+{
char message [4+length];
message[0]=piswarm.get_id();
message[1]=target;
message[2]=id;
message[3]=128 + (success << 6) + (is_command << 4) + function;
- for(int i=0;i<length;i++){
- message[4+i]=data[i];
- }
+ for(int i=0; i<length; i++) {
+ message[4+i]=data[i];
+ }
//Delay the response if it is broadcast and TDMA mode is on
- if(RF_USE_TDMA == 1 && is_broadcast == 1){
- if(tdma_busy == 1){
+ if(RF_USE_TDMA == 1 && is_broadcast == 1) {
+ if(tdma_busy == 1) {
if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n");
- }else{
+ } else {
tdma_busy = 1;
strcpy(waiting_message,message);
waiting_length=length;
tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id());
if (RF_DEBUG==1) pc.printf("TDMA Response pending\n");
}
- }else
- {
+ } else {
piswarm.send_rf_message(message,4+length);
if(RF_DEBUG==1)pc.printf("Response issued");
}
}
// Send a delayed response
-void tdma_response(){
+void tdma_response()
+{
piswarm.send_rf_message(waiting_message,4+waiting_length);
tdma_busy = 0;
if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
}
-// Handle a message that is a response
-void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){
-
-}
-
-// Handle a message that is a (not-user) command
-void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length){
+// Handle a message that is a predefined command
+void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length)
+{
char success = 0;
- switch(function){
+ switch(function) {
case 0: // Stop [0 data]
if(length==0) {
piswarm.stop();
if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - ");
success = 1;
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
- break;
+ break;
case 1: // Forward [2 bytes: 16-bit signed short]
if(length==2) {
int i_speed = (data[0] << 8) + data[1];
@@ -135,7 +183,7 @@
success = 1;
if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed);
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
- break;
+ break;
case 3: // Left [2 bytes: 16-bit signed short]
if(length==2) {
int i_speed = (data[0] << 8) + data[1];
@@ -147,7 +195,7 @@
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
break;
case 4: // Right [2 bytes: 16-bit signed short]
- if(length==2) {
+ if(length==2) {
int i_speed = (data[0] << 8) + data[1];
float speed = i_speed / 32768.0;
speed--;
@@ -155,9 +203,9 @@
success = 1;
if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed);
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
- break;
+ break;
case 5: // Left Motor [2 bytes: 16-bit signed short]
- if(length==2) {
+ if(length==2) {
int i_speed = (data[0] << 8) + data[1];
float speed = i_speed / 32768.0;
speed--;
@@ -167,7 +215,7 @@
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
break;
case 6: // Right Motor [2 bytes: 16-bit signed short]
- if(length==2) {
+ if(length==2) {
int i_speed = (data[0] << 8) + data[1];
float speed = i_speed / 32768.0;
speed--;
@@ -177,40 +225,40 @@
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
break;
case 7: // Outer LED Colour [3 bytes: R, G, B]
- if(length==3) {
- piswarm.set_oled_colour (data[0],data[1],data[2]);
+ if(length==3) {
+ piswarm.set_oled_colour (data[0],data[1],data[2]);
success = 1;
if(RF_DEBUG==1) pc.printf(" - Set Outer R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
break;
case 8: // Center LED Colour[3 bytes: R, G, B]
- if(length==3) {
- piswarm.set_cled_colour (data[0],data[1],data[2]);
+ if(length==3) {
+ piswarm.set_cled_colour (data[0],data[1],data[2]);
success = 1;
if(RF_DEBUG==1) pc.printf(" - Set Center R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
break;
case 9: // Outer LED State [2 bytes: [xxxxxx01][23456789] ]
if(length==2) {
- piswarm.set_oleds(0 != (data[0] & (1 << 1)),0 != (data[0] & (1 << 0)),0 != (data[1] & (1 << 7)),0 != (data[1] & (1 << 6)),0 != (data[1] & (1 << 5)),0 != (data[1] & (1 << 4)),0 != (data[1] & (1 << 3)),0 != (data[1] & (1 << 2)),0 != (data[1] & (1 << 1)),0 != (data[1] & (1 << 0)));
+ piswarm.set_oleds(0 != (data[0] & (1 << 1)),0 != (data[0] & (1 << 0)),0 != (data[1] & (1 << 7)),0 != (data[1] & (1 << 6)),0 != (data[1] & (1 << 5)),0 != (data[1] & (1 << 4)),0 != (data[1] & (1 << 3)),0 != (data[1] & (1 << 2)),0 != (data[1] & (1 << 1)),0 != (data[1] & (1 << 0)));
success = 1;
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
break;
case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ]
if(length==1) {
- piswarm.enable_cled (data[0] % 2);
+ piswarm.enable_cled (data[0] % 2);
success = 1;
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
break;
case 11: // Set outer LED [1 byte: [xxxEvvvv] E=enabled vvvv=LED]
if(length==1) {
int led = data[0] % 16;
- if(led < 10){
+ if(led < 10) {
piswarm.set_oled(led, 0!=(data[0] & (1 << 4)));
success = 1;
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
- break;
+ break;
case 12: // Play sound [Minimum 1 byte]
if(length>0) {
piswarm.play_tune(data,length);
@@ -227,30 +275,30 @@
set_time(new_time);
display_system_time();
} else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
- break;
+ break;
case 14: //
break;
case 15: //
- break;
+ break;
}
- if(request_response == 1){
+ if(request_response == 1) {
send_response(sender, is_broadcast, success, id, 1, function, NULL, 0);
}
-
+
}
-//Handle a message that is a (not-user) request
-void handle_request(char sender, char is_broadcast, char request_response, char id, char function){
+//Handle a message that is a predefined request
+void handle_request(char sender, char is_broadcast, char request_response, char id, char function)
+{
int response_length = 0;
char * response = NULL;
char success = 0;
-
- switch(function){
+
+ switch(function) {
case 0: // Null request
success=1;
- break;
- case 1: // Request left motor speed
- {
+ break;
+ case 1: { // Request left motor speed
response_length = 2;
float speed = piswarm.get_left_motor() * 32767;
int a_speed = 32768 + (int) speed;
@@ -260,10 +308,9 @@
response[0]=msb;
response[1]=lsb;
success=1;
- break;
- }
- case 2: // Request right motor speed
- {
+ break;
+ }
+ case 2: { // Request right motor speed
response_length = 2;
float speed = piswarm.get_right_motor() * 32767;
int a_speed = 32768 + (int) speed;
@@ -273,17 +320,15 @@
response[0]=msb;
response[1]=lsb;
success=1;
- break;
- }
- case 3: // Request button state
- {
+ break;
+ }
+ case 3: { // Request button state
response_length = 1;
response = new char[1];
response[0]=piswarm.get_switches();
- break;
- }
- case 4: // Request LED colours
- {
+ break;
+ }
+ case 4: { // Request LED colours
response_length = 6;
response = new char[6];
int oled_colour = piswarm.get_oled_colour();
@@ -294,89 +339,876 @@
response[3] = (char) (cled_colour >> 16);
response[4] = (char) ((cled_colour >> 8) % 256);
response[5] = (char) (cled_colour % 256);
- break;
- }
- case 5: // Request LED states
- break;
- case 6: // Request battery power
- {
+ break;
+ }
+ case 5: { // Request LED states
+ response_length = 2;
+ response = new char[2];
+ response[0] = 0;
+ response[1] = 0;
+ if (piswarm.get_cled_state() == 1) response[0] += 4;
+ if (piswarm.get_oled_state(0) == 1) response[0] += 2;
+ if (piswarm.get_oled_state(1) == 1) response[0] += 1;
+ if (piswarm.get_oled_state(2) == 1) response[1] += 128;
+ if (piswarm.get_oled_state(3) == 1) response[1] += 64;
+ if (piswarm.get_oled_state(4) == 1) response[1] += 32;
+ if (piswarm.get_oled_state(5) == 1) response[1] += 16;
+ if (piswarm.get_oled_state(6) == 1) response[1] += 8;
+ if (piswarm.get_oled_state(7) == 1) response[1] += 4;
+ if (piswarm.get_oled_state(8) == 1) response[1] += 2;
+ if (piswarm.get_oled_state(9) == 1) response[1] += 1;
+ break;
+ }
+ case 6: { // Request battery power
response_length = 2;
response = new char[2];
float fbattery = piswarm.battery() * 1000.0;
unsigned short battery = (unsigned short) fbattery;
response[0] = battery >> 8;
response[1] = battery % 256;
- break;
- }
- case 7: // Request light sensor reading
- {
+ break;
+ }
+ case 7: { // Request light sensor reading
response_length = 2;
response = new char[2];
float flight = piswarm.read_light_sensor() * 655.0;
unsigned short light = (unsigned short) flight;
response[0] = light >> 8;
response[1] = light % 256;
- break;
- }
- case 8: // Request accelerometer reading
+ break;
+ }
+ case 8: { // Request accelerometer reading
+
break;
- case 9: // Request gyroscope reading
- break;
+ }
+ case 9: { // Request gyroscope reading
+ response_length = 2;
+ response = new char[2];
+ float fgyro = piswarm.read_gyro();
+ fgyro += 32768;
+ unsigned short sgyro = (unsigned short) fgyro;
+ response[0] = sgyro >> 8;
+ response[1] = sgyro % 256;
+ break;
+ }
case 10: // Request background IR reading
break;
case 11: // Request illuminated IR reading
- break;
+ break;
case 12: // Request line-tracking IR reading
break;
case 13: // Request uptime
break;
- case 14: //
+ case 14: //
break;
case 15: //
- break;
- }
+ break;
+ }
send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
-
+
}
-void processRadioData(char * data, char length){
- if(RF_USE_LEDS==1) {errorled=0;rx=1;}
- // Decompose the received message
- if(length < 4) errormessage(0);
- else {
- // Establish the sender and target of the packet
- char sender = data[0];
- char target = data[1];
- char id = data[2];
- char command = data[3];
- if(sender<32 || sender>63)errormessage(1);
- else {
- if(target<32 || target>63)errormessage(2);
- else {
- sender -= 32;
- target -= 32;
- decodeMessage(sender,target,id,command,data+4,length-4);
- }
- }
- }
- if(RF_USE_LEDS==1) rx=0;
-}
-void errormessage(int index){
+void errormessage(int index)
+{
if(RF_USE_LEDS==1) errorled=1;
- switch(index){
+ switch(index) {
case 0: //Message to short
if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n");
break;
case 1: //Sender out of valid range
- if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");
+ if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");
break;
- case 2: //Target out of valid range
+ case 2: //Target out of valid range
if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n");
break;
case 3:
-
+
+ break;
+ case 4:
+ break;
+ }
+}
+
+
+
+// Handle a message that is a response
+void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
+{
+ char outcome = 0;
+ if(is_command == 0) {
+ // Response is a data_request_response
+ switch(function) {
+ case 0: {
+ if(swarm[sender].status_rf_request_null == 1) {
+ if(length == 0) {
+ swarm[sender].status_rf_request_null = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_null= 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 1: { //Left motor speed
+ if(swarm[sender].status_rf_request_left_motor_speed == 1) {
+ if(length == 2) {
+ swarm[sender].status_rf_request_left_motor_speed = 2;
+ int value = (data [0] << 8) + data[1];
+ value -= 32768;
+ float val = value;
+ val /= 32767.0;
+ swarm[sender].left_motor_speed = val;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_left_motor_speed= 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 2: { //Right motor speed
+ if(swarm[sender].status_rf_request_right_motor_speed == 1) {
+ if(length == 2) {
+ swarm[sender].status_rf_request_right_motor_speed = 2;
+ int value = (data [0] << 8) + data[1];
+ value -= 32768;
+ float val = value;
+ val /= 32767.0;
+ swarm[sender].right_motor_speed = val;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_right_motor_speed = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 3: { //Button state
+ if(swarm[sender].status_rf_request_button_state == 1) {
+ if(length == 2) {
+ swarm[sender].status_rf_request_button_state = 2;
+ swarm[sender].button_state = data[0];
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_button_state = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 4: { //LED Colour
+ if(swarm[sender].status_rf_request_led_colour == 1) {
+ if(length == 6) {
+ swarm[sender].status_rf_request_led_colour = 2;
+ for(int i=0; i<3; i++) {
+ swarm[sender].outer_led_colour[i] = data[i];
+ swarm[sender].center_led_colour[i] = data[i+3];
+ }
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_led_colour = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 5: { //LED States
+ if(swarm[sender].status_rf_request_led_states == 1) {
+ if(length == 2) {
+ swarm[sender].status_rf_request_led_states = 2;
+ for(int i=0; i<3; i++) {
+ swarm[sender].led_states[0] = data[0];
+ swarm[sender].led_states[1] = data[1];
+ }
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_led_states = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+
+ case 6: { //Battery
+ if(swarm[sender].status_rf_request_battery == 1) {
+ if(length == 2) {
+ swarm[sender].status_rf_request_battery = 2;
+ int fbattery = data[0] * 256;
+ fbattery += data[1];
+ swarm[sender].battery = (float) fbattery / 1000.0;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_battery = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 7: { //Light sensor
+ if(swarm[sender].status_rf_request_light_sensor == 1) {
+ if(length == 2) {
+ swarm[sender].status_rf_request_light_sensor = 2;
+ int ilight = data[0] * 256;
+ ilight += data[1];
+ float flight = (float) (ilight) / 655.0;
+ swarm[sender].light_sensor = flight;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_light_sensor = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 8: { //Accelerometer
+ if(swarm[sender].status_rf_request_accelerometer == 1) {
+ if(length == 6) {
+ swarm[sender].status_rf_request_accelerometer = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_accelerometer = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 9: { //Gyroscope
+ if(swarm[sender].status_rf_request_gyroscope == 1) {
+ if(length == 2) {
+ swarm[sender].status_rf_request_gyroscope = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_gyroscope = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 10: { //Background IR
+ if(swarm[sender].status_rf_request_background_ir == 1) {
+ if(length == 16) {
+ swarm[sender].status_rf_request_background_ir = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_background_ir = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 11: { //Reflected IR
+ if(swarm[sender].status_rf_request_background_ir == 1) {
+ if(length == 16) {
+ swarm[sender].status_rf_request_background_ir = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_background_ir = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 12: { // Distance IR
+ if(swarm[sender].status_rf_request_distance_ir == 1) {
+ if(length == 16) {
+ swarm[sender].status_rf_request_distance_ir = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_distance_ir = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 13: { // Line following IR
+ if(swarm[sender].status_rf_request_line_following_ir == 1) {
+ if(length == 10) {
+ swarm[sender].status_rf_request_line_following_ir = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_line_following_ir = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 14: { // Request uptime
+ if(swarm[sender].status_rf_request_uptime == 1) {
+ if(length == 4) {
+ swarm[sender].status_rf_request_uptime = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_uptime = 3;
+ outcome = 3;
+ }
+ } else outcome = 2;
+ }
break;
- case 4: break;
+
+ }
+ } else {
+ // Response to a command
+ }
+
+ if(RF_DEBUG) {
+ switch(outcome) {
+ case 0 :
+ pc.printf("Unknown RF response received");
+ case 1 :
+ pc.printf("RF response received, data updated.");
+ case 2 :
+ pc.printf("Unexpected RF response received, ignored.");
+ case 3 :
+ pc.printf("Invalid RF response received, ignored.");
+ }
+ }
+}
+
+void send_rf_request_null ( char target )
+{
+ char command = 0x80;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) { //Request broadcast to all recipients
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_null = 1;
+ }
+ } else swarm[target].status_rf_request_null = 1;
+ send_rf_message(target,command,data,length);
+}
+
+
+void send_rf_request_left_motor_speed ( char target )
+{
+ char command = 0x81;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_left_motor_speed = 1;
+ }
+ } else swarm[target].status_rf_request_left_motor_speed = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_right_motor_speed ( char target )
+{
+ char command = 0x82;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_right_motor_speed = 1;
+ }
+ } else swarm[target].status_rf_request_right_motor_speed = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_button_state ( char target )
+{
+ char command = 0x83;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_button_state = 1;
+ }
+ } else swarm[target].status_rf_request_button_state = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_led_colour ( char target )
+{
+ char command = 0x84;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_led_colour = 1;
+ }
+ } else swarm[target].status_rf_request_led_colour = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_led_states ( char target )
+{
+ char command = 0x85;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_led_states = 1;
+ }
+ } else swarm[target].status_rf_request_led_states = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_battery ( char target )
+{
+ char command = 0x86;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_battery = 1;
+ }
+ } else swarm[target].status_rf_request_battery = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_light_sensor ( char target )
+{
+ char command = 0x87;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_light_sensor = 1;
+ }
+ } else swarm[target].status_rf_request_light_sensor = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_accelerometer ( char target )
+{
+ char command = 0x88;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_accelerometer = 1;
+ }
+ } else swarm[target].status_rf_request_accelerometer = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_gyroscope ( char target )
+{
+ char command = 0x89;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_gyroscope = 1;
+ }
+ } else swarm[target].status_rf_request_gyroscope = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_background_ir ( char target )
+{
+ char command = 0x8A;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_background_ir = 1;
+ }
+ } else swarm[target].status_rf_request_background_ir = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_reflected_ir ( char target )
+{
+ char command = 0x8B;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_reflected_ir = 1;
+ }
+ } else swarm[target].status_rf_request_reflected_ir = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_distance_ir ( char target )
+{
+ char command = 0x8C;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_distance_ir = 1;
+ }
+ } else swarm[target].status_rf_request_distance_ir = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_line_following_ir ( char target )
+{
+ char command = 0x8D;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_line_following_ir = 1;
+ }
+ } else swarm[target].status_rf_request_line_following_ir = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_request_uptime ( char target )
+{
+ char command = 0x8E;
+ char length = 0;
+ char * data = NULL;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_uptime= 1;
+ }
+ } else swarm[target].status_rf_request_uptime = 1;
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_stop ( char target, char request_response )
+{
+ char command = 0x10;
+ char length = 0;
+ char * data = NULL;
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_stop= 1;
+ }
+ } else swarm[target].status_rf_command_stop = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_forward ( char target, char request_response, float speed )
+{
+ char command = 0x11;
+ if(request_response == 1) command+=128;
+ char length = 2;
+ char data [2];
+ float qspeed = speed + 1;
+ qspeed *= 32768.0;
+ int ispeed = (int) qspeed;
+ data[0] = (char) (ispeed >> 8);
+ data[1] = (char) (ispeed % 256);
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_forward= 1;
+ }
+ } else swarm[target].status_rf_command_forward = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+
+void send_rf_command_backward ( char target, char request_response, float speed )
+{
+ char command = 0x12;
+ if(request_response == 1) command+=128;
+ char length = 2;
+ char data [2];
+ float qspeed = speed + 1;
+ qspeed *= 32768.0;
+ int ispeed = (int) qspeed;
+ data[0] = (char) (ispeed >> 8);
+ data[1] = (char) (ispeed % 256);
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_backward= 1;
+ }
+ } else swarm[target].status_rf_command_backward = 1;
}
-}
\ No newline at end of file
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_left ( char target, char request_response, float speed )
+{
+ char command = 0x13;
+ if(request_response == 1) command+=128;
+ char length = 2;
+ char data [2];
+ float qspeed = speed + 1;
+ qspeed *= 32768.0;
+ int ispeed = (int) qspeed;
+ data[0] = (char) (ispeed >> 8);
+ data[1] = (char) (ispeed % 256);
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_left = 1;
+ }
+ } else swarm[target].status_rf_command_left = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_right ( char target, char request_response, float speed )
+{
+ char command = 0x14;
+ if(request_response == 1) command+=128;
+ char length = 2;
+ char data [2];
+ float qspeed = speed + 1;
+ qspeed *= 32768.0;
+ int ispeed = (int) qspeed;
+ data[0] = (char) (ispeed >> 8);
+ data[1] = (char) (ispeed % 256);
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_right = 1;
+ }
+ } else swarm[target].status_rf_command_right = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_left_motor ( char target, char request_response, float speed )
+{
+ char command = 0x15;
+ if(request_response == 1) command+=128;
+ char length = 2;
+ char data [2];
+ float qspeed = speed + 1;
+ qspeed *= 32768.0;
+ int ispeed = (int) qspeed;
+ data[0] = (char) (ispeed >> 8);
+ data[1] = (char) (ispeed % 256);
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_left_motor = 1;
+ }
+ } else swarm[target].status_rf_command_left_motor = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_right_motor ( char target, char request_response, float speed )
+{
+ char command = 0x16;
+ if(request_response == 1) command+=128;
+ char length = 2;
+ char data [2];
+ float qspeed = speed + 1;
+ qspeed *= 32768.0;
+ int ispeed = (int) qspeed;
+ data[0] = (char) (ispeed >> 8);
+ data[1] = (char) (ispeed % 256);
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_right_motor = 1;
+ }
+ } else swarm[target].status_rf_command_right_motor = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_oled_colour ( char target, char request_response, char red, char green, char blue )
+{
+ char command = 0x17;
+ if(request_response == 1) command+=128;
+ char length = 3;
+ char data [3];
+ data[0] = red;
+ data[1] = green;
+ data[2] = blue;
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_oled_colour = 1;
+ }
+ } else swarm[target].status_rf_command_oled_colour = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_cled_colour ( char target, char request_response, char red, char green, char blue )
+{
+ char command = 0x18;
+ if(request_response == 1) command+=128;
+ char length = 3;
+ char data [3];
+ data[0] = red;
+ data[1] = green;
+ data[2] = blue;
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_cled_colour = 1;
+ }
+ } else swarm[target].status_rf_command_cled_colour = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_oled_state ( char target, char request_response, char led0, char led1, char led2, char led3, char led4, char led5, char led6, char led7, char led8, char led9 )
+{
+ char command = 0x19;
+ if(request_response == 1) command+=128;
+ char length = 2;
+ char data [2];
+ data[0] = 0;
+ data[1] = 0;
+ if( led0 == 1) data[0] += 2;
+ if( led1 == 1) data[0] += 1;
+ if( led2 == 1) data[1] += 128;
+ if( led3 == 1) data[1] += 64;
+ if( led4 == 1) data[1] += 32;
+ if( led5 == 1) data[1] += 16;
+ if( led6 == 1) data[1] += 8;
+ if( led7 == 1) data[1] += 4;
+ if( led8 == 1) data[1] += 2;
+ if( led9 == 1) data[1] += 1;
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_oled_state = 1;
+ }
+ } else swarm[target].status_rf_command_oled_state = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_cled_state ( char target, char request_response, char enable )
+{
+ char command = 0x1A;
+ if(request_response == 1) command+=128;
+ char length = 1;
+ char data [1];
+ data[0] = enable;
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_cled_state = 1;
+ }
+ } else swarm[target].status_rf_command_cled_state = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_set_oled ( char target, char request_response, char oled, char enable )
+{
+ char command = 0x1B;
+ if(request_response == 1) command+=128;
+ char length = 1;
+ char data [1];
+ data[0] = oled;
+ if(enable == 1) oled+= 32;
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_set_oled = 1;
+ }
+ } else swarm[target].status_rf_command_set_oled = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_play_tune ( char target, char request_response, char * data, char length )
+{
+ char command = 0x1C;
+ if(request_response == 1) command+=128;
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_play_tune = 1;
+ }
+ } else swarm[target].status_rf_command_play_tune = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_sync_time ( char target, char request_response )
+{
+ char command = 0x1D;
+ if(request_response == 1) command+=128;
+ char length = 1;
+ char data [1];
+ data[0] = 0;
+ if(request_response == 1) {
+ command+=128;
+ if(target == 0) {
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_command_sync_time = 1;
+ }
+ } else swarm[target].status_rf_command_sync_time = 1;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+//Resets the recorded swarm data tables
+void setup_communications()
+{
+ for(int i=0; i<SWARM_SIZE; i++) {
+ swarm[i].status_rf_request_null = 0;
+ swarm[i].status_rf_request_left_motor_speed = 0;
+ swarm[i].status_rf_request_right_motor_speed = 0;
+ swarm[i].status_rf_request_button_state = 0;
+ swarm[i].status_rf_request_led_colour = 0;
+ swarm[i].status_rf_request_led_states = 0;
+ swarm[i].status_rf_request_battery = 0;
+ swarm[i].status_rf_request_light_sensor = 0;
+ swarm[i].status_rf_request_accelerometer = 0;
+ swarm[i].status_rf_request_gyroscope = 0;
+ swarm[i].status_rf_request_background_ir = 0;
+ swarm[i].status_rf_request_reflected_ir = 0;
+ swarm[i].status_rf_request_distance_ir = 0;
+ swarm[i].status_rf_request_line_following_ir = 0;
+ swarm[i].status_rf_request_uptime = 0;
+ swarm[i].status_rf_command_stop = 0;
+ swarm[i].status_rf_command_forward = 0;
+ swarm[i].status_rf_command_backward = 0;
+ swarm[i].status_rf_command_left = 0;
+ swarm[i].status_rf_command_right = 0;
+ swarm[i].status_rf_command_left_motor = 0;
+ swarm[i].status_rf_command_right_motor = 0;
+ swarm[i].status_rf_command_oled_colour = 0;
+ swarm[i].status_rf_command_cled_colour = 0;
+ swarm[i].status_rf_command_oled_state = 0;
+ swarm[i].status_rf_command_cled_state = 0;
+ swarm[i].status_rf_command_set_oled = 0;
+ swarm[i].status_rf_command_play_tune = 0;
+ swarm[i].status_rf_command_sync_time = 0;
+ swarm[i].left_motor_speed = 0.0;
+ swarm[i].right_motor_speed = 0.0;
+ swarm[i].button_state = 0;
+ for(int k=0; k<3; k++) {
+ swarm[i].outer_led_colour [k]=0;
+ swarm[i].center_led_colour [k]=0;
+ swarm[i].accelerometer [k]=0;
+ }
+ swarm[i].led_states[0]=0;
+ swarm[i].led_states[1]=0;
+ swarm[i].battery = 0.0;
+ swarm[i].light_sensor = 0.0;
+ swarm[i].gyro = 0.0;
+ }
+}