James Hilder / Pi_Swarm_Library

V0.5 library for the Pi Swarm robot

communications.cpp@1:b067a08ff54e, 2014-02-02 (annotated)

Committer:
jah128
Date:
Sun Feb 02 18:05:58 2014 +0000
Revision:
1:b067a08ff54e
Parent:
0:9ffe8ebd1c40
Child:
2:0a739218ab11
updated comms library;

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 1:b067a08ff54e 29 void send_rf_message(char target, char command, char * data, char length)
jah128 1:b067a08ff54e 30 {
jah128 1:b067a08ff54e 31 char message [4+length];
jah128 1:b067a08ff54e 32 message[0]=piswarm.get_id();
jah128 1:b067a08ff54e 33 message[1]=target;
jah128 1:b067a08ff54e 34 message_id++;
jah128 1:b067a08ff54e 35 message[2]=message_id % 256;
jah128 1:b067a08ff54e 36 message[3]=command;
jah128 1:b067a08ff54e 37 for(int i=0; i<length; i++) {
jah128 1:b067a08ff54e 38 message[4+i]=data[i];
jah128 1:b067a08ff54e 39 }
jah128 1:b067a08ff54e 40 piswarm.send_rf_message(message,4+length);
jah128 1:b067a08ff54e 41 if(RF_DEBUG==1)pc.printf("RF message sent");
jah128 1:b067a08ff54e 42 }
jah128 1:b067a08ff54e 43
jah128 0:9ffe8ebd1c40 44
jah128 1:b067a08ff54e 45 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 1:b067a08ff54e 46 // Internal Functions
jah128 1:b067a08ff54e 47 // In general these functions should not be called by user code
jah128 1:b067a08ff54e 48 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:9ffe8ebd1c40 49
jah128 1:b067a08ff54e 50 // Decode the received message header. Check it is min. 4 bytes long, that the sender and target are valid
jah128 1:b067a08ff54e 51 void processRadioData(char * data, char length)
jah128 1:b067a08ff54e 52 {
jah128 1:b067a08ff54e 53 if(RF_USE_LEDS==1) {
jah128 1:b067a08ff54e 54 errorled=0;
jah128 1:b067a08ff54e 55 rx=1;
jah128 1:b067a08ff54e 56 }
jah128 1:b067a08ff54e 57 // Decompose the received message
jah128 1:b067a08ff54e 58 if(length < 4) errormessage(0);
jah128 1:b067a08ff54e 59 else {
jah128 1:b067a08ff54e 60 // Establish the sender and target of the packet
jah128 1:b067a08ff54e 61 char sender = data[0];
jah128 1:b067a08ff54e 62 char target = data[1];
jah128 1:b067a08ff54e 63 char id = data[2];
jah128 1:b067a08ff54e 64 char command = data[3];
jah128 1:b067a08ff54e 65 if(sender<32 || sender>63)errormessage(1);
jah128 1:b067a08ff54e 66 else {
jah128 1:b067a08ff54e 67 if(target<32 || target>63)errormessage(2);
jah128 1:b067a08ff54e 68 else {
jah128 1:b067a08ff54e 69 sender -= 32;
jah128 1:b067a08ff54e 70 target -= 32;
jah128 1:b067a08ff54e 71 decodeMessage(sender,target,id,command,data+4,length-4);
jah128 1:b067a08ff54e 72 }
jah128 1:b067a08ff54e 73 }
jah128 1:b067a08ff54e 74 }
jah128 1:b067a08ff54e 75 if(RF_USE_LEDS==1) rx=0;
jah128 1:b067a08ff54e 76 }
jah128 1:b067a08ff54e 77
jah128 1:b067a08ff54e 78 //Decode the received message, action it if it is valid and for me [called in alpha433.cpp]
jah128 1:b067a08ff54e 79 void decodeMessage(char sender, char target, char id, char command, char * data, char length)
jah128 1:b067a08ff54e 80 {
jah128 0:9ffe8ebd1c40 81 char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0;
jah128 1:b067a08ff54e 82
jah128 1:b067a08ff54e 83 if(target==0) broadcast_message = 1;
jah128 0:9ffe8ebd1c40 84 is_response = 0 != (command & (1 << 7));
jah128 0:9ffe8ebd1c40 85 request_response = 0 != (command & (1 << 6));
jah128 0:9ffe8ebd1c40 86 is_user = 0 != (command & (1 << 5));
jah128 0:9ffe8ebd1c40 87 is_command = 0 != (command & (1 << 4));
jah128 0:9ffe8ebd1c40 88 function = command % 16;
jah128 1:b067a08ff54e 89
jah128 0:9ffe8ebd1c40 90 if (RF_DEBUG==1) {
jah128 0:9ffe8ebd1c40 91 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 92 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 93 }
jah128 1:b067a08ff54e 94
jah128 0:9ffe8ebd1c40 95 //Action the message only if I am a recipient
jah128 1:b067a08ff54e 96 if(target==0 || target==piswarm.get_id()) {
jah128 0:9ffe8ebd1c40 97 if(RF_USE_LEDS==1) actioning = 1;
jah128 1:b067a08ff54e 98 if(is_response == 1) {
jah128 1:b067a08ff54e 99 if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 1:b067a08ff54e 100 else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 1:b067a08ff54e 101 } else {
jah128 1:b067a08ff54e 102 if(is_command == 1) {
jah128 1:b067a08ff54e 103 if(RF_ALLOW_COMMANDS == 1) {
jah128 1:b067a08ff54e 104 if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 1:b067a08ff54e 105 else handle_command(sender, broadcast_message, request_response, id, function, data, length);
jah128 1:b067a08ff54e 106 } else if (RF_DEBUG==1) pc.printf(" - Blocked\n");
jah128 1:b067a08ff54e 107 } else {
jah128 0:9ffe8ebd1c40 108 //A information request has no extra parameters
jah128 1:b067a08ff54e 109 if(length == 0) {
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_request(sender, broadcast_message, request_response, id, function);
jah128 1:b067a08ff54e 112 } else if (RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 113 }
jah128 1:b067a08ff54e 114 }
jah128 1:b067a08ff54e 115 if(RF_USE_LEDS==1) actioning = 0;
jah128 0:9ffe8ebd1c40 116 } else if (RF_DEBUG==1) pc.printf(" - Ignored\n");
jah128 0:9ffe8ebd1c40 117 }
jah128 0:9ffe8ebd1c40 118
jah128 0:9ffe8ebd1c40 119 //Send a response message
jah128 1:b067a08ff54e 120 void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
jah128 1:b067a08ff54e 121 {
jah128 0:9ffe8ebd1c40 122 char message [4+length];
jah128 0:9ffe8ebd1c40 123 message[0]=piswarm.get_id();
jah128 0:9ffe8ebd1c40 124 message[1]=target;
jah128 0:9ffe8ebd1c40 125 message[2]=id;
jah128 0:9ffe8ebd1c40 126 message[3]=128 + (success << 6) + (is_command << 4) + function;
jah128 1:b067a08ff54e 127 for(int i=0; i<length; i++) {
jah128 1:b067a08ff54e 128 message[4+i]=data[i];
jah128 1:b067a08ff54e 129 }
jah128 0:9ffe8ebd1c40 130 //Delay the response if it is broadcast and TDMA mode is on
jah128 1:b067a08ff54e 131 if(RF_USE_TDMA == 1 && is_broadcast == 1) {
jah128 1:b067a08ff54e 132 if(tdma_busy == 1) {
jah128 0:9ffe8ebd1c40 133 if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n");
jah128 1:b067a08ff54e 134 } else {
jah128 0:9ffe8ebd1c40 135 tdma_busy = 1;
jah128 0:9ffe8ebd1c40 136 strcpy(waiting_message,message);
jah128 0:9ffe8ebd1c40 137 waiting_length=length;
jah128 0:9ffe8ebd1c40 138 tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id());
jah128 0:9ffe8ebd1c40 139 if (RF_DEBUG==1) pc.printf("TDMA Response pending\n");
jah128 0:9ffe8ebd1c40 140 }
jah128 1:b067a08ff54e 141 } else {
jah128 0:9ffe8ebd1c40 142 piswarm.send_rf_message(message,4+length);
jah128 0:9ffe8ebd1c40 143 if(RF_DEBUG==1)pc.printf("Response issued");
jah128 0:9ffe8ebd1c40 144 }
jah128 0:9ffe8ebd1c40 145 }
jah128 0:9ffe8ebd1c40 146
jah128 0:9ffe8ebd1c40 147 // Send a delayed response
jah128 1:b067a08ff54e 148 void tdma_response()
jah128 1:b067a08ff54e 149 {
jah128 0:9ffe8ebd1c40 150 piswarm.send_rf_message(waiting_message,4+waiting_length);
jah128 0:9ffe8ebd1c40 151 tdma_busy = 0;
jah128 0:9ffe8ebd1c40 152 if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
jah128 0:9ffe8ebd1c40 153 }
jah128 0:9ffe8ebd1c40 154
jah128 1:b067a08ff54e 155 // Handle a message that is a predefined command
jah128 1:b067a08ff54e 156 void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length)
jah128 1:b067a08ff54e 157 {
jah128 0:9ffe8ebd1c40 158 char success = 0;
jah128 1:b067a08ff54e 159 switch(function) {
jah128 0:9ffe8ebd1c40 160 case 0: // Stop [0 data]
jah128 0:9ffe8ebd1c40 161 if(length==0) {
jah128 0:9ffe8ebd1c40 162 piswarm.stop();
jah128 0:9ffe8ebd1c40 163 if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - ");
jah128 0:9ffe8ebd1c40 164 success = 1;
jah128 0:9ffe8ebd1c40 165 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 166 break;
jah128 0:9ffe8ebd1c40 167 case 1: // Forward [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 168 if(length==2) {
jah128 0:9ffe8ebd1c40 169 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 170 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 171 speed--;
jah128 0:9ffe8ebd1c40 172 piswarm.forward(speed);
jah128 0:9ffe8ebd1c40 173 success = 1;
jah128 0:9ffe8ebd1c40 174 if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 175 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 176 break;
jah128 0:9ffe8ebd1c40 177 case 2: // Backward [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 178 if(length==2) {
jah128 0:9ffe8ebd1c40 179 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 180 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 181 speed--;
jah128 0:9ffe8ebd1c40 182 piswarm.backward(speed);
jah128 0:9ffe8ebd1c40 183 success = 1;
jah128 0:9ffe8ebd1c40 184 if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 185 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 186 break;
jah128 0:9ffe8ebd1c40 187 case 3: // Left [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 188 if(length==2) {
jah128 0:9ffe8ebd1c40 189 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 190 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 191 speed--;
jah128 0:9ffe8ebd1c40 192 piswarm.left(speed);
jah128 0:9ffe8ebd1c40 193 success = 1;
jah128 0:9ffe8ebd1c40 194 if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 195 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 196 break;
jah128 0:9ffe8ebd1c40 197 case 4: // Right [2 bytes: 16-bit signed short]
jah128 1:b067a08ff54e 198 if(length==2) {
jah128 0:9ffe8ebd1c40 199 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 200 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 201 speed--;
jah128 0:9ffe8ebd1c40 202 piswarm.right(speed);
jah128 0:9ffe8ebd1c40 203 success = 1;
jah128 0:9ffe8ebd1c40 204 if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 205 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 206 break;
jah128 0:9ffe8ebd1c40 207 case 5: // Left Motor [2 bytes: 16-bit signed short]
jah128 1:b067a08ff54e 208 if(length==2) {
jah128 0:9ffe8ebd1c40 209 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 210 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 211 speed--;
jah128 0:9ffe8ebd1c40 212 piswarm.left_motor(speed);
jah128 0:9ffe8ebd1c40 213 success = 1;
jah128 0:9ffe8ebd1c40 214 if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 215 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 216 break;
jah128 0:9ffe8ebd1c40 217 case 6: // Right Motor [2 bytes: 16-bit signed short]
jah128 1:b067a08ff54e 218 if(length==2) {
jah128 0:9ffe8ebd1c40 219 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 220 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 221 speed--;
jah128 0:9ffe8ebd1c40 222 piswarm.right_motor(speed);
jah128 0:9ffe8ebd1c40 223 success = 1;
jah128 0:9ffe8ebd1c40 224 if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 225 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 226 break;
jah128 0:9ffe8ebd1c40 227 case 7: // Outer LED Colour [3 bytes: R, G, B]
jah128 1:b067a08ff54e 228 if(length==3) {
jah128 1:b067a08ff54e 229 piswarm.set_oled_colour (data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 230 success = 1;
jah128 0:9ffe8ebd1c40 231 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 232 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 233 break;
jah128 0:9ffe8ebd1c40 234 case 8: // Center LED Colour[3 bytes: R, G, B]
jah128 1:b067a08ff54e 235 if(length==3) {
jah128 1:b067a08ff54e 236 piswarm.set_cled_colour (data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 237 success = 1;
jah128 0:9ffe8ebd1c40 238 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 239 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 240 break;
jah128 0:9ffe8ebd1c40 241 case 9: // Outer LED State [2 bytes: [xxxxxx01][23456789] ]
jah128 0:9ffe8ebd1c40 242 if(length==2) {
jah128 1:b067a08ff54e 243 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 244 success = 1;
jah128 0:9ffe8ebd1c40 245 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 246 break;
jah128 0:9ffe8ebd1c40 247 case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ]
jah128 0:9ffe8ebd1c40 248 if(length==1) {
jah128 1:b067a08ff54e 249 piswarm.enable_cled (data[0] % 2);
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 11: // Set outer LED [1 byte: [xxxEvvvv] E=enabled vvvv=LED]
jah128 0:9ffe8ebd1c40 254 if(length==1) {
jah128 0:9ffe8ebd1c40 255 int led = data[0] % 16;
jah128 1:b067a08ff54e 256 if(led < 10) {
jah128 0:9ffe8ebd1c40 257 piswarm.set_oled(led, 0!=(data[0] & (1 << 4)));
jah128 0:9ffe8ebd1c40 258 success = 1;
jah128 0:9ffe8ebd1c40 259 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 260 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 261 break;
jah128 0:9ffe8ebd1c40 262 case 12: // Play sound [Minimum 1 byte]
jah128 0:9ffe8ebd1c40 263 if(length>0) {
jah128 0:9ffe8ebd1c40 264 piswarm.play_tune(data,length);
jah128 0:9ffe8ebd1c40 265 success = 1;
jah128 0:9ffe8ebd1c40 266 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 267 break;
jah128 0:9ffe8ebd1c40 268 case 13: // Sync time
jah128 0:9ffe8ebd1c40 269 if(length==4) {
jah128 0:9ffe8ebd1c40 270 unsigned int new_time = 0;
jah128 0:9ffe8ebd1c40 271 new_time+=((unsigned int)data[0] << 24);
jah128 0:9ffe8ebd1c40 272 new_time+=((unsigned int)data[1] << 16);
jah128 0:9ffe8ebd1c40 273 new_time+=((unsigned int)data[2] << 8);
jah128 0:9ffe8ebd1c40 274 new_time+=(unsigned int)data[3];
jah128 0:9ffe8ebd1c40 275 set_time(new_time);
jah128 0:9ffe8ebd1c40 276 display_system_time();
jah128 0:9ffe8ebd1c40 277 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 1:b067a08ff54e 278 break;
jah128 0:9ffe8ebd1c40 279 case 14: //
jah128 0:9ffe8ebd1c40 280 break;
jah128 0:9ffe8ebd1c40 281 case 15: //
jah128 1:b067a08ff54e 282 break;
jah128 0:9ffe8ebd1c40 283 }
jah128 1:b067a08ff54e 284 if(request_response == 1) {
jah128 0:9ffe8ebd1c40 285 send_response(sender, is_broadcast, success, id, 1, function, NULL, 0);
jah128 0:9ffe8ebd1c40 286 }
jah128 1:b067a08ff54e 287
jah128 0:9ffe8ebd1c40 288 }
jah128 0:9ffe8ebd1c40 289
jah128 1:b067a08ff54e 290 //Handle a message that is a predefined request
jah128 1:b067a08ff54e 291 void handle_request(char sender, char is_broadcast, char request_response, char id, char function)
jah128 1:b067a08ff54e 292 {
jah128 0:9ffe8ebd1c40 293 int response_length = 0;
jah128 0:9ffe8ebd1c40 294 char * response = NULL;
jah128 0:9ffe8ebd1c40 295 char success = 0;
jah128 1:b067a08ff54e 296
jah128 1:b067a08ff54e 297 switch(function) {
jah128 0:9ffe8ebd1c40 298 case 0: // Null request
jah128 0:9ffe8ebd1c40 299 success=1;
jah128 1:b067a08ff54e 300 break;
jah128 1:b067a08ff54e 301 case 1: { // Request left motor speed
jah128 0:9ffe8ebd1c40 302 response_length = 2;
jah128 0:9ffe8ebd1c40 303 float speed = piswarm.get_left_motor() * 32767;
jah128 0:9ffe8ebd1c40 304 int a_speed = 32768 + (int) speed;
jah128 0:9ffe8ebd1c40 305 char msb = (char) (a_speed / 256);
jah128 0:9ffe8ebd1c40 306 char lsb = (char) (a_speed % 256);
jah128 0:9ffe8ebd1c40 307 response = new char[2];
jah128 0:9ffe8ebd1c40 308 response[0]=msb;
jah128 0:9ffe8ebd1c40 309 response[1]=lsb;
jah128 0:9ffe8ebd1c40 310 success=1;
jah128 1:b067a08ff54e 311 break;
jah128 1:b067a08ff54e 312 }
jah128 1:b067a08ff54e 313 case 2: { // Request right motor speed
jah128 0:9ffe8ebd1c40 314 response_length = 2;
jah128 0:9ffe8ebd1c40 315 float speed = piswarm.get_right_motor() * 32767;
jah128 0:9ffe8ebd1c40 316 int a_speed = 32768 + (int) speed;
jah128 0:9ffe8ebd1c40 317 char msb = (char) (a_speed / 256);
jah128 0:9ffe8ebd1c40 318 char lsb = (char) (a_speed % 256);
jah128 0:9ffe8ebd1c40 319 response = new char[2];
jah128 0:9ffe8ebd1c40 320 response[0]=msb;
jah128 0:9ffe8ebd1c40 321 response[1]=lsb;
jah128 0:9ffe8ebd1c40 322 success=1;
jah128 1:b067a08ff54e 323 break;
jah128 1:b067a08ff54e 324 }
jah128 1:b067a08ff54e 325 case 3: { // Request button state
jah128 0:9ffe8ebd1c40 326 response_length = 1;
jah128 0:9ffe8ebd1c40 327 response = new char[1];
jah128 0:9ffe8ebd1c40 328 response[0]=piswarm.get_switches();
jah128 1:b067a08ff54e 329 break;
jah128 1:b067a08ff54e 330 }
jah128 1:b067a08ff54e 331 case 4: { // Request LED colours
jah128 0:9ffe8ebd1c40 332 response_length = 6;
jah128 0:9ffe8ebd1c40 333 response = new char[6];
jah128 0:9ffe8ebd1c40 334 int oled_colour = piswarm.get_oled_colour();
jah128 0:9ffe8ebd1c40 335 int cled_colour = piswarm.get_cled_colour();
jah128 0:9ffe8ebd1c40 336 response[0] = (char) (oled_colour >> 16);
jah128 0:9ffe8ebd1c40 337 response[1] = (char) ((oled_colour >> 8) % 256);
jah128 0:9ffe8ebd1c40 338 response[2] = (char) (oled_colour % 256);
jah128 0:9ffe8ebd1c40 339 response[3] = (char) (cled_colour >> 16);
jah128 0:9ffe8ebd1c40 340 response[4] = (char) ((cled_colour >> 8) % 256);
jah128 0:9ffe8ebd1c40 341 response[5] = (char) (cled_colour % 256);
jah128 1:b067a08ff54e 342 break;
jah128 1:b067a08ff54e 343 }
jah128 1:b067a08ff54e 344 case 5: { // Request LED states
jah128 1:b067a08ff54e 345 response_length = 2;
jah128 1:b067a08ff54e 346 response = new char[2];
jah128 1:b067a08ff54e 347 response[0] = 0;
jah128 1:b067a08ff54e 348 response[1] = 0;
jah128 1:b067a08ff54e 349 if (piswarm.get_cled_state() == 1) response[0] += 4;
jah128 1:b067a08ff54e 350 if (piswarm.get_oled_state(0) == 1) response[0] += 2;
jah128 1:b067a08ff54e 351 if (piswarm.get_oled_state(1) == 1) response[0] += 1;
jah128 1:b067a08ff54e 352 if (piswarm.get_oled_state(2) == 1) response[1] += 128;
jah128 1:b067a08ff54e 353 if (piswarm.get_oled_state(3) == 1) response[1] += 64;
jah128 1:b067a08ff54e 354 if (piswarm.get_oled_state(4) == 1) response[1] += 32;
jah128 1:b067a08ff54e 355 if (piswarm.get_oled_state(5) == 1) response[1] += 16;
jah128 1:b067a08ff54e 356 if (piswarm.get_oled_state(6) == 1) response[1] += 8;
jah128 1:b067a08ff54e 357 if (piswarm.get_oled_state(7) == 1) response[1] += 4;
jah128 1:b067a08ff54e 358 if (piswarm.get_oled_state(8) == 1) response[1] += 2;
jah128 1:b067a08ff54e 359 if (piswarm.get_oled_state(9) == 1) response[1] += 1;
jah128 1:b067a08ff54e 360 break;
jah128 1:b067a08ff54e 361 }
jah128 1:b067a08ff54e 362 case 6: { // Request battery power
jah128 0:9ffe8ebd1c40 363 response_length = 2;
jah128 0:9ffe8ebd1c40 364 response = new char[2];
jah128 0:9ffe8ebd1c40 365 float fbattery = piswarm.battery() * 1000.0;
jah128 0:9ffe8ebd1c40 366 unsigned short battery = (unsigned short) fbattery;
jah128 0:9ffe8ebd1c40 367 response[0] = battery >> 8;
jah128 0:9ffe8ebd1c40 368 response[1] = battery % 256;
jah128 1:b067a08ff54e 369 break;
jah128 1:b067a08ff54e 370 }
jah128 1:b067a08ff54e 371 case 7: { // Request light sensor reading
jah128 0:9ffe8ebd1c40 372 response_length = 2;
jah128 0:9ffe8ebd1c40 373 response = new char[2];
jah128 0:9ffe8ebd1c40 374 float flight = piswarm.read_light_sensor() * 655.0;
jah128 0:9ffe8ebd1c40 375 unsigned short light = (unsigned short) flight;
jah128 0:9ffe8ebd1c40 376 response[0] = light >> 8;
jah128 0:9ffe8ebd1c40 377 response[1] = light % 256;
jah128 1:b067a08ff54e 378 break;
jah128 1:b067a08ff54e 379 }
jah128 1:b067a08ff54e 380 case 8: { // Request accelerometer reading
jah128 1:b067a08ff54e 381
jah128 0:9ffe8ebd1c40 382 break;
jah128 1:b067a08ff54e 383 }
jah128 1:b067a08ff54e 384 case 9: { // Request gyroscope reading
jah128 1:b067a08ff54e 385 response_length = 2;
jah128 1:b067a08ff54e 386 response = new char[2];
jah128 1:b067a08ff54e 387 float fgyro = piswarm.read_gyro();
jah128 1:b067a08ff54e 388 fgyro += 32768;
jah128 1:b067a08ff54e 389 unsigned short sgyro = (unsigned short) fgyro;
jah128 1:b067a08ff54e 390 response[0] = sgyro >> 8;
jah128 1:b067a08ff54e 391 response[1] = sgyro % 256;
jah128 1:b067a08ff54e 392 break;
jah128 1:b067a08ff54e 393 }
jah128 0:9ffe8ebd1c40 394 case 10: // Request background IR reading
jah128 0:9ffe8ebd1c40 395 break;
jah128 0:9ffe8ebd1c40 396 case 11: // Request illuminated IR reading
jah128 1:b067a08ff54e 397 break;
jah128 0:9ffe8ebd1c40 398 case 12: // Request line-tracking IR reading
jah128 0:9ffe8ebd1c40 399 break;
jah128 0:9ffe8ebd1c40 400 case 13: // Request uptime
jah128 0:9ffe8ebd1c40 401 break;
jah128 1:b067a08ff54e 402 case 14: //
jah128 0:9ffe8ebd1c40 403 break;
jah128 0:9ffe8ebd1c40 404 case 15: //
jah128 1:b067a08ff54e 405 break;
jah128 1:b067a08ff54e 406 }
jah128 0:9ffe8ebd1c40 407 send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
jah128 1:b067a08ff54e 408
jah128 0:9ffe8ebd1c40 409 }
jah128 0:9ffe8ebd1c40 410
jah128 0:9ffe8ebd1c40 411
jah128 1:b067a08ff54e 412 void errormessage(int index)
jah128 1:b067a08ff54e 413 {
jah128 0:9ffe8ebd1c40 414 if(RF_USE_LEDS==1) errorled=1;
jah128 1:b067a08ff54e 415 switch(index) {
jah128 0:9ffe8ebd1c40 416 case 0: //Message to short
jah128 0:9ffe8ebd1c40 417 if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n");
jah128 0:9ffe8ebd1c40 418 break;
jah128 0:9ffe8ebd1c40 419 case 1: //Sender out of valid range
jah128 1:b067a08ff54e 420 if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");
jah128 0:9ffe8ebd1c40 421 break;
jah128 1:b067a08ff54e 422 case 2: //Target out of valid range
jah128 0:9ffe8ebd1c40 423 if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n");
jah128 0:9ffe8ebd1c40 424 break;
jah128 0:9ffe8ebd1c40 425 case 3:
jah128 1:b067a08ff54e 426
jah128 1:b067a08ff54e 427 break;
jah128 1:b067a08ff54e 428 case 4:
jah128 1:b067a08ff54e 429 break;
jah128 1:b067a08ff54e 430 }
jah128 1:b067a08ff54e 431 }
jah128 1:b067a08ff54e 432
jah128 1:b067a08ff54e 433
jah128 1:b067a08ff54e 434
jah128 1:b067a08ff54e 435 // Handle a message that is a response
jah128 1:b067a08ff54e 436 void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length)
jah128 1:b067a08ff54e 437 {
jah128 1:b067a08ff54e 438 char outcome = 0;
jah128 1:b067a08ff54e 439 if(is_command == 0) {
jah128 1:b067a08ff54e 440 // Response is a data_request_response
jah128 1:b067a08ff54e 441 switch(function) {
jah128 1:b067a08ff54e 442 case 0: {
jah128 1:b067a08ff54e 443 if(swarm[sender].status_rf_request_null == 1) {
jah128 1:b067a08ff54e 444 if(length == 0) {
jah128 1:b067a08ff54e 445 swarm[sender].status_rf_request_null = 2;
jah128 1:b067a08ff54e 446 outcome = 1;
jah128 1:b067a08ff54e 447 } else {
jah128 1:b067a08ff54e 448 swarm[sender].status_rf_request_null= 3;
jah128 1:b067a08ff54e 449 outcome = 3;
jah128 1:b067a08ff54e 450 }
jah128 1:b067a08ff54e 451 } else outcome = 2;
jah128 1:b067a08ff54e 452 }
jah128 1:b067a08ff54e 453 break;
jah128 1:b067a08ff54e 454
jah128 1:b067a08ff54e 455 case 1: { //Left motor speed
jah128 1:b067a08ff54e 456 if(swarm[sender].status_rf_request_left_motor_speed == 1) {
jah128 1:b067a08ff54e 457 if(length == 2) {
jah128 1:b067a08ff54e 458 swarm[sender].status_rf_request_left_motor_speed = 2;
jah128 1:b067a08ff54e 459 int value = (data [0] << 8) + data[1];
jah128 1:b067a08ff54e 460 value -= 32768;
jah128 1:b067a08ff54e 461 float val = value;
jah128 1:b067a08ff54e 462 val /= 32767.0;
jah128 1:b067a08ff54e 463 swarm[sender].left_motor_speed = val;
jah128 1:b067a08ff54e 464 outcome = 1;
jah128 1:b067a08ff54e 465 } else {
jah128 1:b067a08ff54e 466 swarm[sender].status_rf_request_left_motor_speed= 3;
jah128 1:b067a08ff54e 467 outcome = 3;
jah128 1:b067a08ff54e 468 }
jah128 1:b067a08ff54e 469 } else outcome = 2;
jah128 1:b067a08ff54e 470 }
jah128 1:b067a08ff54e 471 break;
jah128 1:b067a08ff54e 472
jah128 1:b067a08ff54e 473 case 2: { //Right motor speed
jah128 1:b067a08ff54e 474 if(swarm[sender].status_rf_request_right_motor_speed == 1) {
jah128 1:b067a08ff54e 475 if(length == 2) {
jah128 1:b067a08ff54e 476 swarm[sender].status_rf_request_right_motor_speed = 2;
jah128 1:b067a08ff54e 477 int value = (data [0] << 8) + data[1];
jah128 1:b067a08ff54e 478 value -= 32768;
jah128 1:b067a08ff54e 479 float val = value;
jah128 1:b067a08ff54e 480 val /= 32767.0;
jah128 1:b067a08ff54e 481 swarm[sender].right_motor_speed = val;
jah128 1:b067a08ff54e 482 outcome = 1;
jah128 1:b067a08ff54e 483 } else {
jah128 1:b067a08ff54e 484 swarm[sender].status_rf_request_right_motor_speed = 3;
jah128 1:b067a08ff54e 485 outcome = 3;
jah128 1:b067a08ff54e 486 }
jah128 1:b067a08ff54e 487 } else outcome = 2;
jah128 1:b067a08ff54e 488 }
jah128 1:b067a08ff54e 489 break;
jah128 1:b067a08ff54e 490
jah128 1:b067a08ff54e 491 case 3: { //Button state
jah128 1:b067a08ff54e 492 if(swarm[sender].status_rf_request_button_state == 1) {
jah128 1:b067a08ff54e 493 if(length == 2) {
jah128 1:b067a08ff54e 494 swarm[sender].status_rf_request_button_state = 2;
jah128 1:b067a08ff54e 495 swarm[sender].button_state = data[0];
jah128 1:b067a08ff54e 496 outcome = 1;
jah128 1:b067a08ff54e 497 } else {
jah128 1:b067a08ff54e 498 swarm[sender].status_rf_request_button_state = 3;
jah128 1:b067a08ff54e 499 outcome = 3;
jah128 1:b067a08ff54e 500 }
jah128 1:b067a08ff54e 501 } else outcome = 2;
jah128 1:b067a08ff54e 502 }
jah128 1:b067a08ff54e 503 break;
jah128 1:b067a08ff54e 504
jah128 1:b067a08ff54e 505 case 4: { //LED Colour
jah128 1:b067a08ff54e 506 if(swarm[sender].status_rf_request_led_colour == 1) {
jah128 1:b067a08ff54e 507 if(length == 6) {
jah128 1:b067a08ff54e 508 swarm[sender].status_rf_request_led_colour = 2;
jah128 1:b067a08ff54e 509 for(int i=0; i<3; i++) {
jah128 1:b067a08ff54e 510 swarm[sender].outer_led_colour[i] = data[i];
jah128 1:b067a08ff54e 511 swarm[sender].center_led_colour[i] = data[i+3];
jah128 1:b067a08ff54e 512 }
jah128 1:b067a08ff54e 513 outcome = 1;
jah128 1:b067a08ff54e 514 } else {
jah128 1:b067a08ff54e 515 swarm[sender].status_rf_request_led_colour = 3;
jah128 1:b067a08ff54e 516 outcome = 3;
jah128 1:b067a08ff54e 517 }
jah128 1:b067a08ff54e 518 } else outcome = 2;
jah128 1:b067a08ff54e 519 }
jah128 1:b067a08ff54e 520 break;
jah128 1:b067a08ff54e 521
jah128 1:b067a08ff54e 522 case 5: { //LED States
jah128 1:b067a08ff54e 523 if(swarm[sender].status_rf_request_led_states == 1) {
jah128 1:b067a08ff54e 524 if(length == 2) {
jah128 1:b067a08ff54e 525 swarm[sender].status_rf_request_led_states = 2;
jah128 1:b067a08ff54e 526 for(int i=0; i<3; i++) {
jah128 1:b067a08ff54e 527 swarm[sender].led_states[0] = data[0];
jah128 1:b067a08ff54e 528 swarm[sender].led_states[1] = data[1];
jah128 1:b067a08ff54e 529 }
jah128 1:b067a08ff54e 530 outcome = 1;
jah128 1:b067a08ff54e 531 } else {
jah128 1:b067a08ff54e 532 swarm[sender].status_rf_request_led_states = 3;
jah128 1:b067a08ff54e 533 outcome = 3;
jah128 1:b067a08ff54e 534 }
jah128 1:b067a08ff54e 535 } else outcome = 2;
jah128 1:b067a08ff54e 536 }
jah128 1:b067a08ff54e 537 break;
jah128 1:b067a08ff54e 538
jah128 1:b067a08ff54e 539
jah128 1:b067a08ff54e 540 case 6: { //Battery
jah128 1:b067a08ff54e 541 if(swarm[sender].status_rf_request_battery == 1) {
jah128 1:b067a08ff54e 542 if(length == 2) {
jah128 1:b067a08ff54e 543 swarm[sender].status_rf_request_battery = 2;
jah128 1:b067a08ff54e 544 int fbattery = data[0] * 256;
jah128 1:b067a08ff54e 545 fbattery += data[1];
jah128 1:b067a08ff54e 546 swarm[sender].battery = (float) fbattery / 1000.0;
jah128 1:b067a08ff54e 547 outcome = 1;
jah128 1:b067a08ff54e 548 } else {
jah128 1:b067a08ff54e 549 swarm[sender].status_rf_request_battery = 3;
jah128 1:b067a08ff54e 550 outcome = 3;
jah128 1:b067a08ff54e 551 }
jah128 1:b067a08ff54e 552 } else outcome = 2;
jah128 1:b067a08ff54e 553 }
jah128 1:b067a08ff54e 554 break;
jah128 1:b067a08ff54e 555
jah128 1:b067a08ff54e 556 case 7: { //Light sensor
jah128 1:b067a08ff54e 557 if(swarm[sender].status_rf_request_light_sensor == 1) {
jah128 1:b067a08ff54e 558 if(length == 2) {
jah128 1:b067a08ff54e 559 swarm[sender].status_rf_request_light_sensor = 2;
jah128 1:b067a08ff54e 560 int ilight = data[0] * 256;
jah128 1:b067a08ff54e 561 ilight += data[1];
jah128 1:b067a08ff54e 562 float flight = (float) (ilight) / 655.0;
jah128 1:b067a08ff54e 563 swarm[sender].light_sensor = flight;
jah128 1:b067a08ff54e 564 outcome = 1;
jah128 1:b067a08ff54e 565 } else {
jah128 1:b067a08ff54e 566 swarm[sender].status_rf_request_light_sensor = 3;
jah128 1:b067a08ff54e 567 outcome = 3;
jah128 1:b067a08ff54e 568 }
jah128 1:b067a08ff54e 569 } else outcome = 2;
jah128 1:b067a08ff54e 570 }
jah128 1:b067a08ff54e 571 break;
jah128 1:b067a08ff54e 572
jah128 1:b067a08ff54e 573 case 8: { //Accelerometer
jah128 1:b067a08ff54e 574 if(swarm[sender].status_rf_request_accelerometer == 1) {
jah128 1:b067a08ff54e 575 if(length == 6) {
jah128 1:b067a08ff54e 576 swarm[sender].status_rf_request_accelerometer = 2;
jah128 1:b067a08ff54e 577 outcome = 1;
jah128 1:b067a08ff54e 578 } else {
jah128 1:b067a08ff54e 579 swarm[sender].status_rf_request_accelerometer = 3;
jah128 1:b067a08ff54e 580 outcome = 3;
jah128 1:b067a08ff54e 581 }
jah128 1:b067a08ff54e 582 } else outcome = 2;
jah128 1:b067a08ff54e 583 }
jah128 1:b067a08ff54e 584 break;
jah128 1:b067a08ff54e 585
jah128 1:b067a08ff54e 586 case 9: { //Gyroscope
jah128 1:b067a08ff54e 587 if(swarm[sender].status_rf_request_gyroscope == 1) {
jah128 1:b067a08ff54e 588 if(length == 2) {
jah128 1:b067a08ff54e 589 swarm[sender].status_rf_request_gyroscope = 2;
jah128 1:b067a08ff54e 590 outcome = 1;
jah128 1:b067a08ff54e 591 } else {
jah128 1:b067a08ff54e 592 swarm[sender].status_rf_request_gyroscope = 3;
jah128 1:b067a08ff54e 593 outcome = 3;
jah128 1:b067a08ff54e 594 }
jah128 1:b067a08ff54e 595 } else outcome = 2;
jah128 1:b067a08ff54e 596 }
jah128 1:b067a08ff54e 597 break;
jah128 1:b067a08ff54e 598
jah128 1:b067a08ff54e 599 case 10: { //Background IR
jah128 1:b067a08ff54e 600 if(swarm[sender].status_rf_request_background_ir == 1) {
jah128 1:b067a08ff54e 601 if(length == 16) {
jah128 1:b067a08ff54e 602 swarm[sender].status_rf_request_background_ir = 2;
jah128 1:b067a08ff54e 603 outcome = 1;
jah128 1:b067a08ff54e 604 } else {
jah128 1:b067a08ff54e 605 swarm[sender].status_rf_request_background_ir = 3;
jah128 1:b067a08ff54e 606 outcome = 3;
jah128 1:b067a08ff54e 607 }
jah128 1:b067a08ff54e 608 } else outcome = 2;
jah128 1:b067a08ff54e 609 }
jah128 1:b067a08ff54e 610 break;
jah128 1:b067a08ff54e 611
jah128 1:b067a08ff54e 612 case 11: { //Reflected IR
jah128 1:b067a08ff54e 613 if(swarm[sender].status_rf_request_background_ir == 1) {
jah128 1:b067a08ff54e 614 if(length == 16) {
jah128 1:b067a08ff54e 615 swarm[sender].status_rf_request_background_ir = 2;
jah128 1:b067a08ff54e 616 outcome = 1;
jah128 1:b067a08ff54e 617 } else {
jah128 1:b067a08ff54e 618 swarm[sender].status_rf_request_background_ir = 3;
jah128 1:b067a08ff54e 619 outcome = 3;
jah128 1:b067a08ff54e 620 }
jah128 1:b067a08ff54e 621 } else outcome = 2;
jah128 1:b067a08ff54e 622 }
jah128 1:b067a08ff54e 623 break;
jah128 1:b067a08ff54e 624
jah128 1:b067a08ff54e 625 case 12: { // Distance IR
jah128 1:b067a08ff54e 626 if(swarm[sender].status_rf_request_distance_ir == 1) {
jah128 1:b067a08ff54e 627 if(length == 16) {
jah128 1:b067a08ff54e 628 swarm[sender].status_rf_request_distance_ir = 2;
jah128 1:b067a08ff54e 629 outcome = 1;
jah128 1:b067a08ff54e 630 } else {
jah128 1:b067a08ff54e 631 swarm[sender].status_rf_request_distance_ir = 3;
jah128 1:b067a08ff54e 632 outcome = 3;
jah128 1:b067a08ff54e 633 }
jah128 1:b067a08ff54e 634 } else outcome = 2;
jah128 1:b067a08ff54e 635 }
jah128 1:b067a08ff54e 636 break;
jah128 1:b067a08ff54e 637
jah128 1:b067a08ff54e 638 case 13: { // Line following IR
jah128 1:b067a08ff54e 639 if(swarm[sender].status_rf_request_line_following_ir == 1) {
jah128 1:b067a08ff54e 640 if(length == 10) {
jah128 1:b067a08ff54e 641 swarm[sender].status_rf_request_line_following_ir = 2;
jah128 1:b067a08ff54e 642 outcome = 1;
jah128 1:b067a08ff54e 643 } else {
jah128 1:b067a08ff54e 644 swarm[sender].status_rf_request_line_following_ir = 3;
jah128 1:b067a08ff54e 645 outcome = 3;
jah128 1:b067a08ff54e 646 }
jah128 1:b067a08ff54e 647 } else outcome = 2;
jah128 1:b067a08ff54e 648 }
jah128 1:b067a08ff54e 649 break;
jah128 1:b067a08ff54e 650
jah128 1:b067a08ff54e 651 case 14: { // Request uptime
jah128 1:b067a08ff54e 652 if(swarm[sender].status_rf_request_uptime == 1) {
jah128 1:b067a08ff54e 653 if(length == 4) {
jah128 1:b067a08ff54e 654 swarm[sender].status_rf_request_uptime = 2;
jah128 1:b067a08ff54e 655 outcome = 1;
jah128 1:b067a08ff54e 656 } else {
jah128 1:b067a08ff54e 657 swarm[sender].status_rf_request_uptime = 3;
jah128 1:b067a08ff54e 658 outcome = 3;
jah128 1:b067a08ff54e 659 }
jah128 1:b067a08ff54e 660 } else outcome = 2;
jah128 1:b067a08ff54e 661 }
jah128 0:9ffe8ebd1c40 662 break;
jah128 1:b067a08ff54e 663
jah128 1:b067a08ff54e 664 }
jah128 1:b067a08ff54e 665 } else {
jah128 1:b067a08ff54e 666 // Response to a command
jah128 1:b067a08ff54e 667 }
jah128 1:b067a08ff54e 668
jah128 1:b067a08ff54e 669 if(RF_DEBUG) {
jah128 1:b067a08ff54e 670 switch(outcome) {
jah128 1:b067a08ff54e 671 case 0 :
jah128 1:b067a08ff54e 672 pc.printf("Unknown RF response received");
jah128 1:b067a08ff54e 673 case 1 :
jah128 1:b067a08ff54e 674 pc.printf("RF response received, data updated.");
jah128 1:b067a08ff54e 675 case 2 :
jah128 1:b067a08ff54e 676 pc.printf("Unexpected RF response received, ignored.");
jah128 1:b067a08ff54e 677 case 3 :
jah128 1:b067a08ff54e 678 pc.printf("Invalid RF response received, ignored.");
jah128 1:b067a08ff54e 679 }
jah128 1:b067a08ff54e 680 }
jah128 1:b067a08ff54e 681 }
jah128 1:b067a08ff54e 682
jah128 1:b067a08ff54e 683 void send_rf_request_null ( char target )
jah128 1:b067a08ff54e 684 {
jah128 1:b067a08ff54e 685 char command = 0x80;
jah128 1:b067a08ff54e 686 char length = 0;
jah128 1:b067a08ff54e 687 char * data = NULL;
jah128 1:b067a08ff54e 688 if(target == 0) { //Request broadcast to all recipients
jah128 1:b067a08ff54e 689 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 690 swarm[i].status_rf_request_null = 1;
jah128 1:b067a08ff54e 691 }
jah128 1:b067a08ff54e 692 } else swarm[target].status_rf_request_null = 1;
jah128 1:b067a08ff54e 693 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 694 }
jah128 1:b067a08ff54e 695
jah128 1:b067a08ff54e 696
jah128 1:b067a08ff54e 697 void send_rf_request_left_motor_speed ( char target )
jah128 1:b067a08ff54e 698 {
jah128 1:b067a08ff54e 699 char command = 0x81;
jah128 1:b067a08ff54e 700 char length = 0;
jah128 1:b067a08ff54e 701 char * data = NULL;
jah128 1:b067a08ff54e 702 if(target == 0) {
jah128 1:b067a08ff54e 703 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 704 swarm[i].status_rf_request_left_motor_speed = 1;
jah128 1:b067a08ff54e 705 }
jah128 1:b067a08ff54e 706 } else swarm[target].status_rf_request_left_motor_speed = 1;
jah128 1:b067a08ff54e 707 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 708 }
jah128 1:b067a08ff54e 709
jah128 1:b067a08ff54e 710 void send_rf_request_right_motor_speed ( char target )
jah128 1:b067a08ff54e 711 {
jah128 1:b067a08ff54e 712 char command = 0x82;
jah128 1:b067a08ff54e 713 char length = 0;
jah128 1:b067a08ff54e 714 char * data = NULL;
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_request_right_motor_speed = 1;
jah128 1:b067a08ff54e 718 }
jah128 1:b067a08ff54e 719 } else swarm[target].status_rf_request_right_motor_speed = 1;
jah128 1:b067a08ff54e 720 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 721 }
jah128 1:b067a08ff54e 722
jah128 1:b067a08ff54e 723 void send_rf_request_button_state ( char target )
jah128 1:b067a08ff54e 724 {
jah128 1:b067a08ff54e 725 char command = 0x83;
jah128 1:b067a08ff54e 726 char length = 0;
jah128 1:b067a08ff54e 727 char * data = NULL;
jah128 1:b067a08ff54e 728 if(target == 0) {
jah128 1:b067a08ff54e 729 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 730 swarm[i].status_rf_request_button_state = 1;
jah128 1:b067a08ff54e 731 }
jah128 1:b067a08ff54e 732 } else swarm[target].status_rf_request_button_state = 1;
jah128 1:b067a08ff54e 733 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 734 }
jah128 1:b067a08ff54e 735
jah128 1:b067a08ff54e 736 void send_rf_request_led_colour ( char target )
jah128 1:b067a08ff54e 737 {
jah128 1:b067a08ff54e 738 char command = 0x84;
jah128 1:b067a08ff54e 739 char length = 0;
jah128 1:b067a08ff54e 740 char * data = NULL;
jah128 1:b067a08ff54e 741 if(target == 0) {
jah128 1:b067a08ff54e 742 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 743 swarm[i].status_rf_request_led_colour = 1;
jah128 1:b067a08ff54e 744 }
jah128 1:b067a08ff54e 745 } else swarm[target].status_rf_request_led_colour = 1;
jah128 1:b067a08ff54e 746 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 747 }
jah128 1:b067a08ff54e 748
jah128 1:b067a08ff54e 749 void send_rf_request_led_states ( char target )
jah128 1:b067a08ff54e 750 {
jah128 1:b067a08ff54e 751 char command = 0x85;
jah128 1:b067a08ff54e 752 char length = 0;
jah128 1:b067a08ff54e 753 char * data = NULL;
jah128 1:b067a08ff54e 754 if(target == 0) {
jah128 1:b067a08ff54e 755 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 756 swarm[i].status_rf_request_led_states = 1;
jah128 1:b067a08ff54e 757 }
jah128 1:b067a08ff54e 758 } else swarm[target].status_rf_request_led_states = 1;
jah128 1:b067a08ff54e 759 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 760 }
jah128 1:b067a08ff54e 761
jah128 1:b067a08ff54e 762 void send_rf_request_battery ( char target )
jah128 1:b067a08ff54e 763 {
jah128 1:b067a08ff54e 764 char command = 0x86;
jah128 1:b067a08ff54e 765 char length = 0;
jah128 1:b067a08ff54e 766 char * data = NULL;
jah128 1:b067a08ff54e 767 if(target == 0) {
jah128 1:b067a08ff54e 768 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 769 swarm[i].status_rf_request_battery = 1;
jah128 1:b067a08ff54e 770 }
jah128 1:b067a08ff54e 771 } else swarm[target].status_rf_request_battery = 1;
jah128 1:b067a08ff54e 772 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 773 }
jah128 1:b067a08ff54e 774
jah128 1:b067a08ff54e 775 void send_rf_request_light_sensor ( char target )
jah128 1:b067a08ff54e 776 {
jah128 1:b067a08ff54e 777 char command = 0x87;
jah128 1:b067a08ff54e 778 char length = 0;
jah128 1:b067a08ff54e 779 char * data = NULL;
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_request_light_sensor = 1;
jah128 1:b067a08ff54e 783 }
jah128 1:b067a08ff54e 784 } else swarm[target].status_rf_request_light_sensor = 1;
jah128 1:b067a08ff54e 785 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 786 }
jah128 1:b067a08ff54e 787
jah128 1:b067a08ff54e 788 void send_rf_request_accelerometer ( char target )
jah128 1:b067a08ff54e 789 {
jah128 1:b067a08ff54e 790 char command = 0x88;
jah128 1:b067a08ff54e 791 char length = 0;
jah128 1:b067a08ff54e 792 char * data = NULL;
jah128 1:b067a08ff54e 793 if(target == 0) {
jah128 1:b067a08ff54e 794 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 795 swarm[i].status_rf_request_accelerometer = 1;
jah128 1:b067a08ff54e 796 }
jah128 1:b067a08ff54e 797 } else swarm[target].status_rf_request_accelerometer = 1;
jah128 1:b067a08ff54e 798 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 799 }
jah128 1:b067a08ff54e 800
jah128 1:b067a08ff54e 801 void send_rf_request_gyroscope ( char target )
jah128 1:b067a08ff54e 802 {
jah128 1:b067a08ff54e 803 char command = 0x89;
jah128 1:b067a08ff54e 804 char length = 0;
jah128 1:b067a08ff54e 805 char * data = NULL;
jah128 1:b067a08ff54e 806 if(target == 0) {
jah128 1:b067a08ff54e 807 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 808 swarm[i].status_rf_request_gyroscope = 1;
jah128 1:b067a08ff54e 809 }
jah128 1:b067a08ff54e 810 } else swarm[target].status_rf_request_gyroscope = 1;
jah128 1:b067a08ff54e 811 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 812 }
jah128 1:b067a08ff54e 813
jah128 1:b067a08ff54e 814 void send_rf_request_background_ir ( char target )
jah128 1:b067a08ff54e 815 {
jah128 1:b067a08ff54e 816 char command = 0x8A;
jah128 1:b067a08ff54e 817 char length = 0;
jah128 1:b067a08ff54e 818 char * data = NULL;
jah128 1:b067a08ff54e 819 if(target == 0) {
jah128 1:b067a08ff54e 820 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 821 swarm[i].status_rf_request_background_ir = 1;
jah128 1:b067a08ff54e 822 }
jah128 1:b067a08ff54e 823 } else swarm[target].status_rf_request_background_ir = 1;
jah128 1:b067a08ff54e 824 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 825 }
jah128 1:b067a08ff54e 826
jah128 1:b067a08ff54e 827 void send_rf_request_reflected_ir ( char target )
jah128 1:b067a08ff54e 828 {
jah128 1:b067a08ff54e 829 char command = 0x8B;
jah128 1:b067a08ff54e 830 char length = 0;
jah128 1:b067a08ff54e 831 char * data = NULL;
jah128 1:b067a08ff54e 832 if(target == 0) {
jah128 1:b067a08ff54e 833 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 834 swarm[i].status_rf_request_reflected_ir = 1;
jah128 1:b067a08ff54e 835 }
jah128 1:b067a08ff54e 836 } else swarm[target].status_rf_request_reflected_ir = 1;
jah128 1:b067a08ff54e 837 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 838 }
jah128 1:b067a08ff54e 839
jah128 1:b067a08ff54e 840 void send_rf_request_distance_ir ( char target )
jah128 1:b067a08ff54e 841 {
jah128 1:b067a08ff54e 842 char command = 0x8C;
jah128 1:b067a08ff54e 843 char length = 0;
jah128 1:b067a08ff54e 844 char * data = NULL;
jah128 1:b067a08ff54e 845 if(target == 0) {
jah128 1:b067a08ff54e 846 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 847 swarm[i].status_rf_request_distance_ir = 1;
jah128 1:b067a08ff54e 848 }
jah128 1:b067a08ff54e 849 } else swarm[target].status_rf_request_distance_ir = 1;
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_request_line_following_ir ( char target )
jah128 1:b067a08ff54e 854 {
jah128 1:b067a08ff54e 855 char command = 0x8D;
jah128 1:b067a08ff54e 856 char length = 0;
jah128 1:b067a08ff54e 857 char * data = NULL;
jah128 1:b067a08ff54e 858 if(target == 0) {
jah128 1:b067a08ff54e 859 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 860 swarm[i].status_rf_request_line_following_ir = 1;
jah128 1:b067a08ff54e 861 }
jah128 1:b067a08ff54e 862 } else swarm[target].status_rf_request_line_following_ir = 1;
jah128 1:b067a08ff54e 863 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 864 }
jah128 1:b067a08ff54e 865
jah128 1:b067a08ff54e 866 void send_rf_request_uptime ( char target )
jah128 1:b067a08ff54e 867 {
jah128 1:b067a08ff54e 868 char command = 0x8E;
jah128 1:b067a08ff54e 869 char length = 0;
jah128 1:b067a08ff54e 870 char * data = NULL;
jah128 1:b067a08ff54e 871 if(target == 0) {
jah128 1:b067a08ff54e 872 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 873 swarm[i].status_rf_request_uptime= 1;
jah128 1:b067a08ff54e 874 }
jah128 1:b067a08ff54e 875 } else swarm[target].status_rf_request_uptime = 1;
jah128 1:b067a08ff54e 876 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 877 }
jah128 1:b067a08ff54e 878
jah128 1:b067a08ff54e 879 void send_rf_command_stop ( char target, char request_response )
jah128 1:b067a08ff54e 880 {
jah128 1:b067a08ff54e 881 char command = 0x10;
jah128 1:b067a08ff54e 882 char length = 0;
jah128 1:b067a08ff54e 883 char * data = NULL;
jah128 1:b067a08ff54e 884 if(request_response == 1) {
jah128 1:b067a08ff54e 885 command+=128;
jah128 1:b067a08ff54e 886 if(target == 0) {
jah128 1:b067a08ff54e 887 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 888 swarm[i].status_rf_command_stop= 1;
jah128 1:b067a08ff54e 889 }
jah128 1:b067a08ff54e 890 } else swarm[target].status_rf_command_stop = 1;
jah128 1:b067a08ff54e 891 }
jah128 1:b067a08ff54e 892 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 893 }
jah128 1:b067a08ff54e 894
jah128 1:b067a08ff54e 895 void send_rf_command_forward ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 896 {
jah128 1:b067a08ff54e 897 char command = 0x11;
jah128 1:b067a08ff54e 898 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 899 char length = 2;
jah128 1:b067a08ff54e 900 char data [2];
jah128 1:b067a08ff54e 901 float qspeed = speed + 1;
jah128 1:b067a08ff54e 902 qspeed *= 32768.0;
jah128 1:b067a08ff54e 903 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 904 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 905 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 906 if(request_response == 1) {
jah128 1:b067a08ff54e 907 command+=128;
jah128 1:b067a08ff54e 908 if(target == 0) {
jah128 1:b067a08ff54e 909 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 910 swarm[i].status_rf_command_forward= 1;
jah128 1:b067a08ff54e 911 }
jah128 1:b067a08ff54e 912 } else swarm[target].status_rf_command_forward = 1;
jah128 1:b067a08ff54e 913 }
jah128 1:b067a08ff54e 914 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 915 }
jah128 1:b067a08ff54e 916
jah128 1:b067a08ff54e 917
jah128 1:b067a08ff54e 918 void send_rf_command_backward ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 919 {
jah128 1:b067a08ff54e 920 char command = 0x12;
jah128 1:b067a08ff54e 921 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 922 char length = 2;
jah128 1:b067a08ff54e 923 char data [2];
jah128 1:b067a08ff54e 924 float qspeed = speed + 1;
jah128 1:b067a08ff54e 925 qspeed *= 32768.0;
jah128 1:b067a08ff54e 926 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 927 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 928 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 929 if(request_response == 1) {
jah128 1:b067a08ff54e 930 command+=128;
jah128 1:b067a08ff54e 931 if(target == 0) {
jah128 1:b067a08ff54e 932 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 933 swarm[i].status_rf_command_backward= 1;
jah128 1:b067a08ff54e 934 }
jah128 1:b067a08ff54e 935 } else swarm[target].status_rf_command_backward = 1;
jah128 0:9ffe8ebd1c40 936 }
jah128 1:b067a08ff54e 937 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 938 }
jah128 1:b067a08ff54e 939
jah128 1:b067a08ff54e 940 void send_rf_command_left ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 941 {
jah128 1:b067a08ff54e 942 char command = 0x13;
jah128 1:b067a08ff54e 943 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 944 char length = 2;
jah128 1:b067a08ff54e 945 char data [2];
jah128 1:b067a08ff54e 946 float qspeed = speed + 1;
jah128 1:b067a08ff54e 947 qspeed *= 32768.0;
jah128 1:b067a08ff54e 948 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 949 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 950 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 951 if(request_response == 1) {
jah128 1:b067a08ff54e 952 command+=128;
jah128 1:b067a08ff54e 953 if(target == 0) {
jah128 1:b067a08ff54e 954 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 955 swarm[i].status_rf_command_left = 1;
jah128 1:b067a08ff54e 956 }
jah128 1:b067a08ff54e 957 } else swarm[target].status_rf_command_left = 1;
jah128 1:b067a08ff54e 958 }
jah128 1:b067a08ff54e 959 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 960 }
jah128 1:b067a08ff54e 961
jah128 1:b067a08ff54e 962 void send_rf_command_right ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 963 {
jah128 1:b067a08ff54e 964 char command = 0x14;
jah128 1:b067a08ff54e 965 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 966 char length = 2;
jah128 1:b067a08ff54e 967 char data [2];
jah128 1:b067a08ff54e 968 float qspeed = speed + 1;
jah128 1:b067a08ff54e 969 qspeed *= 32768.0;
jah128 1:b067a08ff54e 970 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 971 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 972 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 973 if(request_response == 1) {
jah128 1:b067a08ff54e 974 command+=128;
jah128 1:b067a08ff54e 975 if(target == 0) {
jah128 1:b067a08ff54e 976 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 977 swarm[i].status_rf_command_right = 1;
jah128 1:b067a08ff54e 978 }
jah128 1:b067a08ff54e 979 } else swarm[target].status_rf_command_right = 1;
jah128 1:b067a08ff54e 980 }
jah128 1:b067a08ff54e 981 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 982 }
jah128 1:b067a08ff54e 983
jah128 1:b067a08ff54e 984 void send_rf_command_left_motor ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 985 {
jah128 1:b067a08ff54e 986 char command = 0x15;
jah128 1:b067a08ff54e 987 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 988 char length = 2;
jah128 1:b067a08ff54e 989 char data [2];
jah128 1:b067a08ff54e 990 float qspeed = speed + 1;
jah128 1:b067a08ff54e 991 qspeed *= 32768.0;
jah128 1:b067a08ff54e 992 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 993 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 994 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 995 if(request_response == 1) {
jah128 1:b067a08ff54e 996 command+=128;
jah128 1:b067a08ff54e 997 if(target == 0) {
jah128 1:b067a08ff54e 998 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 999 swarm[i].status_rf_command_left_motor = 1;
jah128 1:b067a08ff54e 1000 }
jah128 1:b067a08ff54e 1001 } else swarm[target].status_rf_command_left_motor = 1;
jah128 1:b067a08ff54e 1002 }
jah128 1:b067a08ff54e 1003 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1004 }
jah128 1:b067a08ff54e 1005
jah128 1:b067a08ff54e 1006 void send_rf_command_right_motor ( char target, char request_response, float speed )
jah128 1:b067a08ff54e 1007 {
jah128 1:b067a08ff54e 1008 char command = 0x16;
jah128 1:b067a08ff54e 1009 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 1010 char length = 2;
jah128 1:b067a08ff54e 1011 char data [2];
jah128 1:b067a08ff54e 1012 float qspeed = speed + 1;
jah128 1:b067a08ff54e 1013 qspeed *= 32768.0;
jah128 1:b067a08ff54e 1014 int ispeed = (int) qspeed;
jah128 1:b067a08ff54e 1015 data[0] = (char) (ispeed >> 8);
jah128 1:b067a08ff54e 1016 data[1] = (char) (ispeed % 256);
jah128 1:b067a08ff54e 1017 if(request_response == 1) {
jah128 1:b067a08ff54e 1018 command+=128;
jah128 1:b067a08ff54e 1019 if(target == 0) {
jah128 1:b067a08ff54e 1020 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1021 swarm[i].status_rf_command_right_motor = 1;
jah128 1:b067a08ff54e 1022 }
jah128 1:b067a08ff54e 1023 } else swarm[target].status_rf_command_right_motor = 1;
jah128 1:b067a08ff54e 1024 }
jah128 1:b067a08ff54e 1025 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1026 }
jah128 1:b067a08ff54e 1027
jah128 1:b067a08ff54e 1028 void send_rf_command_oled_colour ( char target, char request_response, char red, char green, char blue )
jah128 1:b067a08ff54e 1029 {
jah128 1:b067a08ff54e 1030 char command = 0x17;
jah128 1:b067a08ff54e 1031 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 1032 char length = 3;
jah128 1:b067a08ff54e 1033 char data [3];
jah128 1:b067a08ff54e 1034 data[0] = red;
jah128 1:b067a08ff54e 1035 data[1] = green;
jah128 1:b067a08ff54e 1036 data[2] = blue;
jah128 1:b067a08ff54e 1037 if(request_response == 1) {
jah128 1:b067a08ff54e 1038 command+=128;
jah128 1:b067a08ff54e 1039 if(target == 0) {
jah128 1:b067a08ff54e 1040 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1041 swarm[i].status_rf_command_oled_colour = 1;
jah128 1:b067a08ff54e 1042 }
jah128 1:b067a08ff54e 1043 } else swarm[target].status_rf_command_oled_colour = 1;
jah128 1:b067a08ff54e 1044 }
jah128 1:b067a08ff54e 1045 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1046 }
jah128 1:b067a08ff54e 1047
jah128 1:b067a08ff54e 1048 void send_rf_command_cled_colour ( char target, char request_response, char red, char green, char blue )
jah128 1:b067a08ff54e 1049 {
jah128 1:b067a08ff54e 1050 char command = 0x18;
jah128 1:b067a08ff54e 1051 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 1052 char length = 3;
jah128 1:b067a08ff54e 1053 char data [3];
jah128 1:b067a08ff54e 1054 data[0] = red;
jah128 1:b067a08ff54e 1055 data[1] = green;
jah128 1:b067a08ff54e 1056 data[2] = blue;
jah128 1:b067a08ff54e 1057 if(request_response == 1) {
jah128 1:b067a08ff54e 1058 command+=128;
jah128 1:b067a08ff54e 1059 if(target == 0) {
jah128 1:b067a08ff54e 1060 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1061 swarm[i].status_rf_command_cled_colour = 1;
jah128 1:b067a08ff54e 1062 }
jah128 1:b067a08ff54e 1063 } else swarm[target].status_rf_command_cled_colour = 1;
jah128 1:b067a08ff54e 1064 }
jah128 1:b067a08ff54e 1065 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1066 }
jah128 1:b067a08ff54e 1067
jah128 1:b067a08ff54e 1068 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 1069 {
jah128 1:b067a08ff54e 1070 char command = 0x19;
jah128 1:b067a08ff54e 1071 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 1072 char length = 2;
jah128 1:b067a08ff54e 1073 char data [2];
jah128 1:b067a08ff54e 1074 data[0] = 0;
jah128 1:b067a08ff54e 1075 data[1] = 0;
jah128 1:b067a08ff54e 1076 if( led0 == 1) data[0] += 2;
jah128 1:b067a08ff54e 1077 if( led1 == 1) data[0] += 1;
jah128 1:b067a08ff54e 1078 if( led2 == 1) data[1] += 128;
jah128 1:b067a08ff54e 1079 if( led3 == 1) data[1] += 64;
jah128 1:b067a08ff54e 1080 if( led4 == 1) data[1] += 32;
jah128 1:b067a08ff54e 1081 if( led5 == 1) data[1] += 16;
jah128 1:b067a08ff54e 1082 if( led6 == 1) data[1] += 8;
jah128 1:b067a08ff54e 1083 if( led7 == 1) data[1] += 4;
jah128 1:b067a08ff54e 1084 if( led8 == 1) data[1] += 2;
jah128 1:b067a08ff54e 1085 if( led9 == 1) data[1] += 1;
jah128 1:b067a08ff54e 1086 if(request_response == 1) {
jah128 1:b067a08ff54e 1087 command+=128;
jah128 1:b067a08ff54e 1088 if(target == 0) {
jah128 1:b067a08ff54e 1089 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1090 swarm[i].status_rf_command_oled_state = 1;
jah128 1:b067a08ff54e 1091 }
jah128 1:b067a08ff54e 1092 } else swarm[target].status_rf_command_oled_state = 1;
jah128 1:b067a08ff54e 1093 }
jah128 1:b067a08ff54e 1094 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1095 }
jah128 1:b067a08ff54e 1096
jah128 1:b067a08ff54e 1097 void send_rf_command_cled_state ( char target, char request_response, char enable )
jah128 1:b067a08ff54e 1098 {
jah128 1:b067a08ff54e 1099 char command = 0x1A;
jah128 1:b067a08ff54e 1100 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 1101 char length = 1;
jah128 1:b067a08ff54e 1102 char data [1];
jah128 1:b067a08ff54e 1103 data[0] = enable;
jah128 1:b067a08ff54e 1104 if(request_response == 1) {
jah128 1:b067a08ff54e 1105 command+=128;
jah128 1:b067a08ff54e 1106 if(target == 0) {
jah128 1:b067a08ff54e 1107 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1108 swarm[i].status_rf_command_cled_state = 1;
jah128 1:b067a08ff54e 1109 }
jah128 1:b067a08ff54e 1110 } else swarm[target].status_rf_command_cled_state = 1;
jah128 1:b067a08ff54e 1111 }
jah128 1:b067a08ff54e 1112 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1113 }
jah128 1:b067a08ff54e 1114
jah128 1:b067a08ff54e 1115 void send_rf_command_set_oled ( char target, char request_response, char oled, char enable )
jah128 1:b067a08ff54e 1116 {
jah128 1:b067a08ff54e 1117 char command = 0x1B;
jah128 1:b067a08ff54e 1118 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 1119 char length = 1;
jah128 1:b067a08ff54e 1120 char data [1];
jah128 1:b067a08ff54e 1121 data[0] = oled;
jah128 1:b067a08ff54e 1122 if(enable == 1) oled+= 32;
jah128 1:b067a08ff54e 1123 if(request_response == 1) {
jah128 1:b067a08ff54e 1124 command+=128;
jah128 1:b067a08ff54e 1125 if(target == 0) {
jah128 1:b067a08ff54e 1126 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1127 swarm[i].status_rf_command_set_oled = 1;
jah128 1:b067a08ff54e 1128 }
jah128 1:b067a08ff54e 1129 } else swarm[target].status_rf_command_set_oled = 1;
jah128 1:b067a08ff54e 1130 }
jah128 1:b067a08ff54e 1131 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1132 }
jah128 1:b067a08ff54e 1133
jah128 1:b067a08ff54e 1134 void send_rf_command_play_tune ( char target, char request_response, char * data, char length )
jah128 1:b067a08ff54e 1135 {
jah128 1:b067a08ff54e 1136 char command = 0x1C;
jah128 1:b067a08ff54e 1137 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 1138 if(request_response == 1) {
jah128 1:b067a08ff54e 1139 command+=128;
jah128 1:b067a08ff54e 1140 if(target == 0) {
jah128 1:b067a08ff54e 1141 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1142 swarm[i].status_rf_command_play_tune = 1;
jah128 1:b067a08ff54e 1143 }
jah128 1:b067a08ff54e 1144 } else swarm[target].status_rf_command_play_tune = 1;
jah128 1:b067a08ff54e 1145 }
jah128 1:b067a08ff54e 1146 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1147 }
jah128 1:b067a08ff54e 1148
jah128 1:b067a08ff54e 1149 void send_rf_command_sync_time ( char target, char request_response )
jah128 1:b067a08ff54e 1150 {
jah128 1:b067a08ff54e 1151 char command = 0x1D;
jah128 1:b067a08ff54e 1152 if(request_response == 1) command+=128;
jah128 1:b067a08ff54e 1153 char length = 1;
jah128 1:b067a08ff54e 1154 char data [1];
jah128 1:b067a08ff54e 1155 data[0] = 0;
jah128 1:b067a08ff54e 1156 if(request_response == 1) {
jah128 1:b067a08ff54e 1157 command+=128;
jah128 1:b067a08ff54e 1158 if(target == 0) {
jah128 1:b067a08ff54e 1159 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1160 swarm[i].status_rf_command_sync_time = 1;
jah128 1:b067a08ff54e 1161 }
jah128 1:b067a08ff54e 1162 } else swarm[target].status_rf_command_sync_time = 1;
jah128 1:b067a08ff54e 1163 }
jah128 1:b067a08ff54e 1164 send_rf_message(target,command,data,length);
jah128 1:b067a08ff54e 1165 }
jah128 1:b067a08ff54e 1166
jah128 1:b067a08ff54e 1167 //Resets the recorded swarm data tables
jah128 1:b067a08ff54e 1168 void setup_communications()
jah128 1:b067a08ff54e 1169 {
jah128 1:b067a08ff54e 1170 for(int i=0; i<SWARM_SIZE; i++) {
jah128 1:b067a08ff54e 1171 swarm[i].status_rf_request_null = 0;
jah128 1:b067a08ff54e 1172 swarm[i].status_rf_request_left_motor_speed = 0;
jah128 1:b067a08ff54e 1173 swarm[i].status_rf_request_right_motor_speed = 0;
jah128 1:b067a08ff54e 1174 swarm[i].status_rf_request_button_state = 0;
jah128 1:b067a08ff54e 1175 swarm[i].status_rf_request_led_colour = 0;
jah128 1:b067a08ff54e 1176 swarm[i].status_rf_request_led_states = 0;
jah128 1:b067a08ff54e 1177 swarm[i].status_rf_request_battery = 0;
jah128 1:b067a08ff54e 1178 swarm[i].status_rf_request_light_sensor = 0;
jah128 1:b067a08ff54e 1179 swarm[i].status_rf_request_accelerometer = 0;
jah128 1:b067a08ff54e 1180 swarm[i].status_rf_request_gyroscope = 0;
jah128 1:b067a08ff54e 1181 swarm[i].status_rf_request_background_ir = 0;
jah128 1:b067a08ff54e 1182 swarm[i].status_rf_request_reflected_ir = 0;
jah128 1:b067a08ff54e 1183 swarm[i].status_rf_request_distance_ir = 0;
jah128 1:b067a08ff54e 1184 swarm[i].status_rf_request_line_following_ir = 0;
jah128 1:b067a08ff54e 1185 swarm[i].status_rf_request_uptime = 0;
jah128 1:b067a08ff54e 1186 swarm[i].status_rf_command_stop = 0;
jah128 1:b067a08ff54e 1187 swarm[i].status_rf_command_forward = 0;
jah128 1:b067a08ff54e 1188 swarm[i].status_rf_command_backward = 0;
jah128 1:b067a08ff54e 1189 swarm[i].status_rf_command_left = 0;
jah128 1:b067a08ff54e 1190 swarm[i].status_rf_command_right = 0;
jah128 1:b067a08ff54e 1191 swarm[i].status_rf_command_left_motor = 0;
jah128 1:b067a08ff54e 1192 swarm[i].status_rf_command_right_motor = 0;
jah128 1:b067a08ff54e 1193 swarm[i].status_rf_command_oled_colour = 0;
jah128 1:b067a08ff54e 1194 swarm[i].status_rf_command_cled_colour = 0;
jah128 1:b067a08ff54e 1195 swarm[i].status_rf_command_oled_state = 0;
jah128 1:b067a08ff54e 1196 swarm[i].status_rf_command_cled_state = 0;
jah128 1:b067a08ff54e 1197 swarm[i].status_rf_command_set_oled = 0;
jah128 1:b067a08ff54e 1198 swarm[i].status_rf_command_play_tune = 0;
jah128 1:b067a08ff54e 1199 swarm[i].status_rf_command_sync_time = 0;
jah128 1:b067a08ff54e 1200 swarm[i].left_motor_speed = 0.0;
jah128 1:b067a08ff54e 1201 swarm[i].right_motor_speed = 0.0;
jah128 1:b067a08ff54e 1202 swarm[i].button_state = 0;
jah128 1:b067a08ff54e 1203 for(int k=0; k<3; k++) {
jah128 1:b067a08ff54e 1204 swarm[i].outer_led_colour [k]=0;
jah128 1:b067a08ff54e 1205 swarm[i].center_led_colour [k]=0;
jah128 1:b067a08ff54e 1206 swarm[i].accelerometer [k]=0;
jah128 1:b067a08ff54e 1207 }
jah128 1:b067a08ff54e 1208 swarm[i].led_states[0]=0;
jah128 1:b067a08ff54e 1209 swarm[i].led_states[1]=0;
jah128 1:b067a08ff54e 1210 swarm[i].battery = 0.0;
jah128 1:b067a08ff54e 1211 swarm[i].light_sensor = 0.0;
jah128 1:b067a08ff54e 1212 swarm[i].gyro = 0.0;
jah128 1:b067a08ff54e 1213 }
jah128 1:b067a08ff54e 1214 }