Unfinished version 0.6 library for the Pi Swarm robot. NOTE: This library is not yet finished or fully tested - it will change.
Dependents: Pi_Swarm_Blank Aggregation-Flocking_2 Pi_Swarm_User_Command_RF_Test
Fork of Pi_Swarm_Library by
communications.cpp@0:9ffe8ebd1c40, 2014-01-31 (annotated)
- Committer:
- jah128
- Date:
- Fri Jan 31 22:59:25 2014 +0000
- Revision:
- 0:9ffe8ebd1c40
- Child:
- 1:b067a08ff54e
First commit
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:9ffe8ebd1c40 | 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 | 0:9ffe8ebd1c40 | 10 | |
jah128 | 0:9ffe8ebd1c40 | 11 | #include "communications.h" |
jah128 | 0:9ffe8ebd1c40 | 12 | #include "piswarm.h" |
jah128 | 0:9ffe8ebd1c40 | 13 | #include "main.h" |
jah128 | 0:9ffe8ebd1c40 | 14 | |
jah128 | 0:9ffe8ebd1c40 | 15 | DigitalOut actioning (LED1); |
jah128 | 0:9ffe8ebd1c40 | 16 | DigitalOut errorled (LED2); |
jah128 | 0:9ffe8ebd1c40 | 17 | DigitalOut tx (LED3); |
jah128 | 0:9ffe8ebd1c40 | 18 | DigitalOut rx (LED4); |
jah128 | 0:9ffe8ebd1c40 | 19 | Timeout tdma_timeout; |
jah128 | 0:9ffe8ebd1c40 | 20 | char tdma_busy = 0; |
jah128 | 0:9ffe8ebd1c40 | 21 | char waiting_message [64]; |
jah128 | 0:9ffe8ebd1c40 | 22 | char waiting_length = 0; |
jah128 | 0:9ffe8ebd1c40 | 23 | |
jah128 | 0:9ffe8ebd1c40 | 24 | const char * const requests_array[] = { "Req.Ack", "Req.LM", "Req.RM", "Req.But","Req.LEDCol","Req.LED","Req.Lgt","Req.Acc","Req.Gyro","Req.BIR","Req.RIR","Req.DIR","Req.LFS","Req.UpT","Req.???","Req.???"}; |
jah128 | 0:9ffe8ebd1c40 | 25 | const char * const commands_array[] = { "Cm.Stop","Cm.Fward","Cm.Bward","Cm.Left","Cm.Right","Cm.LMot","Cm.RMot","Cm.OLEDC","Cm.CLEDC","Cm.OLEDS","Cm.CLEDS","Cm.SetLED","Cm.Play","Cm.Sync","Cm.???","Cm.???"}; |
jah128 | 0:9ffe8ebd1c40 | 26 | |
jah128 | 0:9ffe8ebd1c40 | 27 | //Decode the received message, action it if it is valid and for me |
jah128 | 0:9ffe8ebd1c40 | 28 | void decodeMessage(char sender, char target, char id, char command, char * data, char length){ |
jah128 | 0:9ffe8ebd1c40 | 29 | char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0; |
jah128 | 0:9ffe8ebd1c40 | 30 | |
jah128 | 0:9ffe8ebd1c40 | 31 | if(target==0) broadcast_message = 1; |
jah128 | 0:9ffe8ebd1c40 | 32 | is_response = 0 != (command & (1 << 7)); |
jah128 | 0:9ffe8ebd1c40 | 33 | request_response = 0 != (command & (1 << 6)); |
jah128 | 0:9ffe8ebd1c40 | 34 | is_user = 0 != (command & (1 << 5)); |
jah128 | 0:9ffe8ebd1c40 | 35 | is_command = 0 != (command & (1 << 4)); |
jah128 | 0:9ffe8ebd1c40 | 36 | function = command % 16; |
jah128 | 0:9ffe8ebd1c40 | 37 | |
jah128 | 0:9ffe8ebd1c40 | 38 | if (RF_DEBUG==1) { |
jah128 | 0:9ffe8ebd1c40 | 39 | 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 | 40 | 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 | 41 | } |
jah128 | 0:9ffe8ebd1c40 | 42 | |
jah128 | 0:9ffe8ebd1c40 | 43 | //Action the message only if I am a recipient |
jah128 | 0:9ffe8ebd1c40 | 44 | if(target==0 || target==piswarm.get_id()){ |
jah128 | 0:9ffe8ebd1c40 | 45 | if(RF_USE_LEDS==1) actioning = 1; |
jah128 | 0:9ffe8ebd1c40 | 46 | if(is_response == 1) { |
jah128 | 0:9ffe8ebd1c40 | 47 | if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length); |
jah128 | 0:9ffe8ebd1c40 | 48 | else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length); |
jah128 | 0:9ffe8ebd1c40 | 49 | }else { |
jah128 | 0:9ffe8ebd1c40 | 50 | if(is_command == 1) { |
jah128 | 0:9ffe8ebd1c40 | 51 | if(RF_ALLOW_COMMANDS == 1){ |
jah128 | 0:9ffe8ebd1c40 | 52 | if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length); |
jah128 | 0:9ffe8ebd1c40 | 53 | else handle_command(sender, broadcast_message, request_response, id, function, data, length); |
jah128 | 0:9ffe8ebd1c40 | 54 | } else if (RF_DEBUG==1) pc.printf(" - Blocked\n"); |
jah128 | 0:9ffe8ebd1c40 | 55 | } |
jah128 | 0:9ffe8ebd1c40 | 56 | else { |
jah128 | 0:9ffe8ebd1c40 | 57 | //A information request has no extra parameters |
jah128 | 0:9ffe8ebd1c40 | 58 | if(length == 0){ |
jah128 | 0:9ffe8ebd1c40 | 59 | if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length); |
jah128 | 0:9ffe8ebd1c40 | 60 | else handle_request(sender, broadcast_message, request_response, id, function); |
jah128 | 0:9ffe8ebd1c40 | 61 | } else if (RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 62 | } |
jah128 | 0:9ffe8ebd1c40 | 63 | } |
jah128 | 0:9ffe8ebd1c40 | 64 | if(RF_USE_LEDS==1) actioning = 0; |
jah128 | 0:9ffe8ebd1c40 | 65 | } else if (RF_DEBUG==1) pc.printf(" - Ignored\n"); |
jah128 | 0:9ffe8ebd1c40 | 66 | } |
jah128 | 0:9ffe8ebd1c40 | 67 | |
jah128 | 0:9ffe8ebd1c40 | 68 | //Send a response message |
jah128 | 0:9ffe8ebd1c40 | 69 | void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){ |
jah128 | 0:9ffe8ebd1c40 | 70 | char message [4+length]; |
jah128 | 0:9ffe8ebd1c40 | 71 | message[0]=piswarm.get_id(); |
jah128 | 0:9ffe8ebd1c40 | 72 | message[1]=target; |
jah128 | 0:9ffe8ebd1c40 | 73 | message[2]=id; |
jah128 | 0:9ffe8ebd1c40 | 74 | message[3]=128 + (success << 6) + (is_command << 4) + function; |
jah128 | 0:9ffe8ebd1c40 | 75 | for(int i=0;i<length;i++){ |
jah128 | 0:9ffe8ebd1c40 | 76 | message[4+i]=data[i]; |
jah128 | 0:9ffe8ebd1c40 | 77 | } |
jah128 | 0:9ffe8ebd1c40 | 78 | //Delay the response if it is broadcast and TDMA mode is on |
jah128 | 0:9ffe8ebd1c40 | 79 | if(RF_USE_TDMA == 1 && is_broadcast == 1){ |
jah128 | 0:9ffe8ebd1c40 | 80 | if(tdma_busy == 1){ |
jah128 | 0:9ffe8ebd1c40 | 81 | if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n"); |
jah128 | 0:9ffe8ebd1c40 | 82 | }else{ |
jah128 | 0:9ffe8ebd1c40 | 83 | tdma_busy = 1; |
jah128 | 0:9ffe8ebd1c40 | 84 | strcpy(waiting_message,message); |
jah128 | 0:9ffe8ebd1c40 | 85 | waiting_length=length; |
jah128 | 0:9ffe8ebd1c40 | 86 | tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id()); |
jah128 | 0:9ffe8ebd1c40 | 87 | if (RF_DEBUG==1) pc.printf("TDMA Response pending\n"); |
jah128 | 0:9ffe8ebd1c40 | 88 | } |
jah128 | 0:9ffe8ebd1c40 | 89 | }else |
jah128 | 0:9ffe8ebd1c40 | 90 | { |
jah128 | 0:9ffe8ebd1c40 | 91 | piswarm.send_rf_message(message,4+length); |
jah128 | 0:9ffe8ebd1c40 | 92 | if(RF_DEBUG==1)pc.printf("Response issued"); |
jah128 | 0:9ffe8ebd1c40 | 93 | } |
jah128 | 0:9ffe8ebd1c40 | 94 | } |
jah128 | 0:9ffe8ebd1c40 | 95 | |
jah128 | 0:9ffe8ebd1c40 | 96 | // Send a delayed response |
jah128 | 0:9ffe8ebd1c40 | 97 | void tdma_response(){ |
jah128 | 0:9ffe8ebd1c40 | 98 | piswarm.send_rf_message(waiting_message,4+waiting_length); |
jah128 | 0:9ffe8ebd1c40 | 99 | tdma_busy = 0; |
jah128 | 0:9ffe8ebd1c40 | 100 | if (RF_DEBUG==1) pc.printf("TDMA Response issued\n"); |
jah128 | 0:9ffe8ebd1c40 | 101 | } |
jah128 | 0:9ffe8ebd1c40 | 102 | |
jah128 | 0:9ffe8ebd1c40 | 103 | // Handle a message that is a response |
jah128 | 0:9ffe8ebd1c40 | 104 | void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){ |
jah128 | 0:9ffe8ebd1c40 | 105 | |
jah128 | 0:9ffe8ebd1c40 | 106 | } |
jah128 | 0:9ffe8ebd1c40 | 107 | |
jah128 | 0:9ffe8ebd1c40 | 108 | // Handle a message that is a (not-user) command |
jah128 | 0:9ffe8ebd1c40 | 109 | void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length){ |
jah128 | 0:9ffe8ebd1c40 | 110 | char success = 0; |
jah128 | 0:9ffe8ebd1c40 | 111 | switch(function){ |
jah128 | 0:9ffe8ebd1c40 | 112 | case 0: // Stop [0 data] |
jah128 | 0:9ffe8ebd1c40 | 113 | if(length==0) { |
jah128 | 0:9ffe8ebd1c40 | 114 | piswarm.stop(); |
jah128 | 0:9ffe8ebd1c40 | 115 | if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - "); |
jah128 | 0:9ffe8ebd1c40 | 116 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 117 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 118 | break; |
jah128 | 0:9ffe8ebd1c40 | 119 | case 1: // Forward [2 bytes: 16-bit signed short] |
jah128 | 0:9ffe8ebd1c40 | 120 | if(length==2) { |
jah128 | 0:9ffe8ebd1c40 | 121 | int i_speed = (data[0] << 8) + data[1]; |
jah128 | 0:9ffe8ebd1c40 | 122 | float speed = i_speed / 32768.0; |
jah128 | 0:9ffe8ebd1c40 | 123 | speed--; |
jah128 | 0:9ffe8ebd1c40 | 124 | piswarm.forward(speed); |
jah128 | 0:9ffe8ebd1c40 | 125 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 126 | if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed); |
jah128 | 0:9ffe8ebd1c40 | 127 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 128 | break; |
jah128 | 0:9ffe8ebd1c40 | 129 | case 2: // Backward [2 bytes: 16-bit signed short] |
jah128 | 0:9ffe8ebd1c40 | 130 | if(length==2) { |
jah128 | 0:9ffe8ebd1c40 | 131 | int i_speed = (data[0] << 8) + data[1]; |
jah128 | 0:9ffe8ebd1c40 | 132 | float speed = i_speed / 32768.0; |
jah128 | 0:9ffe8ebd1c40 | 133 | speed--; |
jah128 | 0:9ffe8ebd1c40 | 134 | piswarm.backward(speed); |
jah128 | 0:9ffe8ebd1c40 | 135 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 136 | if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed); |
jah128 | 0:9ffe8ebd1c40 | 137 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 138 | break; |
jah128 | 0:9ffe8ebd1c40 | 139 | case 3: // Left [2 bytes: 16-bit signed short] |
jah128 | 0:9ffe8ebd1c40 | 140 | if(length==2) { |
jah128 | 0:9ffe8ebd1c40 | 141 | int i_speed = (data[0] << 8) + data[1]; |
jah128 | 0:9ffe8ebd1c40 | 142 | float speed = i_speed / 32768.0; |
jah128 | 0:9ffe8ebd1c40 | 143 | speed--; |
jah128 | 0:9ffe8ebd1c40 | 144 | piswarm.left(speed); |
jah128 | 0:9ffe8ebd1c40 | 145 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 146 | if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed); |
jah128 | 0:9ffe8ebd1c40 | 147 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 148 | break; |
jah128 | 0:9ffe8ebd1c40 | 149 | case 4: // Right [2 bytes: 16-bit signed short] |
jah128 | 0:9ffe8ebd1c40 | 150 | if(length==2) { |
jah128 | 0:9ffe8ebd1c40 | 151 | int i_speed = (data[0] << 8) + data[1]; |
jah128 | 0:9ffe8ebd1c40 | 152 | float speed = i_speed / 32768.0; |
jah128 | 0:9ffe8ebd1c40 | 153 | speed--; |
jah128 | 0:9ffe8ebd1c40 | 154 | piswarm.right(speed); |
jah128 | 0:9ffe8ebd1c40 | 155 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 156 | if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed); |
jah128 | 0:9ffe8ebd1c40 | 157 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 158 | break; |
jah128 | 0:9ffe8ebd1c40 | 159 | case 5: // Left Motor [2 bytes: 16-bit signed short] |
jah128 | 0:9ffe8ebd1c40 | 160 | if(length==2) { |
jah128 | 0:9ffe8ebd1c40 | 161 | int i_speed = (data[0] << 8) + data[1]; |
jah128 | 0:9ffe8ebd1c40 | 162 | float speed = i_speed / 32768.0; |
jah128 | 0:9ffe8ebd1c40 | 163 | speed--; |
jah128 | 0:9ffe8ebd1c40 | 164 | piswarm.left_motor(speed); |
jah128 | 0:9ffe8ebd1c40 | 165 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 166 | if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed); |
jah128 | 0:9ffe8ebd1c40 | 167 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 168 | break; |
jah128 | 0:9ffe8ebd1c40 | 169 | case 6: // Right Motor [2 bytes: 16-bit signed short] |
jah128 | 0:9ffe8ebd1c40 | 170 | if(length==2) { |
jah128 | 0:9ffe8ebd1c40 | 171 | int i_speed = (data[0] << 8) + data[1]; |
jah128 | 0:9ffe8ebd1c40 | 172 | float speed = i_speed / 32768.0; |
jah128 | 0:9ffe8ebd1c40 | 173 | speed--; |
jah128 | 0:9ffe8ebd1c40 | 174 | piswarm.right_motor(speed); |
jah128 | 0:9ffe8ebd1c40 | 175 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 176 | if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed); |
jah128 | 0:9ffe8ebd1c40 | 177 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 178 | break; |
jah128 | 0:9ffe8ebd1c40 | 179 | case 7: // Outer LED Colour [3 bytes: R, G, B] |
jah128 | 0:9ffe8ebd1c40 | 180 | if(length==3) { |
jah128 | 0:9ffe8ebd1c40 | 181 | piswarm.set_oled_colour (data[0],data[1],data[2]); |
jah128 | 0:9ffe8ebd1c40 | 182 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 183 | 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 | 184 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 185 | break; |
jah128 | 0:9ffe8ebd1c40 | 186 | case 8: // Center LED Colour[3 bytes: R, G, B] |
jah128 | 0:9ffe8ebd1c40 | 187 | if(length==3) { |
jah128 | 0:9ffe8ebd1c40 | 188 | piswarm.set_cled_colour (data[0],data[1],data[2]); |
jah128 | 0:9ffe8ebd1c40 | 189 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 190 | 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 | 191 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 192 | break; |
jah128 | 0:9ffe8ebd1c40 | 193 | case 9: // Outer LED State [2 bytes: [xxxxxx01][23456789] ] |
jah128 | 0:9ffe8ebd1c40 | 194 | if(length==2) { |
jah128 | 0:9ffe8ebd1c40 | 195 | 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 | 196 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 197 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 198 | break; |
jah128 | 0:9ffe8ebd1c40 | 199 | case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ] |
jah128 | 0:9ffe8ebd1c40 | 200 | if(length==1) { |
jah128 | 0:9ffe8ebd1c40 | 201 | piswarm.enable_cled (data[0] % 2); |
jah128 | 0:9ffe8ebd1c40 | 202 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 203 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 204 | break; |
jah128 | 0:9ffe8ebd1c40 | 205 | case 11: // Set outer LED [1 byte: [xxxEvvvv] E=enabled vvvv=LED] |
jah128 | 0:9ffe8ebd1c40 | 206 | if(length==1) { |
jah128 | 0:9ffe8ebd1c40 | 207 | int led = data[0] % 16; |
jah128 | 0:9ffe8ebd1c40 | 208 | if(led < 10){ |
jah128 | 0:9ffe8ebd1c40 | 209 | piswarm.set_oled(led, 0!=(data[0] & (1 << 4))); |
jah128 | 0:9ffe8ebd1c40 | 210 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 211 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 212 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 213 | break; |
jah128 | 0:9ffe8ebd1c40 | 214 | case 12: // Play sound [Minimum 1 byte] |
jah128 | 0:9ffe8ebd1c40 | 215 | if(length>0) { |
jah128 | 0:9ffe8ebd1c40 | 216 | piswarm.play_tune(data,length); |
jah128 | 0:9ffe8ebd1c40 | 217 | success = 1; |
jah128 | 0:9ffe8ebd1c40 | 218 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 219 | break; |
jah128 | 0:9ffe8ebd1c40 | 220 | case 13: // Sync time |
jah128 | 0:9ffe8ebd1c40 | 221 | if(length==4) { |
jah128 | 0:9ffe8ebd1c40 | 222 | unsigned int new_time = 0; |
jah128 | 0:9ffe8ebd1c40 | 223 | new_time+=((unsigned int)data[0] << 24); |
jah128 | 0:9ffe8ebd1c40 | 224 | new_time+=((unsigned int)data[1] << 16); |
jah128 | 0:9ffe8ebd1c40 | 225 | new_time+=((unsigned int)data[2] << 8); |
jah128 | 0:9ffe8ebd1c40 | 226 | new_time+=(unsigned int)data[3]; |
jah128 | 0:9ffe8ebd1c40 | 227 | set_time(new_time); |
jah128 | 0:9ffe8ebd1c40 | 228 | display_system_time(); |
jah128 | 0:9ffe8ebd1c40 | 229 | } else if(RF_DEBUG==1) pc.printf(" - Invalid\n"); |
jah128 | 0:9ffe8ebd1c40 | 230 | break; |
jah128 | 0:9ffe8ebd1c40 | 231 | case 14: // |
jah128 | 0:9ffe8ebd1c40 | 232 | break; |
jah128 | 0:9ffe8ebd1c40 | 233 | case 15: // |
jah128 | 0:9ffe8ebd1c40 | 234 | break; |
jah128 | 0:9ffe8ebd1c40 | 235 | } |
jah128 | 0:9ffe8ebd1c40 | 236 | if(request_response == 1){ |
jah128 | 0:9ffe8ebd1c40 | 237 | send_response(sender, is_broadcast, success, id, 1, function, NULL, 0); |
jah128 | 0:9ffe8ebd1c40 | 238 | } |
jah128 | 0:9ffe8ebd1c40 | 239 | |
jah128 | 0:9ffe8ebd1c40 | 240 | } |
jah128 | 0:9ffe8ebd1c40 | 241 | |
jah128 | 0:9ffe8ebd1c40 | 242 | //Handle a message that is a (not-user) request |
jah128 | 0:9ffe8ebd1c40 | 243 | void handle_request(char sender, char is_broadcast, char request_response, char id, char function){ |
jah128 | 0:9ffe8ebd1c40 | 244 | int response_length = 0; |
jah128 | 0:9ffe8ebd1c40 | 245 | char * response = NULL; |
jah128 | 0:9ffe8ebd1c40 | 246 | char success = 0; |
jah128 | 0:9ffe8ebd1c40 | 247 | |
jah128 | 0:9ffe8ebd1c40 | 248 | switch(function){ |
jah128 | 0:9ffe8ebd1c40 | 249 | case 0: // Null request |
jah128 | 0:9ffe8ebd1c40 | 250 | success=1; |
jah128 | 0:9ffe8ebd1c40 | 251 | break; |
jah128 | 0:9ffe8ebd1c40 | 252 | case 1: // Request left motor speed |
jah128 | 0:9ffe8ebd1c40 | 253 | { |
jah128 | 0:9ffe8ebd1c40 | 254 | response_length = 2; |
jah128 | 0:9ffe8ebd1c40 | 255 | float speed = piswarm.get_left_motor() * 32767; |
jah128 | 0:9ffe8ebd1c40 | 256 | int a_speed = 32768 + (int) speed; |
jah128 | 0:9ffe8ebd1c40 | 257 | char msb = (char) (a_speed / 256); |
jah128 | 0:9ffe8ebd1c40 | 258 | char lsb = (char) (a_speed % 256); |
jah128 | 0:9ffe8ebd1c40 | 259 | response = new char[2]; |
jah128 | 0:9ffe8ebd1c40 | 260 | response[0]=msb; |
jah128 | 0:9ffe8ebd1c40 | 261 | response[1]=lsb; |
jah128 | 0:9ffe8ebd1c40 | 262 | success=1; |
jah128 | 0:9ffe8ebd1c40 | 263 | break; |
jah128 | 0:9ffe8ebd1c40 | 264 | } |
jah128 | 0:9ffe8ebd1c40 | 265 | case 2: // Request right motor speed |
jah128 | 0:9ffe8ebd1c40 | 266 | { |
jah128 | 0:9ffe8ebd1c40 | 267 | response_length = 2; |
jah128 | 0:9ffe8ebd1c40 | 268 | float speed = piswarm.get_right_motor() * 32767; |
jah128 | 0:9ffe8ebd1c40 | 269 | int a_speed = 32768 + (int) speed; |
jah128 | 0:9ffe8ebd1c40 | 270 | char msb = (char) (a_speed / 256); |
jah128 | 0:9ffe8ebd1c40 | 271 | char lsb = (char) (a_speed % 256); |
jah128 | 0:9ffe8ebd1c40 | 272 | response = new char[2]; |
jah128 | 0:9ffe8ebd1c40 | 273 | response[0]=msb; |
jah128 | 0:9ffe8ebd1c40 | 274 | response[1]=lsb; |
jah128 | 0:9ffe8ebd1c40 | 275 | success=1; |
jah128 | 0:9ffe8ebd1c40 | 276 | break; |
jah128 | 0:9ffe8ebd1c40 | 277 | } |
jah128 | 0:9ffe8ebd1c40 | 278 | case 3: // Request button state |
jah128 | 0:9ffe8ebd1c40 | 279 | { |
jah128 | 0:9ffe8ebd1c40 | 280 | response_length = 1; |
jah128 | 0:9ffe8ebd1c40 | 281 | response = new char[1]; |
jah128 | 0:9ffe8ebd1c40 | 282 | response[0]=piswarm.get_switches(); |
jah128 | 0:9ffe8ebd1c40 | 283 | break; |
jah128 | 0:9ffe8ebd1c40 | 284 | } |
jah128 | 0:9ffe8ebd1c40 | 285 | case 4: // Request LED colours |
jah128 | 0:9ffe8ebd1c40 | 286 | { |
jah128 | 0:9ffe8ebd1c40 | 287 | response_length = 6; |
jah128 | 0:9ffe8ebd1c40 | 288 | response = new char[6]; |
jah128 | 0:9ffe8ebd1c40 | 289 | int oled_colour = piswarm.get_oled_colour(); |
jah128 | 0:9ffe8ebd1c40 | 290 | int cled_colour = piswarm.get_cled_colour(); |
jah128 | 0:9ffe8ebd1c40 | 291 | response[0] = (char) (oled_colour >> 16); |
jah128 | 0:9ffe8ebd1c40 | 292 | response[1] = (char) ((oled_colour >> 8) % 256); |
jah128 | 0:9ffe8ebd1c40 | 293 | response[2] = (char) (oled_colour % 256); |
jah128 | 0:9ffe8ebd1c40 | 294 | response[3] = (char) (cled_colour >> 16); |
jah128 | 0:9ffe8ebd1c40 | 295 | response[4] = (char) ((cled_colour >> 8) % 256); |
jah128 | 0:9ffe8ebd1c40 | 296 | response[5] = (char) (cled_colour % 256); |
jah128 | 0:9ffe8ebd1c40 | 297 | break; |
jah128 | 0:9ffe8ebd1c40 | 298 | } |
jah128 | 0:9ffe8ebd1c40 | 299 | case 5: // Request LED states |
jah128 | 0:9ffe8ebd1c40 | 300 | break; |
jah128 | 0:9ffe8ebd1c40 | 301 | case 6: // Request battery power |
jah128 | 0:9ffe8ebd1c40 | 302 | { |
jah128 | 0:9ffe8ebd1c40 | 303 | response_length = 2; |
jah128 | 0:9ffe8ebd1c40 | 304 | response = new char[2]; |
jah128 | 0:9ffe8ebd1c40 | 305 | float fbattery = piswarm.battery() * 1000.0; |
jah128 | 0:9ffe8ebd1c40 | 306 | unsigned short battery = (unsigned short) fbattery; |
jah128 | 0:9ffe8ebd1c40 | 307 | response[0] = battery >> 8; |
jah128 | 0:9ffe8ebd1c40 | 308 | response[1] = battery % 256; |
jah128 | 0:9ffe8ebd1c40 | 309 | break; |
jah128 | 0:9ffe8ebd1c40 | 310 | } |
jah128 | 0:9ffe8ebd1c40 | 311 | case 7: // Request light sensor reading |
jah128 | 0:9ffe8ebd1c40 | 312 | { |
jah128 | 0:9ffe8ebd1c40 | 313 | response_length = 2; |
jah128 | 0:9ffe8ebd1c40 | 314 | response = new char[2]; |
jah128 | 0:9ffe8ebd1c40 | 315 | float flight = piswarm.read_light_sensor() * 655.0; |
jah128 | 0:9ffe8ebd1c40 | 316 | unsigned short light = (unsigned short) flight; |
jah128 | 0:9ffe8ebd1c40 | 317 | response[0] = light >> 8; |
jah128 | 0:9ffe8ebd1c40 | 318 | response[1] = light % 256; |
jah128 | 0:9ffe8ebd1c40 | 319 | break; |
jah128 | 0:9ffe8ebd1c40 | 320 | } |
jah128 | 0:9ffe8ebd1c40 | 321 | case 8: // Request accelerometer reading |
jah128 | 0:9ffe8ebd1c40 | 322 | break; |
jah128 | 0:9ffe8ebd1c40 | 323 | case 9: // Request gyroscope reading |
jah128 | 0:9ffe8ebd1c40 | 324 | break; |
jah128 | 0:9ffe8ebd1c40 | 325 | case 10: // Request background IR reading |
jah128 | 0:9ffe8ebd1c40 | 326 | break; |
jah128 | 0:9ffe8ebd1c40 | 327 | case 11: // Request illuminated IR reading |
jah128 | 0:9ffe8ebd1c40 | 328 | break; |
jah128 | 0:9ffe8ebd1c40 | 329 | case 12: // Request line-tracking IR reading |
jah128 | 0:9ffe8ebd1c40 | 330 | break; |
jah128 | 0:9ffe8ebd1c40 | 331 | case 13: // Request uptime |
jah128 | 0:9ffe8ebd1c40 | 332 | break; |
jah128 | 0:9ffe8ebd1c40 | 333 | case 14: // |
jah128 | 0:9ffe8ebd1c40 | 334 | break; |
jah128 | 0:9ffe8ebd1c40 | 335 | case 15: // |
jah128 | 0:9ffe8ebd1c40 | 336 | break; |
jah128 | 0:9ffe8ebd1c40 | 337 | } |
jah128 | 0:9ffe8ebd1c40 | 338 | send_response(sender, is_broadcast, success, id, 0, function, response, response_length); |
jah128 | 0:9ffe8ebd1c40 | 339 | |
jah128 | 0:9ffe8ebd1c40 | 340 | } |
jah128 | 0:9ffe8ebd1c40 | 341 | |
jah128 | 0:9ffe8ebd1c40 | 342 | void processRadioData(char * data, char length){ |
jah128 | 0:9ffe8ebd1c40 | 343 | if(RF_USE_LEDS==1) {errorled=0;rx=1;} |
jah128 | 0:9ffe8ebd1c40 | 344 | // Decompose the received message |
jah128 | 0:9ffe8ebd1c40 | 345 | if(length < 4) errormessage(0); |
jah128 | 0:9ffe8ebd1c40 | 346 | else { |
jah128 | 0:9ffe8ebd1c40 | 347 | // Establish the sender and target of the packet |
jah128 | 0:9ffe8ebd1c40 | 348 | char sender = data[0]; |
jah128 | 0:9ffe8ebd1c40 | 349 | char target = data[1]; |
jah128 | 0:9ffe8ebd1c40 | 350 | char id = data[2]; |
jah128 | 0:9ffe8ebd1c40 | 351 | char command = data[3]; |
jah128 | 0:9ffe8ebd1c40 | 352 | if(sender<32 || sender>63)errormessage(1); |
jah128 | 0:9ffe8ebd1c40 | 353 | else { |
jah128 | 0:9ffe8ebd1c40 | 354 | if(target<32 || target>63)errormessage(2); |
jah128 | 0:9ffe8ebd1c40 | 355 | else { |
jah128 | 0:9ffe8ebd1c40 | 356 | sender -= 32; |
jah128 | 0:9ffe8ebd1c40 | 357 | target -= 32; |
jah128 | 0:9ffe8ebd1c40 | 358 | decodeMessage(sender,target,id,command,data+4,length-4); |
jah128 | 0:9ffe8ebd1c40 | 359 | } |
jah128 | 0:9ffe8ebd1c40 | 360 | } |
jah128 | 0:9ffe8ebd1c40 | 361 | } |
jah128 | 0:9ffe8ebd1c40 | 362 | if(RF_USE_LEDS==1) rx=0; |
jah128 | 0:9ffe8ebd1c40 | 363 | } |
jah128 | 0:9ffe8ebd1c40 | 364 | |
jah128 | 0:9ffe8ebd1c40 | 365 | void errormessage(int index){ |
jah128 | 0:9ffe8ebd1c40 | 366 | if(RF_USE_LEDS==1) errorled=1; |
jah128 | 0:9ffe8ebd1c40 | 367 | switch(index){ |
jah128 | 0:9ffe8ebd1c40 | 368 | case 0: //Message to short |
jah128 | 0:9ffe8ebd1c40 | 369 | if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n"); |
jah128 | 0:9ffe8ebd1c40 | 370 | break; |
jah128 | 0:9ffe8ebd1c40 | 371 | case 1: //Sender out of valid range |
jah128 | 0:9ffe8ebd1c40 | 372 | if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n"); |
jah128 | 0:9ffe8ebd1c40 | 373 | break; |
jah128 | 0:9ffe8ebd1c40 | 374 | case 2: //Target out of valid range |
jah128 | 0:9ffe8ebd1c40 | 375 | if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n"); |
jah128 | 0:9ffe8ebd1c40 | 376 | break; |
jah128 | 0:9ffe8ebd1c40 | 377 | case 3: |
jah128 | 0:9ffe8ebd1c40 | 378 | |
jah128 | 0:9ffe8ebd1c40 | 379 | break; |
jah128 | 0:9ffe8ebd1c40 | 380 | case 4: break; |
jah128 | 0:9ffe8ebd1c40 | 381 | } |
jah128 | 0:9ffe8ebd1c40 | 382 | } |