Alan Millard / Mbed 2 deprecated BeautifulMemeProjectBT

Bluetooth communication for flocking.

Dependencies:   mbed

Fork of BeautifulMemeProject by James Hilder

PsiSwarm/serial.cpp@17:da524989b637, 2016-01-07 (annotated)

Committer:
alanmillard
Date:
Thu Jan 07 17:58:07 2016 +0000
Revision:
17:da524989b637
Parent:
6:ff3c66f7372b
Child:
18:5921c1853e8a
Now stitching two successive partial user messages together, if they are received within 0.1s of each other.

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