James Hilder / Mbed 2 deprecated Pi_Swarm_Handheld_Controller

Handheld controller (RF) for Pi Swarm system

Dependencies:   mbed

Fork of Pi_Swarm_Handheld_Controller by piswarm

communications.cpp@0:d63a63feb104, 2014-06-10 (annotated)

Committer:
jah128
Date:
Tue Jun 10 11:05:23 2014 +0000
Revision:
0:d63a63feb104
Handheld controller for Pi Swarm (old code)

Who changed what in which revision?

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