James Hilder / Pi_Swarm_Library

V0.5 library for the Pi Swarm robot

communications.cpp@3:4c0f2f3de33e, 2014-02-02 (annotated)

Committer:
jah128
Date:
Sun Feb 02 21:18:05 2014 +0000
Revision:
3:4c0f2f3de33e
Parent:
2:0a739218ab11
Child:
4:52b3e4c5a425
updated comms

Who changed what in which revision?

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