James Hilder / Pi_Swarm_Library_7

Unfinished v0.7, added bearing detection

Fork of Pi_Swarm_Library_v06_alpha by piswarm

communications.cpp@11:5ebcb52726cf, 2014-06-30 (annotated)

Committer:
jah128
Date:
Mon Jun 30 07:58:31 2014 +0000
Revision:
11:5ebcb52726cf
Parent:
9:7a4fc1d7e484 
Added prototype bearing detection to pi swarm library;

Who changed what in which revision?

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