James Hilder
Handheld controller (RF) for Pi Swarm system
Fork of
Pi_Swarm_Handheld_Controller
by 
piswarm
Diff: communications.cpp
- Revision:
- 0:d63a63feb104
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/communications.cpp Tue Jun 10 11:05:23 2014 +0000
@@ -0,0 +1,1609 @@
+/*******************************************************************************************
+ *
+ * University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ *
+ * Version 0.5 February 2014
+ *
+ * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
+ *
+ ******************************************************************************************/
+
+// 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 "mbed.h"
+#include "communications.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;
+
+
+// Send a structured message over the RF interface
+// @target - The target recipient (1-31 or 0 for broadcast)
+// @command - The command byte (see manual)
+// @*data - Additional data bytes
+// @length - Length of additional data
+void send_rf_message(char target, char command, char * data, char length)
+{
+ char message [5+length];
+ message_id++;
+ message[0] = 32;
+ message[1] = target+32;
+ message[2] = message_id%256;
+ message[3] = command;
+ for(int i=0;i<length;i++){
+ message[4+i] = data[i];
+ }
+ message[4+length]=NULL;
+ rf.sendString(4+length,message);
+ if(RF_DEBUG==1)pc.printf("RF message sent");
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Internal Functions
+// In general these functions should not be called by user code
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Decode the received message header. Check it is min. 4 bytes long, that the sender and target are valid [called in alpha433.cpp]
+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
+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;
+ 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) {
+ 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 {
+ //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;
+ } else if (RF_DEBUG==1) pc.printf(" - Ignored\n");
+}
+
+//Send a predefined response message
+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]=0;
+ 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];
+ }
+ /*TDMA not relevant to handheld controller
+ //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_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n");
+ } 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 {
+ */
+ rf.sendString(4+length,message);
+ if(RF_DEBUG==1)pc.printf("Response issued");
+ //}
+}
+
+// Send a delayed response
+void tdma_response()
+{
+ rf.sendString(4+waiting_length,waiting_message);
+ tdma_busy = 0;
+ if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
+}
+
+// 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) {
+ 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;
+ case 1: // Forward [2 bytes: 16-bit signed short]
+ if(length==2) {
+ int i_speed = (data[0] << 8) + data[1];
+ float speed = i_speed / 32768.0;
+ speed--;
+ //piswarm.forward(speed);
+ success = 1;
+ if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed);
+ } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+ break;
+ case 2: // Backward [2 bytes: 16-bit signed short]
+ if(length==2) {
+ int i_speed = (data[0] << 8) + data[1];
+ float speed = i_speed / 32768.0;
+ speed--;
+ //piswarm.backward(speed);
+ success = 1;
+ if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed);
+ } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+ break;
+ case 3: // Left [2 bytes: 16-bit signed short]
+ if(length==2) {
+ int i_speed = (data[0] << 8) + data[1];
+ float speed = i_speed / 32768.0;
+ speed--;
+ //piswarm.left(speed);
+ success = 1;
+ if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed);
+ } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+ break;
+ case 4: // Right [2 bytes: 16-bit signed short]
+ if(length==2) {
+ int i_speed = (data[0] << 8) + data[1];
+ float speed = i_speed / 32768.0;
+ speed--;
+ //piswarm.right(speed);
+ success = 1;
+ if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed);
+ } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+ break;
+ case 5: // Left Motor [2 bytes: 16-bit signed short]
+ if(length==2) {
+ int i_speed = (data[0] << 8) + data[1];
+ float speed = i_speed / 32768.0;
+ speed--;
+ //piswarm.left_motor(speed);
+ success = 1;
+ if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed);
+ } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+ break;
+ case 6: // Right Motor [2 bytes: 16-bit signed short]
+ if(length==2) {
+ int i_speed = (data[0] << 8) + data[1];
+ float speed = i_speed / 32768.0;
+ speed--;
+ //piswarm.right_motor(speed);
+ success = 1;
+ if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed);
+ } 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]);
+ 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]);
+ 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)));
+ 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);
+ 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) {
+ // 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;
+ case 12: // Play sound [Minimum 1 byte]
+ if(length>0) {
+ //piswarm.play_tune(data,length);
+ success = 1;
+ } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+ break;
+ case 13: // Sync time
+ if(length==4) {
+ unsigned int new_time = 0;
+ new_time+=((unsigned int)data[0] << 24);
+ new_time+=((unsigned int)data[1] << 16);
+ new_time+=((unsigned int)data[2] << 8);
+ new_time+=(unsigned int)data[3];
+ set_time(new_time);
+ //display_system_time();
+ } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
+ break;
+ case 14: //
+ break;
+ case 15: //
+ break;
+ }
+ if(request_response == 1) {
+ send_response(sender, is_broadcast, success, id, 1, function, NULL, 0);
+ }
+
+}
+
+//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) {
+ case 0: // Null request
+ success=1;
+ break;
+ case 1: { // Request left motor speed
+ response_length = 2;
+ float speed = 0;//piswarm.get_left_motor() * 32767;
+ int a_speed = 32768 + (int) speed;
+ char msb = (char) (a_speed / 256);
+ char lsb = (char) (a_speed % 256);
+ response = new char[2];
+ response[0]=msb;
+ response[1]=lsb;
+ success=1;
+ break;
+ }
+ case 2: { // Request right motor speed
+ response_length = 2;
+ float speed = 0;//piswarm.get_right_motor() * 32767;
+ int a_speed = 32768 + (int) speed;
+ char msb = (char) (a_speed / 256);
+ char lsb = (char) (a_speed % 256);
+ response = new char[2];
+ response[0]=msb;
+ response[1]=lsb;
+ success=1;
+ break;
+ }
+ case 3: { // Request button state
+ response_length = 1;
+ response = new char[1];
+ response[0]=0;//piswarm.get_switches();
+ break;
+ }
+ case 4: { // Request LED colours
+ response_length = 6;
+ response = new char[6];
+ int oled_colour = 0;//piswarm.get_oled_colour();
+ int cled_colour = 0;//piswarm.get_cled_colour();
+ response[0] = (char) (oled_colour >> 16);
+ response[1] = (char) ((oled_colour >> 8) % 256);
+ response[2] = (char) (oled_colour % 256);
+ 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
+ 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 = 0;//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
+ response_length = 2;
+ response = new char[2];
+ float flight = 0;//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
+ response_length = 6;
+ response = new char[6];
+ int acc_x = 0;//(int)piswarm.read_accelerometer_x();
+ int acc_y = 0;//(int)piswarm.read_accelerometer_y();
+ int acc_z = 0;//(int)piswarm.read_accelerometer_z();
+ acc_x += 32768;
+ acc_y += 32768;
+ acc_z += 32768;
+ response[0]=acc_x >> 8;
+ response[1]=acc_x % 256;
+ response[2]=acc_y >> 8;
+ response[3]=acc_y % 256;
+ response[4]=acc_z >> 8;
+ response[5]=acc_z % 256;
+ break;
+ }
+ case 9: { // Request gyroscope reading
+ response_length = 2;
+ response = new char[2];
+ float fgyro = 0;//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
+ response_length = 16;
+ response = new char[16];
+ for(int sensor = 0; sensor < 8; sensor ++){
+ int offset = sensor * 2;
+ unsigned short m_val = 0;//piswarm.read_adc_value(sensor);
+ response[offset]=m_val >> 8;
+ response[offset+1]=m_val % 256;
+ }
+ break;
+ }
+ case 11: { // Request illuminated IR reading
+ response_length = 16;
+ response = new char[16];
+ for(int sensor = 0; sensor < 8; sensor ++){
+ int offset = sensor * 2;
+ unsigned short m_val = 0;//piswarm.read_illuminated_raw_ir_value(sensor);
+ response[offset]=m_val >> 8;
+ response[offset+1]=m_val % 256;
+ }
+ break;
+ }
+
+ case 12: { // Request distance IR reading
+ response_length = 16;
+ response = new char[16];
+ for(int sensor = 0; sensor < 8; sensor ++){
+ int offset = sensor * 2;
+ float f_val = 0;//piswarm.read_reflected_ir_distance(sensor);
+ //f_val ranges from 0 to 100.0;
+ f_val *= 655;
+ unsigned short m_val = (unsigned short) f_val;
+ response[offset]=m_val >> 8;
+ response[offset+1]=m_val % 256;
+ }
+ break;
+ }
+
+ case 13: // Request line-tracking IR reading
+ break;
+ case 14: // Request uptime
+ break;
+ case 15: //
+ break;
+ }
+ send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
+
+}
+
+
+void errormessage(int index)
+{
+ if(RF_USE_LEDS==1) errorled=1;
+ 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");
+ break;
+ 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;
+ }
+}
+
+
+
+
+
+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 [0];
+ if(request_response == 1) {
+ command+=64;
+ 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;
+ 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+=64;
+ 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;
+ 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+=64;
+ 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;
+ }
+ send_rf_message(target,command,data,length);
+}
+
+void send_rf_command_left ( char target, char request_response, float speed )
+{
+ char command = 0x13;
+ 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+=64;
+ 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;
+ 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+=64;
+ 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;
+ 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+=64;
+ 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;
+ 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+=64;
+ 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;
+ char length = 3;
+ char data [3];
+ data[0] = red;
+ data[1] = green;
+ data[2] = blue;
+ if(request_response == 1) {
+ command+=64;
+ 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;
+ char length = 3;
+ char data [3];
+ data[0] = red;
+ data[1] = green;
+ data[2] = blue;
+ if(request_response == 1) {
+ command+=64;
+ 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;
+ 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+=64;
+ 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;
+ char length = 1;
+ char data [1];
+ data[0] = enable;
+ if(request_response == 1) {
+ command+=64;
+ 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;
+ char length = 1;
+ char data [1];
+ data[0] = oled;
+ if(enable == 1) oled+= 32;
+ if(request_response == 1) {
+ command+=64;
+ 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+=64;
+ 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;
+ char length = 1;
+ char data [1];
+ data[0] = 0;
+ if(request_response == 1) {
+ command+=64;
+ 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;
+ for(int k=0; k<8; k++) {
+ swarm[i].background_ir[k] = 0;
+ swarm[i].reflected_ir[k] = 0;
+ swarm[i].distance_ir[k] = 0;
+ }
+ }
+}
+
+
+// Handle a message that is a response to a predefined (non-user) command or request
+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(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_request_null = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_null= 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_request_null = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 1: { //Left motor speed
+ if(swarm[sender].status_rf_request_left_motor_speed == 1) {
+ if(success == 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 {
+ swarm[sender].status_rf_request_left_motor_speed = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 2: { //Right motor speed
+ if(swarm[sender].status_rf_request_right_motor_speed == 1) {
+ if(success == 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 {
+ swarm[sender].status_rf_request_right_motor_speed = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 3: { //Button state
+ if(swarm[sender].status_rf_request_button_state == 1) {
+ if(success == 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 {
+ swarm[sender].status_rf_request_button_state = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 4: { //LED Colour
+ if(swarm[sender].status_rf_request_led_colour == 1) {
+ if(success == 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 {
+ swarm[sender].status_rf_request_led_colour = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 5: { //LED States
+ if(swarm[sender].status_rf_request_led_states == 1) {
+ if(success == 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 {
+ swarm[sender].status_rf_request_led_states = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+
+ case 6: { //Battery
+ if(swarm[sender].status_rf_request_battery == 1) {
+ if(success == 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 {
+ swarm[sender].status_rf_request_battery = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 7: { //Light sensor
+ if(swarm[sender].status_rf_request_light_sensor == 1) {
+ if(success == 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 {
+ swarm[sender].status_rf_request_light_sensor = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 8: { //Accelerometer
+ if(swarm[sender].status_rf_request_accelerometer == 1) {
+ if(success == 1) {
+ if(length == 6) {
+ swarm[sender].status_rf_request_accelerometer = 2;
+ int acc_x = (data[0] * 256) + data[1];
+ int acc_y = (data[2] * 256) + data[3];
+ int acc_z = (data[4] * 256) + data[5];
+ swarm[sender].accelerometer[0] = (float) acc_x - 32768;
+ swarm[sender].accelerometer[1] = (float) acc_y - 32768;
+ swarm[sender].accelerometer[2] = (float) acc_z - 32768;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_accelerometer = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_request_accelerometer = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 9: { //Gyroscope
+ if(swarm[sender].status_rf_request_gyroscope == 1) {
+ if(success == 1) {
+ if(length == 2) {
+ swarm[sender].status_rf_request_gyroscope = 2;
+ int gyro = (data [0] * 256) + data[1];
+ swarm[sender].gyro = (float) gyro - 32768;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_gyroscope = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_request_gyroscope = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 10: { //Background IR
+ if(swarm[sender].status_rf_request_background_ir == 1) {
+ if(success == 1) {
+ if(length == 16) {
+ swarm[sender].status_rf_request_background_ir = 2;
+ for(int i=0;i<8;i++){
+ int offset = i * 2;
+ unsigned short value = (unsigned short) data[offset] << 8;
+ value += data[offset+1];
+ swarm[sender].background_ir[i]=value;
+ }
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_background_ir = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_request_background_ir = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 11: { //Reflected IR
+ if(swarm[sender].status_rf_request_reflected_ir == 1) {
+ if(success == 1) {
+ if(length == 16) {
+ swarm[sender].status_rf_request_reflected_ir = 2;
+ for(int i=0;i<8;i++){
+ int offset = i * 2;
+ unsigned short value = (unsigned short) data[offset] << 8;
+ value += data[offset+1];
+ swarm[sender].reflected_ir[i]=value;
+ }
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_reflected_ir = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_request_reflected_ir = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 12: { // Distance IR
+ if(swarm[sender].status_rf_request_distance_ir == 1) {
+ if(success == 1) {
+ if(length == 16) {
+ swarm[sender].status_rf_request_distance_ir = 2;
+ for(int i=0;i<8;i++){
+ int offset = i * 2;
+ unsigned short value = (unsigned short) data[offset] << 8;
+ value += data[offset+1];
+ float adjusted = (float) value / 655.0;
+ swarm[sender].distance_ir[i]=adjusted;
+ }
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_distance_ir = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_request_distance_ir = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 13: { // Line following IR
+ if(swarm[sender].status_rf_request_line_following_ir == 1) {
+ if(success == 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 {
+ swarm[sender].status_rf_request_line_following_ir = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ case 14: { // Request uptime
+ if(swarm[sender].status_rf_request_uptime == 1) {
+ if(success == 1) {
+ if(length == 4) {
+ swarm[sender].status_rf_request_uptime = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_request_uptime = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_request_uptime = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+
+ }
+ } else {
+ // Response to a command
+ switch(function) {
+ case 0: {
+ if(swarm[sender].status_rf_command_stop == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_stop = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_stop = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_stop = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 1: {
+ if(swarm[sender].status_rf_command_forward == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_forward = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_forward = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_forward = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 2: {
+ if(swarm[sender].status_rf_command_backward == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_backward = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_backward = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_backward = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 3: {
+ if(swarm[sender].status_rf_command_left == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_left = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_left = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_left = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 4: {
+ if(swarm[sender].status_rf_command_right == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_right = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_right = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_right = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 5: {
+ if(swarm[sender].status_rf_command_left_motor == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_left_motor = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_left_motor = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_left_motor = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 6: {
+ if(swarm[sender].status_rf_command_right_motor == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_right_motor = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_right_motor = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_right_motor = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 7: {
+ if(swarm[sender].status_rf_command_oled_colour == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_oled_colour = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_oled_colour = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_oled_colour = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 8: {
+ if(swarm[sender].status_rf_command_cled_colour == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_cled_colour = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_cled_colour = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_cled_colour = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 9: {
+ if(swarm[sender].status_rf_command_oled_state == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_oled_state = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_oled_state = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_oled_state = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 10: {
+ if(swarm[sender].status_rf_command_cled_state == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_cled_state = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_cled_state = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_cled_state = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 11: {
+ if(swarm[sender].status_rf_command_set_oled == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_set_oled = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_set_oled = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_set_oled = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 12: {
+ if(swarm[sender].status_rf_command_play_tune == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_play_tune = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_play_tune = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_play_tune = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ case 14: {
+ if(swarm[sender].status_rf_command_sync_time == 1) {
+ if(success == 1){
+ if(length == 0) {
+ swarm[sender].status_rf_command_sync_time = 2;
+ outcome = 1;
+ } else {
+ swarm[sender].status_rf_command_sync_time = 3;
+ outcome = 3;
+ }
+ } else {
+ swarm[sender].status_rf_command_sync_time = 4;
+ outcome = 4;
+ }
+ } else outcome = 2;
+ }
+ break;
+ }
+ }
+
+ 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.");
+ case 4 :
+ pc.printf("RF response received: unsuccessful operation.");
+ }
+ }
+}
\ No newline at end of file