James Hilder
/
Pi_Swarm_Library
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
communications.cpp
00001 /******************************************************************************************* 00002 * 00003 * University of York Robot Lab Pi Swarm Library: Swarm Communications Handler 00004 * 00005 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York 00006 * 00007 * Version 0.5 February 2014 00008 * 00009 * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 00010 * 00011 ******************************************************************************************/ 00012 00013 // Important note: The communication stack is enabled by setting the "USE_COMMUNICATION_STACK" flag to 1 00014 // When being used, all received messages are decoded using the decodeMessage() function 00015 // See manual for more info on using the communication handler 00016 #include "communications.h" 00017 #include "piswarm.h" 00018 #include "main.h" 00019 00020 struct swarm_member swarm[SWARM_SIZE]; // Array to hold received information about other swarm members 00021 DigitalOut actioning (LED1); 00022 DigitalOut errorled (LED2); 00023 DigitalOut tx (LED3); 00024 DigitalOut rx (LED4); 00025 Timeout tdma_timeout; 00026 char tdma_busy = 0; 00027 char waiting_message [64]; 00028 char waiting_length = 0; 00029 int message_id = 0; 00030 00031 00032 // Send a structured message over the RF interface 00033 // @target - The target recipient (1-31 or 0 for broadcast) 00034 // @command - The command byte (see manual) 00035 // @*data - Additional data bytes 00036 // @length - Length of additional data 00037 void send_rf_message(char target, char command, char * data, char length) 00038 { 00039 char message [4+length]; 00040 message[0]=piswarm.get_id() + 32; 00041 message[1]=target + 32; 00042 message_id++; 00043 message[2]=message_id % 256; 00044 message[3]=command; 00045 for(int i=0; i<length; i++) { 00046 message[4+i]=data[i]; 00047 } 00048 piswarm.send_rf_message(message,4+length); 00049 if(RF_DEBUG==1)pc.printf("RF message sent"); 00050 } 00051 00052 00053 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00054 // Internal Functions 00055 // In general these functions should not be called by user code 00056 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00057 00058 // Decode the received message header. Check it is min. 4 bytes long, that the sender and target are valid [called in alpha433.cpp] 00059 void processRadioData(char * data, char length) 00060 { 00061 if(RF_USE_LEDS==1) { 00062 errorled=0; 00063 rx=1; 00064 } 00065 // Decompose the received message 00066 if(length < 4) errormessage(0); 00067 else { 00068 // Establish the sender and target of the packet 00069 char sender = data[0]; 00070 char target = data[1]; 00071 char id = data[2]; 00072 char command = data[3]; 00073 if(sender<32 || sender>63)errormessage(1); 00074 else { 00075 if(target<32 || target>63)errormessage(2); 00076 else { 00077 sender -= 32; 00078 target -= 32; 00079 decodeMessage(sender,target,id,command,data+4,length-4); 00080 } 00081 } 00082 } 00083 if(RF_USE_LEDS==1) rx=0; 00084 } 00085 00086 //Decode the received message, action it if it is valid and for me 00087 void decodeMessage(char sender, char target, char id, char command, char * data, char length) 00088 { 00089 char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0; 00090 00091 if(target==0) broadcast_message = 1; 00092 is_response = 0 != (command & (1 << 7)); 00093 request_response = 0 != (command & (1 << 6)); 00094 is_user = 0 != (command & (1 << 5)); 00095 is_command = 0 != (command & (1 << 4)); 00096 function = command % 16; 00097 00098 if (RF_DEBUG==1) { 00099 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); 00100 else pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, requests_array[function],length); 00101 } 00102 00103 //Action the message only if I am a recipient 00104 if(target==0 || target==piswarm.get_id()) { 00105 if(RF_USE_LEDS==1) actioning = 1; 00106 if(is_response == 1) { 00107 if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length); 00108 else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length); 00109 } else { 00110 if(is_command == 1) { 00111 if(RF_ALLOW_COMMANDS == 1) { 00112 if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length); 00113 else handle_command(sender, broadcast_message, request_response, id, function, data, length); 00114 } else if (RF_DEBUG==1) pc.printf(" - Blocked\n"); 00115 } else { 00116 //A information request has no extra parameters 00117 if(length == 0) { 00118 if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length); 00119 else handle_request(sender, broadcast_message, request_response, id, function); 00120 } else if (RF_DEBUG==1) pc.printf(" - Invalid\n"); 00121 } 00122 } 00123 if(RF_USE_LEDS==1) actioning = 0; 00124 } else if (RF_DEBUG==1) pc.printf(" - Ignored\n"); 00125 } 00126 00127 //Send a predefined response message 00128 void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length) 00129 { 00130 char message [4+length]; 00131 message[0]=piswarm.get_id(); 00132 message[1]=target; 00133 message[2]=id; 00134 message[3]=128 + (success << 6) + (is_command << 4) + function; 00135 for(int i=0; i<length; i++) { 00136 message[4+i]=data[i]; 00137 } 00138 //Delay the response if it is broadcast and TDMA mode is on 00139 if(RF_USE_TDMA == 1 && is_broadcast == 1) { 00140 if(tdma_busy == 1) { 00141 if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n"); 00142 } else { 00143 tdma_busy = 1; 00144 strcpy(waiting_message,message); 00145 waiting_length=length; 00146 tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id()); 00147 if (RF_DEBUG==1) pc.printf("TDMA Response pending\n"); 00148 } 00149 } else { 00150 piswarm.send_rf_message(message,4+length); 00151 if(RF_DEBUG==1)pc.printf("Response issued"); 00152 } 00153 } 00154 00155 // Send a delayed response 00156 void tdma_response() 00157 { 00158 piswarm.send_rf_message(waiting_message,4+waiting_length); 00159 tdma_busy = 0; 00160 if (RF_DEBUG==1) pc.printf("TDMA Response issued\n"); 00161 } 00162 00163 // Handle a message that is a predefined command 00164 void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length) 00165 { 00166 char success = 0; 00167 switch(function) { 00168 case 0: // Stop [0 data] 00169 if(length==0) { 00170 piswarm.stop(); 00171 if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - "); 00172 success = 1; 00173 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00174 break; 00175 case 1: // Forward [2 bytes: 16-bit signed short] 00176 if(length==2) { 00177 int i_speed = (data[0] << 8) + data[1]; 00178 float speed = i_speed / 32768.0; 00179 speed--; 00180 piswarm.forward(speed); 00181 success = 1; 00182 if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed); 00183 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00184 break; 00185 case 2: // Backward [2 bytes: 16-bit signed short] 00186 if(length==2) { 00187 int i_speed = (data[0] << 8) + data[1]; 00188 float speed = i_speed / 32768.0; 00189 speed--; 00190 piswarm.backward(speed); 00191 success = 1; 00192 if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed); 00193 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00194 break; 00195 case 3: // Left [2 bytes: 16-bit signed short] 00196 if(length==2) { 00197 int i_speed = (data[0] << 8) + data[1]; 00198 float speed = i_speed / 32768.0; 00199 speed--; 00200 piswarm.left(speed); 00201 success = 1; 00202 if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed); 00203 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00204 break; 00205 case 4: // Right [2 bytes: 16-bit signed short] 00206 if(length==2) { 00207 int i_speed = (data[0] << 8) + data[1]; 00208 float speed = i_speed / 32768.0; 00209 speed--; 00210 piswarm.right(speed); 00211 success = 1; 00212 if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed); 00213 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00214 break; 00215 case 5: // Left Motor [2 bytes: 16-bit signed short] 00216 if(length==2) { 00217 int i_speed = (data[0] << 8) + data[1]; 00218 float speed = i_speed / 32768.0; 00219 speed--; 00220 piswarm.left_motor(speed); 00221 success = 1; 00222 if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed); 00223 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00224 break; 00225 case 6: // Right Motor [2 bytes: 16-bit signed short] 00226 if(length==2) { 00227 int i_speed = (data[0] << 8) + data[1]; 00228 float speed = i_speed / 32768.0; 00229 speed--; 00230 piswarm.right_motor(speed); 00231 success = 1; 00232 if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed); 00233 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00234 break; 00235 case 7: // Outer LED Colour [3 bytes: R, G, B] 00236 if(length==3) { 00237 piswarm.set_oled_colour (data[0],data[1],data[2]); 00238 success = 1; 00239 if(RF_DEBUG==1) pc.printf(" - Set Outer R%i G%i B%i Command Issued - ",data[0],data[1],data[2]); 00240 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00241 break; 00242 case 8: // Center LED Colour[3 bytes: R, G, B] 00243 if(length==3) { 00244 piswarm.set_cled_colour (data[0],data[1],data[2]); 00245 success = 1; 00246 if(RF_DEBUG==1) pc.printf(" - Set Center R%i G%i B%i Command Issued - ",data[0],data[1],data[2]); 00247 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00248 break; 00249 case 9: // Outer LED State [2 bytes: [xxxxxx01][23456789] ] 00250 if(length==2) { 00251 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))); 00252 success = 1; 00253 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00254 break; 00255 case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ] 00256 if(length==1) { 00257 piswarm.enable_cled (data[0] % 2); 00258 success = 1; 00259 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00260 break; 00261 case 11: // Set outer LED [1 byte: [xxxEvvvv] E=enabled vvvv=LED] 00262 if(length==1) { 00263 int led = data[0] % 16; 00264 if(led < 10) { 00265 piswarm.set_oled(led, 0!=(data[0] & (1 << 4))); 00266 success = 1; 00267 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00268 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00269 break; 00270 case 12: // Play sound [Minimum 1 byte] 00271 if(length>0) { 00272 piswarm.play_tune(data,length); 00273 success = 1; 00274 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00275 break; 00276 case 13: // Sync time 00277 if(length==4) { 00278 unsigned int new_time = 0; 00279 new_time+=((unsigned int)data[0] << 24); 00280 new_time+=((unsigned int)data[1] << 16); 00281 new_time+=((unsigned int)data[2] << 8); 00282 new_time+=(unsigned int)data[3]; 00283 set_time(new_time); 00284 display_system_time(); 00285 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); 00286 break; 00287 case 14: // 00288 break; 00289 case 15: // 00290 break; 00291 } 00292 if(request_response == 1) { 00293 send_response(sender, is_broadcast, success, id, 1, function, NULL, 0); 00294 } 00295 00296 } 00297 00298 //Handle a message that is a predefined request 00299 void handle_request(char sender, char is_broadcast, char request_response, char id, char function) 00300 { 00301 int response_length = 0; 00302 char * response = NULL; 00303 char success = 0; 00304 00305 switch(function) { 00306 case 0: // Null request 00307 success=1; 00308 break; 00309 case 1: { // Request left motor speed 00310 response_length = 2; 00311 float speed = piswarm.get_left_motor() * 32767; 00312 int a_speed = 32768 + (int) speed; 00313 char msb = (char) (a_speed / 256); 00314 char lsb = (char) (a_speed % 256); 00315 response = new char[2]; 00316 response[0]=msb; 00317 response[1]=lsb; 00318 success=1; 00319 break; 00320 } 00321 case 2: { // Request right motor speed 00322 response_length = 2; 00323 float speed = piswarm.get_right_motor() * 32767; 00324 int a_speed = 32768 + (int) speed; 00325 char msb = (char) (a_speed / 256); 00326 char lsb = (char) (a_speed % 256); 00327 response = new char[2]; 00328 response[0]=msb; 00329 response[1]=lsb; 00330 success=1; 00331 break; 00332 } 00333 case 3: { // Request button state 00334 response_length = 1; 00335 response = new char[1]; 00336 response[0]=piswarm.get_switches(); 00337 break; 00338 } 00339 case 4: { // Request LED colours 00340 response_length = 6; 00341 response = new char[6]; 00342 int oled_colour = piswarm.get_oled_colour(); 00343 int cled_colour = piswarm.get_cled_colour(); 00344 response[0] = (char) (oled_colour >> 16); 00345 response[1] = (char) ((oled_colour >> 8) % 256); 00346 response[2] = (char) (oled_colour % 256); 00347 response[3] = (char) (cled_colour >> 16); 00348 response[4] = (char) ((cled_colour >> 8) % 256); 00349 response[5] = (char) (cled_colour % 256); 00350 break; 00351 } 00352 case 5: { // Request LED states 00353 response_length = 2; 00354 response = new char[2]; 00355 response[0] = 0; 00356 response[1] = 0; 00357 if (piswarm.get_cled_state() == 1) response[0] += 4; 00358 if (piswarm.get_oled_state(0) == 1) response[0] += 2; 00359 if (piswarm.get_oled_state(1) == 1) response[0] += 1; 00360 if (piswarm.get_oled_state(2) == 1) response[1] += 128; 00361 if (piswarm.get_oled_state(3) == 1) response[1] += 64; 00362 if (piswarm.get_oled_state(4) == 1) response[1] += 32; 00363 if (piswarm.get_oled_state(5) == 1) response[1] += 16; 00364 if (piswarm.get_oled_state(6) == 1) response[1] += 8; 00365 if (piswarm.get_oled_state(7) == 1) response[1] += 4; 00366 if (piswarm.get_oled_state(8) == 1) response[1] += 2; 00367 if (piswarm.get_oled_state(9) == 1) response[1] += 1; 00368 break; 00369 } 00370 case 6: { // Request battery power 00371 response_length = 2; 00372 response = new char[2]; 00373 float fbattery = piswarm.battery() * 1000.0; 00374 unsigned short battery = (unsigned short) fbattery; 00375 response[0] = battery >> 8; 00376 response[1] = battery % 256; 00377 break; 00378 } 00379 case 7: { // Request light sensor reading 00380 response_length = 2; 00381 response = new char[2]; 00382 float flight = piswarm.read_light_sensor() * 655.0; 00383 unsigned short light = (unsigned short) flight; 00384 response[0] = light >> 8; 00385 response[1] = light % 256; 00386 break; 00387 } 00388 case 8: { // Request accelerometer reading 00389 response_length = 6; 00390 response = new char[6]; 00391 int acc_x = (int)piswarm.read_accelerometer_x(); 00392 int acc_y = (int)piswarm.read_accelerometer_y(); 00393 int acc_z = (int)piswarm.read_accelerometer_z(); 00394 acc_x += 32768; 00395 acc_y += 32768; 00396 acc_z += 32768; 00397 response[0]=acc_x >> 8; 00398 response[1]=acc_x % 256; 00399 response[2]=acc_y >> 8; 00400 response[3]=acc_y % 256; 00401 response[4]=acc_z >> 8; 00402 response[5]=acc_z % 256; 00403 break; 00404 } 00405 case 9: { // Request gyroscope reading 00406 response_length = 2; 00407 response = new char[2]; 00408 float fgyro = piswarm.read_gyro(); 00409 fgyro += 32768; 00410 unsigned short sgyro = (unsigned short) fgyro; 00411 response[0] = sgyro >> 8; 00412 response[1] = sgyro % 256; 00413 break; 00414 } 00415 case 10: { // Request background IR reading 00416 response_length = 16; 00417 response = new char[16]; 00418 for(int sensor = 0; sensor < 8; sensor ++){ 00419 int offset = sensor * 2; 00420 unsigned short m_val = piswarm.read_adc_value(sensor); 00421 response[offset]=m_val >> 8; 00422 response[offset+1]=m_val % 256; 00423 } 00424 break; 00425 } 00426 case 11: { // Request illuminated IR reading 00427 response_length = 16; 00428 response = new char[16]; 00429 for(int sensor = 0; sensor < 8; sensor ++){ 00430 int offset = sensor * 2; 00431 unsigned short m_val = piswarm.read_illuminated_raw_ir_value(sensor); 00432 response[offset]=m_val >> 8; 00433 response[offset+1]=m_val % 256; 00434 } 00435 break; 00436 } 00437 00438 case 12: { // Request distance IR reading 00439 response_length = 16; 00440 response = new char[16]; 00441 for(int sensor = 0; sensor < 8; sensor ++){ 00442 int offset = sensor * 2; 00443 float f_val = piswarm.read_reflected_ir_distance(sensor); 00444 //f_val ranges from 0 to 100.0; 00445 f_val *= 655; 00446 unsigned short m_val = (unsigned short) f_val; 00447 response[offset]=m_val >> 8; 00448 response[offset+1]=m_val % 256; 00449 } 00450 break; 00451 } 00452 00453 case 13: // Request line-tracking IR reading 00454 break; 00455 case 14: // Request uptime 00456 break; 00457 case 15: // 00458 break; 00459 } 00460 send_response(sender, is_broadcast, success, id, 0, function, response, response_length); 00461 00462 } 00463 00464 00465 void errormessage(int index) 00466 { 00467 if(RF_USE_LEDS==1) errorled=1; 00468 switch(index) { 00469 case 0: //Message to short 00470 if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n"); 00471 break; 00472 case 1: //Sender out of valid range 00473 if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n"); 00474 break; 00475 case 2: //Target out of valid range 00476 if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n"); 00477 break; 00478 case 3: 00479 00480 break; 00481 case 4: 00482 break; 00483 } 00484 } 00485 00486 00487 00488 00489 00490 void send_rf_request_null ( char target ) 00491 { 00492 char command = 0x80; 00493 char length = 0; 00494 char * data = NULL; 00495 if(target == 0) { //Request broadcast to all recipients 00496 for(int i=0; i<SWARM_SIZE; i++) { 00497 swarm[i].status_rf_request_null = 1; 00498 } 00499 } else swarm[target].status_rf_request_null = 1; 00500 send_rf_message(target,command,data,length); 00501 } 00502 00503 00504 void send_rf_request_left_motor_speed ( char target ) 00505 { 00506 char command = 0x81; 00507 char length = 0; 00508 char * data = NULL; 00509 if(target == 0) { 00510 for(int i=0; i<SWARM_SIZE; i++) { 00511 swarm[i].status_rf_request_left_motor_speed = 1; 00512 } 00513 } else swarm[target].status_rf_request_left_motor_speed = 1; 00514 send_rf_message(target,command,data,length); 00515 } 00516 00517 void send_rf_request_right_motor_speed ( char target ) 00518 { 00519 char command = 0x82; 00520 char length = 0; 00521 char * data = NULL; 00522 if(target == 0) { 00523 for(int i=0; i<SWARM_SIZE; i++) { 00524 swarm[i].status_rf_request_right_motor_speed = 1; 00525 } 00526 } else swarm[target].status_rf_request_right_motor_speed = 1; 00527 send_rf_message(target,command,data,length); 00528 } 00529 00530 void send_rf_request_button_state ( char target ) 00531 { 00532 char command = 0x83; 00533 char length = 0; 00534 char * data = NULL; 00535 if(target == 0) { 00536 for(int i=0; i<SWARM_SIZE; i++) { 00537 swarm[i].status_rf_request_button_state = 1; 00538 } 00539 } else swarm[target].status_rf_request_button_state = 1; 00540 send_rf_message(target,command,data,length); 00541 } 00542 00543 void send_rf_request_led_colour ( char target ) 00544 { 00545 char command = 0x84; 00546 char length = 0; 00547 char * data = NULL; 00548 if(target == 0) { 00549 for(int i=0; i<SWARM_SIZE; i++) { 00550 swarm[i].status_rf_request_led_colour = 1; 00551 } 00552 } else swarm[target].status_rf_request_led_colour = 1; 00553 send_rf_message(target,command,data,length); 00554 } 00555 00556 void send_rf_request_led_states ( char target ) 00557 { 00558 char command = 0x85; 00559 char length = 0; 00560 char * data = NULL; 00561 if(target == 0) { 00562 for(int i=0; i<SWARM_SIZE; i++) { 00563 swarm[i].status_rf_request_led_states = 1; 00564 } 00565 } else swarm[target].status_rf_request_led_states = 1; 00566 send_rf_message(target,command,data,length); 00567 } 00568 00569 void send_rf_request_battery ( char target ) 00570 { 00571 char command = 0x86; 00572 char length = 0; 00573 char * data = NULL; 00574 if(target == 0) { 00575 for(int i=0; i<SWARM_SIZE; i++) { 00576 swarm[i].status_rf_request_battery = 1; 00577 } 00578 } else swarm[target].status_rf_request_battery = 1; 00579 send_rf_message(target,command,data,length); 00580 } 00581 00582 void send_rf_request_light_sensor ( char target ) 00583 { 00584 char command = 0x87; 00585 char length = 0; 00586 char * data = NULL; 00587 if(target == 0) { 00588 for(int i=0; i<SWARM_SIZE; i++) { 00589 swarm[i].status_rf_request_light_sensor = 1; 00590 } 00591 } else swarm[target].status_rf_request_light_sensor = 1; 00592 send_rf_message(target,command,data,length); 00593 } 00594 00595 void send_rf_request_accelerometer ( char target ) 00596 { 00597 char command = 0x88; 00598 char length = 0; 00599 char * data = NULL; 00600 if(target == 0) { 00601 for(int i=0; i<SWARM_SIZE; i++) { 00602 swarm[i].status_rf_request_accelerometer = 1; 00603 } 00604 } else swarm[target].status_rf_request_accelerometer = 1; 00605 send_rf_message(target,command,data,length); 00606 } 00607 00608 void send_rf_request_gyroscope ( char target ) 00609 { 00610 char command = 0x89; 00611 char length = 0; 00612 char * data = NULL; 00613 if(target == 0) { 00614 for(int i=0; i<SWARM_SIZE; i++) { 00615 swarm[i].status_rf_request_gyroscope = 1; 00616 } 00617 } else swarm[target].status_rf_request_gyroscope = 1; 00618 send_rf_message(target,command,data,length); 00619 } 00620 00621 void send_rf_request_background_ir ( char target ) 00622 { 00623 char command = 0x8A; 00624 char length = 0; 00625 char * data = NULL; 00626 if(target == 0) { 00627 for(int i=0; i<SWARM_SIZE; i++) { 00628 swarm[i].status_rf_request_background_ir = 1; 00629 } 00630 } else swarm[target].status_rf_request_background_ir = 1; 00631 send_rf_message(target,command,data,length); 00632 } 00633 00634 void send_rf_request_reflected_ir ( char target ) 00635 { 00636 char command = 0x8B; 00637 char length = 0; 00638 char * data = NULL; 00639 if(target == 0) { 00640 for(int i=0; i<SWARM_SIZE; i++) { 00641 swarm[i].status_rf_request_reflected_ir = 1; 00642 } 00643 } else swarm[target].status_rf_request_reflected_ir = 1; 00644 send_rf_message(target,command,data,length); 00645 } 00646 00647 void send_rf_request_distance_ir ( char target ) 00648 { 00649 char command = 0x8C; 00650 char length = 0; 00651 char * data = NULL; 00652 if(target == 0) { 00653 for(int i=0; i<SWARM_SIZE; i++) { 00654 swarm[i].status_rf_request_distance_ir = 1; 00655 } 00656 } else swarm[target].status_rf_request_distance_ir = 1; 00657 send_rf_message(target,command,data,length); 00658 } 00659 00660 void send_rf_request_line_following_ir ( char target ) 00661 { 00662 char command = 0x8D; 00663 char length = 0; 00664 char * data = NULL; 00665 if(target == 0) { 00666 for(int i=0; i<SWARM_SIZE; i++) { 00667 swarm[i].status_rf_request_line_following_ir = 1; 00668 } 00669 } else swarm[target].status_rf_request_line_following_ir = 1; 00670 send_rf_message(target,command,data,length); 00671 } 00672 00673 void send_rf_request_uptime ( char target ) 00674 { 00675 char command = 0x8E; 00676 char length = 0; 00677 char * data = NULL; 00678 if(target == 0) { 00679 for(int i=0; i<SWARM_SIZE; i++) { 00680 swarm[i].status_rf_request_uptime= 1; 00681 } 00682 } else swarm[target].status_rf_request_uptime = 1; 00683 send_rf_message(target,command,data,length); 00684 } 00685 00686 void send_rf_command_stop ( char target, char request_response ) 00687 { 00688 char command = 0x10; 00689 char length = 0; 00690 char * data = NULL; 00691 if(request_response == 1) { 00692 command+=128; 00693 if(target == 0) { 00694 for(int i=0; i<SWARM_SIZE; i++) { 00695 swarm[i].status_rf_command_stop= 1; 00696 } 00697 } else swarm[target].status_rf_command_stop = 1; 00698 } 00699 send_rf_message(target,command,data,length); 00700 } 00701 00702 void send_rf_command_forward ( char target, char request_response, float speed ) 00703 { 00704 char command = 0x11; 00705 if(request_response == 1) command+=128; 00706 char length = 2; 00707 char data [2]; 00708 float qspeed = speed + 1; 00709 qspeed *= 32768.0; 00710 int ispeed = (int) qspeed; 00711 data[0] = (char) (ispeed >> 8); 00712 data[1] = (char) (ispeed % 256); 00713 if(request_response == 1) { 00714 command+=128; 00715 if(target == 0) { 00716 for(int i=0; i<SWARM_SIZE; i++) { 00717 swarm[i].status_rf_command_forward= 1; 00718 } 00719 } else swarm[target].status_rf_command_forward = 1; 00720 } 00721 send_rf_message(target,command,data,length); 00722 } 00723 00724 00725 void send_rf_command_backward ( char target, char request_response, float speed ) 00726 { 00727 char command = 0x12; 00728 if(request_response == 1) command+=128; 00729 char length = 2; 00730 char data [2]; 00731 float qspeed = speed + 1; 00732 qspeed *= 32768.0; 00733 int ispeed = (int) qspeed; 00734 data[0] = (char) (ispeed >> 8); 00735 data[1] = (char) (ispeed % 256); 00736 if(request_response == 1) { 00737 command+=128; 00738 if(target == 0) { 00739 for(int i=0; i<SWARM_SIZE; i++) { 00740 swarm[i].status_rf_command_backward= 1; 00741 } 00742 } else swarm[target].status_rf_command_backward = 1; 00743 } 00744 send_rf_message(target,command,data,length); 00745 } 00746 00747 void send_rf_command_left ( char target, char request_response, float speed ) 00748 { 00749 char command = 0x13; 00750 if(request_response == 1) command+=128; 00751 char length = 2; 00752 char data [2]; 00753 float qspeed = speed + 1; 00754 qspeed *= 32768.0; 00755 int ispeed = (int) qspeed; 00756 data[0] = (char) (ispeed >> 8); 00757 data[1] = (char) (ispeed % 256); 00758 if(request_response == 1) { 00759 command+=128; 00760 if(target == 0) { 00761 for(int i=0; i<SWARM_SIZE; i++) { 00762 swarm[i].status_rf_command_left = 1; 00763 } 00764 } else swarm[target].status_rf_command_left = 1; 00765 } 00766 send_rf_message(target,command,data,length); 00767 } 00768 00769 void send_rf_command_right ( char target, char request_response, float speed ) 00770 { 00771 char command = 0x14; 00772 if(request_response == 1) command+=128; 00773 char length = 2; 00774 char data [2]; 00775 float qspeed = speed + 1; 00776 qspeed *= 32768.0; 00777 int ispeed = (int) qspeed; 00778 data[0] = (char) (ispeed >> 8); 00779 data[1] = (char) (ispeed % 256); 00780 if(request_response == 1) { 00781 command+=128; 00782 if(target == 0) { 00783 for(int i=0; i<SWARM_SIZE; i++) { 00784 swarm[i].status_rf_command_right = 1; 00785 } 00786 } else swarm[target].status_rf_command_right = 1; 00787 } 00788 send_rf_message(target,command,data,length); 00789 } 00790 00791 void send_rf_command_left_motor ( char target, char request_response, float speed ) 00792 { 00793 char command = 0x15; 00794 if(request_response == 1) command+=128; 00795 char length = 2; 00796 char data [2]; 00797 float qspeed = speed + 1; 00798 qspeed *= 32768.0; 00799 int ispeed = (int) qspeed; 00800 data[0] = (char) (ispeed >> 8); 00801 data[1] = (char) (ispeed % 256); 00802 if(request_response == 1) { 00803 command+=128; 00804 if(target == 0) { 00805 for(int i=0; i<SWARM_SIZE; i++) { 00806 swarm[i].status_rf_command_left_motor = 1; 00807 } 00808 } else swarm[target].status_rf_command_left_motor = 1; 00809 } 00810 send_rf_message(target,command,data,length); 00811 } 00812 00813 void send_rf_command_right_motor ( char target, char request_response, float speed ) 00814 { 00815 char command = 0x16; 00816 if(request_response == 1) command+=128; 00817 char length = 2; 00818 char data [2]; 00819 float qspeed = speed + 1; 00820 qspeed *= 32768.0; 00821 int ispeed = (int) qspeed; 00822 data[0] = (char) (ispeed >> 8); 00823 data[1] = (char) (ispeed % 256); 00824 if(request_response == 1) { 00825 command+=128; 00826 if(target == 0) { 00827 for(int i=0; i<SWARM_SIZE; i++) { 00828 swarm[i].status_rf_command_right_motor = 1; 00829 } 00830 } else swarm[target].status_rf_command_right_motor = 1; 00831 } 00832 send_rf_message(target,command,data,length); 00833 } 00834 00835 void send_rf_command_oled_colour ( char target, char request_response, char red, char green, char blue ) 00836 { 00837 char command = 0x17; 00838 if(request_response == 1) command+=128; 00839 char length = 3; 00840 char data [3]; 00841 data[0] = red; 00842 data[1] = green; 00843 data[2] = blue; 00844 if(request_response == 1) { 00845 command+=128; 00846 if(target == 0) { 00847 for(int i=0; i<SWARM_SIZE; i++) { 00848 swarm[i].status_rf_command_oled_colour = 1; 00849 } 00850 } else swarm[target].status_rf_command_oled_colour = 1; 00851 } 00852 send_rf_message(target,command,data,length); 00853 } 00854 00855 void send_rf_command_cled_colour ( char target, char request_response, char red, char green, char blue ) 00856 { 00857 char command = 0x18; 00858 if(request_response == 1) command+=128; 00859 char length = 3; 00860 char data [3]; 00861 data[0] = red; 00862 data[1] = green; 00863 data[2] = blue; 00864 if(request_response == 1) { 00865 command+=128; 00866 if(target == 0) { 00867 for(int i=0; i<SWARM_SIZE; i++) { 00868 swarm[i].status_rf_command_cled_colour = 1; 00869 } 00870 } else swarm[target].status_rf_command_cled_colour = 1; 00871 } 00872 send_rf_message(target,command,data,length); 00873 } 00874 00875 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 ) 00876 { 00877 char command = 0x19; 00878 if(request_response == 1) command+=128; 00879 char length = 2; 00880 char data [2]; 00881 data[0] = 0; 00882 data[1] = 0; 00883 if( led0 == 1) data[0] += 2; 00884 if( led1 == 1) data[0] += 1; 00885 if( led2 == 1) data[1] += 128; 00886 if( led3 == 1) data[1] += 64; 00887 if( led4 == 1) data[1] += 32; 00888 if( led5 == 1) data[1] += 16; 00889 if( led6 == 1) data[1] += 8; 00890 if( led7 == 1) data[1] += 4; 00891 if( led8 == 1) data[1] += 2; 00892 if( led9 == 1) data[1] += 1; 00893 if(request_response == 1) { 00894 command+=128; 00895 if(target == 0) { 00896 for(int i=0; i<SWARM_SIZE; i++) { 00897 swarm[i].status_rf_command_oled_state = 1; 00898 } 00899 } else swarm[target].status_rf_command_oled_state = 1; 00900 } 00901 send_rf_message(target,command,data,length); 00902 } 00903 00904 void send_rf_command_cled_state ( char target, char request_response, char enable ) 00905 { 00906 char command = 0x1A; 00907 if(request_response == 1) command+=128; 00908 char length = 1; 00909 char data [1]; 00910 data[0] = enable; 00911 if(request_response == 1) { 00912 command+=128; 00913 if(target == 0) { 00914 for(int i=0; i<SWARM_SIZE; i++) { 00915 swarm[i].status_rf_command_cled_state = 1; 00916 } 00917 } else swarm[target].status_rf_command_cled_state = 1; 00918 } 00919 send_rf_message(target,command,data,length); 00920 } 00921 00922 void send_rf_command_set_oled ( char target, char request_response, char oled, char enable ) 00923 { 00924 char command = 0x1B; 00925 if(request_response == 1) command+=128; 00926 char length = 1; 00927 char data [1]; 00928 data[0] = oled; 00929 if(enable == 1) oled+= 32; 00930 if(request_response == 1) { 00931 command+=128; 00932 if(target == 0) { 00933 for(int i=0; i<SWARM_SIZE; i++) { 00934 swarm[i].status_rf_command_set_oled = 1; 00935 } 00936 } else swarm[target].status_rf_command_set_oled = 1; 00937 } 00938 send_rf_message(target,command,data,length); 00939 } 00940 00941 void send_rf_command_play_tune ( char target, char request_response, char * data, char length ) 00942 { 00943 char command = 0x1C; 00944 if(request_response == 1) command+=128; 00945 if(request_response == 1) { 00946 command+=128; 00947 if(target == 0) { 00948 for(int i=0; i<SWARM_SIZE; i++) { 00949 swarm[i].status_rf_command_play_tune = 1; 00950 } 00951 } else swarm[target].status_rf_command_play_tune = 1; 00952 } 00953 send_rf_message(target,command,data,length); 00954 } 00955 00956 void send_rf_command_sync_time ( char target, char request_response ) 00957 { 00958 char command = 0x1D; 00959 if(request_response == 1) command+=128; 00960 char length = 1; 00961 char data [1]; 00962 data[0] = 0; 00963 if(request_response == 1) { 00964 command+=128; 00965 if(target == 0) { 00966 for(int i=0; i<SWARM_SIZE; i++) { 00967 swarm[i].status_rf_command_sync_time = 1; 00968 } 00969 } else swarm[target].status_rf_command_sync_time = 1; 00970 } 00971 send_rf_message(target,command,data,length); 00972 } 00973 00974 //Resets the recorded swarm data tables 00975 void setup_communications() 00976 { 00977 for(int i=0; i<SWARM_SIZE; i++) { 00978 swarm[i].status_rf_request_null = 0; 00979 swarm[i].status_rf_request_left_motor_speed = 0; 00980 swarm[i].status_rf_request_right_motor_speed = 0; 00981 swarm[i].status_rf_request_button_state = 0; 00982 swarm[i].status_rf_request_led_colour = 0; 00983 swarm[i].status_rf_request_led_states = 0; 00984 swarm[i].status_rf_request_battery = 0; 00985 swarm[i].status_rf_request_light_sensor = 0; 00986 swarm[i].status_rf_request_accelerometer = 0; 00987 swarm[i].status_rf_request_gyroscope = 0; 00988 swarm[i].status_rf_request_background_ir = 0; 00989 swarm[i].status_rf_request_reflected_ir = 0; 00990 swarm[i].status_rf_request_distance_ir = 0; 00991 swarm[i].status_rf_request_line_following_ir = 0; 00992 swarm[i].status_rf_request_uptime = 0; 00993 swarm[i].status_rf_command_stop = 0; 00994 swarm[i].status_rf_command_forward = 0; 00995 swarm[i].status_rf_command_backward = 0; 00996 swarm[i].status_rf_command_left = 0; 00997 swarm[i].status_rf_command_right = 0; 00998 swarm[i].status_rf_command_left_motor = 0; 00999 swarm[i].status_rf_command_right_motor = 0; 01000 swarm[i].status_rf_command_oled_colour = 0; 01001 swarm[i].status_rf_command_cled_colour = 0; 01002 swarm[i].status_rf_command_oled_state = 0; 01003 swarm[i].status_rf_command_cled_state = 0; 01004 swarm[i].status_rf_command_set_oled = 0; 01005 swarm[i].status_rf_command_play_tune = 0; 01006 swarm[i].status_rf_command_sync_time = 0; 01007 swarm[i].left_motor_speed = 0.0; 01008 swarm[i].right_motor_speed = 0.0; 01009 swarm[i].button_state = 0; 01010 for(int k=0; k<3; k++) { 01011 swarm[i].outer_led_colour [k]=0; 01012 swarm[i].center_led_colour [k]=0; 01013 swarm[i].accelerometer [k]=0; 01014 } 01015 swarm[i].led_states[0]=0; 01016 swarm[i].led_states[1]=0; 01017 swarm[i].battery = 0.0; 01018 swarm[i].light_sensor = 0.0; 01019 swarm[i].gyro = 0.0; 01020 for(int k=0; k<8; k++) { 01021 swarm[i].background_ir[k] = 0; 01022 swarm[i].reflected_ir[k] = 0; 01023 swarm[i].distance_ir[k] = 0; 01024 } 01025 } 01026 } 01027 01028 01029 // Handle a message that is a response to a predefined (non-user) command or request 01030 void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length) 01031 { 01032 char outcome = 0; 01033 if(is_command == 0) { 01034 // Response is a data_request_response 01035 switch(function) { 01036 case 0: { 01037 if(swarm[sender].status_rf_request_null == 1) { 01038 if(success == 1){ 01039 if(length == 0) { 01040 swarm[sender].status_rf_request_null = 2; 01041 outcome = 1; 01042 } else { 01043 swarm[sender].status_rf_request_null= 3; 01044 outcome = 3; 01045 } 01046 } else { 01047 swarm[sender].status_rf_request_null = 4; 01048 outcome = 4; 01049 } 01050 } else outcome = 2; 01051 } 01052 break; 01053 01054 case 1: { //Left motor speed 01055 if(swarm[sender].status_rf_request_left_motor_speed == 1) { 01056 if(success == 1){ 01057 if(length == 2) { 01058 swarm[sender].status_rf_request_left_motor_speed = 2; 01059 int value = (data [0] << 8) + data[1]; 01060 value -= 32768; 01061 float val = value; 01062 val /= 32767.0; 01063 swarm[sender].left_motor_speed = val; 01064 outcome = 1; 01065 } else { 01066 swarm[sender].status_rf_request_left_motor_speed= 3; 01067 outcome = 3; 01068 } 01069 } else { 01070 swarm[sender].status_rf_request_left_motor_speed = 4; 01071 outcome = 4; 01072 } 01073 } else outcome = 2; 01074 } 01075 break; 01076 01077 case 2: { //Right motor speed 01078 if(swarm[sender].status_rf_request_right_motor_speed == 1) { 01079 if(success == 1){ 01080 if(length == 2) { 01081 swarm[sender].status_rf_request_right_motor_speed = 2; 01082 int value = (data [0] << 8) + data[1]; 01083 value -= 32768; 01084 float val = value; 01085 val /= 32767.0; 01086 swarm[sender].right_motor_speed = val; 01087 outcome = 1; 01088 } else { 01089 swarm[sender].status_rf_request_right_motor_speed = 3; 01090 outcome = 3; 01091 } 01092 } else { 01093 swarm[sender].status_rf_request_right_motor_speed = 4; 01094 outcome = 4; 01095 } 01096 } else outcome = 2; 01097 } 01098 break; 01099 01100 case 3: { //Button state 01101 if(swarm[sender].status_rf_request_button_state == 1) { 01102 if(success == 1){ 01103 if(length == 2) { 01104 swarm[sender].status_rf_request_button_state = 2; 01105 swarm[sender].button_state = data[0]; 01106 outcome = 1; 01107 } else { 01108 swarm[sender].status_rf_request_button_state = 3; 01109 outcome = 3; 01110 } 01111 } else { 01112 swarm[sender].status_rf_request_button_state = 4; 01113 outcome = 4; 01114 } 01115 } else outcome = 2; 01116 } 01117 break; 01118 01119 case 4: { //LED Colour 01120 if(swarm[sender].status_rf_request_led_colour == 1) { 01121 if(success == 1) { 01122 if(length == 6) { 01123 swarm[sender].status_rf_request_led_colour = 2; 01124 for(int i=0; i<3; i++) { 01125 swarm[sender].outer_led_colour[i] = data[i]; 01126 swarm[sender].center_led_colour[i] = data[i+3]; 01127 } 01128 outcome = 1; 01129 } else { 01130 swarm[sender].status_rf_request_led_colour = 3; 01131 outcome = 3; 01132 } 01133 } else { 01134 swarm[sender].status_rf_request_led_colour = 4; 01135 outcome = 4; 01136 } 01137 } else outcome = 2; 01138 } 01139 break; 01140 01141 case 5: { //LED States 01142 if(swarm[sender].status_rf_request_led_states == 1) { 01143 if(success == 1) { 01144 if(length == 2) { 01145 swarm[sender].status_rf_request_led_states = 2; 01146 for(int i=0; i<3; i++) { 01147 swarm[sender].led_states[0] = data[0]; 01148 swarm[sender].led_states[1] = data[1]; 01149 } 01150 outcome = 1; 01151 } else { 01152 swarm[sender].status_rf_request_led_states = 3; 01153 outcome = 3; 01154 } 01155 } else { 01156 swarm[sender].status_rf_request_led_states = 4; 01157 outcome = 4; 01158 } 01159 } else outcome = 2; 01160 } 01161 break; 01162 01163 01164 case 6: { //Battery 01165 if(swarm[sender].status_rf_request_battery == 1) { 01166 if(success == 1) { 01167 if(length == 2) { 01168 swarm[sender].status_rf_request_battery = 2; 01169 int fbattery = data[0] * 256; 01170 fbattery += data[1]; 01171 swarm[sender].battery = (float) fbattery / 1000.0; 01172 outcome = 1; 01173 } else { 01174 swarm[sender].status_rf_request_battery = 3; 01175 outcome = 3; 01176 } 01177 } else { 01178 swarm[sender].status_rf_request_battery = 4; 01179 outcome = 4; 01180 } 01181 } else outcome = 2; 01182 } 01183 break; 01184 01185 case 7: { //Light sensor 01186 if(swarm[sender].status_rf_request_light_sensor == 1) { 01187 if(success == 1) { 01188 if(length == 2) { 01189 swarm[sender].status_rf_request_light_sensor = 2; 01190 int ilight = data[0] * 256; 01191 ilight += data[1]; 01192 float flight = (float) (ilight) / 655.0; 01193 swarm[sender].light_sensor = flight; 01194 outcome = 1; 01195 } else { 01196 swarm[sender].status_rf_request_light_sensor = 3; 01197 outcome = 3; 01198 } 01199 } else { 01200 swarm[sender].status_rf_request_light_sensor = 4; 01201 outcome = 4; 01202 } 01203 } else outcome = 2; 01204 } 01205 break; 01206 01207 case 8: { //Accelerometer 01208 if(swarm[sender].status_rf_request_accelerometer == 1) { 01209 if(success == 1) { 01210 if(length == 6) { 01211 swarm[sender].status_rf_request_accelerometer = 2; 01212 int acc_x = (data[0] * 256) + data[1]; 01213 int acc_y = (data[2] * 256) + data[3]; 01214 int acc_z = (data[4] * 256) + data[5]; 01215 swarm[sender].accelerometer[0] = (float) acc_x - 32768; 01216 swarm[sender].accelerometer[1] = (float) acc_y - 32768; 01217 swarm[sender].accelerometer[2] = (float) acc_z - 32768; 01218 outcome = 1; 01219 } else { 01220 swarm[sender].status_rf_request_accelerometer = 3; 01221 outcome = 3; 01222 } 01223 } else { 01224 swarm[sender].status_rf_request_accelerometer = 4; 01225 outcome = 4; 01226 } 01227 } else outcome = 2; 01228 } 01229 break; 01230 01231 case 9: { //Gyroscope 01232 if(swarm[sender].status_rf_request_gyroscope == 1) { 01233 if(success == 1) { 01234 if(length == 2) { 01235 swarm[sender].status_rf_request_gyroscope = 2; 01236 int gyro = (data [0] * 256) + data[1]; 01237 swarm[sender].gyro = (float) gyro - 32768; 01238 outcome = 1; 01239 } else { 01240 swarm[sender].status_rf_request_gyroscope = 3; 01241 outcome = 3; 01242 } 01243 } else { 01244 swarm[sender].status_rf_request_gyroscope = 4; 01245 outcome = 4; 01246 } 01247 } else outcome = 2; 01248 } 01249 break; 01250 01251 case 10: { //Background IR 01252 if(swarm[sender].status_rf_request_background_ir == 1) { 01253 if(success == 1) { 01254 if(length == 16) { 01255 swarm[sender].status_rf_request_background_ir = 2; 01256 for(int i=0;i<8;i++){ 01257 int offset = i * 2; 01258 unsigned short value = (unsigned short) data[offset] << 8; 01259 value += data[offset+1]; 01260 swarm[sender].background_ir[i]=value; 01261 } 01262 outcome = 1; 01263 } else { 01264 swarm[sender].status_rf_request_background_ir = 3; 01265 outcome = 3; 01266 } 01267 } else { 01268 swarm[sender].status_rf_request_background_ir = 4; 01269 outcome = 4; 01270 } 01271 } else outcome = 2; 01272 } 01273 break; 01274 01275 case 11: { //Reflected IR 01276 if(swarm[sender].status_rf_request_reflected_ir == 1) { 01277 if(success == 1) { 01278 if(length == 16) { 01279 swarm[sender].status_rf_request_reflected_ir = 2; 01280 for(int i=0;i<8;i++){ 01281 int offset = i * 2; 01282 unsigned short value = (unsigned short) data[offset] << 8; 01283 value += data[offset+1]; 01284 swarm[sender].reflected_ir[i]=value; 01285 } 01286 outcome = 1; 01287 } else { 01288 swarm[sender].status_rf_request_reflected_ir = 3; 01289 outcome = 3; 01290 } 01291 } else { 01292 swarm[sender].status_rf_request_reflected_ir = 4; 01293 outcome = 4; 01294 } 01295 } else outcome = 2; 01296 } 01297 break; 01298 01299 case 12: { // Distance IR 01300 if(swarm[sender].status_rf_request_distance_ir == 1) { 01301 if(success == 1) { 01302 if(length == 16) { 01303 swarm[sender].status_rf_request_distance_ir = 2; 01304 for(int i=0;i<8;i++){ 01305 int offset = i * 2; 01306 unsigned short value = (unsigned short) data[offset] << 8; 01307 value += data[offset+1]; 01308 float adjusted = (float) value / 655.0; 01309 swarm[sender].distance_ir[i]=adjusted; 01310 } 01311 outcome = 1; 01312 } else { 01313 swarm[sender].status_rf_request_distance_ir = 3; 01314 outcome = 3; 01315 } 01316 } else { 01317 swarm[sender].status_rf_request_distance_ir = 4; 01318 outcome = 4; 01319 } 01320 } else outcome = 2; 01321 } 01322 break; 01323 01324 case 13: { // Line following IR 01325 if(swarm[sender].status_rf_request_line_following_ir == 1) { 01326 if(success == 1) { 01327 if(length == 10) { 01328 swarm[sender].status_rf_request_line_following_ir = 2; 01329 outcome = 1; 01330 } else { 01331 swarm[sender].status_rf_request_line_following_ir = 3; 01332 outcome = 3; 01333 } 01334 } else { 01335 swarm[sender].status_rf_request_line_following_ir = 4; 01336 outcome = 4; 01337 } 01338 } else outcome = 2; 01339 } 01340 break; 01341 01342 case 14: { // Request uptime 01343 if(swarm[sender].status_rf_request_uptime == 1) { 01344 if(success == 1) { 01345 if(length == 4) { 01346 swarm[sender].status_rf_request_uptime = 2; 01347 outcome = 1; 01348 } else { 01349 swarm[sender].status_rf_request_uptime = 3; 01350 outcome = 3; 01351 } 01352 } else { 01353 swarm[sender].status_rf_request_uptime = 4; 01354 outcome = 4; 01355 } 01356 } else outcome = 2; 01357 } 01358 break; 01359 01360 } 01361 } else { 01362 // Response to a command 01363 switch(function) { 01364 case 0: { 01365 if(swarm[sender].status_rf_command_stop == 1) { 01366 if(success == 1){ 01367 if(length == 0) { 01368 swarm[sender].status_rf_command_stop = 2; 01369 outcome = 1; 01370 } else { 01371 swarm[sender].status_rf_command_stop = 3; 01372 outcome = 3; 01373 } 01374 } else { 01375 swarm[sender].status_rf_command_stop = 4; 01376 outcome = 4; 01377 } 01378 } else outcome = 2; 01379 } 01380 break; 01381 case 1: { 01382 if(swarm[sender].status_rf_command_forward == 1) { 01383 if(success == 1){ 01384 if(length == 0) { 01385 swarm[sender].status_rf_command_forward = 2; 01386 outcome = 1; 01387 } else { 01388 swarm[sender].status_rf_command_forward = 3; 01389 outcome = 3; 01390 } 01391 } else { 01392 swarm[sender].status_rf_command_forward = 4; 01393 outcome = 4; 01394 } 01395 } else outcome = 2; 01396 } 01397 break; 01398 case 2: { 01399 if(swarm[sender].status_rf_command_backward == 1) { 01400 if(success == 1){ 01401 if(length == 0) { 01402 swarm[sender].status_rf_command_backward = 2; 01403 outcome = 1; 01404 } else { 01405 swarm[sender].status_rf_command_backward = 3; 01406 outcome = 3; 01407 } 01408 } else { 01409 swarm[sender].status_rf_command_backward = 4; 01410 outcome = 4; 01411 } 01412 } else outcome = 2; 01413 } 01414 break; 01415 case 3: { 01416 if(swarm[sender].status_rf_command_left == 1) { 01417 if(success == 1){ 01418 if(length == 0) { 01419 swarm[sender].status_rf_command_left = 2; 01420 outcome = 1; 01421 } else { 01422 swarm[sender].status_rf_command_left = 3; 01423 outcome = 3; 01424 } 01425 } else { 01426 swarm[sender].status_rf_command_left = 4; 01427 outcome = 4; 01428 } 01429 } else outcome = 2; 01430 } 01431 break; 01432 case 4: { 01433 if(swarm[sender].status_rf_command_right == 1) { 01434 if(success == 1){ 01435 if(length == 0) { 01436 swarm[sender].status_rf_command_right = 2; 01437 outcome = 1; 01438 } else { 01439 swarm[sender].status_rf_command_right = 3; 01440 outcome = 3; 01441 } 01442 } else { 01443 swarm[sender].status_rf_command_right = 4; 01444 outcome = 4; 01445 } 01446 } else outcome = 2; 01447 } 01448 break; 01449 case 5: { 01450 if(swarm[sender].status_rf_command_left_motor == 1) { 01451 if(success == 1){ 01452 if(length == 0) { 01453 swarm[sender].status_rf_command_left_motor = 2; 01454 outcome = 1; 01455 } else { 01456 swarm[sender].status_rf_command_left_motor = 3; 01457 outcome = 3; 01458 } 01459 } else { 01460 swarm[sender].status_rf_command_left_motor = 4; 01461 outcome = 4; 01462 } 01463 } else outcome = 2; 01464 } 01465 break; 01466 case 6: { 01467 if(swarm[sender].status_rf_command_right_motor == 1) { 01468 if(success == 1){ 01469 if(length == 0) { 01470 swarm[sender].status_rf_command_right_motor = 2; 01471 outcome = 1; 01472 } else { 01473 swarm[sender].status_rf_command_right_motor = 3; 01474 outcome = 3; 01475 } 01476 } else { 01477 swarm[sender].status_rf_command_right_motor = 4; 01478 outcome = 4; 01479 } 01480 } else outcome = 2; 01481 } 01482 break; 01483 case 7: { 01484 if(swarm[sender].status_rf_command_oled_colour == 1) { 01485 if(success == 1){ 01486 if(length == 0) { 01487 swarm[sender].status_rf_command_oled_colour = 2; 01488 outcome = 1; 01489 } else { 01490 swarm[sender].status_rf_command_oled_colour = 3; 01491 outcome = 3; 01492 } 01493 } else { 01494 swarm[sender].status_rf_command_oled_colour = 4; 01495 outcome = 4; 01496 } 01497 } else outcome = 2; 01498 } 01499 break; 01500 case 8: { 01501 if(swarm[sender].status_rf_command_cled_colour == 1) { 01502 if(success == 1){ 01503 if(length == 0) { 01504 swarm[sender].status_rf_command_cled_colour = 2; 01505 outcome = 1; 01506 } else { 01507 swarm[sender].status_rf_command_cled_colour = 3; 01508 outcome = 3; 01509 } 01510 } else { 01511 swarm[sender].status_rf_command_cled_colour = 4; 01512 outcome = 4; 01513 } 01514 } else outcome = 2; 01515 } 01516 break; 01517 case 9: { 01518 if(swarm[sender].status_rf_command_oled_state == 1) { 01519 if(success == 1){ 01520 if(length == 0) { 01521 swarm[sender].status_rf_command_oled_state = 2; 01522 outcome = 1; 01523 } else { 01524 swarm[sender].status_rf_command_oled_state = 3; 01525 outcome = 3; 01526 } 01527 } else { 01528 swarm[sender].status_rf_command_oled_state = 4; 01529 outcome = 4; 01530 } 01531 } else outcome = 2; 01532 } 01533 break; 01534 case 10: { 01535 if(swarm[sender].status_rf_command_cled_state == 1) { 01536 if(success == 1){ 01537 if(length == 0) { 01538 swarm[sender].status_rf_command_cled_state = 2; 01539 outcome = 1; 01540 } else { 01541 swarm[sender].status_rf_command_cled_state = 3; 01542 outcome = 3; 01543 } 01544 } else { 01545 swarm[sender].status_rf_command_cled_state = 4; 01546 outcome = 4; 01547 } 01548 } else outcome = 2; 01549 } 01550 break; 01551 case 11: { 01552 if(swarm[sender].status_rf_command_set_oled == 1) { 01553 if(success == 1){ 01554 if(length == 0) { 01555 swarm[sender].status_rf_command_set_oled = 2; 01556 outcome = 1; 01557 } else { 01558 swarm[sender].status_rf_command_set_oled = 3; 01559 outcome = 3; 01560 } 01561 } else { 01562 swarm[sender].status_rf_command_set_oled = 4; 01563 outcome = 4; 01564 } 01565 } else outcome = 2; 01566 } 01567 break; 01568 case 12: { 01569 if(swarm[sender].status_rf_command_play_tune == 1) { 01570 if(success == 1){ 01571 if(length == 0) { 01572 swarm[sender].status_rf_command_play_tune = 2; 01573 outcome = 1; 01574 } else { 01575 swarm[sender].status_rf_command_play_tune = 3; 01576 outcome = 3; 01577 } 01578 } else { 01579 swarm[sender].status_rf_command_play_tune = 4; 01580 outcome = 4; 01581 } 01582 } else outcome = 2; 01583 } 01584 break; 01585 case 14: { 01586 if(swarm[sender].status_rf_command_sync_time == 1) { 01587 if(success == 1){ 01588 if(length == 0) { 01589 swarm[sender].status_rf_command_sync_time = 2; 01590 outcome = 1; 01591 } else { 01592 swarm[sender].status_rf_command_sync_time = 3; 01593 outcome = 3; 01594 } 01595 } else { 01596 swarm[sender].status_rf_command_sync_time = 4; 01597 outcome = 4; 01598 } 01599 } else outcome = 2; 01600 } 01601 break; 01602 } 01603 } 01604 01605 if(RF_DEBUG) { 01606 switch(outcome) { 01607 case 0 : 01608 pc.printf("Unknown RF response received"); 01609 case 1 : 01610 pc.printf("RF response received, data updated."); 01611 case 2 : 01612 pc.printf("Unexpected RF response received, ignored."); 01613 case 3 : 01614 pc.printf("Invalid RF response received, ignored."); 01615 case 4 : 01616 pc.printf("RF response received: unsuccessful operation."); 01617 } 01618 } 01619 }
Generated on Wed Jul 27 2022 21:17:49 by
1.7.2