Alan Millard / Mbed 2 deprecated BeautifulMemeProjectBT

Bluetooth communication for flocking.

Dependencies:   mbed

Fork of BeautifulMemeProject by James Hilder

PsiSwarm/serial.cpp@27:7eb032772bc2, 2016-01-31 (annotated)

Committer:
alanmillard
Date:
Sun Jan 31 15:14:54 2016 +0000
Revision:
27:7eb032772bc2
Parent:
21:e5ab8c56a769 
Flocking seems to work better now, however the robot controller sometimes "crashes" (unsure why).

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:8a5497a2e366 1 /* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File
jah128 0:8a5497a2e366 2 *
jah128 0:8a5497a2e366 3 * File: serial.cpp
jah128 0:8a5497a2e366 4 *
jah128 6:ff3c66f7372b 5 * (C) Dept. Electronics & Computer Science, University of York
jah128 6:ff3c66f7372b 6 * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 0:8a5497a2e366 7 *
jah128 6:ff3c66f7372b 8 * PsiSwarm Library Version: 0.3
jah128 0:8a5497a2e366 9 *
jah128 6:ff3c66f7372b 10 * October 2015
jah128 0:8a5497a2e366 11 *
jah128 0:8a5497a2e366 12 */
jah128 0:8a5497a2e366 13
jah128 0:8a5497a2e366 14 #include "psiswarm.h"
jah128 0:8a5497a2e366 15
jah128 0:8a5497a2e366 16 static float command_timeout_period = 0.1f; //If a complete command message is not received in 0.1s then consider it a user message
jah128 0:8a5497a2e366 17 char pc_command_message_started = 0;
jah128 0:8a5497a2e366 18 char pc_command_message_byte = 0;
jah128 0:8a5497a2e366 19 char pc_command_message[3];
jah128 0:8a5497a2e366 20 char bt_command_message_started = 0;
jah128 0:8a5497a2e366 21 char bt_command_message_byte = 0;
jah128 0:8a5497a2e366 22 char bt_command_message[3];
jah128 0:8a5497a2e366 23
jah128 0:8a5497a2e366 24 char allow_commands = 1;
jah128 0:8a5497a2e366 25 char allow_requests = 1;
jah128 0:8a5497a2e366 26
jah128 0:8a5497a2e366 27 Timeout pc_command_timeout;
jah128 0:8a5497a2e366 28 Timeout bt_command_timeout;
jah128 0:8a5497a2e366 29
jah128 0:8a5497a2e366 30 // A predefined message structure for command messages is as follows:
jah128 0:8a5497a2e366 31 // [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4]
jah128 0:8a5497a2e366 32 // Byte 0 and Byte 4 must be equal to COMMAND_MESSAGE_BYTE [in psiswarm.h] or message is treated as a user message
jah128 0:8a5497a2e366 33
jah128 0:8a5497a2e366 34 void handle_user_serial_message(char * message, char length, char interface)
jah128 0:8a5497a2e366 35 {
alanmillard 18:5921c1853e8a 36 // This is where user code for handling a (non-system) serial message should go
jah128 0:8a5497a2e366 37 //
alanmillard 18:5921c1853e8a 38 // message = pointer to message char array
alanmillard 18:5921c1853e8a 39 // length = length of message
alanmillard 18:5921c1853e8a 40 // interface = 0 for PC serial connection, 1 for Bluetooth
alanmillard 17:da524989b637 41
alanmillard 18:5921c1853e8a 42 if(interface)
alanmillard 18:5921c1853e8a 43 {
alanmillard 18:5921c1853e8a 44 if(length == 8)
alanmillard 18:5921c1853e8a 45 {
alanmillard 18:5921c1853e8a 46 for(int i = 0; i < length; i++)
alanmillard 18:5921c1853e8a 47 {
alanmillard 21:e5ab8c56a769 48 // Convert single byte value into a beacon heading in the range +/-180 degrees
alanmillard 21:e5ab8c56a769 49 float beacon_heading = message[i];
alanmillard 21:e5ab8c56a769 50 float degrees_per_value = 256.0f / 360.0f;
alanmillard 21:e5ab8c56a769 51
alanmillard 21:e5ab8c56a769 52 if(beacon_heading != 0)
alanmillard 21:e5ab8c56a769 53 beacon_heading /= degrees_per_value;
alanmillard 21:e5ab8c56a769 54
alanmillard 21:e5ab8c56a769 55 beacon_heading -= 180;
alanmillard 21:e5ab8c56a769 56
alanmillard 21:e5ab8c56a769 57 flocking_headings[i] = beacon_heading;
alanmillard 21:e5ab8c56a769 58
alanmillard 21:e5ab8c56a769 59 debug("%d, ", flocking_headings[i]);
alanmillard 21:e5ab8c56a769 60 //debug("%f, ", beacon_heading);
alanmillard 18:5921c1853e8a 61 }
alanmillard 18:5921c1853e8a 62
alanmillard 21:e5ab8c56a769 63 debug("\n");
alanmillard 18:5921c1853e8a 64 }
alanmillard 18:5921c1853e8a 65 }
jah128 0:8a5497a2e366 66 }
jah128 0:8a5497a2e366 67
jah128 0:8a5497a2e366 68 void IF_handle_user_serial_message(char * message, char length, char interface)
jah128 0:8a5497a2e366 69 {
alanmillard 18:5921c1853e8a 70 char buffer[255];
alanmillard 18:5921c1853e8a 71 sprintf(buffer,message,length);
alanmillard 18:5921c1853e8a 72 for(int i=0; i<length; i++) {
alanmillard 18:5921c1853e8a 73 buffer[i]=message[i];
jah128 0:8a5497a2e366 74 }
alanmillard 18:5921c1853e8a 75 buffer[length]=0;
alanmillard 20:e08376c0b4ea 76 // if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
alanmillard 20:e08376c0b4ea 77 // else debug("Received USB message:%s [%d chars]\n",buffer,length);
alanmillard 18:5921c1853e8a 78 handle_user_serial_message(message,length,interface);
jah128 0:8a5497a2e366 79 }
jah128 0:8a5497a2e366 80
jah128 0:8a5497a2e366 81 void IF_handle_command_serial_message(char message[3], char interface)
jah128 0:8a5497a2e366 82 {
jah128 0:8a5497a2e366 83 char iface [4];
jah128 0:8a5497a2e366 84 if(interface) strcpy(iface,"BT");
jah128 0:8a5497a2e366 85 else strcpy(iface,"USB");
jah128 0:8a5497a2e366 86 char command [26];
jah128 0:8a5497a2e366 87 char subcommand[30];
jah128 0:8a5497a2e366 88 float dec;
jah128 0:8a5497a2e366 89 float l_dec;
jah128 0:8a5497a2e366 90 float r_dec;
jah128 0:8a5497a2e366 91 int irp_delay;
jah128 0:8a5497a2e366 92 char colour_string[7];
jah128 0:8a5497a2e366 93 char ret_message[50];
jah128 0:8a5497a2e366 94 char send_message = 0;
jah128 0:8a5497a2e366 95 char command_status = 0;
jah128 0:8a5497a2e366 96 // command_status values:
jah128 0:8a5497a2e366 97 // 0 - unrecognised command
jah128 0:8a5497a2e366 98 // 1 - command actioned
jah128 0:8a5497a2e366 99 // 2 - command blocked
jah128 0:8a5497a2e366 100 // 3 - invalid parameters
jah128 0:8a5497a2e366 101
jah128 0:8a5497a2e366 102 subcommand[0]=0;
jah128 0:8a5497a2e366 103 command[0]=0;
jah128 0:8a5497a2e366 104 switch(message[0]) {
jah128 0:8a5497a2e366 105
jah128 0:8a5497a2e366 106 // MOTOR COMMANDS
jah128 0:8a5497a2e366 107
jah128 0:8a5497a2e366 108 case 1:
jah128 0:8a5497a2e366 109 strcpy(command,"SET LEFT MOTOR");
jah128 0:8a5497a2e366 110 dec = IF_decode_float(message[1],message[2]);
jah128 0:8a5497a2e366 111 sprintf(subcommand,"%1.5f",dec);
jah128 0:8a5497a2e366 112 if(allow_commands) {
jah128 0:8a5497a2e366 113 command_status = 1;
jah128 0:8a5497a2e366 114 set_left_motor_speed(dec);
jah128 0:8a5497a2e366 115 } else command_status = 2;
jah128 0:8a5497a2e366 116 break;
jah128 0:8a5497a2e366 117 case 2:
jah128 0:8a5497a2e366 118 strcpy(command,"SET RIGHT MOTOR");
jah128 0:8a5497a2e366 119 dec = IF_decode_float(message[1],message[2]);
jah128 0:8a5497a2e366 120 sprintf(subcommand,"%1.5f",dec);
jah128 0:8a5497a2e366 121 if(allow_commands) {
jah128 0:8a5497a2e366 122 set_right_motor_speed(dec);
jah128 0:8a5497a2e366 123 command_status = 1;
jah128 0:8a5497a2e366 124 } else command_status = 2;
jah128 0:8a5497a2e366 125 break;
jah128 0:8a5497a2e366 126 case 3:
jah128 0:8a5497a2e366 127 strcpy(command,"SET BOTH MOTORS");
jah128 0:8a5497a2e366 128 dec = IF_decode_float(message[1],message[2]);
jah128 0:8a5497a2e366 129 sprintf(subcommand,"%1.5f",dec);
jah128 0:8a5497a2e366 130 if(allow_commands) {
jah128 0:8a5497a2e366 131 command_status = 1;
jah128 0:8a5497a2e366 132 forward(dec);
jah128 0:8a5497a2e366 133 } else command_status = 2;
jah128 0:8a5497a2e366 134 break;
jah128 0:8a5497a2e366 135 case 4:
jah128 0:8a5497a2e366 136 strcpy(command,"BRAKE LEFT MOTOR");
jah128 0:8a5497a2e366 137 sprintf(subcommand,"");
jah128 0:8a5497a2e366 138 if(allow_commands) {
jah128 0:8a5497a2e366 139 command_status = 1;
jah128 0:8a5497a2e366 140 brake_left_motor();
jah128 0:8a5497a2e366 141 } else command_status = 2;
jah128 0:8a5497a2e366 142 break;
jah128 0:8a5497a2e366 143 case 5:
jah128 0:8a5497a2e366 144 strcpy(command,"BRAKE RIGHT MOTOR");
jah128 0:8a5497a2e366 145 sprintf(subcommand,"");
jah128 0:8a5497a2e366 146 if(allow_commands) {
jah128 0:8a5497a2e366 147 command_status = 1;
jah128 0:8a5497a2e366 148 brake_right_motor();
jah128 0:8a5497a2e366 149 } else command_status = 2;
jah128 0:8a5497a2e366 150 break;
jah128 0:8a5497a2e366 151 case 6:
jah128 0:8a5497a2e366 152 strcpy(command,"BRAKE BOTH MOTORS");
jah128 0:8a5497a2e366 153 sprintf(subcommand,"");
jah128 0:8a5497a2e366 154 if(allow_commands) {
jah128 0:8a5497a2e366 155 command_status = 1;
jah128 0:8a5497a2e366 156 brake();
jah128 0:8a5497a2e366 157 } else command_status = 2;
jah128 0:8a5497a2e366 158 break;
jah128 0:8a5497a2e366 159 case 7:
jah128 0:8a5497a2e366 160 strcpy(command,"STOP BOTH MOTORS");
jah128 0:8a5497a2e366 161 sprintf(subcommand,"");
jah128 0:8a5497a2e366 162 if(allow_commands) {
jah128 0:8a5497a2e366 163 command_status = 1;
jah128 0:8a5497a2e366 164 stop();
jah128 0:8a5497a2e366 165 } else command_status = 2;
jah128 0:8a5497a2e366 166 break;
jah128 0:8a5497a2e366 167 case 8:
jah128 0:8a5497a2e366 168 strcpy(command,"TURN ON SPOT");
jah128 0:8a5497a2e366 169 dec = IF_decode_float(message[1],message[2]);
jah128 0:8a5497a2e366 170 sprintf(subcommand,"%1.5f",dec);
jah128 0:8a5497a2e366 171 if(allow_commands) {
jah128 0:8a5497a2e366 172 command_status = 1;
jah128 0:8a5497a2e366 173 turn(dec);
jah128 0:8a5497a2e366 174 } else command_status = 2;
jah128 0:8a5497a2e366 175 break;
jah128 0:8a5497a2e366 176 case 9:
jah128 0:8a5497a2e366 177 strcpy(command,"SET EACH MOTOR");
jah128 0:8a5497a2e366 178 l_dec = IF_decode_float(message[1]);
jah128 0:8a5497a2e366 179 r_dec = IF_decode_float(message[2]);
jah128 0:8a5497a2e366 180 sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
jah128 0:8a5497a2e366 181 if(allow_commands) {
jah128 0:8a5497a2e366 182 command_status = 1;
jah128 0:8a5497a2e366 183
jah128 0:8a5497a2e366 184 set_left_motor_speed(l_dec);
jah128 0:8a5497a2e366 185 set_right_motor_speed(r_dec);
jah128 0:8a5497a2e366 186 } else command_status = 2;
jah128 0:8a5497a2e366 187 break;
jah128 0:8a5497a2e366 188 // LED COMMANDS
jah128 0:8a5497a2e366 189
jah128 0:8a5497a2e366 190 case 10:
jah128 0:8a5497a2e366 191 strcpy(command,"SET LED STATES");
jah128 0:8a5497a2e366 192 sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2]));
jah128 0:8a5497a2e366 193 if(allow_commands) {
jah128 0:8a5497a2e366 194 command_status = 1;
jah128 0:8a5497a2e366 195 set_leds(message[1],message[2]);
jah128 0:8a5497a2e366 196 } else command_status = 2;
jah128 0:8a5497a2e366 197 break;
jah128 0:8a5497a2e366 198 case 11:
jah128 0:8a5497a2e366 199 strcpy(command,"SET RED LED STATES");
jah128 0:8a5497a2e366 200 sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
jah128 0:8a5497a2e366 201 if(allow_commands) {
jah128 0:8a5497a2e366 202 command_status = 1;
jah128 0:8a5497a2e366 203 set_red_leds(message[1]);
jah128 0:8a5497a2e366 204 } else command_status = 2;
jah128 0:8a5497a2e366 205 break;
jah128 0:8a5497a2e366 206 case 12:
jah128 0:8a5497a2e366 207 strcpy(command,"SET GREEN LED STATES");
jah128 0:8a5497a2e366 208 sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
jah128 0:8a5497a2e366 209 if(allow_commands) {
jah128 0:8a5497a2e366 210 command_status = 1;
jah128 0:8a5497a2e366 211 set_green_leds(message[1]);
jah128 0:8a5497a2e366 212 } else command_status = 2;
jah128 0:8a5497a2e366 213 break;
jah128 0:8a5497a2e366 214 case 13:
jah128 0:8a5497a2e366 215 strcpy(command,"SET LED");
jah128 0:8a5497a2e366 216 switch(message[2]) {
jah128 0:8a5497a2e366 217 case 1:
jah128 0:8a5497a2e366 218 strcpy(colour_string,"RED");
jah128 0:8a5497a2e366 219 break;
jah128 0:8a5497a2e366 220 case 2:
jah128 0:8a5497a2e366 221 strcpy(colour_string,"GREEN");
jah128 0:8a5497a2e366 222 break;
jah128 0:8a5497a2e366 223 case 3:
jah128 0:8a5497a2e366 224 strcpy(colour_string,"BOTH");
jah128 0:8a5497a2e366 225 break;
jah128 0:8a5497a2e366 226 case 0:
jah128 0:8a5497a2e366 227 strcpy(colour_string,"OFF");
jah128 0:8a5497a2e366 228 break;
jah128 0:8a5497a2e366 229 }
jah128 0:8a5497a2e366 230 if(message[1] < 8 && message[2] < 4) {
jah128 0:8a5497a2e366 231 sprintf(subcommand,"%d %s",message[1],colour_string);
jah128 0:8a5497a2e366 232 if(allow_commands) {
jah128 0:8a5497a2e366 233 command_status = 1;
jah128 0:8a5497a2e366 234 set_led(message[1],message[2]);
jah128 0:8a5497a2e366 235 } else command_status = 2;
jah128 0:8a5497a2e366 236 } else {
jah128 0:8a5497a2e366 237 sprintf(subcommand,"[INVALID CODE]");
jah128 0:8a5497a2e366 238 command_status = 3;
jah128 0:8a5497a2e366 239 }
jah128 0:8a5497a2e366 240 break;
jah128 0:8a5497a2e366 241 case 14:
jah128 0:8a5497a2e366 242 strcpy(command,"SET CENTER LED STATE");
jah128 0:8a5497a2e366 243 switch(message[1]) {
jah128 0:8a5497a2e366 244 case 1:
jah128 0:8a5497a2e366 245 strcpy(colour_string,"RED");
jah128 0:8a5497a2e366 246 break;
jah128 0:8a5497a2e366 247 case 2:
jah128 0:8a5497a2e366 248 strcpy(colour_string,"GREEN");
jah128 0:8a5497a2e366 249 break;
jah128 0:8a5497a2e366 250 case 3:
jah128 0:8a5497a2e366 251 strcpy(colour_string,"BOTH");
jah128 0:8a5497a2e366 252 break;
jah128 0:8a5497a2e366 253 case 0:
jah128 0:8a5497a2e366 254 strcpy(colour_string,"OFF");
jah128 0:8a5497a2e366 255 break;
jah128 0:8a5497a2e366 256 }
jah128 0:8a5497a2e366 257 if(message[1] < 4) {
jah128 0:8a5497a2e366 258 sprintf(subcommand,"%s",colour_string);
jah128 0:8a5497a2e366 259 if(allow_commands) {
jah128 0:8a5497a2e366 260 command_status = 1;
jah128 0:8a5497a2e366 261 set_center_led(message[1]);
jah128 0:8a5497a2e366 262 } else command_status = 2;
jah128 0:8a5497a2e366 263 } else {
jah128 0:8a5497a2e366 264 sprintf(subcommand,"[INVALID CODE]");
jah128 0:8a5497a2e366 265 command_status = 3;
jah128 0:8a5497a2e366 266 }
jah128 0:8a5497a2e366 267 break;
jah128 0:8a5497a2e366 268 case 15:
jah128 0:8a5497a2e366 269 strcpy(command,"SET C.LED BRIGHTNESS");
jah128 0:8a5497a2e366 270 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:8a5497a2e366 271 sprintf(subcommand,"%1.5f",dec);
jah128 0:8a5497a2e366 272 if(allow_commands) {
jah128 0:8a5497a2e366 273 command_status = 1;
jah128 0:8a5497a2e366 274 set_center_led_brightness(dec);
jah128 0:8a5497a2e366 275 } else command_status = 2;
jah128 0:8a5497a2e366 276 break;
jah128 0:8a5497a2e366 277 case 16:
jah128 0:8a5497a2e366 278 strcpy(command,"SET MBED LEDS");
jah128 0:8a5497a2e366 279 sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1]));
jah128 0:8a5497a2e366 280 if(allow_commands) {
jah128 0:8a5497a2e366 281 command_status = 1;
jah128 0:8a5497a2e366 282 mbed_led1 = (message[1] & 128) >> 7;
jah128 0:8a5497a2e366 283 mbed_led2 = (message[1] & 64) >> 6;
jah128 0:8a5497a2e366 284 mbed_led3 = (message[1] & 32) >> 5;
jah128 0:8a5497a2e366 285 mbed_led4 = (message[1] & 16) >> 4;
jah128 0:8a5497a2e366 286 } else command_status = 2;
jah128 0:8a5497a2e366 287 break;
jah128 0:8a5497a2e366 288 case 17:
jah128 0:8a5497a2e366 289 strcpy(command,"BLINK OUTER LEDS");
jah128 0:8a5497a2e366 290 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:8a5497a2e366 291 sprintf(subcommand,"FOR %1.5fS",dec);
jah128 0:8a5497a2e366 292 if(allow_commands) {
jah128 0:8a5497a2e366 293 command_status = 1;
jah128 0:8a5497a2e366 294 blink_leds(dec);
jah128 0:8a5497a2e366 295 } else command_status = 2;
jah128 0:8a5497a2e366 296 break;
jah128 0:8a5497a2e366 297 case 18:
jah128 0:8a5497a2e366 298 strcpy(command,"SET BASE LED STATE");
jah128 0:8a5497a2e366 299 switch(message[1]) {
jah128 0:8a5497a2e366 300 case 1:
jah128 0:8a5497a2e366 301 strcpy(subcommand,"ON");
jah128 0:8a5497a2e366 302 break;
jah128 0:8a5497a2e366 303 case 0:
jah128 0:8a5497a2e366 304 strcpy(subcommand,"OFF");
jah128 0:8a5497a2e366 305 break;
jah128 0:8a5497a2e366 306 }
jah128 0:8a5497a2e366 307 //Function not yet implemented
jah128 0:8a5497a2e366 308 break;
jah128 0:8a5497a2e366 309 case 19:
jah128 0:8a5497a2e366 310 strcpy(command,"SET CENTER LED ");
jah128 0:8a5497a2e366 311 switch(message[1]) {
jah128 0:8a5497a2e366 312 case 1:
jah128 0:8a5497a2e366 313 strcpy(colour_string,"RED");
jah128 0:8a5497a2e366 314 break;
jah128 0:8a5497a2e366 315 case 2:
jah128 0:8a5497a2e366 316 strcpy(colour_string,"GREEN");
jah128 0:8a5497a2e366 317 break;
jah128 0:8a5497a2e366 318 case 3:
jah128 0:8a5497a2e366 319 strcpy(colour_string,"BOTH");
jah128 0:8a5497a2e366 320 break;
jah128 0:8a5497a2e366 321 case 0:
jah128 0:8a5497a2e366 322 strcpy(colour_string,"OFF");
jah128 0:8a5497a2e366 323 break;
jah128 0:8a5497a2e366 324 }
jah128 0:8a5497a2e366 325 dec = IF_decode_unsigned_float(message[2]);
jah128 0:8a5497a2e366 326 sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec);
jah128 0:8a5497a2e366 327 if(allow_commands) {
jah128 0:8a5497a2e366 328 command_status = 1;
jah128 0:8a5497a2e366 329 set_center_led(message[1],dec);
jah128 0:8a5497a2e366 330 } else command_status = 2;
jah128 0:8a5497a2e366 331 break;
jah128 0:8a5497a2e366 332
jah128 0:8a5497a2e366 333 // DISPLAY COMMANDS
jah128 0:8a5497a2e366 334
jah128 0:8a5497a2e366 335 case 20:
jah128 0:8a5497a2e366 336 strcpy(command,"SET DISPLAY ");
jah128 0:8a5497a2e366 337 switch(message[1]) {
jah128 0:8a5497a2e366 338 case 0:
jah128 0:8a5497a2e366 339 strcpy(subcommand,"CLEAR");
jah128 0:8a5497a2e366 340 if(allow_commands) {
jah128 0:8a5497a2e366 341 command_status = 1;
jah128 0:8a5497a2e366 342 display.clear_display();
jah128 0:8a5497a2e366 343 } else command_status = 2;
jah128 0:8a5497a2e366 344 break;
jah128 0:8a5497a2e366 345 case 1:
jah128 0:8a5497a2e366 346 strcpy(subcommand,"MESSAGE 1");
jah128 0:8a5497a2e366 347 if(allow_commands) {
jah128 0:8a5497a2e366 348 command_status = 1;
jah128 0:8a5497a2e366 349 display.clear_display();
jah128 0:8a5497a2e366 350 display.home();
jah128 0:8a5497a2e366 351 display.write_string("PC CONNECTION");
jah128 0:8a5497a2e366 352 display.set_position(1,0);
jah128 0:8a5497a2e366 353 display.write_string("STARTED");
jah128 0:8a5497a2e366 354 } else command_status = 2;
jah128 0:8a5497a2e366 355 break;
jah128 0:8a5497a2e366 356 case 2:
jah128 0:8a5497a2e366 357 strcpy(subcommand,"MESSAGE 2");
jah128 0:8a5497a2e366 358 if(allow_commands) {
jah128 0:8a5497a2e366 359 command_status = 1;
jah128 0:8a5497a2e366 360 display.clear_display();
jah128 0:8a5497a2e366 361 display.home();
jah128 0:8a5497a2e366 362 display.write_string("PC CONNECTION");
jah128 0:8a5497a2e366 363 display.set_position(1,0);
jah128 0:8a5497a2e366 364 display.write_string("TERMINATED");
jah128 0:8a5497a2e366 365 } else command_status = 2;
jah128 0:8a5497a2e366 366 break;
jah128 0:8a5497a2e366 367 case 3:
jah128 0:8a5497a2e366 368 strcpy(subcommand,"MESSAGE 3");
jah128 0:8a5497a2e366 369 if(allow_commands) {
jah128 0:8a5497a2e366 370 command_status = 1;
jah128 0:8a5497a2e366 371 display.clear_display();
jah128 0:8a5497a2e366 372 display.home();
jah128 0:8a5497a2e366 373 display.write_string("ANDROID DEVICE");
jah128 0:8a5497a2e366 374 display.set_position(1,0);
jah128 0:8a5497a2e366 375 display.write_string("CONNECTED");
jah128 0:8a5497a2e366 376 } else command_status = 2;
jah128 0:8a5497a2e366 377 break;
jah128 0:8a5497a2e366 378 case 4:
jah128 0:8a5497a2e366 379 strcpy(subcommand,"MESSAGE 4");
jah128 0:8a5497a2e366 380 if(allow_commands) {
jah128 0:8a5497a2e366 381 command_status = 1;
jah128 0:8a5497a2e366 382 display.clear_display();
jah128 0:8a5497a2e366 383 display.home();
jah128 0:8a5497a2e366 384 display.write_string("ANDROID DEVICE");
jah128 0:8a5497a2e366 385 display.set_position(1,0);
jah128 0:8a5497a2e366 386 display.write_string("DISCONNECTED");
jah128 0:8a5497a2e366 387 } else command_status = 2;
jah128 0:8a5497a2e366 388 break;
jah128 0:8a5497a2e366 389 }
jah128 0:8a5497a2e366 390 break;
jah128 0:8a5497a2e366 391 case 21:
jah128 0:8a5497a2e366 392 strcpy(command,"SET CURSOR ");
jah128 0:8a5497a2e366 393 if(message[1] < 2 && message[2] < 16) {
jah128 0:8a5497a2e366 394 sprintf(subcommand,"[%d,%d]",message[1],message[2]);
jah128 0:8a5497a2e366 395 if(allow_commands) {
jah128 0:8a5497a2e366 396 display.set_position(message[1],message[2]);
jah128 0:8a5497a2e366 397 } else command_status = 2;
jah128 0:8a5497a2e366 398 } else {
jah128 0:8a5497a2e366 399 sprintf(subcommand,"[INVALID]");
jah128 0:8a5497a2e366 400 command_status = 3;
jah128 0:8a5497a2e366 401 }
jah128 0:8a5497a2e366 402 break;
jah128 0:8a5497a2e366 403 case 22:
jah128 0:8a5497a2e366 404 strcpy(command,"PRINT CHARACTERS ");
jah128 0:8a5497a2e366 405 char print_message[2];
jah128 0:8a5497a2e366 406 print_message[0]=message[1];
jah128 0:8a5497a2e366 407 print_message[1]=message[2];
jah128 0:8a5497a2e366 408 sprintf(subcommand,"[%c,%c]",message[1],message[2]);
jah128 0:8a5497a2e366 409 if(allow_commands) {
jah128 0:8a5497a2e366 410 display.write_string(print_message,2);
jah128 0:8a5497a2e366 411 } else command_status = 2;
jah128 0:8a5497a2e366 412 break;
jah128 0:8a5497a2e366 413 case 23:
jah128 0:8a5497a2e366 414 strcpy(command,"SET DISPLAY B.NESS");
jah128 0:8a5497a2e366 415 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:8a5497a2e366 416 sprintf(subcommand,"%1.5f",dec);
jah128 0:8a5497a2e366 417 if(allow_commands) {
jah128 0:8a5497a2e366 418 command_status = 1;
jah128 0:8a5497a2e366 419 display.set_backlight_brightness(dec);
jah128 0:8a5497a2e366 420 } else command_status = 2;
jah128 0:8a5497a2e366 421 break;
jah128 0:8a5497a2e366 422
jah128 0:8a5497a2e366 423 case 30:
jah128 0:8a5497a2e366 424 strcpy(command,"SET DEBUG MODE");
jah128 0:8a5497a2e366 425 switch(message[1]) {
jah128 0:8a5497a2e366 426 case 1:
jah128 0:8a5497a2e366 427 strcpy(subcommand,"ON");
jah128 0:8a5497a2e366 428 break;
jah128 0:8a5497a2e366 429 case 0:
jah128 0:8a5497a2e366 430 strcpy(subcommand,"OFF");
jah128 0:8a5497a2e366 431 break;
jah128 0:8a5497a2e366 432 }
jah128 0:8a5497a2e366 433 if(message[2] & 1) strcat (subcommand,"-PC");
jah128 0:8a5497a2e366 434 if(message[2] & 2) strcat (subcommand,"-BT");
jah128 0:8a5497a2e366 435 if(message[2] & 4) strcat (subcommand,"-DISP");
jah128 0:8a5497a2e366 436 if(allow_commands) {
jah128 0:8a5497a2e366 437 command_status = 1;
jah128 0:8a5497a2e366 438 debug_mode = message[1];
jah128 0:8a5497a2e366 439 debug_output = message[2];
jah128 0:8a5497a2e366 440 } else command_status = 2;
jah128 0:8a5497a2e366 441 break;
jah128 0:8a5497a2e366 442 case 31:
jah128 0:8a5497a2e366 443 strcpy(command,"SET DEMO MODE");
jah128 0:8a5497a2e366 444 switch(message[1] % 2) {
jah128 0:8a5497a2e366 445 case 1:
jah128 0:8a5497a2e366 446 strcpy(subcommand,"ON");
jah128 0:8a5497a2e366 447 break;
jah128 0:8a5497a2e366 448 case 0:
jah128 0:8a5497a2e366 449 strcpy(subcommand,"OFF");
jah128 0:8a5497a2e366 450 break;
jah128 0:8a5497a2e366 451 }
jah128 0:8a5497a2e366 452 if(allow_commands) {
jah128 0:8a5497a2e366 453 command_status = 1;
jah128 0:8a5497a2e366 454 demo_on = message[1] % 2;
jah128 0:8a5497a2e366 455 if(demo_on == 1) {
jah128 0:8a5497a2e366 456 user_code_restore_mode = user_code_running;
jah128 0:8a5497a2e366 457 user_code_running = 0;
jah128 0:8a5497a2e366 458 } else {
jah128 0:8a5497a2e366 459 user_code_running = user_code_restore_mode;
jah128 0:8a5497a2e366 460 }
jah128 0:8a5497a2e366 461 } else command_status = 2;
jah128 0:8a5497a2e366 462 break;
jah128 0:8a5497a2e366 463 case 32:
jah128 0:8a5497a2e366 464 strcpy(command,"SET USER CODE");
jah128 0:8a5497a2e366 465 switch(message[1] % 2) {
jah128 0:8a5497a2e366 466 case 1:
jah128 0:8a5497a2e366 467 strcpy(subcommand,"ON");
jah128 0:8a5497a2e366 468 break;
jah128 0:8a5497a2e366 469 case 0:
jah128 0:8a5497a2e366 470 strcpy(subcommand,"OFF");
jah128 0:8a5497a2e366 471 break;
jah128 0:8a5497a2e366 472 }
jah128 0:8a5497a2e366 473 if(allow_commands) {
jah128 0:8a5497a2e366 474 command_status = 1;
jah128 0:8a5497a2e366 475 user_code_running = message[1] % 2;
jah128 0:8a5497a2e366 476 } else command_status = 2;
jah128 0:8a5497a2e366 477 break;
jah128 0:8a5497a2e366 478 case 33:
jah128 0:8a5497a2e366 479 strcpy(command,"PAUSE USER CODE");
jah128 0:8a5497a2e366 480 dec = IF_decode_unsigned_float(message[1],message[2]) * 10;
jah128 0:8a5497a2e366 481 sprintf(subcommand,"FOR %2.3fS",dec);
jah128 0:8a5497a2e366 482 if(allow_commands) {
jah128 0:8a5497a2e366 483 command_status = 1;
jah128 0:8a5497a2e366 484 pause_user_code(dec);
jah128 0:8a5497a2e366 485 } else command_status = 2;
jah128 0:8a5497a2e366 486 break;
jah128 0:8a5497a2e366 487
jah128 0:8a5497a2e366 488 case 34:
jah128 0:8a5497a2e366 489 strcpy(command,"RESET ENCODERS");
jah128 0:8a5497a2e366 490 if(allow_commands) {
jah128 0:8a5497a2e366 491 command_status = 1;
jah128 0:8a5497a2e366 492 reset_encoders();
jah128 0:8a5497a2e366 493 } else command_status = 2;
jah128 0:8a5497a2e366 494 break;
jah128 0:8a5497a2e366 495
jah128 0:8a5497a2e366 496 case 35:
jah128 0:8a5497a2e366 497 strcpy(command,"SET ALLOW COMMANDS");
jah128 0:8a5497a2e366 498 switch(message[1] % 2) {
jah128 0:8a5497a2e366 499 case 1:
jah128 0:8a5497a2e366 500 strcpy(subcommand,"ON");
jah128 0:8a5497a2e366 501 break;
jah128 0:8a5497a2e366 502 case 0:
jah128 0:8a5497a2e366 503 strcpy(subcommand,"OFF");
jah128 0:8a5497a2e366 504 break;
jah128 0:8a5497a2e366 505 }
jah128 0:8a5497a2e366 506 allow_commands = message[1] % 2;
jah128 0:8a5497a2e366 507 command_status = 1;
jah128 0:8a5497a2e366 508 break;
jah128 0:8a5497a2e366 509
jah128 0:8a5497a2e366 510 case 36:
jah128 0:8a5497a2e366 511 irp_delay = (message[1] << 8) + message[2];
jah128 0:8a5497a2e366 512 sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay);
jah128 0:8a5497a2e366 513 if(allow_commands) {
jah128 0:8a5497a2e366 514 command_status = 1;
jah128 0:8a5497a2e366 515 ir_pulse_delay = irp_delay;
jah128 0:8a5497a2e366 516 } else command_status = 2;
jah128 0:8a5497a2e366 517 break;
jah128 0:8a5497a2e366 518 case 37:
jah128 0:8a5497a2e366 519 irp_delay = (message[1] << 8) + message[2];
jah128 0:8a5497a2e366 520 sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay);
jah128 0:8a5497a2e366 521 if(allow_commands) {
jah128 0:8a5497a2e366 522 command_status = 1;
jah128 0:8a5497a2e366 523 base_ir_pulse_delay = irp_delay;
jah128 0:8a5497a2e366 524 } else command_status = 2;
jah128 0:8a5497a2e366 525 break;
jah128 0:8a5497a2e366 526
jah128 0:8a5497a2e366 527 // MOTOR REQUESTS
jah128 0:8a5497a2e366 528 case 40:
jah128 0:8a5497a2e366 529 strcpy(command,"GET LEFT MOTOR SPEED");
jah128 0:8a5497a2e366 530 sprintf(ret_message,"%1.5f",motor_left_speed);
jah128 0:8a5497a2e366 531 send_message = 1;
jah128 0:8a5497a2e366 532 break;
jah128 0:8a5497a2e366 533
jah128 0:8a5497a2e366 534 case 41:
jah128 0:8a5497a2e366 535 strcpy(command,"GET RIGHT MOTOR SPEED");
jah128 0:8a5497a2e366 536 sprintf(ret_message,"%1.5f",motor_right_speed);
jah128 0:8a5497a2e366 537 send_message = 1;
jah128 0:8a5497a2e366 538 break;
jah128 0:8a5497a2e366 539 case 42:
jah128 0:8a5497a2e366 540 strcpy(command,"GET BRAKE STATES");
jah128 0:8a5497a2e366 541 sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
jah128 0:8a5497a2e366 542 send_message = 1;
jah128 0:8a5497a2e366 543 break;
jah128 0:8a5497a2e366 544 case 43:
jah128 0:8a5497a2e366 545 strcpy(command,"GET MOTOR STATES");
jah128 0:8a5497a2e366 546 //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
jah128 0:8a5497a2e366 547 send_message = 1;
jah128 0:8a5497a2e366 548 break;
jah128 0:8a5497a2e366 549 case 44:
jah128 0:8a5497a2e366 550 strcpy(command,"GET ENCODERS");
jah128 0:8a5497a2e366 551 sprintf(ret_message,"%d,%d",left_encoder,right_encoder);
jah128 0:8a5497a2e366 552 send_message = 1;
jah128 0:8a5497a2e366 553 break;
jah128 0:8a5497a2e366 554
jah128 0:8a5497a2e366 555 // LED REQUESTS
jah128 0:8a5497a2e366 556 case 50:
jah128 0:8a5497a2e366 557 strcpy(command,"GET LED STATES");
jah128 0:8a5497a2e366 558 sprintf(ret_message,"%04x",get_led_states());
jah128 0:8a5497a2e366 559 send_message = 1;
jah128 0:8a5497a2e366 560 break;
jah128 0:8a5497a2e366 561
jah128 0:8a5497a2e366 562 // GENERAL REQUESTS
jah128 0:8a5497a2e366 563 case 60:
jah128 0:8a5497a2e366 564 strcpy(command,"GET SOFTWARE VERSION");
jah128 0:8a5497a2e366 565 sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
jah128 0:8a5497a2e366 566 send_message = 1;
jah128 0:8a5497a2e366 567 break;
jah128 0:8a5497a2e366 568
jah128 0:8a5497a2e366 569 case 61:
jah128 0:8a5497a2e366 570 strcpy(command,"GET UPTIME");
jah128 0:8a5497a2e366 571 sprintf(ret_message,"%6.2f",get_uptime());
jah128 0:8a5497a2e366 572 send_message = 1;
jah128 0:8a5497a2e366 573 break;
jah128 0:8a5497a2e366 574
jah128 0:8a5497a2e366 575 case 62:
jah128 0:8a5497a2e366 576 strcpy(command,"GET ID");
jah128 0:8a5497a2e366 577 sprintf(ret_message,"%d",robot_id);
jah128 0:8a5497a2e366 578 send_message = 1;
jah128 0:8a5497a2e366 579 break;
jah128 0:8a5497a2e366 580
jah128 0:8a5497a2e366 581 case 63:
jah128 0:8a5497a2e366 582 strcpy(command,"GET SWITCH BYTE");
jah128 0:8a5497a2e366 583 sprintf(ret_message,"%02x",switch_byte);
jah128 0:8a5497a2e366 584 send_message = 1;
jah128 0:8a5497a2e366 585 break;
jah128 0:8a5497a2e366 586 case 64:
jah128 0:8a5497a2e366 587 strcpy(command,"GET USER CODE");
jah128 0:8a5497a2e366 588 sprintf(ret_message,"%d",user_code_running);
jah128 0:8a5497a2e366 589 send_message = 1;
jah128 0:8a5497a2e366 590 break;
jah128 0:8a5497a2e366 591 case 65:
jah128 0:8a5497a2e366 592 strcpy(command,"GET RESPONSE STRING");
jah128 0:8a5497a2e366 593 sprintf(ret_message,"PSI");
jah128 0:8a5497a2e366 594 send_message = 1;
jah128 0:8a5497a2e366 595 break;
jah128 0:8a5497a2e366 596 case 66:
jah128 0:8a5497a2e366 597 strcpy(command,"GET PROGRAM NAME");
jah128 0:8a5497a2e366 598 sprintf(ret_message,"%s",program_name);
jah128 0:8a5497a2e366 599 send_message = 1;
jah128 0:8a5497a2e366 600 break;
jah128 0:8a5497a2e366 601 case 67:
jah128 0:8a5497a2e366 602 strcpy(command,"GET AUTHOR NAME");
jah128 0:8a5497a2e366 603 sprintf(ret_message,"%s",author_name);
jah128 0:8a5497a2e366 604 send_message = 1;
jah128 0:8a5497a2e366 605 break;
jah128 0:8a5497a2e366 606 case 68:
jah128 0:8a5497a2e366 607 strcpy(command,"GET DEBUG MODE");
jah128 0:8a5497a2e366 608 sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
jah128 0:8a5497a2e366 609 send_message = 1;
jah128 0:8a5497a2e366 610 break;
jah128 0:8a5497a2e366 611 case 69:
jah128 0:8a5497a2e366 612 strcpy(command,"GET SYSTEM WARNINGS");
jah128 0:8a5497a2e366 613 sprintf(ret_message,"%d",system_warnings);
jah128 0:8a5497a2e366 614 send_message = 1;
jah128 0:8a5497a2e366 615 break;
jah128 0:8a5497a2e366 616
jah128 0:8a5497a2e366 617
jah128 0:8a5497a2e366 618 // Sensors
jah128 0:8a5497a2e366 619 case 80:
jah128 0:8a5497a2e366 620 strcpy(command,"STORE BG. IR VALUES");
jah128 0:8a5497a2e366 621 if(allow_commands) {
jah128 0:8a5497a2e366 622 command_status = 1;
jah128 0:8a5497a2e366 623 store_background_raw_ir_values();
jah128 0:8a5497a2e366 624 } else command_status = 2;
jah128 0:8a5497a2e366 625 break;
jah128 0:8a5497a2e366 626 case 81:
jah128 0:8a5497a2e366 627 strcpy(command,"STORE IL. IR VALUES");
jah128 0:8a5497a2e366 628 if(allow_commands) {
jah128 0:8a5497a2e366 629 command_status = 1;
jah128 0:8a5497a2e366 630 store_illuminated_raw_ir_values();
jah128 0:8a5497a2e366 631 } else command_status = 2;
jah128 0:8a5497a2e366 632 break;
jah128 0:8a5497a2e366 633 case 82:
jah128 0:8a5497a2e366 634 strcpy(command,"STORE IR VALUES");
jah128 0:8a5497a2e366 635 if(allow_commands) {
jah128 0:8a5497a2e366 636 command_status = 1;
jah128 0:8a5497a2e366 637 store_ir_values();
jah128 0:8a5497a2e366 638 } else command_status = 2;
jah128 0:8a5497a2e366 639 break;
jah128 0:8a5497a2e366 640 case 83:
jah128 0:8a5497a2e366 641 strcpy(command,"STORE BG BASE IR VALUES");
jah128 0:8a5497a2e366 642 if(allow_commands) {
jah128 0:8a5497a2e366 643 command_status = 1;
jah128 0:8a5497a2e366 644 store_background_base_ir_values();
jah128 0:8a5497a2e366 645 } else command_status = 2;
jah128 0:8a5497a2e366 646 break;
jah128 0:8a5497a2e366 647 case 84:
jah128 0:8a5497a2e366 648 strcpy(command,"STORE IL. BASE IR VALUES");
jah128 0:8a5497a2e366 649 if(allow_commands) {
jah128 0:8a5497a2e366 650 command_status = 1;
jah128 0:8a5497a2e366 651 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 652 } else command_status = 2;
jah128 0:8a5497a2e366 653 break;
jah128 0:8a5497a2e366 654 case 85:
jah128 0:8a5497a2e366 655 strcpy(command,"STORE BASE IR VALUES");
jah128 0:8a5497a2e366 656 if(allow_commands) {
jah128 0:8a5497a2e366 657 command_status = 1;
jah128 0:8a5497a2e366 658 store_base_ir_values();
jah128 0:8a5497a2e366 659 } else command_status = 2;
jah128 0:8a5497a2e366 660 break;
jah128 0:8a5497a2e366 661 case 86:
jah128 0:8a5497a2e366 662 strcpy(command,"STORE ALL IR VALUES");
jah128 0:8a5497a2e366 663 if(allow_commands) {
jah128 0:8a5497a2e366 664 command_status = 1;
jah128 0:8a5497a2e366 665 store_ir_values();
jah128 0:8a5497a2e366 666 store_base_ir_values();
jah128 0:8a5497a2e366 667 } else command_status = 2;
jah128 0:8a5497a2e366 668 break;
jah128 0:8a5497a2e366 669 case 90:
jah128 0:8a5497a2e366 670 sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
jah128 0:8a5497a2e366 671 sprintf(ret_message,"%d",get_background_raw_ir_value(message[1]));
jah128 0:8a5497a2e366 672 send_message = 1;
jah128 0:8a5497a2e366 673 break;
jah128 0:8a5497a2e366 674 case 91:
jah128 0:8a5497a2e366 675 sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
jah128 0:8a5497a2e366 676 sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1]));
jah128 0:8a5497a2e366 677 send_message = 1;
jah128 0:8a5497a2e366 678 break;
jah128 0:8a5497a2e366 679 case 92:
jah128 0:8a5497a2e366 680 strcpy(command,"GET BG IR VALUES");
jah128 0:8a5497a2e366 681 sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",get_background_raw_ir_value(0),get_background_raw_ir_value(1),get_background_raw_ir_value(2),get_background_raw_ir_value(3),get_background_raw_ir_value(4),get_background_raw_ir_value(5),get_background_raw_ir_value(6),get_background_raw_ir_value(7));
jah128 0:8a5497a2e366 682 send_message = 1;
jah128 0:8a5497a2e366 683 break;
jah128 0:8a5497a2e366 684 case 93:
jah128 0:8a5497a2e366 685 strcpy(command,"GET ILLUMINATED IR VALUES");
jah128 0:8a5497a2e366 686 sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",get_illuminated_raw_ir_value(0),get_illuminated_raw_ir_value(1),get_illuminated_raw_ir_value(2),get_illuminated_raw_ir_value(3),get_illuminated_raw_ir_value(4),get_illuminated_raw_ir_value(5),get_illuminated_raw_ir_value(6),get_illuminated_raw_ir_value(7));
jah128 0:8a5497a2e366 687 send_message = 1;
jah128 0:8a5497a2e366 688 break;
jah128 0:8a5497a2e366 689 case 94:
jah128 0:8a5497a2e366 690 sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
jah128 0:8a5497a2e366 691 sprintf(ret_message,"%d",get_background_base_ir_value(message[1]));
jah128 0:8a5497a2e366 692 send_message = 1;
jah128 0:8a5497a2e366 693 break;
jah128 0:8a5497a2e366 694 case 95:
jah128 0:8a5497a2e366 695 sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
jah128 0:8a5497a2e366 696 sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1]));
jah128 0:8a5497a2e366 697 send_message = 1;
jah128 0:8a5497a2e366 698 break;
jah128 0:8a5497a2e366 699 case 96:
jah128 0:8a5497a2e366 700 strcpy(command,"GET BG BASE IR VALUES");
jah128 0:8a5497a2e366 701 sprintf(ret_message,"%03X%03X%03X%03X%03X",get_background_base_ir_value(0),get_background_base_ir_value(1),get_background_base_ir_value(2),get_background_base_ir_value(3),get_background_base_ir_value(4));
jah128 0:8a5497a2e366 702 send_message = 1;
jah128 0:8a5497a2e366 703 break;
jah128 0:8a5497a2e366 704 case 97:
jah128 0:8a5497a2e366 705 strcpy(command,"GET IL BASE IR VALUES");
jah128 0:8a5497a2e366 706 sprintf(ret_message,"%03X%03X%03X%03X%03X",get_illuminated_base_ir_value(0),get_illuminated_base_ir_value(1),get_illuminated_base_ir_value(2),get_illuminated_base_ir_value(3),get_illuminated_base_ir_value(4));
jah128 0:8a5497a2e366 707 send_message = 1;
jah128 0:8a5497a2e366 708 break;
jah128 0:8a5497a2e366 709 }
jah128 0:8a5497a2e366 710
jah128 0:8a5497a2e366 711
jah128 0:8a5497a2e366 712 if(send_message) {
jah128 0:8a5497a2e366 713 char message_length = strlen(ret_message);
jah128 0:8a5497a2e366 714 switch(interface) {
jah128 0:8a5497a2e366 715 case 0:
jah128 0:8a5497a2e366 716 pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 0:8a5497a2e366 717 break;
jah128 0:8a5497a2e366 718 case 1:
jah128 0:8a5497a2e366 719 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 0:8a5497a2e366 720 break;
jah128 0:8a5497a2e366 721 }
jah128 0:8a5497a2e366 722 debug("Received %s request message: %s %s [%02x%02x%02x]\nReply: %s [%d ch]\n",iface, command, subcommand,message[0],message[1],message[2],ret_message,message_length);
jah128 0:8a5497a2e366 723 } else {
jah128 0:8a5497a2e366 724 switch(interface) {
jah128 0:8a5497a2e366 725 case 0:
jah128 0:8a5497a2e366 726 pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 0:8a5497a2e366 727 break;
jah128 0:8a5497a2e366 728 case 1:
jah128 0:8a5497a2e366 729 bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 0:8a5497a2e366 730 break;
jah128 0:8a5497a2e366 731 }
jah128 0:8a5497a2e366 732 switch(command_status) {
jah128 0:8a5497a2e366 733 case 0:
jah128 0:8a5497a2e366 734 debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
jah128 0:8a5497a2e366 735 break;
jah128 0:8a5497a2e366 736 case 1:
jah128 0:8a5497a2e366 737 debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:8a5497a2e366 738 break;
jah128 0:8a5497a2e366 739 case 2:
jah128 0:8a5497a2e366 740 debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:8a5497a2e366 741 break;
jah128 0:8a5497a2e366 742 case 3:
jah128 0:8a5497a2e366 743 debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:8a5497a2e366 744 break;
jah128 0:8a5497a2e366 745 }
jah128 0:8a5497a2e366 746 }
jah128 0:8a5497a2e366 747 }
jah128 0:8a5497a2e366 748
jah128 0:8a5497a2e366 749 char * IF_nibble_to_binary_char(char in)
jah128 0:8a5497a2e366 750 {
jah128 0:8a5497a2e366 751 char * ret = (char*)malloc(sizeof(char)*5);
jah128 0:8a5497a2e366 752 for(int i=0; i<4; i++) {
jah128 0:8a5497a2e366 753 if(in & (128 >> i)) ret[i]='1';
jah128 0:8a5497a2e366 754 else ret[i]='0';
jah128 0:8a5497a2e366 755 }
jah128 0:8a5497a2e366 756 ret[4]=0;
jah128 0:8a5497a2e366 757 return ret;
jah128 0:8a5497a2e366 758 }
jah128 0:8a5497a2e366 759
jah128 0:8a5497a2e366 760 char * IF_char_to_binary_char(char in)
jah128 0:8a5497a2e366 761 {
jah128 0:8a5497a2e366 762 char * ret = (char*)malloc(sizeof(char)*9);
jah128 0:8a5497a2e366 763 for(int i=0; i<8; i++) {
jah128 0:8a5497a2e366 764 if(in & (128 >> i)) ret[i]='1';
jah128 0:8a5497a2e366 765 else ret[i]='0';
jah128 0:8a5497a2e366 766 }
jah128 0:8a5497a2e366 767 ret[8]=0;
jah128 0:8a5497a2e366 768 return ret;
jah128 0:8a5497a2e366 769 }
jah128 0:8a5497a2e366 770
jah128 0:8a5497a2e366 771 float IF_decode_unsigned_float(char byte0, char byte1)
jah128 0:8a5497a2e366 772 {
jah128 0:8a5497a2e366 773 unsigned short sval = (byte0) << 8;
jah128 0:8a5497a2e366 774 sval += byte1;
jah128 0:8a5497a2e366 775 float scaled = sval / 65535.0f;
jah128 0:8a5497a2e366 776 return scaled;
jah128 0:8a5497a2e366 777 }
jah128 0:8a5497a2e366 778
jah128 0:8a5497a2e366 779 float IF_decode_float(char byte0, char byte1)
jah128 0:8a5497a2e366 780 {
jah128 0:8a5497a2e366 781 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 0:8a5497a2e366 782 char sign = byte0 / 128;
jah128 0:8a5497a2e366 783 short sval = (byte0 % 128) << 8;
jah128 0:8a5497a2e366 784 sval += byte1;
jah128 0:8a5497a2e366 785 float scaled = sval / 32767.0f;
jah128 0:8a5497a2e366 786 if(sign == 0) scaled = 0-scaled;
jah128 0:8a5497a2e366 787 return scaled;
jah128 0:8a5497a2e366 788 }
jah128 0:8a5497a2e366 789
jah128 0:8a5497a2e366 790 float IF_decode_unsigned_float(char byte0)
jah128 0:8a5497a2e366 791 {
jah128 0:8a5497a2e366 792 unsigned short sval = (byte0);
jah128 0:8a5497a2e366 793 float scaled = sval / 255.0f;
jah128 0:8a5497a2e366 794 return scaled;
jah128 0:8a5497a2e366 795 }
jah128 0:8a5497a2e366 796
jah128 0:8a5497a2e366 797 float IF_decode_float(char byte0)
jah128 0:8a5497a2e366 798 {
jah128 0:8a5497a2e366 799 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 0:8a5497a2e366 800 char sign = byte0 / 128;
jah128 0:8a5497a2e366 801 short sval = (byte0 % 128);
jah128 0:8a5497a2e366 802 float scaled = sval / 127.0f;
jah128 0:8a5497a2e366 803 if(sign == 0) scaled = 0-scaled;
jah128 0:8a5497a2e366 804 return scaled;
jah128 0:8a5497a2e366 805 }
jah128 0:8a5497a2e366 806
jah128 0:8a5497a2e366 807 void IF_setup_serial_interfaces()
jah128 0:8a5497a2e366 808 {
jah128 0:8a5497a2e366 809 if(ENABLE_PC_SERIAL) {
jah128 0:8a5497a2e366 810 pc.baud(PC_BAUD);
jah128 0:8a5497a2e366 811 pc.attach(&IF_pc_rx_callback, Serial::RxIrq);
jah128 0:8a5497a2e366 812 }
jah128 0:8a5497a2e366 813 if(ENABLE_BLUETOOTH) {
jah128 0:8a5497a2e366 814 bt.baud(BLUETOOTH_BAUD);
jah128 0:8a5497a2e366 815 bt.attach(&IF_bt_rx_callback, Serial::RxIrq);
jah128 0:8a5497a2e366 816 }
jah128 0:8a5497a2e366 817 }
jah128 0:8a5497a2e366 818
jah128 0:8a5497a2e366 819 void IF_pc_rx_command_timeout()
jah128 0:8a5497a2e366 820 {
jah128 0:8a5497a2e366 821 char message_array[6];
jah128 0:8a5497a2e366 822 char length = 1 + pc_command_message_byte;
jah128 0:8a5497a2e366 823 pc_command_message_started = 0;
jah128 0:8a5497a2e366 824 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:8a5497a2e366 825 for(int k=0; k<pc_command_message_byte; k++) {
jah128 0:8a5497a2e366 826 message_array[k+1] = pc_command_message[k];
jah128 0:8a5497a2e366 827 }
jah128 0:8a5497a2e366 828 IF_handle_user_serial_message(message_array, length, 0);
jah128 0:8a5497a2e366 829 }
jah128 0:8a5497a2e366 830
jah128 0:8a5497a2e366 831 void IF_bt_rx_command_timeout()
jah128 0:8a5497a2e366 832 {
jah128 0:8a5497a2e366 833 char message_array[6];
jah128 0:8a5497a2e366 834 char length = 1 + bt_command_message_byte;
jah128 0:8a5497a2e366 835 bt_command_message_started = 0;
jah128 0:8a5497a2e366 836 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:8a5497a2e366 837 for(int k=0; k<bt_command_message_byte; k++) {
jah128 0:8a5497a2e366 838 message_array[k+1] = bt_command_message[k];
jah128 0:8a5497a2e366 839 }
jah128 0:8a5497a2e366 840 IF_handle_user_serial_message(message_array, length, 1);
jah128 0:8a5497a2e366 841 }
jah128 0:8a5497a2e366 842
jah128 0:8a5497a2e366 843 void IF_pc_rx_callback()
jah128 0:8a5497a2e366 844 {
jah128 0:8a5497a2e366 845 int count = 0;
jah128 0:8a5497a2e366 846 char message_array[255];
jah128 0:8a5497a2e366 847
jah128 0:8a5497a2e366 848 while(pc.readable()) {
jah128 0:8a5497a2e366 849 char tc = pc.getc();
jah128 0:8a5497a2e366 850 message_array[count] = tc;
jah128 0:8a5497a2e366 851 count ++;
jah128 0:8a5497a2e366 852 if(pc_command_message_started == 1) {
jah128 0:8a5497a2e366 853 if(pc_command_message_byte == 3) {
jah128 0:8a5497a2e366 854 pc_command_timeout.detach();
jah128 0:8a5497a2e366 855 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:8a5497a2e366 856 // A complete command message succesfully received, call handler
jah128 0:8a5497a2e366 857 pc_command_message_started = 0;
jah128 0:8a5497a2e366 858 count = 0;
jah128 0:8a5497a2e366 859 IF_handle_command_serial_message(pc_command_message , 0);
jah128 0:8a5497a2e366 860 } else {
jah128 0:8a5497a2e366 861 // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
jah128 0:8a5497a2e366 862 pc_command_message_started = 0;
jah128 0:8a5497a2e366 863 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:8a5497a2e366 864 message_array[1] = pc_command_message[0];
jah128 0:8a5497a2e366 865 message_array[2] = pc_command_message[1];
jah128 0:8a5497a2e366 866 message_array[3] = pc_command_message[2];
jah128 0:8a5497a2e366 867 message_array[4] = tc;
jah128 0:8a5497a2e366 868 count = 5;
jah128 0:8a5497a2e366 869 }
jah128 0:8a5497a2e366 870 } else {
jah128 0:8a5497a2e366 871 pc_command_message[pc_command_message_byte] = tc;
jah128 0:8a5497a2e366 872 pc_command_message_byte ++;
jah128 0:8a5497a2e366 873 }
jah128 0:8a5497a2e366 874 } else {
jah128 0:8a5497a2e366 875 if(count == 1) {
jah128 0:8a5497a2e366 876 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:8a5497a2e366 877 pc_command_timeout.attach(&IF_pc_rx_command_timeout,command_timeout_period);
jah128 0:8a5497a2e366 878 pc_command_message_started = 1;
jah128 0:8a5497a2e366 879 pc_command_message_byte = 0;
jah128 0:8a5497a2e366 880
jah128 0:8a5497a2e366 881 }
jah128 0:8a5497a2e366 882 }
jah128 0:8a5497a2e366 883 }
jah128 0:8a5497a2e366 884 }
jah128 0:8a5497a2e366 885 if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0);
jah128 0:8a5497a2e366 886 }
alanmillard 20:e08376c0b4ea 887
alanmillard 20:e08376c0b4ea 888 Timeout bt_message_timeout;
alanmillard 20:e08376c0b4ea 889 static float bt_message_timeout_period = 0.001; // 1 millisecond
alanmillard 20:e08376c0b4ea 890 char bt_buffer[255];
alanmillard 20:e08376c0b4ea 891 int bt_buffer_index = 0;
alanmillard 20:e08376c0b4ea 892
alanmillard 20:e08376c0b4ea 893 void IF_bt_message_timeout()
alanmillard 20:e08376c0b4ea 894 {
alanmillard 20:e08376c0b4ea 895 char buffer[255];
alanmillard 20:e08376c0b4ea 896
alanmillard 20:e08376c0b4ea 897 sprintf(buffer, bt_buffer, bt_buffer_index);
alanmillard 20:e08376c0b4ea 898 buffer[bt_buffer_index] = 0;
alanmillard 20:e08376c0b4ea 899
alanmillard 20:e08376c0b4ea 900 // debug("BT message timeout: %s [%d chars]\n", buffer, bt_buffer_index);
alanmillard 20:e08376c0b4ea 901
alanmillard 20:e08376c0b4ea 902 IF_handle_user_serial_message(bt_buffer, bt_buffer_index, 1);
alanmillard 20:e08376c0b4ea 903
alanmillard 20:e08376c0b4ea 904 bt_buffer_index = 0;
alanmillard 20:e08376c0b4ea 905 }
alanmillard 20:e08376c0b4ea 906
jah128 0:8a5497a2e366 907 void IF_bt_rx_callback()
jah128 0:8a5497a2e366 908 {
alanmillard 20:e08376c0b4ea 909 while(bt.readable())
alanmillard 20:e08376c0b4ea 910 {
alanmillard 20:e08376c0b4ea 911 char byte = bt.getc();
alanmillard 20:e08376c0b4ea 912
alanmillard 20:e08376c0b4ea 913 bt_buffer[bt_buffer_index] = byte;
alanmillard 20:e08376c0b4ea 914 bt_buffer_index++;
alanmillard 20:e08376c0b4ea 915 }
alanmillard 20:e08376c0b4ea 916
alanmillard 20:e08376c0b4ea 917 bt_message_timeout.attach(&IF_bt_message_timeout, bt_message_timeout_period);
alanmillard 20:e08376c0b4ea 918 }
alanmillard 20:e08376c0b4ea 919
alanmillard 20:e08376c0b4ea 920 //void IF_bt_rx_callback()
alanmillard 20:e08376c0b4ea 921 //{
alanmillard 18:5921c1853e8a 922 // int count = 0;
alanmillard 18:5921c1853e8a 923 // char message_array[255];
alanmillard 18:5921c1853e8a 924 //
alanmillard 18:5921c1853e8a 925 // wait_ms(500); // Wait 0.5ms to allow a complete message to arrive before atttempting to process it
alanmillard 18:5921c1853e8a 926 //
alanmillard 18:5921c1853e8a 927 // while(bt.readable()) {
alanmillard 18:5921c1853e8a 928 // char tc = bt.getc();
alanmillard 18:5921c1853e8a 929 // message_array[count] = tc;
alanmillard 18:5921c1853e8a 930 // count ++;
alanmillard 18:5921c1853e8a 931 // if(bt_command_message_started == 1) {
alanmillard 18:5921c1853e8a 932 // if(bt_command_message_byte == 3) {
alanmillard 18:5921c1853e8a 933 // bt_command_timeout.detach();
alanmillard 18:5921c1853e8a 934 // if(tc == COMMAND_MESSAGE_BYTE) {
alanmillard 18:5921c1853e8a 935 // // A complete command message succesfully received, call handler
alanmillard 18:5921c1853e8a 936 // bt_command_message_started = 0;
alanmillard 18:5921c1853e8a 937 // count = 0;
alanmillard 18:5921c1853e8a 938 // IF_handle_command_serial_message(bt_command_message , 1);
alanmillard 18:5921c1853e8a 939 // } else {
alanmillard 18:5921c1853e8a 940 // // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
alanmillard 18:5921c1853e8a 941 // bt_command_message_started = 0;
alanmillard 18:5921c1853e8a 942 // message_array[0] = COMMAND_MESSAGE_BYTE;
alanmillard 18:5921c1853e8a 943 // message_array[1] = bt_command_message[0];
alanmillard 18:5921c1853e8a 944 // message_array[2] = bt_command_message[1];
alanmillard 18:5921c1853e8a 945 // message_array[3] = bt_command_message[2];
alanmillard 18:5921c1853e8a 946 // message_array[4] = tc;
alanmillard 18:5921c1853e8a 947 // count = 5;
alanmillard 18:5921c1853e8a 948 // }
alanmillard 18:5921c1853e8a 949 // } else {
alanmillard 18:5921c1853e8a 950 // bt_command_timeout.attach(&IF_bt_rx_command_timeout,command_timeout_period);
alanmillard 18:5921c1853e8a 951 // bt_command_message[bt_command_message_byte] = tc;
alanmillard 18:5921c1853e8a 952 // bt_command_message_byte ++;
alanmillard 17:da524989b637 953 // }
alanmillard 18:5921c1853e8a 954 // } else {
alanmillard 18:5921c1853e8a 955 // if(count == 1) {
alanmillard 18:5921c1853e8a 956 // if(tc == COMMAND_MESSAGE_BYTE) {
alanmillard 18:5921c1853e8a 957 // bt_command_message_started = 1;
alanmillard 18:5921c1853e8a 958 // bt_command_message_byte = 0;
alanmillard 18:5921c1853e8a 959 //
alanmillard 18:5921c1853e8a 960 // }
alanmillard 18:5921c1853e8a 961 // }
alanmillard 17:da524989b637 962 // }
alanmillard 17:da524989b637 963 // }
alanmillard 18:5921c1853e8a 964 // if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1);
alanmillard 20:e08376c0b4ea 965 //}