James Hilder / Pi_Swarm_Library

V0.5 library for the Pi Swarm robot

communications.cpp@8:08dec9c7d3f6, 2014-02-03 (annotated)

Committer:
jah128
Date:
Mon Feb 03 12:59:51 2014 +0000
Revision:
8:08dec9c7d3f6
Parent:
4:52b3e4c5a425 
Minor comments added

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