James Hilder / Pi_Swarm_Library

V0.5 library for the Pi Swarm robot

communications.cpp@2:0a739218ab11, 2014-02-02 (annotated)

Committer:
jah128
Date:
Sun Feb 02 20:37:48 2014 +0000
Revision:
2:0a739218ab11
Parent:
1:b067a08ff54e
Child:
3:4c0f2f3de33e
updated comms;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:9ffe8ebd1c40 1 /* University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
jah128 0:9ffe8ebd1c40 2 *
jah128 0:9ffe8ebd1c40 3 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
jah128 1:b067a08ff54e 4 *
jah128 0:9ffe8ebd1c40 5 * Version 0.4 January 2014
jah128 0:9ffe8ebd1c40 6 *
jah128 0:9ffe8ebd1c40 7 * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
jah128 0:9ffe8ebd1c40 8 *
jah128 0:9ffe8ebd1c40 9 */
jah128 1:b067a08ff54e 10
jah128 1:b067a08ff54e 11 // Important note: The communication stack is enabled by setting the "USE_COMMUNICATION_STACK" flag to 1
jah128 1:b067a08ff54e 12 // When being used, all received messages are decoded using the decodeMessage() function
jah128 1:b067a08ff54e 13 // See manual for more info on using the communication handler
jah128 1:b067a08ff54e 14 #include "communications.h"
jah128 1:b067a08ff54e 15 #include "piswarm.h"
jah128 1:b067a08ff54e 16 #include "main.h"
jah128 1:b067a08ff54e 17
jah128 1:b067a08ff54e 18 struct swarm_member swarm[SWARM_SIZE]; // Array to hold received information about other swarm members
jah128 1:b067a08ff54e 19 DigitalOut actioning (LED1);
jah128 1:b067a08ff54e 20 DigitalOut errorled (LED2);
jah128 1:b067a08ff54e 21 DigitalOut tx (LED3);
jah128 1:b067a08ff54e 22 DigitalOut rx (LED4);
jah128 1:b067a08ff54e 23 Timeout tdma_timeout;
jah128 1:b067a08ff54e 24 char tdma_busy = 0;
jah128 1:b067a08ff54e 25 char waiting_message [64];
jah128 1:b067a08ff54e 26 char waiting_length = 0;
jah128 1:b067a08ff54e 27 int message_id = 0;
jah128 1:b067a08ff54e 28
jah128 2:0a739218ab11 29
jah128 2:0a739218ab11 30 // Send a structured message over the RF interface
jah128 2:0a739218ab11 31 // @target - The target recipient (1-31 or 0 for broadcast)
jah128 2:0a739218ab11 32 // @command - The command byte (see manual)
jah128 2:0a739218ab11 33 // @*data - Additional data bytes
jah128 2:0a739218ab11 34 // @length - Length of additional data
jah128 1:b067a08ff54e 35 void send_rf_message(char target, char command, char * data, char length)
jah128 1:b067a08ff54e 36 {
jah128 1:b067a08ff54e 37 char message [4+length];
jah128 2:0a739218ab11 38 message[0]=piswarm.get_id() + 32;
jah128 2:0a739218ab11 39 message[1]=target + 32;
jah128 1:b067a08ff54e 40 message_id++;
jah128 1:b067a08ff54e 41 message[2]=message_id % 256;
jah128 1:b067a08ff54e 42 message[3]=command;
jah128 1:b067a08ff54e 43 for(int i=0; i<length; i++) {
jah128 1:b067a08ff54e 44 message[4+i]=data[i];
jah128 1:b067a08ff54e 45 }
jah128 1:b067a08ff54e 46 piswarm.send_rf_message(message,4+length);
jah128 1:b067a08ff54e 47 if(RF_DEBUG==1)pc.printf("RF message sent");
jah128 1:b067a08ff54e 48 }
jah128 1:b067a08ff54e 49
jah128 0:9ffe8ebd1c40 50
jah128 1:b067a08ff54e 51 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 1:b067a08ff54e 52 // Internal Functions
jah128 1:b067a08ff54e 53 // In general these functions should not be called by user code
jah128 1:b067a08ff54e 54 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:9ffe8ebd1c40 55
jah128 2:0a739218ab11 56 // Decode the received message header. Check it is min. 4 bytes long, that the sender and target are valid [called in alpha433.cpp]
jah128 1:b067a08ff54e 57 void processRadioData(char * data, char length)
jah128 1:b067a08ff54e 58 {
jah128 1:b067a08ff54e 59 if(RF_USE_LEDS==1) {
jah128 1:b067a08ff54e 60 errorled=0;
jah128 1:b067a08ff54e 61 rx=1;
jah128 1:b067a08ff54e 62 }
jah128 1:b067a08ff54e 63 // Decompose the received message
jah128 1:b067a08ff54e 64 if(length < 4) errormessage(0);
jah128 1:b067a08ff54e 65 else {
jah128 1:b067a08ff54e 66 // Establish the sender and target of the packet
jah128 1:b067a08ff54e 67 char sender = data[0];
jah128 1:b067a08ff54e 68 char target = data[1];
jah128 1:b067a08ff54e 69 char id = data[2];
jah128 1:b067a08ff54e 70 char command = data[3];
jah128 1:b067a08ff54e 71 if(sender<32 || sender>63)errormessage(1);
jah128 1:b067a08ff54e 72 else {
jah128 1:b067a08ff54e 73 if(target<32 || target>63)errormessage(2);
jah128 1:b067a08ff54e 74 else {
jah128 1:b067a08ff54e 75 sender -= 32;
jah128 1:b067a08ff54e 76 target -= 32;
jah128 1:b067a08ff54e 77 decodeMessage(sender,target,id,command,data+4,length-4);
jah128 1:b067a08ff54e 78 }
jah128 1:b067a08ff54e 79 }
jah128 1:b067a08ff54e 80 }
jah128 1:b067a08ff54e 81 if(RF_USE_LEDS==1) rx=0;
jah128 1:b067a08ff54e 82 }
jah128 1:b067a08ff54e 83
jah128 2:0a739218ab11 84 //Decode the received message, action it if it is valid and for me
jah128 1:b067a08ff54e 85 void decodeMessage(char sender, char target, char id, char command, char * data, char length)
jah128 1:b067a08ff54e 86 {
jah128 0:9ffe8ebd1c40 87 char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0;
jah128 1:b067a08ff54e 88
jah128 1:b067a08ff54e 89 if(target==0) broadcast_message = 1;
jah128 0:9ffe8ebd1c40 90 is_response = 0 != (command & (1 << 7));
jah128 0:9ffe8ebd1c40 91 request_response = 0 != (command & (1 << 6));
jah128 0:9ffe8ebd1c40 92 is_user = 0 != (command & (1 << 5));
jah128 0:9ffe8ebd1c40 93 is_command = 0 != (command & (1 << 4));
jah128 0:9ffe8ebd1c40 94 function = command % 16;
jah128 1:b067a08ff54e 95
jah128 0:9ffe8ebd1c40 96 if (RF_DEBUG==1) {
jah128 0:9ffe8ebd1c40 97 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);
jah128 0:9ffe8ebd1c40 98 else pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, requests_array[function],length);
jah128 0:9ffe8ebd1c40 99 }
jah128 1:b067a08ff54e 100
jah128 0:9ffe8ebd1c40 101 //Action the message only if I am a recipient
jah128 1:b067a08ff54e 102 if(target==0 || target==piswarm.get_id()) {
jah128 0:9ffe8ebd1c40 103 if(RF_USE_LEDS==1) actioning = 1;
jah128 1:b067a08ff54e 104 if(is_response == 1) {
jah128 1:b067a08ff54e 105 if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 1:b067a08ff54e 106 else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 1:b067a08ff54e 107 } else {
jah128 1:b067a08ff54e 108 if(is_command == 1) {
jah128 1:b067a08ff54e 109 if(RF_ALLOW_COMMANDS == 1) {
jah128 1:b067a08ff54e 110 if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 1:b067a08ff54e 111 else handle_command(sender, broadcast_message, request_response, id, function, data, length);
jah128 1:b067a08ff54e 112 } else if (RF_DEBUG==1) pc.printf(" - Blocked\n");
jah128 1:b067a08ff54e 113 } else {
jah128 0:9ffe8ebd1c40 114 //A information request has no extra parameters
jah128 1:b067a08ff54e 115 if(length == 0) {
jah128 1:b067a08ff54e 116 if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 1:b067a08ff54e 117 else handle_request(sender, broadcast_message, request_response, id, function);
jah128 1:b067a08ff54e 118 } else if (RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 119 }
jah128 1:b067a08ff54e 120 }
jah128 1:b067a08ff54e 121 if(RF_USE_LEDS==1) actioning = 0;
jah128 0:9ffe8ebd1c40 122 } else if (RF_DEBUG==1) pc.printf(" - Ignored\n");
jah128 0:9ffe8ebd1c40 123 }
jah128 0:9ffe8ebd1c40 124
jah128 2:0a739218ab11 125 //Send a predefined response message
jah128 1:b067a08ff54e 126 void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
jah128 1:b067a08ff54e 127 {
jah128 0:9ffe8ebd1c40 128 char message [4+length];
jah128 0:9ffe8ebd1c40 129 message[0]=piswarm.get_id();
jah128 0:9ffe8ebd1c40 130 message[1]=target;
jah128 0:9ffe8ebd1c40 131 message[2]=id;
jah128 0:9ffe8ebd1c40 132 message[3]=128 + (success << 6) + (is_command << 4) + function;
jah128 1:b067a08ff54e 133 for(int i=0; i<length; i++) {
jah128 1:b067a08ff54e 134 message[4+i]=data[i];
jah128 1:b067a08ff54e 135 }
jah128 0:9ffe8ebd1c40 136 //Delay the response if it is broadcast and TDMA mode is on
jah128 1:b067a08ff54e 137 if(RF_USE_TDMA == 1 && is_broadcast == 1) {
jah128 1:b067a08ff54e 138 if(tdma_busy == 1) {
jah128 0:9ffe8ebd1c40 139 if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n");
jah128 1:b067a08ff54e 140 } else {
jah128 0:9ffe8ebd1c40 141 tdma_busy = 1;
jah128 0:9ffe8ebd1c40 142 strcpy(waiting_message,message);
jah128 0:9ffe8ebd1c40 143 waiting_length=length;
jah128 0:9ffe8ebd1c40 144 tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id());
jah128 0:9ffe8ebd1c40 145 if (RF_DEBUG==1) pc.printf("TDMA Response pending\n");
jah128 0:9ffe8ebd1c40 146 }
jah128 1:b067a08ff54e 147 } else {
jah128 0:9ffe8ebd1c40 148 piswarm.send_rf_message(message,4+length);
jah128 0:9ffe8ebd1c40 149 if(RF_DEBUG==1)pc.printf("Response issued");
jah128 0:9ffe8ebd1c40 150 }
jah128 0:9ffe8ebd1c40 151 }
jah128 0:9ffe8ebd1c40 152
jah128 0:9ffe8ebd1c40 153 // Send a delayed response
jah128 1:b067a08ff54e 154 void tdma_response()
jah128 1:b067a08ff54e 155 {
jah128 0:9ffe8ebd1c40 156 piswarm.send_rf_message(waiting_message,4+waiting_length);
jah128 0:9ffe8ebd1c40 157 tdma_busy = 0;
jah128 0:9ffe8ebd1c40 158 if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
jah128 0:9ffe8ebd1c40 159 }
jah128 0:9ffe8ebd1c40 160
jah128 1:b067a08ff54e 161 // Handle a message that is a predefined command
jah128 1:b067a08ff54e 162 void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length)
jah128 1:b067a08ff54e 163 {
jah128 0:9ffe8ebd1c40 164 char success = 0;
jah128 1:b067a08ff54e 165 switch(function) {
jah128 0:9ffe8ebd1c40 166 case 0: // Stop [0 data]
jah128 0:9ffe8ebd1c40 167 if(length==0) {
jah128 0:9ffe8ebd1c40 168 piswarm.stop();
jah128 0:9ffe8ebd1c40 169 if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - ");
jah128 0:9ffe8ebd1c40 170 success = 1;
jah128 0:9ffe8ebd1c40 171 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 172 break;
jah128 0:9ffe8ebd1c40 173 case 1: // Forward [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 174 if(length==2) {
jah128 0:9ffe8ebd1c40 175 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 176 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 177 speed--;
jah128 0:9ffe8ebd1c40 178 piswarm.forward(speed);
jah128 0:9ffe8ebd1c40 179 success = 1;
jah128 0:9ffe8ebd1c40 180 if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 181 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 182 break;
jah128 0:9ffe8ebd1c40 183 case 2: // Backward [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 184 if(length==2) {
jah128 0:9ffe8ebd1c40 185 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 186 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 187 speed--;
jah128 0:9ffe8ebd1c40 188 piswarm.backward(speed);
jah128 0:9ffe8ebd1c40 189 success = 1;
jah128 0:9ffe8ebd1c40 190 if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 191 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 192 break;
jah128 0:9ffe8ebd1c40 193 case 3: // Left [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 194 if(length==2) {
jah128 0:9ffe8ebd1c40 195 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 196 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 197 speed--;
jah128 0:9ffe8ebd1c40 198 piswarm.left(speed);
jah128 0:9ffe8ebd1c40 199 success = 1;
jah128 0:9ffe8ebd1c40 200 if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 201 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 202 break;
jah128 0:9ffe8ebd1c40 203 case 4: // Right [2 bytes: 16-bit signed short]
jah128 1:b067a08ff54e 204 if(length==2) {
jah128 0:9ffe8ebd1c40 205 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 206 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 207 speed--;
jah128 0:9ffe8ebd1c40 208 piswarm.right(speed);
jah128 0:9ffe8ebd1c40 209 success = 1;
jah128 0:9ffe8ebd1c40 210 if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 211 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 212 break;
jah128 0:9ffe8ebd1c40 213 case 5: // Left Motor [2 bytes: 16-bit signed short]
jah128 1:b067a08ff54e 214 if(length==2) {
jah128 0:9ffe8ebd1c40 215 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 216 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 217 speed--;
jah128 0:9ffe8ebd1c40 218 piswarm.left_motor(speed);
jah128 0:9ffe8ebd1c40 219 success = 1;
jah128 0:9ffe8ebd1c40 220 if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 221 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 222 break;
jah128 0:9ffe8ebd1c40 223 case 6: // Right Motor [2 bytes: 16-bit signed short]
jah128 1:b067a08ff54e 224 if(length==2) {
jah128 0:9ffe8ebd1c40 225 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 226 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 227 speed--;
jah128 0:9ffe8ebd1c40 228 piswarm.right_motor(speed);
jah128 0:9ffe8ebd1c40 229 success = 1;
jah128 0:9ffe8ebd1c40 230 if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 231 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 232 break;
jah128 0:9ffe8ebd1c40 233 case 7: // Outer LED Colour [3 bytes: R, G, B]
jah128 1:b067a08ff54e 234 if(length==3) {
jah128 1:b067a08ff54e 235 piswarm.set_oled_colour (data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 236 success = 1;
jah128 0:9ffe8ebd1c40 237 if(RF_DEBUG==1) pc.printf(" - Set Outer R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 238 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 239 break;
jah128 0:9ffe8ebd1c40 240 case 8: // Center LED Colour[3 bytes: R, G, B]
jah128 1:b067a08ff54e 241 if(length==3) {
jah128 1:b067a08ff54e 242 piswarm.set_cled_colour (data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 243 success = 1;
jah128 0:9ffe8ebd1c40 244 if(RF_DEBUG==1) pc.printf(" - Set Center R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 245 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 246 break;
jah128 0:9ffe8ebd1c40 247 case 9: // Outer LED State [2 bytes: [xxxxxx01][23456789] ]
jah128 0:9ffe8ebd1c40 248 if(length==2) {
jah128 1:b067a08ff54e 249 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)));
jah128 0:9ffe8ebd1c40 250 success = 1;
jah128 0:9ffe8ebd1c40 251 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 252 break;
jah128 0:9ffe8ebd1c40 253 case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ]
jah128 0:9ffe8ebd1c40 254 if(length==1) {
jah128 1:b067a08ff54e 255 piswarm.enable_cled (data[0] % 2);
jah128 0:9ffe8ebd1c40 256 success = 1;
jah128 0:9ffe8ebd1c40 257 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 258 break;
jah128 0:9ffe8ebd1c40 259 case 11: // Set outer LED [1 byte: [xxxEvvvv] E=enabled vvvv=LED]
jah128 0:9ffe8ebd1c40 260 if(length==1) {
jah128 0:9ffe8ebd1c40 261 int led = data[0] % 16;
jah128 1:b067a08ff54e 262 if(led < 10) {
jah128 0:9ffe8ebd1c40 263 piswarm.set_oled(led, 0!=(data[0] & (1 << 4)));
jah128 0:9ffe8ebd1c40 264 success = 1;
jah128 0:9ffe8ebd1c40 265 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 266 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 267 break;
jah128 0:9ffe8ebd1c40 268 case 12: // Play sound [Minimum 1 byte]
jah128 0:9ffe8ebd1c40 269 if(length>0) {
jah128 0:9ffe8ebd1c40 270 piswarm.play_tune(data,length);
jah128 0:9ffe8ebd1c40 271 success = 1;
jah128 0:9ffe8ebd1c40 272 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 273 break;
jah128 0:9ffe8ebd1c40 274 case 13: // Sync time
jah128 0:9ffe8ebd1c40 275 if(length==4) {
jah128 0:9ffe8ebd1c40 276 unsigned int new_time = 0;
jah128 0:9ffe8ebd1c40 277 new_time+=((unsigned int)data[0] << 24);
jah128 0:9ffe8ebd1c40 278 new_time+=((unsigned int)data[1] << 16);
jah128 0:9ffe8ebd1c40 279 new_time+=((unsigned int)data[2] << 8);
jah128 0:9ffe8ebd1c40 280 new_time+=(unsigned int)data[3];
jah128 0:9ffe8ebd1c40 281 set_time(new_time);
jah128 0:9ffe8ebd1c40 282 display_system_time();
jah128 0:9ffe8ebd1c40 283 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 284 break;
jah128 0:9ffe8ebd1c40 285 case 14: //
jah128 0:9ffe8ebd1c40 286 break;
jah128 0:9ffe8ebd1c40 287 case 15: //
jah128 1:b067a08ff54e 288 break;
jah128 0:9ffe8ebd1c40 289 }
jah128 1:b067a08ff54e 290 if(request_response == 1) {
jah128 0:9ffe8ebd1c40 291 send_response(sender, is_broadcast, success, id, 1, function, NULL, 0);
jah128 0:9ffe8ebd1c40 292 }
jah128 1:b067a08ff54e 293
jah128 0:9ffe8ebd1c40 294 }
jah128 0:9ffe8ebd1c40 295
jah128 1:b067a08ff54e 296 //Handle a message that is a predefined request
jah128 1:b067a08ff54e 297 void handle_request(char sender, char is_broadcast, char request_response, char id, char function)
jah128 1:b067a08ff54e 298 {
jah128 0:9ffe8ebd1c40 299 int response_length = 0;
jah128 0:9ffe8ebd1c40 300 char * response = NULL;
jah128 0:9ffe8ebd1c40 301 char success = 0;
jah128 1:b067a08ff54e 302
jah128 1:b067a08ff54e 303 switch(function) {
jah128 0:9ffe8ebd1c40 304 case 0: // Null request
jah128 0:9ffe8ebd1c40 305 success=1;
jah128 1:b067a08ff54e 306 break;
jah128 1:b067a08ff54e 307 case 1: { // Request left motor speed
jah128 0:9ffe8ebd1c40 308 response_length = 2;
jah128 0:9ffe8ebd1c40 309 float speed = piswarm.get_left_motor() * 32767;
jah128 0:9ffe8ebd1c40 310 int a_speed = 32768 + (int) speed;
jah128 0:9ffe8ebd1c40 311 char msb = (char) (a_speed / 256);
jah128 0:9ffe8ebd1c40 312 char lsb = (char) (a_speed % 256);
jah128 0:9ffe8ebd1c40 313 response = new char[2];
jah128 0:9ffe8ebd1c40 314 response[0]=msb;
jah128 0:9ffe8ebd1c40 315 response[1]=lsb;
jah128 0:9ffe8ebd1c40 316 success=1;
jah128 1:b067a08ff54e 317 break;
jah128 1:b067a08ff54e 318 }
jah128 1:b067a08ff54e 319 case 2: { // Request right motor speed
jah128 0:9ffe8ebd1c40 320 response_length = 2;
jah128 0:9ffe8ebd1c40 321 float speed = piswarm.get_right_motor() * 32767;
jah128 0:9ffe8ebd1c40 322 int a_speed = 32768 + (int) speed;
jah128 0:9ffe8ebd1c40 323 char msb = (char) (a_speed / 256);
jah128 0:9ffe8ebd1c40 324 char lsb = (char) (a_speed % 256);
jah128 0:9ffe8ebd1c40 325 response = new char[2];
jah128 0:9ffe8ebd1c40 326 response[0]=msb;
jah128 0:9ffe8ebd1c40 327 response[1]=lsb;
jah128 0:9ffe8ebd1c40 328 success=1;
jah128 1:b067a08ff54e 329 break;
jah128 1:b067a08ff54e 330 }
jah128 1:b067a08ff54e 331 case 3: { // Request button state
jah128 0:9ffe8ebd1c40 332 response_length = 1;
jah128 0:9ffe8ebd1c40 333 response = new char[1];
jah128 0:9ffe8ebd1c40 334 response[0]=piswarm.get_switches();
jah128 1:b067a08ff54e 335 break;
jah128 1:b067a08ff54e 336 }
jah128 1:b067a08ff54e 337 case 4: { // Request LED colours
jah128 0:9ffe8ebd1c40 338 response_length = 6;
jah128 0:9ffe8ebd1c40 339 response = new char[6];
jah128 0:9ffe8ebd1c40 340 int oled_colour = piswarm.get_oled_colour();
jah128 0:9ffe8ebd1c40 341 int cled_colour = piswarm.get_cled_colour();
jah128 0:9ffe8ebd1c40 342 response[0] = (char) (oled_colour >> 16);
jah128 0:9ffe8ebd1c40 343 response[1] = (char) ((oled_colour >> 8) % 256);
jah128 0:9ffe8ebd1c40 344 response[2] = (char) (oled_colour % 256);
jah128 0:9ffe8ebd1c40 345 response[3] = (char) (cled_colour >> 16);
jah128 0:9ffe8ebd1c40 346 response[4] = (char) ((cled_colour >> 8) % 256);
jah128 0:9ffe8ebd1c40 347 response[5] = (char) (cled_colour % 256);
jah128 1:b067a08ff54e 348 break;
jah128 1:b067a08ff54e 349 }
jah128 1:b067a08ff54e 350 case 5: { // Request LED states
jah128 1:b067a08ff54e 351 response_length = 2;
jah128 1:b067a08ff54e 352 response = new char[2];
jah128 1:b067a08ff54e 353 response[0] = 0;
jah128 1:b067a08ff54e 354 response[1] = 0;
jah128 1:b067a08ff54e 355 if (piswarm.get_cled_state() == 1) response[0] += 4;
jah128 1:b067a08ff54e 356 if (piswarm.get_oled_state(0) == 1) response[0] += 2;
jah128 1:b067a08ff54e 357 if (piswarm.get_oled_state(1) == 1) response[0] += 1;
jah128 1:b067a08ff54e 358 if (piswarm.get_oled_state(2) == 1) response[1] += 128;
jah128 1:b067a08ff54e 359 if (piswarm.get_oled_state(3) == 1) response[1] += 64;
jah128 1:b067a08ff54e 360 if (piswarm.get_oled_state(4) == 1) response[1] += 32;
jah128 1:b067a08ff54e 361 if (piswarm.get_oled_state(5) == 1) response[1] += 16;
jah128 1:b067a08ff54e 362 if (piswarm.get_oled_state(6) == 1) response[1] += 8;
jah128 1:b067a08ff54e 363 if (piswarm.get_oled_state(7) == 1) response[1] += 4;
jah128 1:b067a08ff54e 364 if (piswarm.get_oled_state(8) == 1) response[1] += 2;
jah128 1:b067a08ff54e 365 if (piswarm.get_oled_state(9) == 1) response[1] += 1;
jah128 1:b067a08ff54e 366 break;
jah128 1:b067a08ff54e 367 }
jah128 1:b067a08ff54e 368 case 6: { // Request battery power
jah128 0:9ffe8ebd1c40 369 response_length = 2;
jah128 0:9ffe8ebd1c40 370 response = new char[2];
jah128 0:9ffe8ebd1c40 371 float fbattery = piswarm.battery() * 1000.0;
jah128 0:9ffe8ebd1c40 372 unsigned short battery = (unsigned short) fbattery;
jah128 0:9ffe8ebd1c40 373 response[0] = battery >> 8;
jah128 0:9ffe8ebd1c40 374 response[1] = battery % 256;
jah128 1:b067a08ff54e 375 break;
jah128 1:b067a08ff54e 376 }
jah128 1:b067a08ff54e 377 case 7: { // Request light sensor reading
jah128 0:9ffe8ebd1c40 378 response_length = 2;
jah128 0:9ffe8ebd1c40 379 response = new char[2];
jah128 0:9ffe8ebd1c40 380 float flight = piswarm.read_light_sensor() * 655.0;
jah128 0:9ffe8ebd1c40 381 unsigned short light = (unsigned short) flight;
jah128 0:9ffe8ebd1c40 382 response[0] = light >> 8;
jah128 0:9ffe8ebd1c40 383 response[1] = light % 256;
jah128 1:b067a08ff54e 384 break;
jah128 1:b067a08ff54e 385 }
jah128 1:b067a08ff54e 386 case 8: { // Request accelerometer reading
jah128 2:0a739218ab11 387 response_length = 6;
jah128 2:0a739218ab11 388 response = new char[6];
jah128 2:0a739218ab11 389 int acc_x = (int)piswarm.read_accelerometer_x();
jah128 2:0a739218ab11 390 int acc_y = (int)piswarm.read_accelerometer_y();
jah128 2:0a739218ab11 391 int acc_z = (int)piswarm.read_accelerometer_z();
jah128 2:0a739218ab11 392 acc_x += 32768;
jah128 2:0a739218ab11 393 acc_y += 32768;
jah128 2:0a739218ab11 394 acc_z += 32768;
jah128 2:0a739218ab11 395 response[0]=acc_x >> 8;
jah128 2:0a739218ab11 396 response[1]=acc_x % 256;
jah128 2:0a739218ab11 397 response[2]=acc_y >> 8;
jah128 2:0a739218ab11 398 response[3]=acc_y % 256;
jah128 2:0a739218ab11 399 response[4]=acc_z >> 8;
jah128 2:0a739218ab11 400 response[5]=acc_z % 256;
jah128 0:9ffe8ebd1c40 401 break;
jah128 1:b067a08ff54e 402 }
jah128 1:b067a08ff54e 403 case 9: { // Request gyroscope reading
jah128 1:b067a08ff54e 404 response_length = 2;
jah128 1:b067a08ff54e 405 response = new char[2];
jah128 1:b067a08ff54e 406 float fgyro = piswarm.read_gyro();
jah128 1:b067a08ff54e 407 fgyro += 32768;
jah128 1:b067a08ff54e 408 unsigned short sgyro = (unsigned short) fgyro;
jah128 1:b067a08ff54e 409 response[0] = sgyro >> 8;
jah128 1:b067a08ff54e 410 response[1] = sgyro % 256;
jah128 1:b067a08ff54e 411 break;
jah128 1:b067a08ff54e 412 }
jah128 0:9ffe8ebd1c40 413 case 10: // Request background IR reading
jah128 0:9ffe8ebd1c40 414 break;
jah128 0:9ffe8ebd1c40 415 case 11: // Request illuminated IR reading
jah128 1:b067a08ff54e 416 break;
jah128 0:9ffe8ebd1c40 417 case 12: // Request line-tracking IR reading
jah128 0:9ffe8ebd1c40 418 break;
jah128 0:9ffe8ebd1c40 419 case 13: // Request uptime
jah128 0:9ffe8ebd1c40 420 break;
jah128 1:b067a08ff54e 421 case 14: //
jah128 0:9ffe8ebd1c40 422 break;
jah128 0:9ffe8ebd1c40 423 case 15: //
jah128 1:b067a08ff54e 424 break;
jah128 1:b067a08ff54e 425 }
jah128 0:9ffe8ebd1c40 426 send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
jah128 1:b067a08ff54e 427
jah128 0:9ffe8ebd1c40 428 }
jah128 0:9ffe8ebd1c40 429
jah128 0:9ffe8ebd1c40 430
jah128 1:b067a08ff54e 431 void errormessage(int index)
jah128 1:b067a08ff54e 432 {
jah128 0:9ffe8ebd1c40 433 if(RF_USE_LEDS==1) errorled=1;
jah128 1:b067a08ff54e 434 switch(index) {
jah128 0:9ffe8ebd1c40 435 case 0: //Message to short
jah128 0:9ffe8ebd1c40 436 if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n");
jah128 0:9ffe8ebd1c40 437 break;
jah128 0:9ffe8ebd1c40 438 case 1: //Sender out of valid range
jah128 1:b067a08ff54e 439 if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");
jah128 0:9ffe8ebd1c40 440 break;
jah128 1:b067a08ff54e 441 case 2: //Target out of valid range
jah128 0:9ffe8ebd1c40 442 if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n");
jah128 0:9ffe8ebd1c40 443 break;
jah128 0:9ffe8ebd1c40 444 case 3:
jah128 1:b067a08ff54e 445
jah128 1:b067a08ff54e 446 break;
jah128 1:b067a08ff54e 447 case 4:
jah128 1:b067a08ff54e 448 break;
jah128 1:b067a08ff54e 449 }
jah128 1:b067a08ff54e 450 }
jah128 1:b067a08ff54e 451
jah128 1:b067a08ff54e 452
jah128 1:b067a08ff54e 453
jah128 1:b067a08ff54e 454
jah128 1:b067a08ff54e 455
jah128 1:b067a08ff54e 456 void send_rf_request_null ( char target )
jah128 1:b067a08ff54e 457 {
jah128 1:b067a08ff54e 458 char command = 0x80;
jah128 1:b067a08ff54e 459 char length = 0;
jah128 1:b067a08ff54e 460 char * data = NULL;
jah128 1:b067a08ff54e 461 if(target == 0) { //Request broadcast to all recipients
jah128 1:b067a08ff54e 462 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 463 swarm[i].status_rf_request_null = 1;
jah128 1:b067a08ff54e 464 }
jah128 1:b067a08ff54e 465 } else swarm[target].status_rf_request_null = 1;
jah128 1:b067a08ff54e 466 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 467 }
jah128 1:b067a08ff54e 468
jah128 1:b067a08ff54e 469
jah128 1:b067a08ff54e 470 void send_rf_request_left_motor_speed ( char target )
jah128 1:b067a08ff54e 471 {
jah128 1:b067a08ff54e 472 char command = 0x81;
jah128 1:b067a08ff54e 473 char length = 0;
jah128 1:b067a08ff54e 474 char * data = NULL;
jah128 1:b067a08ff54e 475 if(target == 0) {
jah128 1:b067a08ff54e 476 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 477 swarm[i].status_rf_request_left_motor_speed = 1;
jah128 1:b067a08ff54e 478 }
jah128 1:b067a08ff54e 479 } else swarm[target].status_rf_request_left_motor_speed = 1;
jah128 1:b067a08ff54e 480 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 481 }
jah128 1:b067a08ff54e 482
jah128 1:b067a08ff54e 483 void send_rf_request_right_motor_speed ( char target )
jah128 1:b067a08ff54e 484 {
jah128 1:b067a08ff54e 485 char command = 0x82;
jah128 1:b067a08ff54e 486 char length = 0;
jah128 1:b067a08ff54e 487 char * data = NULL;
jah128 1:b067a08ff54e 488 if(target == 0) {
jah128 1:b067a08ff54e 489 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 490 swarm[i].status_rf_request_right_motor_speed = 1;
jah128 1:b067a08ff54e 491 }
jah128 1:b067a08ff54e 492 } else swarm[target].status_rf_request_right_motor_speed = 1;
jah128 1:b067a08ff54e 493 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 494 }
jah128 1:b067a08ff54e 495
jah128 1:b067a08ff54e 496 void send_rf_request_button_state ( char target )
jah128 1:b067a08ff54e 497 {
jah128 1:b067a08ff54e 498 char command = 0x83;
jah128 1:b067a08ff54e 499 char length = 0;
jah128 1:b067a08ff54e 500 char * data = NULL;
jah128 1:b067a08ff54e 501 if(target == 0) {
jah128 1:b067a08ff54e 502 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 503 swarm[i].status_rf_request_button_state = 1;
jah128 1:b067a08ff54e 504 }
jah128 1:b067a08ff54e 505 } else swarm[target].status_rf_request_button_state = 1;
jah128 1:b067a08ff54e 506 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 507 }
jah128 1:b067a08ff54e 508
jah128 1:b067a08ff54e 509 void send_rf_request_led_colour ( char target )
jah128 1:b067a08ff54e 510 {
jah128 1:b067a08ff54e 511 char command = 0x84;
jah128 1:b067a08ff54e 512 char length = 0;
jah128 1:b067a08ff54e 513 char * data = NULL;
jah128 1:b067a08ff54e 514 if(target == 0) {
jah128 1:b067a08ff54e 515 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 516 swarm[i].status_rf_request_led_colour = 1;
jah128 1:b067a08ff54e 517 }
jah128 1:b067a08ff54e 518 } else swarm[target].status_rf_request_led_colour = 1;
jah128 1:b067a08ff54e 519 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 520 }
jah128 1:b067a08ff54e 521
jah128 1:b067a08ff54e 522 void send_rf_request_led_states ( char target )
jah128 1:b067a08ff54e 523 {
jah128 1:b067a08ff54e 524 char command = 0x85;
jah128 1:b067a08ff54e 525 char length = 0;
jah128 1:b067a08ff54e 526 char * data = NULL;
jah128 1:b067a08ff54e 527 if(target == 0) {
jah128 1:b067a08ff54e 528 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 529 swarm[i].status_rf_request_led_states = 1;
jah128 1:b067a08ff54e 530 }
jah128 1:b067a08ff54e 531 } else swarm[target].status_rf_request_led_states = 1;
jah128 1:b067a08ff54e 532 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 533 }
jah128 1:b067a08ff54e 534
jah128 1:b067a08ff54e 535 void send_rf_request_battery ( char target )
jah128 1:b067a08ff54e 536 {
jah128 1:b067a08ff54e 537 char command = 0x86;
jah128 1:b067a08ff54e 538 char length = 0;
jah128 1:b067a08ff54e 539 char * data = NULL;
jah128 1:b067a08ff54e 540 if(target == 0) {
jah128 1:b067a08ff54e 541 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 542 swarm[i].status_rf_request_battery = 1;
jah128 1:b067a08ff54e 543 }
jah128 1:b067a08ff54e 544 } else swarm[target].status_rf_request_battery = 1;
jah128 1:b067a08ff54e 545 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 546 }
jah128 1:b067a08ff54e 547
jah128 1:b067a08ff54e 548 void send_rf_request_light_sensor ( char target )
jah128 1:b067a08ff54e 549 {
jah128 1:b067a08ff54e 550 char command = 0x87;
jah128 1:b067a08ff54e 551 char length = 0;
jah128 1:b067a08ff54e 552 char * data = NULL;
jah128 1:b067a08ff54e 553 if(target == 0) {
jah128 1:b067a08ff54e 554 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 555 swarm[i].status_rf_request_light_sensor = 1;
jah128 1:b067a08ff54e 556 }
jah128 1:b067a08ff54e 557 } else swarm[target].status_rf_request_light_sensor = 1;
jah128 1:b067a08ff54e 558 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 559 }
jah128 1:b067a08ff54e 560
jah128 1:b067a08ff54e 561 void send_rf_request_accelerometer ( char target )
jah128 1:b067a08ff54e 562 {
jah128 1:b067a08ff54e 563 char command = 0x88;
jah128 1:b067a08ff54e 564 char length = 0;
jah128 1:b067a08ff54e 565 char * data = NULL;
jah128 1:b067a08ff54e 566 if(target == 0) {
jah128 1:b067a08ff54e 567 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 568 swarm[i].status_rf_request_accelerometer = 1;
jah128 1:b067a08ff54e 569 }
jah128 1:b067a08ff54e 570 } else swarm[target].status_rf_request_accelerometer = 1;
jah128 1:b067a08ff54e 571 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 572 }
jah128 1:b067a08ff54e 573
jah128 1:b067a08ff54e 574 void send_rf_request_gyroscope ( char target )
jah128 1:b067a08ff54e 575 {
jah128 1:b067a08ff54e 576 char command = 0x89;
jah128 1:b067a08ff54e 577 char length = 0;
jah128 1:b067a08ff54e 578 char * data = NULL;
jah128 1:b067a08ff54e 579 if(target == 0) {
jah128 1:b067a08ff54e 580 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 581 swarm[i].status_rf_request_gyroscope = 1;
jah128 1:b067a08ff54e 582 }
jah128 1:b067a08ff54e 583 } else swarm[target].status_rf_request_gyroscope = 1;
jah128 1:b067a08ff54e 584 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 585 }
jah128 1:b067a08ff54e 586
jah128 1:b067a08ff54e 587 void send_rf_request_background_ir ( char target )
jah128 1:b067a08ff54e 588 {
jah128 1:b067a08ff54e 589 char command = 0x8A;
jah128 1:b067a08ff54e 590 char length = 0;
jah128 1:b067a08ff54e 591 char * data = NULL;
jah128 1:b067a08ff54e 592 if(target == 0) {
jah128 1:b067a08ff54e 593 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 594 swarm[i].status_rf_request_background_ir = 1;
jah128 1:b067a08ff54e 595 }
jah128 1:b067a08ff54e 596 } else swarm[target].status_rf_request_background_ir = 1;
jah128 1:b067a08ff54e 597 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 598 }
jah128 1:b067a08ff54e 599
jah128 1:b067a08ff54e 600 void send_rf_request_reflected_ir ( char target )
jah128 1:b067a08ff54e 601 {
jah128 1:b067a08ff54e 602 char command = 0x8B;
jah128 1:b067a08ff54e 603 char length = 0;
jah128 1:b067a08ff54e 604 char * data = NULL;
jah128 1:b067a08ff54e 605 if(target == 0) {
jah128 1:b067a08ff54e 606 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 607 swarm[i].status_rf_request_reflected_ir = 1;
jah128 1:b067a08ff54e 608 }
jah128 1:b067a08ff54e 609 } else swarm[target].status_rf_request_reflected_ir = 1;
jah128 1:b067a08ff54e 610 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 611 }
jah128 1:b067a08ff54e 612
jah128 1:b067a08ff54e 613 void send_rf_request_distance_ir ( char target )
jah128 1:b067a08ff54e 614 {
jah128 1:b067a08ff54e 615 char command = 0x8C;
jah128 1:b067a08ff54e 616 char length = 0;
jah128 1:b067a08ff54e 617 char * data = NULL;
jah128 1:b067a08ff54e 618 if(target == 0) {
jah128 1:b067a08ff54e 619 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 620 swarm[i].status_rf_request_distance_ir = 1;
jah128 1:b067a08ff54e 621 }
jah128 1:b067a08ff54e 622 } else swarm[target].status_rf_request_distance_ir = 1;
jah128 1:b067a08ff54e 623 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 624 }
jah128 1:b067a08ff54e 625
jah128 1:b067a08ff54e 626 void send_rf_request_line_following_ir ( char target )
jah128 1:b067a08ff54e 627 {
jah128 1:b067a08ff54e 628 char command = 0x8D;
jah128 1:b067a08ff54e 629 char length = 0;
jah128 1:b067a08ff54e 630 char * data = NULL;
jah128 1:b067a08ff54e 631 if(target == 0) {
jah128 1:b067a08ff54e 632 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 633 swarm[i].status_rf_request_line_following_ir = 1;
jah128 1:b067a08ff54e 634 }
jah128 1:b067a08ff54e 635 } else swarm[target].status_rf_request_line_following_ir = 1;
jah128 1:b067a08ff54e 636 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 637 }
jah128 1:b067a08ff54e 638
jah128 1:b067a08ff54e 639 void send_rf_request_uptime ( char target )
jah128 1:b067a08ff54e 640 {
jah128 1:b067a08ff54e 641 char command = 0x8E;
jah128 1:b067a08ff54e 642 char length = 0;
jah128 1:b067a08ff54e 643 char * data = NULL;
jah128 1:b067a08ff54e 644 if(target == 0) {
jah128 1:b067a08ff54e 645 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 646 swarm[i].status_rf_request_uptime= 1;
jah128 1:b067a08ff54e 647 }
jah128 1:b067a08ff54e 648 } else swarm[target].status_rf_request_uptime = 1;
jah128 1:b067a08ff54e 649 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 650 }
jah128 1:b067a08ff54e 651
jah128 1:b067a08ff54e 652 void send_rf_command_stop ( char target, char request_response )
jah128 1:b067a08ff54e 653 {
jah128 1:b067a08ff54e 654 char command = 0x10;
jah128 1:b067a08ff54e 655 char length = 0;
jah128 1:b067a08ff54e 656 char * data = NULL;
jah128 1:b067a08ff54e 657 if(request_response == 1) {
jah128 1:b067a08ff54e 658 command+=128;
jah128 1:b067a08ff54e 659 if(target == 0) {
jah128 1:b067a08ff54e 660 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 661 swarm[i].status_rf_command_stop= 1;
jah128 1:b067a08ff54e 662 }
jah128 1:b067a08ff54e 663 } else swarm[target].status_rf_command_stop = 1;
jah128 1:b067a08ff54e 664 }
jah128 1:b067a08ff54e 665 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 666 }
jah128 1:b067a08ff54e 667
jah128 1:b067a08ff54e 668 void send_rf_command_forward ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 669 {
jah128 1:b067a08ff54e 670 char command = 0x11;
jah128 1:b067a08ff54e 671 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 672 char length = 2;
jah128 1:b067a08ff54e 673 char data [2];
jah128 1:b067a08ff54e 674 float qspeed = speed + 1;
jah128 1:b067a08ff54e 675 qspeed *= 32768.0;
jah128 1:b067a08ff54e 676 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 677 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 678 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 679 if(request_response == 1) {
jah128 1:b067a08ff54e 680 command+=128;
jah128 1:b067a08ff54e 681 if(target == 0) {
jah128 1:b067a08ff54e 682 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 683 swarm[i].status_rf_command_forward= 1;
jah128 1:b067a08ff54e 684 }
jah128 1:b067a08ff54e 685 } else swarm[target].status_rf_command_forward = 1;
jah128 1:b067a08ff54e 686 }
jah128 1:b067a08ff54e 687 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 688 }
jah128 1:b067a08ff54e 689
jah128 1:b067a08ff54e 690
jah128 1:b067a08ff54e 691 void send_rf_command_backward ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 692 {
jah128 1:b067a08ff54e 693 char command = 0x12;
jah128 1:b067a08ff54e 694 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 695 char length = 2;
jah128 1:b067a08ff54e 696 char data [2];
jah128 1:b067a08ff54e 697 float qspeed = speed + 1;
jah128 1:b067a08ff54e 698 qspeed *= 32768.0;
jah128 1:b067a08ff54e 699 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 700 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 701 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 702 if(request_response == 1) {
jah128 1:b067a08ff54e 703 command+=128;
jah128 1:b067a08ff54e 704 if(target == 0) {
jah128 1:b067a08ff54e 705 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 706 swarm[i].status_rf_command_backward= 1;
jah128 1:b067a08ff54e 707 }
jah128 1:b067a08ff54e 708 } else swarm[target].status_rf_command_backward = 1;
jah128 0:9ffe8ebd1c40 709 }
jah128 1:b067a08ff54e 710 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 711 }
jah128 1:b067a08ff54e 712
jah128 1:b067a08ff54e 713 void send_rf_command_left ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 714 {
jah128 1:b067a08ff54e 715 char command = 0x13;
jah128 1:b067a08ff54e 716 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 717 char length = 2;
jah128 1:b067a08ff54e 718 char data [2];
jah128 1:b067a08ff54e 719 float qspeed = speed + 1;
jah128 1:b067a08ff54e 720 qspeed *= 32768.0;
jah128 1:b067a08ff54e 721 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 722 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 723 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 724 if(request_response == 1) {
jah128 1:b067a08ff54e 725 command+=128;
jah128 1:b067a08ff54e 726 if(target == 0) {
jah128 1:b067a08ff54e 727 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 728 swarm[i].status_rf_command_left = 1;
jah128 1:b067a08ff54e 729 }
jah128 1:b067a08ff54e 730 } else swarm[target].status_rf_command_left = 1;
jah128 1:b067a08ff54e 731 }
jah128 1:b067a08ff54e 732 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 733 }
jah128 1:b067a08ff54e 734
jah128 1:b067a08ff54e 735 void send_rf_command_right ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 736 {
jah128 1:b067a08ff54e 737 char command = 0x14;
jah128 1:b067a08ff54e 738 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 739 char length = 2;
jah128 1:b067a08ff54e 740 char data [2];
jah128 1:b067a08ff54e 741 float qspeed = speed + 1;
jah128 1:b067a08ff54e 742 qspeed *= 32768.0;
jah128 1:b067a08ff54e 743 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 744 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 745 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 746 if(request_response == 1) {
jah128 1:b067a08ff54e 747 command+=128;
jah128 1:b067a08ff54e 748 if(target == 0) {
jah128 1:b067a08ff54e 749 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 750 swarm[i].status_rf_command_right = 1;
jah128 1:b067a08ff54e 751 }
jah128 1:b067a08ff54e 752 } else swarm[target].status_rf_command_right = 1;
jah128 1:b067a08ff54e 753 }
jah128 1:b067a08ff54e 754 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 755 }
jah128 1:b067a08ff54e 756
jah128 1:b067a08ff54e 757 void send_rf_command_left_motor ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 758 {
jah128 1:b067a08ff54e 759 char command = 0x15;
jah128 1:b067a08ff54e 760 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 761 char length = 2;
jah128 1:b067a08ff54e 762 char data [2];
jah128 1:b067a08ff54e 763 float qspeed = speed + 1;
jah128 1:b067a08ff54e 764 qspeed *= 32768.0;
jah128 1:b067a08ff54e 765 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 766 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 767 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 768 if(request_response == 1) {
jah128 1:b067a08ff54e 769 command+=128;
jah128 1:b067a08ff54e 770 if(target == 0) {
jah128 1:b067a08ff54e 771 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 772 swarm[i].status_rf_command_left_motor = 1;
jah128 1:b067a08ff54e 773 }
jah128 1:b067a08ff54e 774 } else swarm[target].status_rf_command_left_motor = 1;
jah128 1:b067a08ff54e 775 }
jah128 1:b067a08ff54e 776 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 777 }
jah128 1:b067a08ff54e 778
jah128 1:b067a08ff54e 779 void send_rf_command_right_motor ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 780 {
jah128 1:b067a08ff54e 781 char command = 0x16;
jah128 1:b067a08ff54e 782 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 783 char length = 2;
jah128 1:b067a08ff54e 784 char data [2];
jah128 1:b067a08ff54e 785 float qspeed = speed + 1;
jah128 1:b067a08ff54e 786 qspeed *= 32768.0;
jah128 1:b067a08ff54e 787 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 788 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 789 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 790 if(request_response == 1) {
jah128 1:b067a08ff54e 791 command+=128;
jah128 1:b067a08ff54e 792 if(target == 0) {
jah128 1:b067a08ff54e 793 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 794 swarm[i].status_rf_command_right_motor = 1;
jah128 1:b067a08ff54e 795 }
jah128 1:b067a08ff54e 796 } else swarm[target].status_rf_command_right_motor = 1;
jah128 1:b067a08ff54e 797 }
jah128 1:b067a08ff54e 798 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 799 }
jah128 1:b067a08ff54e 800
jah128 1:b067a08ff54e 801 void send_rf_command_oled_colour ( char target, char request_response, char red, char green, char blue )
jah128 1:b067a08ff54e 802 {
jah128 1:b067a08ff54e 803 char command = 0x17;
jah128 1:b067a08ff54e 804 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 805 char length = 3;
jah128 1:b067a08ff54e 806 char data [3];
jah128 1:b067a08ff54e 807 data[0] = red;
jah128 1:b067a08ff54e 808 data[1] = green;
jah128 1:b067a08ff54e 809 data[2] = blue;
jah128 1:b067a08ff54e 810 if(request_response == 1) {
jah128 1:b067a08ff54e 811 command+=128;
jah128 1:b067a08ff54e 812 if(target == 0) {
jah128 1:b067a08ff54e 813 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 814 swarm[i].status_rf_command_oled_colour = 1;
jah128 1:b067a08ff54e 815 }
jah128 1:b067a08ff54e 816 } else swarm[target].status_rf_command_oled_colour = 1;
jah128 1:b067a08ff54e 817 }
jah128 1:b067a08ff54e 818 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 819 }
jah128 1:b067a08ff54e 820
jah128 1:b067a08ff54e 821 void send_rf_command_cled_colour ( char target, char request_response, char red, char green, char blue )
jah128 1:b067a08ff54e 822 {
jah128 1:b067a08ff54e 823 char command = 0x18;
jah128 1:b067a08ff54e 824 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 825 char length = 3;
jah128 1:b067a08ff54e 826 char data [3];
jah128 1:b067a08ff54e 827 data[0] = red;
jah128 1:b067a08ff54e 828 data[1] = green;
jah128 1:b067a08ff54e 829 data[2] = blue;
jah128 1:b067a08ff54e 830 if(request_response == 1) {
jah128 1:b067a08ff54e 831 command+=128;
jah128 1:b067a08ff54e 832 if(target == 0) {
jah128 1:b067a08ff54e 833 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 834 swarm[i].status_rf_command_cled_colour = 1;
jah128 1:b067a08ff54e 835 }
jah128 1:b067a08ff54e 836 } else swarm[target].status_rf_command_cled_colour = 1;
jah128 1:b067a08ff54e 837 }
jah128 1:b067a08ff54e 838 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 839 }
jah128 1:b067a08ff54e 840
jah128 1:b067a08ff54e 841 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 )
jah128 1:b067a08ff54e 842 {
jah128 1:b067a08ff54e 843 char command = 0x19;
jah128 1:b067a08ff54e 844 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 845 char length = 2;
jah128 1:b067a08ff54e 846 char data [2];
jah128 1:b067a08ff54e 847 data[0] = 0;
jah128 1:b067a08ff54e 848 data[1] = 0;
jah128 1:b067a08ff54e 849 if( led0 == 1) data[0] += 2;
jah128 1:b067a08ff54e 850 if( led1 == 1) data[0] += 1;
jah128 1:b067a08ff54e 851 if( led2 == 1) data[1] += 128;
jah128 1:b067a08ff54e 852 if( led3 == 1) data[1] += 64;
jah128 1:b067a08ff54e 853 if( led4 == 1) data[1] += 32;
jah128 1:b067a08ff54e 854 if( led5 == 1) data[1] += 16;
jah128 1:b067a08ff54e 855 if( led6 == 1) data[1] += 8;
jah128 1:b067a08ff54e 856 if( led7 == 1) data[1] += 4;
jah128 1:b067a08ff54e 857 if( led8 == 1) data[1] += 2;
jah128 1:b067a08ff54e 858 if( led9 == 1) data[1] += 1;
jah128 1:b067a08ff54e 859 if(request_response == 1) {
jah128 1:b067a08ff54e 860 command+=128;
jah128 1:b067a08ff54e 861 if(target == 0) {
jah128 1:b067a08ff54e 862 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 863 swarm[i].status_rf_command_oled_state = 1;
jah128 1:b067a08ff54e 864 }
jah128 1:b067a08ff54e 865 } else swarm[target].status_rf_command_oled_state = 1;
jah128 1:b067a08ff54e 866 }
jah128 1:b067a08ff54e 867 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 868 }
jah128 1:b067a08ff54e 869
jah128 1:b067a08ff54e 870 void send_rf_command_cled_state ( char target, char request_response, char enable )
jah128 1:b067a08ff54e 871 {
jah128 1:b067a08ff54e 872 char command = 0x1A;
jah128 1:b067a08ff54e 873 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 874 char length = 1;
jah128 1:b067a08ff54e 875 char data [1];
jah128 1:b067a08ff54e 876 data[0] = enable;
jah128 1:b067a08ff54e 877 if(request_response == 1) {
jah128 1:b067a08ff54e 878 command+=128;
jah128 1:b067a08ff54e 879 if(target == 0) {
jah128 1:b067a08ff54e 880 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 881 swarm[i].status_rf_command_cled_state = 1;
jah128 1:b067a08ff54e 882 }
jah128 1:b067a08ff54e 883 } else swarm[target].status_rf_command_cled_state = 1;
jah128 1:b067a08ff54e 884 }
jah128 1:b067a08ff54e 885 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 886 }
jah128 1:b067a08ff54e 887
jah128 1:b067a08ff54e 888 void send_rf_command_set_oled ( char target, char request_response, char oled, char enable )
jah128 1:b067a08ff54e 889 {
jah128 1:b067a08ff54e 890 char command = 0x1B;
jah128 1:b067a08ff54e 891 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 892 char length = 1;
jah128 1:b067a08ff54e 893 char data [1];
jah128 1:b067a08ff54e 894 data[0] = oled;
jah128 1:b067a08ff54e 895 if(enable == 1) oled+= 32;
jah128 1:b067a08ff54e 896 if(request_response == 1) {
jah128 1:b067a08ff54e 897 command+=128;
jah128 1:b067a08ff54e 898 if(target == 0) {
jah128 1:b067a08ff54e 899 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 900 swarm[i].status_rf_command_set_oled = 1;
jah128 1:b067a08ff54e 901 }
jah128 1:b067a08ff54e 902 } else swarm[target].status_rf_command_set_oled = 1;
jah128 1:b067a08ff54e 903 }
jah128 1:b067a08ff54e 904 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 905 }
jah128 1:b067a08ff54e 906
jah128 1:b067a08ff54e 907 void send_rf_command_play_tune ( char target, char request_response, char * data, char length )
jah128 1:b067a08ff54e 908 {
jah128 1:b067a08ff54e 909 char command = 0x1C;
jah128 1:b067a08ff54e 910 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 911 if(request_response == 1) {
jah128 1:b067a08ff54e 912 command+=128;
jah128 1:b067a08ff54e 913 if(target == 0) {
jah128 1:b067a08ff54e 914 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 915 swarm[i].status_rf_command_play_tune = 1;
jah128 1:b067a08ff54e 916 }
jah128 1:b067a08ff54e 917 } else swarm[target].status_rf_command_play_tune = 1;
jah128 1:b067a08ff54e 918 }
jah128 1:b067a08ff54e 919 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 920 }
jah128 1:b067a08ff54e 921
jah128 1:b067a08ff54e 922 void send_rf_command_sync_time ( char target, char request_response )
jah128 1:b067a08ff54e 923 {
jah128 1:b067a08ff54e 924 char command = 0x1D;
jah128 1:b067a08ff54e 925 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 926 char length = 1;
jah128 1:b067a08ff54e 927 char data [1];
jah128 1:b067a08ff54e 928 data[0] = 0;
jah128 1:b067a08ff54e 929 if(request_response == 1) {
jah128 1:b067a08ff54e 930 command+=128;
jah128 1:b067a08ff54e 931 if(target == 0) {
jah128 1:b067a08ff54e 932 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 933 swarm[i].status_rf_command_sync_time = 1;
jah128 1:b067a08ff54e 934 }
jah128 1:b067a08ff54e 935 } else swarm[target].status_rf_command_sync_time = 1;
jah128 1:b067a08ff54e 936 }
jah128 1:b067a08ff54e 937 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 938 }
jah128 1:b067a08ff54e 939
jah128 1:b067a08ff54e 940 //Resets the recorded swarm data tables
jah128 1:b067a08ff54e 941 void setup_communications()
jah128 1:b067a08ff54e 942 {
jah128 1:b067a08ff54e 943 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 944 swarm[i].status_rf_request_null = 0;
jah128 1:b067a08ff54e 945 swarm[i].status_rf_request_left_motor_speed = 0;
jah128 1:b067a08ff54e 946 swarm[i].status_rf_request_right_motor_speed = 0;
jah128 1:b067a08ff54e 947 swarm[i].status_rf_request_button_state = 0;
jah128 1:b067a08ff54e 948 swarm[i].status_rf_request_led_colour = 0;
jah128 1:b067a08ff54e 949 swarm[i].status_rf_request_led_states = 0;
jah128 1:b067a08ff54e 950 swarm[i].status_rf_request_battery = 0;
jah128 1:b067a08ff54e 951 swarm[i].status_rf_request_light_sensor = 0;
jah128 1:b067a08ff54e 952 swarm[i].status_rf_request_accelerometer = 0;
jah128 1:b067a08ff54e 953 swarm[i].status_rf_request_gyroscope = 0;
jah128 1:b067a08ff54e 954 swarm[i].status_rf_request_background_ir = 0;
jah128 1:b067a08ff54e 955 swarm[i].status_rf_request_reflected_ir = 0;
jah128 1:b067a08ff54e 956 swarm[i].status_rf_request_distance_ir = 0;
jah128 1:b067a08ff54e 957 swarm[i].status_rf_request_line_following_ir = 0;
jah128 1:b067a08ff54e 958 swarm[i].status_rf_request_uptime = 0;
jah128 1:b067a08ff54e 959 swarm[i].status_rf_command_stop = 0;
jah128 1:b067a08ff54e 960 swarm[i].status_rf_command_forward = 0;
jah128 1:b067a08ff54e 961 swarm[i].status_rf_command_backward = 0;
jah128 1:b067a08ff54e 962 swarm[i].status_rf_command_left = 0;
jah128 1:b067a08ff54e 963 swarm[i].status_rf_command_right = 0;
jah128 1:b067a08ff54e 964 swarm[i].status_rf_command_left_motor = 0;
jah128 1:b067a08ff54e 965 swarm[i].status_rf_command_right_motor = 0;
jah128 1:b067a08ff54e 966 swarm[i].status_rf_command_oled_colour = 0;
jah128 1:b067a08ff54e 967 swarm[i].status_rf_command_cled_colour = 0;
jah128 1:b067a08ff54e 968 swarm[i].status_rf_command_oled_state = 0;
jah128 1:b067a08ff54e 969 swarm[i].status_rf_command_cled_state = 0;
jah128 1:b067a08ff54e 970 swarm[i].status_rf_command_set_oled = 0;
jah128 1:b067a08ff54e 971 swarm[i].status_rf_command_play_tune = 0;
jah128 1:b067a08ff54e 972 swarm[i].status_rf_command_sync_time = 0;
jah128 1:b067a08ff54e 973 swarm[i].left_motor_speed = 0.0;
jah128 1:b067a08ff54e 974 swarm[i].right_motor_speed = 0.0;
jah128 1:b067a08ff54e 975 swarm[i].button_state = 0;
jah128 1:b067a08ff54e 976 for(int k=0; k<3; k++) {
jah128 1:b067a08ff54e 977 swarm[i].outer_led_colour [k]=0;
jah128 1:b067a08ff54e 978 swarm[i].center_led_colour [k]=0;
jah128 1:b067a08ff54e 979 swarm[i].accelerometer [k]=0;
jah128 1:b067a08ff54e 980 }
jah128 1:b067a08ff54e 981 swarm[i].led_states[0]=0;
jah128 1:b067a08ff54e 982 swarm[i].led_states[1]=0;
jah128 1:b067a08ff54e 983 swarm[i].battery = 0.0;
jah128 1:b067a08ff54e 984 swarm[i].light_sensor = 0.0;
jah128 1:b067a08ff54e 985 swarm[i].gyro = 0.0;
jah128 1:b067a08ff54e 986 }
jah128 1:b067a08ff54e 987 }
jah128 2:0a739218ab11 988
jah128 2:0a739218ab11 989
jah128 2:0a739218ab11 990 // Handle a message that is a response to a predefined (non-user) command or request
jah128 2:0a739218ab11 991 void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
jah128 2:0a739218ab11 992 {
jah128 2:0a739218ab11 993 char outcome = 0;
jah128 2:0a739218ab11 994 if(is_command == 0) {
jah128 2:0a739218ab11 995 // Response is a data_request_response
jah128 2:0a739218ab11 996 switch(function) {
jah128 2:0a739218ab11 997 case 0: {
jah128 2:0a739218ab11 998 if(swarm[sender].status_rf_request_null == 1) {
jah128 2:0a739218ab11 999 if(success == 1){
jah128 2:0a739218ab11 1000 if(length == 0) {
jah128 2:0a739218ab11 1001 swarm[sender].status_rf_request_null = 2;
jah128 2:0a739218ab11 1002 outcome = 1;
jah128 2:0a739218ab11 1003 } else {
jah128 2:0a739218ab11 1004 swarm[sender].status_rf_request_null= 3;
jah128 2:0a739218ab11 1005 outcome = 3;
jah128 2:0a739218ab11 1006 }
jah128 2:0a739218ab11 1007 } else {
jah128 2:0a739218ab11 1008 swarm[sender].status_rf_request_null = 4;
jah128 2:0a739218ab11 1009 outcome = 4;
jah128 2:0a739218ab11 1010 }
jah128 2:0a739218ab11 1011 } else outcome = 2;
jah128 2:0a739218ab11 1012 }
jah128 2:0a739218ab11 1013 break;
jah128 2:0a739218ab11 1014
jah128 2:0a739218ab11 1015 case 1: { //Left motor speed
jah128 2:0a739218ab11 1016 if(swarm[sender].status_rf_request_left_motor_speed == 1) {
jah128 2:0a739218ab11 1017 if(success == 1){
jah128 2:0a739218ab11 1018 if(length == 2) {
jah128 2:0a739218ab11 1019 swarm[sender].status_rf_request_left_motor_speed = 2;
jah128 2:0a739218ab11 1020 int value = (data [0] << 8) + data[1];
jah128 2:0a739218ab11 1021 value -= 32768;
jah128 2:0a739218ab11 1022 float val = value;
jah128 2:0a739218ab11 1023 val /= 32767.0;
jah128 2:0a739218ab11 1024 swarm[sender].left_motor_speed = val;
jah128 2:0a739218ab11 1025 outcome = 1;
jah128 2:0a739218ab11 1026 } else {
jah128 2:0a739218ab11 1027 swarm[sender].status_rf_request_left_motor_speed= 3;
jah128 2:0a739218ab11 1028 outcome = 3;
jah128 2:0a739218ab11 1029 }
jah128 2:0a739218ab11 1030 } else {
jah128 2:0a739218ab11 1031 swarm[sender].status_rf_request_left_motor_speed = 4;
jah128 2:0a739218ab11 1032 outcome = 4;
jah128 2:0a739218ab11 1033 }
jah128 2:0a739218ab11 1034 } else outcome = 2;
jah128 2:0a739218ab11 1035 }
jah128 2:0a739218ab11 1036 break;
jah128 2:0a739218ab11 1037
jah128 2:0a739218ab11 1038 case 2: { //Right motor speed
jah128 2:0a739218ab11 1039 if(swarm[sender].status_rf_request_right_motor_speed == 1) {
jah128 2:0a739218ab11 1040 if(success == 1){
jah128 2:0a739218ab11 1041 if(length == 2) {
jah128 2:0a739218ab11 1042 swarm[sender].status_rf_request_right_motor_speed = 2;
jah128 2:0a739218ab11 1043 int value = (data [0] << 8) + data[1];
jah128 2:0a739218ab11 1044 value -= 32768;
jah128 2:0a739218ab11 1045 float val = value;
jah128 2:0a739218ab11 1046 val /= 32767.0;
jah128 2:0a739218ab11 1047 swarm[sender].right_motor_speed = val;
jah128 2:0a739218ab11 1048 outcome = 1;
jah128 2:0a739218ab11 1049 } else {
jah128 2:0a739218ab11 1050 swarm[sender].status_rf_request_right_motor_speed = 3;
jah128 2:0a739218ab11 1051 outcome = 3;
jah128 2:0a739218ab11 1052 }
jah128 2:0a739218ab11 1053 } else {
jah128 2:0a739218ab11 1054 swarm[sender].status_rf_request_right_motor_speed = 4;
jah128 2:0a739218ab11 1055 outcome = 4;
jah128 2:0a739218ab11 1056 }
jah128 2:0a739218ab11 1057 } else outcome = 2;
jah128 2:0a739218ab11 1058 }
jah128 2:0a739218ab11 1059 break;
jah128 2:0a739218ab11 1060
jah128 2:0a739218ab11 1061 case 3: { //Button state
jah128 2:0a739218ab11 1062 if(swarm[sender].status_rf_request_button_state == 1) {
jah128 2:0a739218ab11 1063 if(success == 1){
jah128 2:0a739218ab11 1064 if(length == 2) {
jah128 2:0a739218ab11 1065 swarm[sender].status_rf_request_button_state = 2;
jah128 2:0a739218ab11 1066 swarm[sender].button_state = data[0];
jah128 2:0a739218ab11 1067 outcome = 1;
jah128 2:0a739218ab11 1068 } else {
jah128 2:0a739218ab11 1069 swarm[sender].status_rf_request_button_state = 3;
jah128 2:0a739218ab11 1070 outcome = 3;
jah128 2:0a739218ab11 1071 }
jah128 2:0a739218ab11 1072 } else {
jah128 2:0a739218ab11 1073 swarm[sender].status_rf_request_button_state = 4;
jah128 2:0a739218ab11 1074 outcome = 4;
jah128 2:0a739218ab11 1075 }
jah128 2:0a739218ab11 1076 } else outcome = 2;
jah128 2:0a739218ab11 1077 }
jah128 2:0a739218ab11 1078 break;
jah128 2:0a739218ab11 1079
jah128 2:0a739218ab11 1080 case 4: { //LED Colour
jah128 2:0a739218ab11 1081 if(swarm[sender].status_rf_request_led_colour == 1) {
jah128 2:0a739218ab11 1082 if(success == 1) {
jah128 2:0a739218ab11 1083 if(length == 6) {
jah128 2:0a739218ab11 1084 swarm[sender].status_rf_request_led_colour = 2;
jah128 2:0a739218ab11 1085 for(int i=0; i<3; i++) {
jah128 2:0a739218ab11 1086 swarm[sender].outer_led_colour[i] = data[i];
jah128 2:0a739218ab11 1087 swarm[sender].center_led_colour[i] = data[i+3];
jah128 2:0a739218ab11 1088 }
jah128 2:0a739218ab11 1089 outcome = 1;
jah128 2:0a739218ab11 1090 } else {
jah128 2:0a739218ab11 1091 swarm[sender].status_rf_request_led_colour = 3;
jah128 2:0a739218ab11 1092 outcome = 3;
jah128 2:0a739218ab11 1093 }
jah128 2:0a739218ab11 1094 } else {
jah128 2:0a739218ab11 1095 swarm[sender].status_rf_request_led_colour = 4;
jah128 2:0a739218ab11 1096 outcome = 4;
jah128 2:0a739218ab11 1097 }
jah128 2:0a739218ab11 1098 } else outcome = 2;
jah128 2:0a739218ab11 1099 }
jah128 2:0a739218ab11 1100 break;
jah128 2:0a739218ab11 1101
jah128 2:0a739218ab11 1102 case 5: { //LED States
jah128 2:0a739218ab11 1103 if(swarm[sender].status_rf_request_led_states == 1) {
jah128 2:0a739218ab11 1104 if(success == 1) {
jah128 2:0a739218ab11 1105 if(length == 2) {
jah128 2:0a739218ab11 1106 swarm[sender].status_rf_request_led_states = 2;
jah128 2:0a739218ab11 1107 for(int i=0; i<3; i++) {
jah128 2:0a739218ab11 1108 swarm[sender].led_states[0] = data[0];
jah128 2:0a739218ab11 1109 swarm[sender].led_states[1] = data[1];
jah128 2:0a739218ab11 1110 }
jah128 2:0a739218ab11 1111 outcome = 1;
jah128 2:0a739218ab11 1112 } else {
jah128 2:0a739218ab11 1113 swarm[sender].status_rf_request_led_states = 3;
jah128 2:0a739218ab11 1114 outcome = 3;
jah128 2:0a739218ab11 1115 }
jah128 2:0a739218ab11 1116 } else {
jah128 2:0a739218ab11 1117 swarm[sender].status_rf_request_led_states = 4;
jah128 2:0a739218ab11 1118 outcome = 4;
jah128 2:0a739218ab11 1119 }
jah128 2:0a739218ab11 1120 } else outcome = 2;
jah128 2:0a739218ab11 1121 }
jah128 2:0a739218ab11 1122 break;
jah128 2:0a739218ab11 1123
jah128 2:0a739218ab11 1124
jah128 2:0a739218ab11 1125 case 6: { //Battery
jah128 2:0a739218ab11 1126 if(swarm[sender].status_rf_request_battery == 1) {
jah128 2:0a739218ab11 1127 if(success == 1) {
jah128 2:0a739218ab11 1128 if(length == 2) {
jah128 2:0a739218ab11 1129 swarm[sender].status_rf_request_battery = 2;
jah128 2:0a739218ab11 1130 int fbattery = data[0] * 256;
jah128 2:0a739218ab11 1131 fbattery += data[1];
jah128 2:0a739218ab11 1132 swarm[sender].battery = (float) fbattery / 1000.0;
jah128 2:0a739218ab11 1133 outcome = 1;
jah128 2:0a739218ab11 1134 } else {
jah128 2:0a739218ab11 1135 swarm[sender].status_rf_request_battery = 3;
jah128 2:0a739218ab11 1136 outcome = 3;
jah128 2:0a739218ab11 1137 }
jah128 2:0a739218ab11 1138 } else {
jah128 2:0a739218ab11 1139 swarm[sender].status_rf_request_battery = 4;
jah128 2:0a739218ab11 1140 outcome = 4;
jah128 2:0a739218ab11 1141 }
jah128 2:0a739218ab11 1142 } else outcome = 2;
jah128 2:0a739218ab11 1143 }
jah128 2:0a739218ab11 1144 break;
jah128 2:0a739218ab11 1145
jah128 2:0a739218ab11 1146 case 7: { //Light sensor
jah128 2:0a739218ab11 1147 if(swarm[sender].status_rf_request_light_sensor == 1) {
jah128 2:0a739218ab11 1148 if(success == 1) {
jah128 2:0a739218ab11 1149 if(length == 2) {
jah128 2:0a739218ab11 1150 swarm[sender].status_rf_request_light_sensor = 2;
jah128 2:0a739218ab11 1151 int ilight = data[0] * 256;
jah128 2:0a739218ab11 1152 ilight += data[1];
jah128 2:0a739218ab11 1153 float flight = (float) (ilight) / 655.0;
jah128 2:0a739218ab11 1154 swarm[sender].light_sensor = flight;
jah128 2:0a739218ab11 1155 outcome = 1;
jah128 2:0a739218ab11 1156 } else {
jah128 2:0a739218ab11 1157 swarm[sender].status_rf_request_light_sensor = 3;
jah128 2:0a739218ab11 1158 outcome = 3;
jah128 2:0a739218ab11 1159 }
jah128 2:0a739218ab11 1160 } else {
jah128 2:0a739218ab11 1161 swarm[sender].status_rf_request_light_sensor = 4;
jah128 2:0a739218ab11 1162 outcome = 4;
jah128 2:0a739218ab11 1163 }
jah128 2:0a739218ab11 1164 } else outcome = 2;
jah128 2:0a739218ab11 1165 }
jah128 2:0a739218ab11 1166 break;
jah128 2:0a739218ab11 1167
jah128 2:0a739218ab11 1168 case 8: { //Accelerometer
jah128 2:0a739218ab11 1169 if(swarm[sender].status_rf_request_accelerometer == 1) {
jah128 2:0a739218ab11 1170 if(success == 1) {
jah128 2:0a739218ab11 1171 if(length == 6) {
jah128 2:0a739218ab11 1172 swarm[sender].status_rf_request_accelerometer = 2;
jah128 2:0a739218ab11 1173 int acc_x = (data[0] * 256) + data[1];
jah128 2:0a739218ab11 1174 int acc_y = (data[2] * 256) + data[3];
jah128 2:0a739218ab11 1175 int acc_z = (data[4] * 256) + data[5];
jah128 2:0a739218ab11 1176 swarm[sender].accelerometer[0] = (float) acc_x - 32768;
jah128 2:0a739218ab11 1177 swarm[sender].accelerometer[1] = (float) acc_y - 32768;
jah128 2:0a739218ab11 1178 swarm[sender].accelerometer[2] = (float) acc_z - 32768;
jah128 2:0a739218ab11 1179 outcome = 1;
jah128 2:0a739218ab11 1180 } else {
jah128 2:0a739218ab11 1181 swarm[sender].status_rf_request_accelerometer = 3;
jah128 2:0a739218ab11 1182 outcome = 3;
jah128 2:0a739218ab11 1183 }
jah128 2:0a739218ab11 1184 } else {
jah128 2:0a739218ab11 1185 swarm[sender].status_rf_request_accelerometer = 4;
jah128 2:0a739218ab11 1186 outcome = 4;
jah128 2:0a739218ab11 1187 }
jah128 2:0a739218ab11 1188 } else outcome = 2;
jah128 2:0a739218ab11 1189 }
jah128 2:0a739218ab11 1190 break;
jah128 2:0a739218ab11 1191
jah128 2:0a739218ab11 1192 case 9: { //Gyroscope
jah128 2:0a739218ab11 1193 if(swarm[sender].status_rf_request_gyroscope == 1) {
jah128 2:0a739218ab11 1194 if(success == 1) {
jah128 2:0a739218ab11 1195 if(length == 2) {
jah128 2:0a739218ab11 1196 swarm[sender].status_rf_request_gyroscope = 2;
jah128 2:0a739218ab11 1197 int gyro = (data [0] * 256) + data[1];
jah128 2:0a739218ab11 1198 swarm[sender].gyro = (float) gyro - 32768;
jah128 2:0a739218ab11 1199 outcome = 1;
jah128 2:0a739218ab11 1200 } else {
jah128 2:0a739218ab11 1201 swarm[sender].status_rf_request_gyroscope = 3;
jah128 2:0a739218ab11 1202 outcome = 3;
jah128 2:0a739218ab11 1203 }
jah128 2:0a739218ab11 1204 } else {
jah128 2:0a739218ab11 1205 swarm[sender].status_rf_request_gyroscope = 4;
jah128 2:0a739218ab11 1206 outcome = 4;
jah128 2:0a739218ab11 1207 }
jah128 2:0a739218ab11 1208 } else outcome = 2;
jah128 2:0a739218ab11 1209 }
jah128 2:0a739218ab11 1210 break;
jah128 2:0a739218ab11 1211
jah128 2:0a739218ab11 1212 case 10: { //Background IR
jah128 2:0a739218ab11 1213 if(swarm[sender].status_rf_request_background_ir == 1) {
jah128 2:0a739218ab11 1214 if(success == 1) {
jah128 2:0a739218ab11 1215 if(length == 16) {
jah128 2:0a739218ab11 1216 swarm[sender].status_rf_request_background_ir = 2;
jah128 2:0a739218ab11 1217 outcome = 1;
jah128 2:0a739218ab11 1218 } else {
jah128 2:0a739218ab11 1219 swarm[sender].status_rf_request_background_ir = 3;
jah128 2:0a739218ab11 1220 outcome = 3;
jah128 2:0a739218ab11 1221 }
jah128 2:0a739218ab11 1222 } else {
jah128 2:0a739218ab11 1223 swarm[sender].status_rf_request_background_ir = 4;
jah128 2:0a739218ab11 1224 outcome = 4;
jah128 2:0a739218ab11 1225 }
jah128 2:0a739218ab11 1226 } else outcome = 2;
jah128 2:0a739218ab11 1227 }
jah128 2:0a739218ab11 1228 break;
jah128 2:0a739218ab11 1229
jah128 2:0a739218ab11 1230 case 11: { //Reflected IR
jah128 2:0a739218ab11 1231 if(swarm[sender].status_rf_request_reflected_ir == 1) {
jah128 2:0a739218ab11 1232 if(success == 1) {
jah128 2:0a739218ab11 1233 if(length == 16) {
jah128 2:0a739218ab11 1234 swarm[sender].status_rf_request_reflected_ir = 2;
jah128 2:0a739218ab11 1235 outcome = 1;
jah128 2:0a739218ab11 1236 } else {
jah128 2:0a739218ab11 1237 swarm[sender].status_rf_request_reflected_ir = 3;
jah128 2:0a739218ab11 1238 outcome = 3;
jah128 2:0a739218ab11 1239 }
jah128 2:0a739218ab11 1240 } else {
jah128 2:0a739218ab11 1241 swarm[sender].status_rf_request_reflected_ir = 4;
jah128 2:0a739218ab11 1242 outcome = 4;
jah128 2:0a739218ab11 1243 }
jah128 2:0a739218ab11 1244 } else outcome = 2;
jah128 2:0a739218ab11 1245 }
jah128 2:0a739218ab11 1246 break;
jah128 2:0a739218ab11 1247
jah128 2:0a739218ab11 1248 case 12: { // Distance IR
jah128 2:0a739218ab11 1249 if(swarm[sender].status_rf_request_distance_ir == 1) {
jah128 2:0a739218ab11 1250 if(success == 1) {
jah128 2:0a739218ab11 1251 if(length == 16) {
jah128 2:0a739218ab11 1252 swarm[sender].status_rf_request_distance_ir = 2;
jah128 2:0a739218ab11 1253 outcome = 1;
jah128 2:0a739218ab11 1254 } else {
jah128 2:0a739218ab11 1255 swarm[sender].status_rf_request_distance_ir = 3;
jah128 2:0a739218ab11 1256 outcome = 3;
jah128 2:0a739218ab11 1257 }
jah128 2:0a739218ab11 1258 } else {
jah128 2:0a739218ab11 1259 swarm[sender].status_rf_request_distance_ir = 4;
jah128 2:0a739218ab11 1260 outcome = 4;
jah128 2:0a739218ab11 1261 }
jah128 2:0a739218ab11 1262 } else outcome = 2;
jah128 2:0a739218ab11 1263 }
jah128 2:0a739218ab11 1264 break;
jah128 2:0a739218ab11 1265
jah128 2:0a739218ab11 1266 case 13: { // Line following IR
jah128 2:0a739218ab11 1267 if(swarm[sender].status_rf_request_line_following_ir == 1) {
jah128 2:0a739218ab11 1268 if(success == 1) {
jah128 2:0a739218ab11 1269 if(length == 10) {
jah128 2:0a739218ab11 1270 swarm[sender].status_rf_request_line_following_ir = 2;
jah128 2:0a739218ab11 1271 outcome = 1;
jah128 2:0a739218ab11 1272 } else {
jah128 2:0a739218ab11 1273 swarm[sender].status_rf_request_line_following_ir = 3;
jah128 2:0a739218ab11 1274 outcome = 3;
jah128 2:0a739218ab11 1275 }
jah128 2:0a739218ab11 1276 } else {
jah128 2:0a739218ab11 1277 swarm[sender].status_rf_request_line_following_ir = 4;
jah128 2:0a739218ab11 1278 outcome = 4;
jah128 2:0a739218ab11 1279 }
jah128 2:0a739218ab11 1280 } else outcome = 2;
jah128 2:0a739218ab11 1281 }
jah128 2:0a739218ab11 1282 break;
jah128 2:0a739218ab11 1283
jah128 2:0a739218ab11 1284 case 14: { // Request uptime
jah128 2:0a739218ab11 1285 if(swarm[sender].status_rf_request_uptime == 1) {
jah128 2:0a739218ab11 1286 if(success == 1) {
jah128 2:0a739218ab11 1287 if(length == 4) {
jah128 2:0a739218ab11 1288 swarm[sender].status_rf_request_uptime = 2;
jah128 2:0a739218ab11 1289 outcome = 1;
jah128 2:0a739218ab11 1290 } else {
jah128 2:0a739218ab11 1291 swarm[sender].status_rf_request_uptime = 3;
jah128 2:0a739218ab11 1292 outcome = 3;
jah128 2:0a739218ab11 1293 }
jah128 2:0a739218ab11 1294 } else {
jah128 2:0a739218ab11 1295 swarm[sender].status_rf_request_uptime = 4;
jah128 2:0a739218ab11 1296 outcome = 4;
jah128 2:0a739218ab11 1297 }
jah128 2:0a739218ab11 1298 } else outcome = 2;
jah128 2:0a739218ab11 1299 }
jah128 2:0a739218ab11 1300 break;
jah128 2:0a739218ab11 1301
jah128 2:0a739218ab11 1302 }
jah128 2:0a739218ab11 1303 } else {
jah128 2:0a739218ab11 1304 // Response to a command
jah128 2:0a739218ab11 1305 switch(function) {
jah128 2:0a739218ab11 1306 case 0: {
jah128 2:0a739218ab11 1307 if(swarm[sender].status_rf_command_stop == 1) {
jah128 2:0a739218ab11 1308 if(success == 1){
jah128 2:0a739218ab11 1309 if(length == 0) {
jah128 2:0a739218ab11 1310 swarm[sender].status_rf_command_stop = 2;
jah128 2:0a739218ab11 1311 outcome = 1;
jah128 2:0a739218ab11 1312 } else {
jah128 2:0a739218ab11 1313 swarm[sender].status_rf_command_stop = 3;
jah128 2:0a739218ab11 1314 outcome = 3;
jah128 2:0a739218ab11 1315 }
jah128 2:0a739218ab11 1316 } else {
jah128 2:0a739218ab11 1317 swarm[sender].status_rf_command_stop = 4;
jah128 2:0a739218ab11 1318 outcome = 4;
jah128 2:0a739218ab11 1319 }
jah128 2:0a739218ab11 1320 } else outcome = 2;
jah128 2:0a739218ab11 1321 }
jah128 2:0a739218ab11 1322 break;
jah128 2:0a739218ab11 1323 case 1: {
jah128 2:0a739218ab11 1324 if(swarm[sender].status_rf_command_forward == 1) {
jah128 2:0a739218ab11 1325 if(success == 1){
jah128 2:0a739218ab11 1326 if(length == 0) {
jah128 2:0a739218ab11 1327 swarm[sender].status_rf_command_forward = 2;
jah128 2:0a739218ab11 1328 outcome = 1;
jah128 2:0a739218ab11 1329 } else {
jah128 2:0a739218ab11 1330 swarm[sender].status_rf_command_forward = 3;
jah128 2:0a739218ab11 1331 outcome = 3;
jah128 2:0a739218ab11 1332 }
jah128 2:0a739218ab11 1333 } else {
jah128 2:0a739218ab11 1334 swarm[sender].status_rf_command_forward = 4;
jah128 2:0a739218ab11 1335 outcome = 4;
jah128 2:0a739218ab11 1336 }
jah128 2:0a739218ab11 1337 } else outcome = 2;
jah128 2:0a739218ab11 1338 }
jah128 2:0a739218ab11 1339 break;
jah128 2:0a739218ab11 1340 case 2: {
jah128 2:0a739218ab11 1341 if(swarm[sender].status_rf_command_backward == 1) {
jah128 2:0a739218ab11 1342 if(success == 1){
jah128 2:0a739218ab11 1343 if(length == 0) {
jah128 2:0a739218ab11 1344 swarm[sender].status_rf_command_backward = 2;
jah128 2:0a739218ab11 1345 outcome = 1;
jah128 2:0a739218ab11 1346 } else {
jah128 2:0a739218ab11 1347 swarm[sender].status_rf_command_backward = 3;
jah128 2:0a739218ab11 1348 outcome = 3;
jah128 2:0a739218ab11 1349 }
jah128 2:0a739218ab11 1350 } else {
jah128 2:0a739218ab11 1351 swarm[sender].status_rf_command_backward = 4;
jah128 2:0a739218ab11 1352 outcome = 4;
jah128 2:0a739218ab11 1353 }
jah128 2:0a739218ab11 1354 } else outcome = 2;
jah128 2:0a739218ab11 1355 }
jah128 2:0a739218ab11 1356 break;
jah128 2:0a739218ab11 1357 case 3: {
jah128 2:0a739218ab11 1358 if(swarm[sender].status_rf_command_left == 1) {
jah128 2:0a739218ab11 1359 if(success == 1){
jah128 2:0a739218ab11 1360 if(length == 0) {
jah128 2:0a739218ab11 1361 swarm[sender].status_rf_command_left = 2;
jah128 2:0a739218ab11 1362 outcome = 1;
jah128 2:0a739218ab11 1363 } else {
jah128 2:0a739218ab11 1364 swarm[sender].status_rf_command_left = 3;
jah128 2:0a739218ab11 1365 outcome = 3;
jah128 2:0a739218ab11 1366 }
jah128 2:0a739218ab11 1367 } else {
jah128 2:0a739218ab11 1368 swarm[sender].status_rf_command_left = 4;
jah128 2:0a739218ab11 1369 outcome = 4;
jah128 2:0a739218ab11 1370 }
jah128 2:0a739218ab11 1371 } else outcome = 2;
jah128 2:0a739218ab11 1372 }
jah128 2:0a739218ab11 1373 break;
jah128 2:0a739218ab11 1374 case 4: {
jah128 2:0a739218ab11 1375 if(swarm[sender].status_rf_command_right == 1) {
jah128 2:0a739218ab11 1376 if(success == 1){
jah128 2:0a739218ab11 1377 if(length == 0) {
jah128 2:0a739218ab11 1378 swarm[sender].status_rf_command_right = 2;
jah128 2:0a739218ab11 1379 outcome = 1;
jah128 2:0a739218ab11 1380 } else {
jah128 2:0a739218ab11 1381 swarm[sender].status_rf_command_right = 3;
jah128 2:0a739218ab11 1382 outcome = 3;
jah128 2:0a739218ab11 1383 }
jah128 2:0a739218ab11 1384 } else {
jah128 2:0a739218ab11 1385 swarm[sender].status_rf_command_right = 4;
jah128 2:0a739218ab11 1386 outcome = 4;
jah128 2:0a739218ab11 1387 }
jah128 2:0a739218ab11 1388 } else outcome = 2;
jah128 2:0a739218ab11 1389 }
jah128 2:0a739218ab11 1390 break;
jah128 2:0a739218ab11 1391 case 5: {
jah128 2:0a739218ab11 1392 if(swarm[sender].status_rf_command_left_motor == 1) {
jah128 2:0a739218ab11 1393 if(success == 1){
jah128 2:0a739218ab11 1394 if(length == 0) {
jah128 2:0a739218ab11 1395 swarm[sender].status_rf_command_left_motor = 2;
jah128 2:0a739218ab11 1396 outcome = 1;
jah128 2:0a739218ab11 1397 } else {
jah128 2:0a739218ab11 1398 swarm[sender].status_rf_command_left_motor = 3;
jah128 2:0a739218ab11 1399 outcome = 3;
jah128 2:0a739218ab11 1400 }
jah128 2:0a739218ab11 1401 } else {
jah128 2:0a739218ab11 1402 swarm[sender].status_rf_command_left_motor = 4;
jah128 2:0a739218ab11 1403 outcome = 4;
jah128 2:0a739218ab11 1404 }
jah128 2:0a739218ab11 1405 } else outcome = 2;
jah128 2:0a739218ab11 1406 }
jah128 2:0a739218ab11 1407 break;
jah128 2:0a739218ab11 1408 case 6: {
jah128 2:0a739218ab11 1409 if(swarm[sender].status_rf_command_right_motor == 1) {
jah128 2:0a739218ab11 1410 if(success == 1){
jah128 2:0a739218ab11 1411 if(length == 0) {
jah128 2:0a739218ab11 1412 swarm[sender].status_rf_command_right_motor = 2;
jah128 2:0a739218ab11 1413 outcome = 1;
jah128 2:0a739218ab11 1414 } else {
jah128 2:0a739218ab11 1415 swarm[sender].status_rf_command_right_motor = 3;
jah128 2:0a739218ab11 1416 outcome = 3;
jah128 2:0a739218ab11 1417 }
jah128 2:0a739218ab11 1418 } else {
jah128 2:0a739218ab11 1419 swarm[sender].status_rf_command_right_motor = 4;
jah128 2:0a739218ab11 1420 outcome = 4;
jah128 2:0a739218ab11 1421 }
jah128 2:0a739218ab11 1422 } else outcome = 2;
jah128 2:0a739218ab11 1423 }
jah128 2:0a739218ab11 1424 break;
jah128 2:0a739218ab11 1425 case 7: {
jah128 2:0a739218ab11 1426 if(swarm[sender].status_rf_command_oled_colour == 1) {
jah128 2:0a739218ab11 1427 if(success == 1){
jah128 2:0a739218ab11 1428 if(length == 0) {
jah128 2:0a739218ab11 1429 swarm[sender].status_rf_command_oled_colour = 2;
jah128 2:0a739218ab11 1430 outcome = 1;
jah128 2:0a739218ab11 1431 } else {
jah128 2:0a739218ab11 1432 swarm[sender].status_rf_command_oled_colour = 3;
jah128 2:0a739218ab11 1433 outcome = 3;
jah128 2:0a739218ab11 1434 }
jah128 2:0a739218ab11 1435 } else {
jah128 2:0a739218ab11 1436 swarm[sender].status_rf_command_oled_colour = 4;
jah128 2:0a739218ab11 1437 outcome = 4;
jah128 2:0a739218ab11 1438 }
jah128 2:0a739218ab11 1439 } else outcome = 2;
jah128 2:0a739218ab11 1440 }
jah128 2:0a739218ab11 1441 break;
jah128 2:0a739218ab11 1442 case 8: {
jah128 2:0a739218ab11 1443 if(swarm[sender].status_rf_command_cled_colour == 1) {
jah128 2:0a739218ab11 1444 if(success == 1){
jah128 2:0a739218ab11 1445 if(length == 0) {
jah128 2:0a739218ab11 1446 swarm[sender].status_rf_command_cled_colour = 2;
jah128 2:0a739218ab11 1447 outcome = 1;
jah128 2:0a739218ab11 1448 } else {
jah128 2:0a739218ab11 1449 swarm[sender].status_rf_command_cled_colour = 3;
jah128 2:0a739218ab11 1450 outcome = 3;
jah128 2:0a739218ab11 1451 }
jah128 2:0a739218ab11 1452 } else {
jah128 2:0a739218ab11 1453 swarm[sender].status_rf_command_cled_colour = 4;
jah128 2:0a739218ab11 1454 outcome = 4;
jah128 2:0a739218ab11 1455 }
jah128 2:0a739218ab11 1456 } else outcome = 2;
jah128 2:0a739218ab11 1457 }
jah128 2:0a739218ab11 1458 break;
jah128 2:0a739218ab11 1459 case 9: {
jah128 2:0a739218ab11 1460 if(swarm[sender].status_rf_command_oled_state == 1) {
jah128 2:0a739218ab11 1461 if(success == 1){
jah128 2:0a739218ab11 1462 if(length == 0) {
jah128 2:0a739218ab11 1463 swarm[sender].status_rf_command_oled_state = 2;
jah128 2:0a739218ab11 1464 outcome = 1;
jah128 2:0a739218ab11 1465 } else {
jah128 2:0a739218ab11 1466 swarm[sender].status_rf_command_oled_state = 3;
jah128 2:0a739218ab11 1467 outcome = 3;
jah128 2:0a739218ab11 1468 }
jah128 2:0a739218ab11 1469 } else {
jah128 2:0a739218ab11 1470 swarm[sender].status_rf_command_oled_state = 4;
jah128 2:0a739218ab11 1471 outcome = 4;
jah128 2:0a739218ab11 1472 }
jah128 2:0a739218ab11 1473 } else outcome = 2;
jah128 2:0a739218ab11 1474 }
jah128 2:0a739218ab11 1475 break;
jah128 2:0a739218ab11 1476 case 10: {
jah128 2:0a739218ab11 1477 if(swarm[sender].status_rf_command_cled_state == 1) {
jah128 2:0a739218ab11 1478 if(success == 1){
jah128 2:0a739218ab11 1479 if(length == 0) {
jah128 2:0a739218ab11 1480 swarm[sender].status_rf_command_cled_state = 2;
jah128 2:0a739218ab11 1481 outcome = 1;
jah128 2:0a739218ab11 1482 } else {
jah128 2:0a739218ab11 1483 swarm[sender].status_rf_command_cled_state = 3;
jah128 2:0a739218ab11 1484 outcome = 3;
jah128 2:0a739218ab11 1485 }
jah128 2:0a739218ab11 1486 } else {
jah128 2:0a739218ab11 1487 swarm[sender].status_rf_command_cled_state = 4;
jah128 2:0a739218ab11 1488 outcome = 4;
jah128 2:0a739218ab11 1489 }
jah128 2:0a739218ab11 1490 } else outcome = 2;
jah128 2:0a739218ab11 1491 }
jah128 2:0a739218ab11 1492 break;
jah128 2:0a739218ab11 1493 case 11: {
jah128 2:0a739218ab11 1494 if(swarm[sender].status_rf_command_set_oled == 1) {
jah128 2:0a739218ab11 1495 if(success == 1){
jah128 2:0a739218ab11 1496 if(length == 0) {
jah128 2:0a739218ab11 1497 swarm[sender].status_rf_command_set_oled = 2;
jah128 2:0a739218ab11 1498 outcome = 1;
jah128 2:0a739218ab11 1499 } else {
jah128 2:0a739218ab11 1500 swarm[sender].status_rf_command_set_oled = 3;
jah128 2:0a739218ab11 1501 outcome = 3;
jah128 2:0a739218ab11 1502 }
jah128 2:0a739218ab11 1503 } else {
jah128 2:0a739218ab11 1504 swarm[sender].status_rf_command_set_oled = 4;
jah128 2:0a739218ab11 1505 outcome = 4;
jah128 2:0a739218ab11 1506 }
jah128 2:0a739218ab11 1507 } else outcome = 2;
jah128 2:0a739218ab11 1508 }
jah128 2:0a739218ab11 1509 break;
jah128 2:0a739218ab11 1510 case 12: {
jah128 2:0a739218ab11 1511 if(swarm[sender].status_rf_command_play_tune == 1) {
jah128 2:0a739218ab11 1512 if(success == 1){
jah128 2:0a739218ab11 1513 if(length == 0) {
jah128 2:0a739218ab11 1514 swarm[sender].status_rf_command_play_tune = 2;
jah128 2:0a739218ab11 1515 outcome = 1;
jah128 2:0a739218ab11 1516 } else {
jah128 2:0a739218ab11 1517 swarm[sender].status_rf_command_play_tune = 3;
jah128 2:0a739218ab11 1518 outcome = 3;
jah128 2:0a739218ab11 1519 }
jah128 2:0a739218ab11 1520 } else {
jah128 2:0a739218ab11 1521 swarm[sender].status_rf_command_play_tune = 4;
jah128 2:0a739218ab11 1522 outcome = 4;
jah128 2:0a739218ab11 1523 }
jah128 2:0a739218ab11 1524 } else outcome = 2;
jah128 2:0a739218ab11 1525 }
jah128 2:0a739218ab11 1526 break;
jah128 2:0a739218ab11 1527 case 14: {
jah128 2:0a739218ab11 1528 if(swarm[sender].status_rf_command_sync_time == 1) {
jah128 2:0a739218ab11 1529 if(success == 1){
jah128 2:0a739218ab11 1530 if(length == 0) {
jah128 2:0a739218ab11 1531 swarm[sender].status_rf_command_sync_time = 2;
jah128 2:0a739218ab11 1532 outcome = 1;
jah128 2:0a739218ab11 1533 } else {
jah128 2:0a739218ab11 1534 swarm[sender].status_rf_command_sync_time = 3;
jah128 2:0a739218ab11 1535 outcome = 3;
jah128 2:0a739218ab11 1536 }
jah128 2:0a739218ab11 1537 } else {
jah128 2:0a739218ab11 1538 swarm[sender].status_rf_command_sync_time = 4;
jah128 2:0a739218ab11 1539 outcome = 4;
jah128 2:0a739218ab11 1540 }
jah128 2:0a739218ab11 1541 } else outcome = 2;
jah128 2:0a739218ab11 1542 }
jah128 2:0a739218ab11 1543 break;
jah128 2:0a739218ab11 1544 }
jah128 2:0a739218ab11 1545 }
jah128 2:0a739218ab11 1546
jah128 2:0a739218ab11 1547 if(RF_DEBUG) {
jah128 2:0a739218ab11 1548 switch(outcome) {
jah128 2:0a739218ab11 1549 case 0 :
jah128 2:0a739218ab11 1550 pc.printf("Unknown RF response received");
jah128 2:0a739218ab11 1551 case 1 :
jah128 2:0a739218ab11 1552 pc.printf("RF response received, data updated.");
jah128 2:0a739218ab11 1553 case 2 :
jah128 2:0a739218ab11 1554 pc.printf("Unexpected RF response received, ignored.");
jah128 2:0a739218ab11 1555 case 3 :
jah128 2:0a739218ab11 1556 pc.printf("Invalid RF response received, ignored.");
jah128 2:0a739218ab11 1557 case 4 :
jah128 2:0a739218ab11 1558 pc.printf("RF response received: unsuccessful operation.");
jah128 2:0a739218ab11 1559 }
jah128 2:0a739218ab11 1560 }
jah128 2:0a739218ab11 1561 }