UKESF Headstart Summer School / PsiSwarmLab-ScriptingBased

Modified version of the UKESF lab source which can be carried out with no knowledge of C

Fork of PsiSwarm-Headstart by UKESF Headstart Summer School

serial.cpp@2:c6986ee3c7c5, 2016-03-11 (annotated)

Committer:
jah128
Date:
Fri Mar 11 13:51:24 2016 +0000
Revision:
2:c6986ee3c7c5
Parent:
1:060690a934a9
Child:
3:7c0d1f581757
Added Alex' IR functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:d6269d17c8cf 1 /* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File
jah128 0:d6269d17c8cf 2 *
jah128 0:d6269d17c8cf 3 * File: serial.cpp
jah128 0:d6269d17c8cf 4 *
jah128 0:d6269d17c8cf 5 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:d6269d17c8cf 6 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
jah128 0:d6269d17c8cf 7 *
jah128 2:c6986ee3c7c5 8 * PsiSwarm Library Version: 0.41
jah128 0:d6269d17c8cf 9 *
jah128 2:c6986ee3c7c5 10 * March 2016
jah128 0:d6269d17c8cf 11 *
jah128 0:d6269d17c8cf 12 *
jah128 0:d6269d17c8cf 13 */
jah128 0:d6269d17c8cf 14
jah128 0:d6269d17c8cf 15 #include "psiswarm.h"
jah128 0:d6269d17c8cf 16
jah128 0:d6269d17c8cf 17 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:d6269d17c8cf 18 char pc_command_message_started = 0;
jah128 0:d6269d17c8cf 19 char pc_command_message_byte = 0;
jah128 0:d6269d17c8cf 20 char pc_command_message[3];
jah128 0:d6269d17c8cf 21 char bt_command_message_started = 0;
jah128 0:d6269d17c8cf 22 char bt_command_message_byte = 0;
jah128 0:d6269d17c8cf 23 char bt_command_message[3];
jah128 0:d6269d17c8cf 24
jah128 0:d6269d17c8cf 25 char allow_commands = 1;
jah128 0:d6269d17c8cf 26 char allow_requests = 1;
jah128 2:c6986ee3c7c5 27 char file_transfer_state = 0;
jah128 0:d6269d17c8cf 28
jah128 2:c6986ee3c7c5 29 int block_size = 88; // The data block size for file transfer
jah128 2:c6986ee3c7c5 30 char data_block[89]; // Stores the data block to write for Bluetooth file transfer
jah128 2:c6986ee3c7c5 31 int data_written; // Stores if partial data has been written to a file
jah128 2:c6986ee3c7c5 32 int file_length; // Stores the file length for a Bluetooth file transfer
jah128 2:c6986ee3c7c5 33 int final_block; // Stores the index of the final data block for a Bluetooth file transfer
jah128 2:c6986ee3c7c5 34 int block_index; // Stores the current block index for a Bluetooth file transfer
jah128 2:c6986ee3c7c5 35 char filename [21]; // Stores the filename for a Bluetooth file transfer
jah128 1:060690a934a9 36
jah128 1:060690a934a9 37 Timeout ft_timeout;
jah128 0:d6269d17c8cf 38 Timeout pc_command_timeout;
jah128 0:d6269d17c8cf 39 Timeout bt_command_timeout;
jah128 0:d6269d17c8cf 40
jah128 0:d6269d17c8cf 41 // A predefined message structure for command messages is as follows:
jah128 0:d6269d17c8cf 42 // [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4]
jah128 0:d6269d17c8cf 43 // 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:d6269d17c8cf 44
jah128 0:d6269d17c8cf 45
jah128 0:d6269d17c8cf 46
jah128 0:d6269d17c8cf 47 void handle_user_serial_message(char * message, char length, char interface)
jah128 0:d6269d17c8cf 48 {
jah128 0:d6269d17c8cf 49 // This is where user code for handling a (non-system) serial message should go
jah128 0:d6269d17c8cf 50 //
jah128 0:d6269d17c8cf 51 // message = pointer to message char array
jah128 0:d6269d17c8cf 52 // length = length of message
jah128 0:d6269d17c8cf 53 // interface = 0 for PC serial connection, 1 for Bluetooth
jah128 0:d6269d17c8cf 54
jah128 0:d6269d17c8cf 55 if(interface) {
jah128 0:d6269d17c8cf 56 if(length == 8) {
jah128 0:d6269d17c8cf 57 for(int i = 0; i < length; i++) {
jah128 0:d6269d17c8cf 58 // Convert single byte value into a beacon heading in the range +/-180 degrees
jah128 0:d6269d17c8cf 59 float beacon_heading = message[i];
jah128 0:d6269d17c8cf 60 float degrees_per_value = 256.0f / 360.0f;
jah128 0:d6269d17c8cf 61
jah128 0:d6269d17c8cf 62 if(beacon_heading != 0)
jah128 0:d6269d17c8cf 63 beacon_heading /= degrees_per_value;
jah128 0:d6269d17c8cf 64
jah128 0:d6269d17c8cf 65 beacon_heading -= 180;
jah128 0:d6269d17c8cf 66
jah128 0:d6269d17c8cf 67 flocking_headings[i] = beacon_heading;
jah128 0:d6269d17c8cf 68
jah128 0:d6269d17c8cf 69 debug("%d, ", flocking_headings[i]);
jah128 0:d6269d17c8cf 70 //debug("%f, ", beacon_heading);
jah128 0:d6269d17c8cf 71 }
jah128 0:d6269d17c8cf 72
jah128 0:d6269d17c8cf 73 debug("\n");
jah128 0:d6269d17c8cf 74 }
jah128 0:d6269d17c8cf 75 }
jah128 0:d6269d17c8cf 76 }
jah128 0:d6269d17c8cf 77
jah128 1:060690a934a9 78 void IF_start_file_transfer_mode()
jah128 1:060690a934a9 79 {
jah128 1:060690a934a9 80 display.clear_display();
jah128 1:060690a934a9 81 display.set_position(0,0);
jah128 1:060690a934a9 82 display.write_string("FILE TRANSFER");
jah128 1:060690a934a9 83 display.set_position(1,0);
jah128 1:060690a934a9 84 display.write_string("MODE...");
jah128 2:c6986ee3c7c5 85 data_written = 0;
jah128 1:060690a934a9 86 file_transfer_mode = 1;
jah128 1:060690a934a9 87 file_transfer_state = 0;
jah128 1:060690a934a9 88 file_length = 0;
jah128 1:060690a934a9 89 user_code_restore_mode = user_code_running;
jah128 1:060690a934a9 90 user_code_running = 0;
jah128 1:060690a934a9 91 ft_timeout.attach(IF_file_transfer_timeout,2.0);
jah128 1:060690a934a9 92 }
jah128 1:060690a934a9 93
jah128 1:060690a934a9 94
jah128 1:060690a934a9 95 void IF_invalid_transfer(void)
jah128 1:060690a934a9 96 {
jah128 1:060690a934a9 97 debug("File transfer failed\n");
jah128 2:c6986ee3c7c5 98 if(data_written == 1){
jah128 2:c6986ee3c7c5 99 debug("Deleting corrupted file\n");
jah128 2:c6986ee3c7c5 100 remove(filename);
jah128 2:c6986ee3c7c5 101 }
jah128 1:060690a934a9 102 display.clear_display();
jah128 1:060690a934a9 103 display.set_position(0,0);
jah128 1:060690a934a9 104 display.write_string("TRANSFER FAILED");
jah128 1:060690a934a9 105 wait(0.5);
jah128 1:060690a934a9 106 IF_end_file_transfer_mode();
jah128 1:060690a934a9 107 }
jah128 1:060690a934a9 108
jah128 1:060690a934a9 109 void IF_file_transfer_timeout(void)
jah128 1:060690a934a9 110 {
jah128 1:060690a934a9 111 debug("File transfer failed: timeout\n");
jah128 1:060690a934a9 112 display.clear_display();
jah128 1:060690a934a9 113 display.set_position(0,0);
jah128 1:060690a934a9 114 display.write_string("TRANSFER TIMEOUT");
jah128 1:060690a934a9 115 wait(0.5);
jah128 1:060690a934a9 116 IF_end_file_transfer_mode();
jah128 1:060690a934a9 117 }
jah128 1:060690a934a9 118
jah128 1:060690a934a9 119 void IF_end_file_transfer_mode(void)
jah128 1:060690a934a9 120 {
jah128 1:060690a934a9 121 display.clear_display();
jah128 1:060690a934a9 122 file_transfer_mode = 0;
jah128 1:060690a934a9 123 user_code_running = user_code_restore_mode;
jah128 1:060690a934a9 124 }
jah128 1:060690a934a9 125
jah128 0:d6269d17c8cf 126
jah128 0:d6269d17c8cf 127 void IF_handle_file_transfer_serial_message(char * message, char length, char interface)
jah128 0:d6269d17c8cf 128 {
jah128 0:d6269d17c8cf 129 // Code for handling a serial (Bluetooth) message when in file-transfer mode
jah128 0:d6269d17c8cf 130 //
jah128 0:d6269d17c8cf 131 // message = pointer to message char array
jah128 0:d6269d17c8cf 132 // length = length of message
jah128 0:d6269d17c8cf 133 // interface = 0 for PC serial connection, 1 for Bluetooth [NB only Bluetooth used for file transfer in this version]
jah128 0:d6269d17c8cf 134
jah128 2:c6986ee3c7c5 135 if(file_transfer_state < 2)debug("FTM Message:%.*s [%d]\n",length,message,length);
jah128 2:c6986ee3c7c5 136 else debug("FTM data block received (%d bytes)\n",length);
jah128 1:060690a934a9 137 int expected_size;
jah128 1:060690a934a9 138 // The first byte in EVERY message received should be 33; if it isn't, abort the transfer
jah128 1:060690a934a9 139 if(message[0] != 33) {
jah128 1:060690a934a9 140 IF_invalid_transfer();
jah128 1:060690a934a9 141 } else {
jah128 1:060690a934a9 142 switch(file_transfer_state) {
jah128 1:060690a934a9 143 case 0: //First message received is the target filename
jah128 1:060690a934a9 144 //The filenames cannot be more that 8.3 characters long (FAT12 format)
jah128 1:060690a934a9 145 if(length == 1 || length > 13) IF_invalid_transfer();
jah128 1:060690a934a9 146 else {
jah128 2:c6986ee3c7c5 147 strcpy(filename, "/local/");
jah128 2:c6986ee3c7c5 148 strncat(filename, message + 1, length - 1);
jah128 2:c6986ee3c7c5 149 debug("Target filename:%s\n",filename);
jah128 1:060690a934a9 150 //Send acknowledge ("FN")
jah128 1:060690a934a9 151 ft_timeout.detach();
jah128 1:060690a934a9 152 ft_timeout.attach(IF_file_transfer_timeout,2.0);
jah128 1:060690a934a9 153 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,2,"FN");
jah128 1:060690a934a9 154 file_transfer_state = 1;
jah128 1:060690a934a9 155 }
jah128 1:060690a934a9 156 break;
jah128 1:060690a934a9 157 case 1: //Second message is the length of the file in bytes
jah128 1:060690a934a9 158 //Length is encoded as a 3-byte value
jah128 1:060690a934a9 159 if(length != 4) IF_invalid_transfer();
jah128 1:060690a934a9 160 else {
jah128 1:060690a934a9 161 file_length = (message[1]) * 256;
jah128 1:060690a934a9 162 file_length += (message[2]);
jah128 1:060690a934a9 163 file_length *= 256;
jah128 1:060690a934a9 164 file_length += message[3];
jah128 1:060690a934a9 165 file_transfer_state = 2;
jah128 1:060690a934a9 166 display.clear_display();
jah128 1:060690a934a9 167 char display_message[17];
jah128 1:060690a934a9 168 sprintf(display_message,"F:%s",filename);
jah128 1:060690a934a9 169 display.set_position(0,0);
jah128 1:060690a934a9 170 display.write_string(display_message);
jah128 1:060690a934a9 171 display.set_position(1,0);
jah128 1:060690a934a9 172 sprintf(display_message,"S:%d b",file_length);
jah128 1:060690a934a9 173 display.write_string(display_message);
jah128 1:060690a934a9 174 block_index = 0;
jah128 1:060690a934a9 175 //Work out how many blocks the file will be sent in (size = block_size, tested at 100 bytes)
jah128 1:060690a934a9 176 //Allocate memory for the file up to a limit of 16 blocks; larger files will be split across
jah128 1:060690a934a9 177 //multiple blocks....
jah128 1:060690a934a9 178 final_block = file_length / block_size;
jah128 1:060690a934a9 179 if(file_length % block_size != 0) final_block ++;
jah128 2:c6986ee3c7c5 180 //int target_size = file_length;
jah128 2:c6986ee3c7c5 181 //if(file_length > (block_size * 16)) target_size = block_size * 16;
jah128 2:c6986ee3c7c5 182 //file_data = (char *) malloc(target_size);
jah128 2:c6986ee3c7c5 183 debug("File size %d bytes (%d blocks of %d bytes)\n",file_length,final_block,block_size);
jah128 1:060690a934a9 184 ft_timeout.detach();
jah128 1:060690a934a9 185 ft_timeout.attach(IF_file_transfer_timeout,1.0);
jah128 1:060690a934a9 186 //Send acknowledge (size of file)
jah128 1:060690a934a9 187 bt.printf("%c%c%c%c%c",RESPONSE_MESSAGE_BYTE,3,message[1],message[2],message[3]);
jah128 1:060690a934a9 188 }
jah128 1:060690a934a9 189 break;
jah128 1:060690a934a9 190 case 2:
jah128 1:060690a934a9 191 block_index ++;
jah128 2:c6986ee3c7c5 192 display.clear_display();
jah128 2:c6986ee3c7c5 193 display.set_position(0,0);
jah128 2:c6986ee3c7c5 194 display.write_string("FILE TRANSFER");
jah128 2:c6986ee3c7c5 195 display.set_position(1,0);
jah128 2:c6986ee3c7c5 196 char details_string[17];
jah128 2:c6986ee3c7c5 197 sprintf(details_string,"BLOCK %d OF %d",block_index,final_block);
jah128 2:c6986ee3c7c5 198 display.write_string(details_string);
jah128 2:c6986ee3c7c5 199 expected_size = block_size;
jah128 1:060690a934a9 200 if(block_index == final_block) expected_size = file_length % block_size;
jah128 1:060690a934a9 201 if(expected_size == 0) expected_size = block_size;
jah128 1:060690a934a9 202 if(length!=expected_size + 1){
jah128 1:060690a934a9 203 // Unexpected length
jah128 2:c6986ee3c7c5 204 debug("File data unexpected length in packet %d (%d bytes received, %d bytes expected)\n",block_index,length-1,expected_size);
jah128 1:060690a934a9 205 }else{
jah128 2:c6986ee3c7c5 206 char transfer_mode[2]={'a'};
jah128 2:c6986ee3c7c5 207 if(block_index == 1){
jah128 2:c6986ee3c7c5 208 transfer_mode[0]='w';
jah128 2:c6986ee3c7c5 209 }
jah128 2:c6986ee3c7c5 210 FILE *fp = fopen(filename,transfer_mode);
jah128 2:c6986ee3c7c5 211 //strncpy(data_block,message+1,length);
jah128 2:c6986ee3c7c5 212 //data_block[length]=0;
jah128 2:c6986ee3c7c5 213 //fprintf(fp,data_block);
jah128 2:c6986ee3c7c5 214 int bytes_written;
jah128 2:c6986ee3c7c5 215 bytes_written = fwrite(message+1,expected_size,1,fp);
jah128 2:c6986ee3c7c5 216 fclose(fp);
jah128 2:c6986ee3c7c5 217 if(data_written == false && bytes_written > 0) data_written = true;
jah128 2:c6986ee3c7c5 218 debug("Bytes written: %d\n",expected_size * bytes_written);
jah128 2:c6986ee3c7c5 219 if(block_index < final_block){
jah128 2:c6986ee3c7c5 220 debug("Message packet %d received and written\n",block_index);
jah128 2:c6986ee3c7c5 221 //Send acknowledge ("D")
jah128 1:060690a934a9 222 ft_timeout.detach();
jah128 1:060690a934a9 223 ft_timeout.attach(IF_file_transfer_timeout,1.0);
jah128 2:c6986ee3c7c5 224 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"D");
jah128 2:c6986ee3c7c5 225 }
jah128 2:c6986ee3c7c5 226 else{
jah128 2:c6986ee3c7c5 227 //Last data block written
jah128 2:c6986ee3c7c5 228 //[Put file checking code here]
jah128 2:c6986ee3c7c5 229 //Send acknowledge ("P");
jah128 2:c6986ee3c7c5 230 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"F");
jah128 2:c6986ee3c7c5 231 ft_timeout.detach();
jah128 2:c6986ee3c7c5 232 debug("File transfer completed successfully\n");
jah128 2:c6986ee3c7c5 233 wait(0.25);
jah128 2:c6986ee3c7c5 234 //Calculate CRC16 value for file
jah128 2:c6986ee3c7c5 235 IF_calculateCRC16(file_length);
jah128 2:c6986ee3c7c5 236
jah128 2:c6986ee3c7c5 237 display.clear_display();
jah128 2:c6986ee3c7c5 238 display.write_string("FILE TRANSFER");
jah128 2:c6986ee3c7c5 239 display.set_position(1,0);
jah128 2:c6986ee3c7c5 240 display.write_string("COMPLETE");
jah128 2:c6986ee3c7c5 241 wait(1);
jah128 2:c6986ee3c7c5 242 debug("File transfer mode ended\n");
jah128 2:c6986ee3c7c5 243 IF_end_file_transfer_mode();
jah128 2:c6986ee3c7c5 244 }
jah128 1:060690a934a9 245 }
jah128 2:c6986ee3c7c5 246 break;
jah128 1:060690a934a9 247 }
jah128 1:060690a934a9 248 }
jah128 0:d6269d17c8cf 249 }
jah128 0:d6269d17c8cf 250
jah128 0:d6269d17c8cf 251
jah128 0:d6269d17c8cf 252 void IF_handle_user_serial_message(char * message, char length, char interface)
jah128 0:d6269d17c8cf 253 {
jah128 0:d6269d17c8cf 254 char buffer[255];
jah128 0:d6269d17c8cf 255 sprintf(buffer,message,length);
jah128 0:d6269d17c8cf 256 for(int i=0; i<length; i++) {
jah128 0:d6269d17c8cf 257 buffer[i]=message[i];
jah128 0:d6269d17c8cf 258 }
jah128 0:d6269d17c8cf 259 buffer[length]=0;
jah128 0:d6269d17c8cf 260 // if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
jah128 0:d6269d17c8cf 261 // else debug("Received USB message:%s [%d chars]\n",buffer,length);
jah128 0:d6269d17c8cf 262 handle_user_serial_message(message,length,interface);
jah128 0:d6269d17c8cf 263 }
jah128 0:d6269d17c8cf 264
jah128 0:d6269d17c8cf 265 void IF_handle_command_serial_message(char message[3], char interface)
jah128 0:d6269d17c8cf 266 {
jah128 0:d6269d17c8cf 267 char iface [4];
jah128 0:d6269d17c8cf 268 if(interface) strcpy(iface,"BT");
jah128 0:d6269d17c8cf 269 else strcpy(iface,"USB");
jah128 0:d6269d17c8cf 270 char command [26];
jah128 0:d6269d17c8cf 271 char subcommand[30];
jah128 0:d6269d17c8cf 272 float dec;
jah128 0:d6269d17c8cf 273 float l_dec;
jah128 0:d6269d17c8cf 274 float r_dec;
jah128 0:d6269d17c8cf 275 int irp_delay;
jah128 0:d6269d17c8cf 276 char colour_string[7];
jah128 0:d6269d17c8cf 277 char ret_message[50];
jah128 0:d6269d17c8cf 278 char send_message = 0;
jah128 0:d6269d17c8cf 279 char command_status = 0;
jah128 0:d6269d17c8cf 280 // command_status values:
jah128 0:d6269d17c8cf 281 // 0 - unrecognised command
jah128 0:d6269d17c8cf 282 // 1 - command actioned
jah128 0:d6269d17c8cf 283 // 2 - command blocked
jah128 0:d6269d17c8cf 284 // 3 - invalid parameters
jah128 0:d6269d17c8cf 285
jah128 0:d6269d17c8cf 286 subcommand[0]=0;
jah128 0:d6269d17c8cf 287 command[0]=0;
jah128 0:d6269d17c8cf 288 switch(message[0]) {
jah128 0:d6269d17c8cf 289
jah128 0:d6269d17c8cf 290 // MOTOR COMMANDS
jah128 0:d6269d17c8cf 291
jah128 0:d6269d17c8cf 292 case 1:
jah128 0:d6269d17c8cf 293 strcpy(command,"SET LEFT MOTOR");
jah128 0:d6269d17c8cf 294 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 295 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 296 if(allow_commands) {
jah128 0:d6269d17c8cf 297 command_status = 1;
jah128 0:d6269d17c8cf 298 set_left_motor_speed(dec);
jah128 0:d6269d17c8cf 299 } else command_status = 2;
jah128 0:d6269d17c8cf 300 break;
jah128 0:d6269d17c8cf 301 case 2:
jah128 0:d6269d17c8cf 302 strcpy(command,"SET RIGHT MOTOR");
jah128 0:d6269d17c8cf 303 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 304 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 305 if(allow_commands) {
jah128 0:d6269d17c8cf 306 set_right_motor_speed(dec);
jah128 0:d6269d17c8cf 307 command_status = 1;
jah128 0:d6269d17c8cf 308 } else command_status = 2;
jah128 0:d6269d17c8cf 309 break;
jah128 0:d6269d17c8cf 310 case 3:
jah128 0:d6269d17c8cf 311 strcpy(command,"SET BOTH MOTORS");
jah128 0:d6269d17c8cf 312 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 313 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 314 if(allow_commands) {
jah128 0:d6269d17c8cf 315 command_status = 1;
jah128 0:d6269d17c8cf 316 forward(dec);
jah128 0:d6269d17c8cf 317 } else command_status = 2;
jah128 0:d6269d17c8cf 318 break;
jah128 0:d6269d17c8cf 319 case 4:
jah128 0:d6269d17c8cf 320 strcpy(command,"BRAKE LEFT MOTOR");
jah128 0:d6269d17c8cf 321 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 322 if(allow_commands) {
jah128 0:d6269d17c8cf 323 command_status = 1;
jah128 0:d6269d17c8cf 324 brake_left_motor();
jah128 0:d6269d17c8cf 325 } else command_status = 2;
jah128 0:d6269d17c8cf 326 break;
jah128 0:d6269d17c8cf 327 case 5:
jah128 0:d6269d17c8cf 328 strcpy(command,"BRAKE RIGHT MOTOR");
jah128 0:d6269d17c8cf 329 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 330 if(allow_commands) {
jah128 0:d6269d17c8cf 331 command_status = 1;
jah128 0:d6269d17c8cf 332 brake_right_motor();
jah128 0:d6269d17c8cf 333 } else command_status = 2;
jah128 0:d6269d17c8cf 334 break;
jah128 0:d6269d17c8cf 335 case 6:
jah128 0:d6269d17c8cf 336 strcpy(command,"BRAKE BOTH MOTORS");
jah128 0:d6269d17c8cf 337 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 338 if(allow_commands) {
jah128 0:d6269d17c8cf 339 command_status = 1;
jah128 0:d6269d17c8cf 340 brake();
jah128 0:d6269d17c8cf 341 } else command_status = 2;
jah128 0:d6269d17c8cf 342 break;
jah128 0:d6269d17c8cf 343 case 7:
jah128 0:d6269d17c8cf 344 strcpy(command,"STOP BOTH MOTORS");
jah128 0:d6269d17c8cf 345 sprintf(subcommand,"");
jah128 0:d6269d17c8cf 346 if(allow_commands) {
jah128 0:d6269d17c8cf 347 command_status = 1;
jah128 0:d6269d17c8cf 348 stop();
jah128 0:d6269d17c8cf 349 } else command_status = 2;
jah128 0:d6269d17c8cf 350 break;
jah128 0:d6269d17c8cf 351 case 8:
jah128 0:d6269d17c8cf 352 strcpy(command,"TURN ON SPOT");
jah128 0:d6269d17c8cf 353 dec = IF_decode_float(message[1],message[2]);
jah128 0:d6269d17c8cf 354 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 355 if(allow_commands) {
jah128 0:d6269d17c8cf 356 command_status = 1;
jah128 0:d6269d17c8cf 357 turn(dec);
jah128 0:d6269d17c8cf 358 } else command_status = 2;
jah128 0:d6269d17c8cf 359 break;
jah128 0:d6269d17c8cf 360 case 9:
jah128 0:d6269d17c8cf 361 strcpy(command,"SET EACH MOTOR");
jah128 0:d6269d17c8cf 362 l_dec = IF_decode_float(message[1]);
jah128 0:d6269d17c8cf 363 r_dec = IF_decode_float(message[2]);
jah128 0:d6269d17c8cf 364 sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
jah128 0:d6269d17c8cf 365 if(allow_commands) {
jah128 0:d6269d17c8cf 366 command_status = 1;
jah128 0:d6269d17c8cf 367
jah128 0:d6269d17c8cf 368 set_left_motor_speed(l_dec);
jah128 0:d6269d17c8cf 369 set_right_motor_speed(r_dec);
jah128 0:d6269d17c8cf 370 } else command_status = 2;
jah128 0:d6269d17c8cf 371 break;
jah128 0:d6269d17c8cf 372 // LED COMMANDS
jah128 0:d6269d17c8cf 373
jah128 0:d6269d17c8cf 374 case 10:
jah128 0:d6269d17c8cf 375 strcpy(command,"SET LED STATES");
jah128 0:d6269d17c8cf 376 sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2]));
jah128 0:d6269d17c8cf 377 if(allow_commands) {
jah128 0:d6269d17c8cf 378 command_status = 1;
jah128 0:d6269d17c8cf 379 set_leds(message[1],message[2]);
jah128 0:d6269d17c8cf 380 } else command_status = 2;
jah128 0:d6269d17c8cf 381 break;
jah128 0:d6269d17c8cf 382 case 11:
jah128 0:d6269d17c8cf 383 strcpy(command,"SET RED LED STATES");
jah128 0:d6269d17c8cf 384 sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 385 if(allow_commands) {
jah128 0:d6269d17c8cf 386 command_status = 1;
jah128 0:d6269d17c8cf 387 set_red_leds(message[1]);
jah128 0:d6269d17c8cf 388 } else command_status = 2;
jah128 0:d6269d17c8cf 389 break;
jah128 0:d6269d17c8cf 390 case 12:
jah128 0:d6269d17c8cf 391 strcpy(command,"SET GREEN LED STATES");
jah128 0:d6269d17c8cf 392 sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 393 if(allow_commands) {
jah128 0:d6269d17c8cf 394 command_status = 1;
jah128 0:d6269d17c8cf 395 set_green_leds(message[1]);
jah128 0:d6269d17c8cf 396 } else command_status = 2;
jah128 0:d6269d17c8cf 397 break;
jah128 0:d6269d17c8cf 398 case 13:
jah128 0:d6269d17c8cf 399 strcpy(command,"SET LED");
jah128 0:d6269d17c8cf 400 switch(message[2]) {
jah128 0:d6269d17c8cf 401 case 1:
jah128 0:d6269d17c8cf 402 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 403 break;
jah128 0:d6269d17c8cf 404 case 2:
jah128 0:d6269d17c8cf 405 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 406 break;
jah128 0:d6269d17c8cf 407 case 3:
jah128 0:d6269d17c8cf 408 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 409 break;
jah128 0:d6269d17c8cf 410 case 0:
jah128 0:d6269d17c8cf 411 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 412 break;
jah128 0:d6269d17c8cf 413 }
jah128 0:d6269d17c8cf 414 if(message[1] < 8 && message[2] < 4) {
jah128 0:d6269d17c8cf 415 sprintf(subcommand,"%d %s",message[1],colour_string);
jah128 0:d6269d17c8cf 416 if(allow_commands) {
jah128 0:d6269d17c8cf 417 command_status = 1;
jah128 0:d6269d17c8cf 418 set_led(message[1],message[2]);
jah128 0:d6269d17c8cf 419 } else command_status = 2;
jah128 0:d6269d17c8cf 420 } else {
jah128 0:d6269d17c8cf 421 sprintf(subcommand,"[INVALID CODE]");
jah128 0:d6269d17c8cf 422 command_status = 3;
jah128 0:d6269d17c8cf 423 }
jah128 0:d6269d17c8cf 424 break;
jah128 0:d6269d17c8cf 425 case 14:
jah128 0:d6269d17c8cf 426 strcpy(command,"SET CENTER LED STATE");
jah128 0:d6269d17c8cf 427 switch(message[1]) {
jah128 0:d6269d17c8cf 428 case 1:
jah128 0:d6269d17c8cf 429 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 430 break;
jah128 0:d6269d17c8cf 431 case 2:
jah128 0:d6269d17c8cf 432 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 433 break;
jah128 0:d6269d17c8cf 434 case 3:
jah128 0:d6269d17c8cf 435 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 436 break;
jah128 0:d6269d17c8cf 437 case 0:
jah128 0:d6269d17c8cf 438 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 439 break;
jah128 0:d6269d17c8cf 440 }
jah128 0:d6269d17c8cf 441 if(message[1] < 4) {
jah128 0:d6269d17c8cf 442 sprintf(subcommand,"%s",colour_string);
jah128 0:d6269d17c8cf 443 if(allow_commands) {
jah128 0:d6269d17c8cf 444 command_status = 1;
jah128 0:d6269d17c8cf 445 set_center_led(message[1]);
jah128 0:d6269d17c8cf 446 } else command_status = 2;
jah128 0:d6269d17c8cf 447 } else {
jah128 0:d6269d17c8cf 448 sprintf(subcommand,"[INVALID CODE]");
jah128 0:d6269d17c8cf 449 command_status = 3;
jah128 0:d6269d17c8cf 450 }
jah128 0:d6269d17c8cf 451 break;
jah128 0:d6269d17c8cf 452 case 15:
jah128 0:d6269d17c8cf 453 strcpy(command,"SET C.LED BRIGHTNESS");
jah128 0:d6269d17c8cf 454 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 455 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 456 if(allow_commands) {
jah128 0:d6269d17c8cf 457 command_status = 1;
jah128 0:d6269d17c8cf 458 set_center_led_brightness(dec);
jah128 0:d6269d17c8cf 459 } else command_status = 2;
jah128 0:d6269d17c8cf 460 break;
jah128 0:d6269d17c8cf 461 case 16:
jah128 0:d6269d17c8cf 462 strcpy(command,"SET MBED LEDS");
jah128 0:d6269d17c8cf 463 sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1]));
jah128 0:d6269d17c8cf 464 if(allow_commands) {
jah128 0:d6269d17c8cf 465 command_status = 1;
jah128 0:d6269d17c8cf 466 mbed_led1 = (message[1] & 128) >> 7;
jah128 0:d6269d17c8cf 467 mbed_led2 = (message[1] & 64) >> 6;
jah128 0:d6269d17c8cf 468 mbed_led3 = (message[1] & 32) >> 5;
jah128 0:d6269d17c8cf 469 mbed_led4 = (message[1] & 16) >> 4;
jah128 0:d6269d17c8cf 470 } else command_status = 2;
jah128 0:d6269d17c8cf 471 break;
jah128 0:d6269d17c8cf 472 case 17:
jah128 0:d6269d17c8cf 473 strcpy(command,"BLINK OUTER LEDS");
jah128 0:d6269d17c8cf 474 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 475 sprintf(subcommand,"FOR %1.5fS",dec);
jah128 0:d6269d17c8cf 476 if(allow_commands) {
jah128 0:d6269d17c8cf 477 command_status = 1;
jah128 0:d6269d17c8cf 478 blink_leds(dec);
jah128 0:d6269d17c8cf 479 } else command_status = 2;
jah128 0:d6269d17c8cf 480 break;
jah128 0:d6269d17c8cf 481 case 18:
jah128 0:d6269d17c8cf 482 strcpy(command,"SET BASE LED STATE");
jah128 0:d6269d17c8cf 483 switch(message[1]) {
jah128 0:d6269d17c8cf 484 case 1:
jah128 0:d6269d17c8cf 485 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 486 break;
jah128 0:d6269d17c8cf 487 case 0:
jah128 0:d6269d17c8cf 488 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 489 break;
jah128 0:d6269d17c8cf 490 }
jah128 0:d6269d17c8cf 491 //Function not yet implemented
jah128 0:d6269d17c8cf 492 break;
jah128 0:d6269d17c8cf 493 case 19:
jah128 0:d6269d17c8cf 494 strcpy(command,"SET CENTER LED ");
jah128 0:d6269d17c8cf 495 switch(message[1]) {
jah128 0:d6269d17c8cf 496 case 1:
jah128 0:d6269d17c8cf 497 strcpy(colour_string,"RED");
jah128 0:d6269d17c8cf 498 break;
jah128 0:d6269d17c8cf 499 case 2:
jah128 0:d6269d17c8cf 500 strcpy(colour_string,"GREEN");
jah128 0:d6269d17c8cf 501 break;
jah128 0:d6269d17c8cf 502 case 3:
jah128 0:d6269d17c8cf 503 strcpy(colour_string,"BOTH");
jah128 0:d6269d17c8cf 504 break;
jah128 0:d6269d17c8cf 505 case 0:
jah128 0:d6269d17c8cf 506 strcpy(colour_string,"OFF");
jah128 0:d6269d17c8cf 507 break;
jah128 0:d6269d17c8cf 508 }
jah128 0:d6269d17c8cf 509 dec = IF_decode_unsigned_float(message[2]);
jah128 0:d6269d17c8cf 510 sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec);
jah128 0:d6269d17c8cf 511 if(allow_commands) {
jah128 0:d6269d17c8cf 512 command_status = 1;
jah128 0:d6269d17c8cf 513 set_center_led(message[1],dec);
jah128 0:d6269d17c8cf 514 } else command_status = 2;
jah128 0:d6269d17c8cf 515 break;
jah128 0:d6269d17c8cf 516
jah128 0:d6269d17c8cf 517 // DISPLAY COMMANDS
jah128 0:d6269d17c8cf 518
jah128 0:d6269d17c8cf 519 case 20:
jah128 0:d6269d17c8cf 520 strcpy(command,"SET DISPLAY ");
jah128 0:d6269d17c8cf 521 switch(message[1]) {
jah128 0:d6269d17c8cf 522 case 0:
jah128 0:d6269d17c8cf 523 strcpy(subcommand,"CLEAR");
jah128 0:d6269d17c8cf 524 if(allow_commands) {
jah128 0:d6269d17c8cf 525 command_status = 1;
jah128 0:d6269d17c8cf 526 display.clear_display();
jah128 0:d6269d17c8cf 527 } else command_status = 2;
jah128 0:d6269d17c8cf 528 break;
jah128 0:d6269d17c8cf 529 case 1:
jah128 0:d6269d17c8cf 530 strcpy(subcommand,"MESSAGE 1");
jah128 0:d6269d17c8cf 531 if(allow_commands) {
jah128 0:d6269d17c8cf 532 command_status = 1;
jah128 0:d6269d17c8cf 533 display.clear_display();
jah128 0:d6269d17c8cf 534 display.home();
jah128 0:d6269d17c8cf 535 display.write_string("PC CONNECTION");
jah128 0:d6269d17c8cf 536 display.set_position(1,0);
jah128 0:d6269d17c8cf 537 display.write_string("STARTED");
jah128 0:d6269d17c8cf 538 } else command_status = 2;
jah128 0:d6269d17c8cf 539 break;
jah128 0:d6269d17c8cf 540 case 2:
jah128 0:d6269d17c8cf 541 strcpy(subcommand,"MESSAGE 2");
jah128 0:d6269d17c8cf 542 if(allow_commands) {
jah128 0:d6269d17c8cf 543 command_status = 1;
jah128 0:d6269d17c8cf 544 display.clear_display();
jah128 0:d6269d17c8cf 545 display.home();
jah128 0:d6269d17c8cf 546 display.write_string("PC CONNECTION");
jah128 0:d6269d17c8cf 547 display.set_position(1,0);
jah128 0:d6269d17c8cf 548 display.write_string("TERMINATED");
jah128 0:d6269d17c8cf 549 } else command_status = 2;
jah128 0:d6269d17c8cf 550 break;
jah128 0:d6269d17c8cf 551 case 3:
jah128 0:d6269d17c8cf 552 strcpy(subcommand,"MESSAGE 3");
jah128 0:d6269d17c8cf 553 if(allow_commands) {
jah128 0:d6269d17c8cf 554 command_status = 1;
jah128 0:d6269d17c8cf 555 display.clear_display();
jah128 0:d6269d17c8cf 556 display.home();
jah128 0:d6269d17c8cf 557 display.write_string("ANDROID DEVICE");
jah128 0:d6269d17c8cf 558 display.set_position(1,0);
jah128 0:d6269d17c8cf 559 display.write_string("CONNECTED");
jah128 0:d6269d17c8cf 560 } else command_status = 2;
jah128 0:d6269d17c8cf 561 break;
jah128 0:d6269d17c8cf 562 case 4:
jah128 0:d6269d17c8cf 563 strcpy(subcommand,"MESSAGE 4");
jah128 0:d6269d17c8cf 564 if(allow_commands) {
jah128 0:d6269d17c8cf 565 command_status = 1;
jah128 0:d6269d17c8cf 566 display.clear_display();
jah128 0:d6269d17c8cf 567 display.home();
jah128 0:d6269d17c8cf 568 display.write_string("ANDROID DEVICE");
jah128 0:d6269d17c8cf 569 display.set_position(1,0);
jah128 0:d6269d17c8cf 570 display.write_string("DISCONNECTED");
jah128 0:d6269d17c8cf 571 } else command_status = 2;
jah128 0:d6269d17c8cf 572 break;
jah128 0:d6269d17c8cf 573 }
jah128 0:d6269d17c8cf 574 break;
jah128 0:d6269d17c8cf 575 case 21:
jah128 0:d6269d17c8cf 576 strcpy(command,"SET CURSOR ");
jah128 0:d6269d17c8cf 577 if(message[1] < 2 && message[2] < 16) {
jah128 0:d6269d17c8cf 578 sprintf(subcommand,"[%d,%d]",message[1],message[2]);
jah128 0:d6269d17c8cf 579 if(allow_commands) {
jah128 0:d6269d17c8cf 580 display.set_position(message[1],message[2]);
jah128 0:d6269d17c8cf 581 } else command_status = 2;
jah128 0:d6269d17c8cf 582 } else {
jah128 0:d6269d17c8cf 583 sprintf(subcommand,"[INVALID]");
jah128 0:d6269d17c8cf 584 command_status = 3;
jah128 0:d6269d17c8cf 585 }
jah128 0:d6269d17c8cf 586 break;
jah128 0:d6269d17c8cf 587 case 22:
jah128 0:d6269d17c8cf 588 strcpy(command,"PRINT CHARACTERS ");
jah128 0:d6269d17c8cf 589 char print_message[2];
jah128 0:d6269d17c8cf 590 print_message[0]=message[1];
jah128 0:d6269d17c8cf 591 print_message[1]=message[2];
jah128 0:d6269d17c8cf 592 sprintf(subcommand,"[%c,%c]",message[1],message[2]);
jah128 0:d6269d17c8cf 593 if(allow_commands) {
jah128 0:d6269d17c8cf 594 display.write_string(print_message,2);
jah128 0:d6269d17c8cf 595 } else command_status = 2;
jah128 0:d6269d17c8cf 596 break;
jah128 0:d6269d17c8cf 597 case 23:
jah128 0:d6269d17c8cf 598 strcpy(command,"SET DISPLAY B.NESS");
jah128 0:d6269d17c8cf 599 dec = IF_decode_unsigned_float(message[1],message[2]);
jah128 0:d6269d17c8cf 600 sprintf(subcommand,"%1.5f",dec);
jah128 0:d6269d17c8cf 601 if(allow_commands) {
jah128 0:d6269d17c8cf 602 command_status = 1;
jah128 0:d6269d17c8cf 603 display.set_backlight_brightness(dec);
jah128 0:d6269d17c8cf 604 } else command_status = 2;
jah128 0:d6269d17c8cf 605 break;
jah128 0:d6269d17c8cf 606
jah128 0:d6269d17c8cf 607 case 30:
jah128 0:d6269d17c8cf 608 strcpy(command,"SET DEBUG MODE");
jah128 0:d6269d17c8cf 609 switch(message[1]) {
jah128 0:d6269d17c8cf 610 case 1:
jah128 0:d6269d17c8cf 611 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 612 break;
jah128 0:d6269d17c8cf 613 case 0:
jah128 0:d6269d17c8cf 614 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 615 break;
jah128 0:d6269d17c8cf 616 }
jah128 0:d6269d17c8cf 617 if(message[2] & 1) strcat (subcommand,"-PC");
jah128 0:d6269d17c8cf 618 if(message[2] & 2) strcat (subcommand,"-BT");
jah128 0:d6269d17c8cf 619 if(message[2] & 4) strcat (subcommand,"-DISP");
jah128 0:d6269d17c8cf 620 if(allow_commands) {
jah128 0:d6269d17c8cf 621 command_status = 1;
jah128 0:d6269d17c8cf 622 debug_mode = message[1];
jah128 0:d6269d17c8cf 623 debug_output = message[2];
jah128 0:d6269d17c8cf 624 } else command_status = 2;
jah128 0:d6269d17c8cf 625 break;
jah128 0:d6269d17c8cf 626 case 31:
jah128 0:d6269d17c8cf 627 strcpy(command,"SET DEMO MODE");
jah128 0:d6269d17c8cf 628 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 629 case 1:
jah128 0:d6269d17c8cf 630 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 631 break;
jah128 0:d6269d17c8cf 632 case 0:
jah128 0:d6269d17c8cf 633 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 634 break;
jah128 0:d6269d17c8cf 635 }
jah128 0:d6269d17c8cf 636 if(allow_commands) {
jah128 0:d6269d17c8cf 637 command_status = 1;
jah128 0:d6269d17c8cf 638 demo_on = message[1] % 2;
jah128 0:d6269d17c8cf 639 if(demo_on == 1) {
jah128 0:d6269d17c8cf 640 user_code_restore_mode = user_code_running;
jah128 0:d6269d17c8cf 641 user_code_running = 0;
jah128 0:d6269d17c8cf 642 } else {
jah128 0:d6269d17c8cf 643 user_code_running = user_code_restore_mode;
jah128 0:d6269d17c8cf 644 }
jah128 0:d6269d17c8cf 645 } else command_status = 2;
jah128 0:d6269d17c8cf 646 break;
jah128 0:d6269d17c8cf 647 case 32:
jah128 0:d6269d17c8cf 648 strcpy(command,"SET USER CODE");
jah128 0:d6269d17c8cf 649 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 650 case 1:
jah128 0:d6269d17c8cf 651 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 652 break;
jah128 0:d6269d17c8cf 653 case 0:
jah128 0:d6269d17c8cf 654 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 655 break;
jah128 0:d6269d17c8cf 656 }
jah128 0:d6269d17c8cf 657 if(allow_commands) {
jah128 0:d6269d17c8cf 658 command_status = 1;
jah128 0:d6269d17c8cf 659 user_code_running = message[1] % 2;
jah128 0:d6269d17c8cf 660 } else command_status = 2;
jah128 0:d6269d17c8cf 661 break;
jah128 0:d6269d17c8cf 662 case 33:
jah128 0:d6269d17c8cf 663 strcpy(command,"PAUSE USER CODE");
jah128 0:d6269d17c8cf 664 dec = IF_decode_unsigned_float(message[1],message[2]) * 10;
jah128 0:d6269d17c8cf 665 sprintf(subcommand,"FOR %2.3fS",dec);
jah128 0:d6269d17c8cf 666 if(allow_commands) {
jah128 0:d6269d17c8cf 667 command_status = 1;
jah128 0:d6269d17c8cf 668 pause_user_code(dec);
jah128 0:d6269d17c8cf 669 } else command_status = 2;
jah128 0:d6269d17c8cf 670 break;
jah128 0:d6269d17c8cf 671
jah128 0:d6269d17c8cf 672 case 34:
jah128 0:d6269d17c8cf 673 strcpy(command,"RESET ENCODERS");
jah128 0:d6269d17c8cf 674 if(allow_commands) {
jah128 0:d6269d17c8cf 675 command_status = 1;
jah128 0:d6269d17c8cf 676 reset_encoders();
jah128 0:d6269d17c8cf 677 } else command_status = 2;
jah128 0:d6269d17c8cf 678 break;
jah128 0:d6269d17c8cf 679
jah128 0:d6269d17c8cf 680 case 35:
jah128 0:d6269d17c8cf 681 strcpy(command,"SET ALLOW COMMANDS");
jah128 0:d6269d17c8cf 682 switch(message[1] % 2) {
jah128 0:d6269d17c8cf 683 case 1:
jah128 0:d6269d17c8cf 684 strcpy(subcommand,"ON");
jah128 0:d6269d17c8cf 685 break;
jah128 0:d6269d17c8cf 686 case 0:
jah128 0:d6269d17c8cf 687 strcpy(subcommand,"OFF");
jah128 0:d6269d17c8cf 688 break;
jah128 0:d6269d17c8cf 689 }
jah128 0:d6269d17c8cf 690 allow_commands = message[1] % 2;
jah128 0:d6269d17c8cf 691 command_status = 1;
jah128 0:d6269d17c8cf 692 break;
jah128 0:d6269d17c8cf 693
jah128 0:d6269d17c8cf 694 case 36:
jah128 0:d6269d17c8cf 695 irp_delay = (message[1] << 8) + message[2];
jah128 0:d6269d17c8cf 696 sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay);
jah128 0:d6269d17c8cf 697 if(allow_commands) {
jah128 0:d6269d17c8cf 698 command_status = 1;
jah128 0:d6269d17c8cf 699 ir_pulse_delay = irp_delay;
jah128 0:d6269d17c8cf 700 } else command_status = 2;
jah128 0:d6269d17c8cf 701 break;
jah128 0:d6269d17c8cf 702 case 37:
jah128 0:d6269d17c8cf 703 irp_delay = (message[1] << 8) + message[2];
jah128 0:d6269d17c8cf 704 sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay);
jah128 0:d6269d17c8cf 705 if(allow_commands) {
jah128 0:d6269d17c8cf 706 command_status = 1;
jah128 0:d6269d17c8cf 707 base_ir_pulse_delay = irp_delay;
jah128 0:d6269d17c8cf 708 } else command_status = 2;
jah128 0:d6269d17c8cf 709 break;
jah128 0:d6269d17c8cf 710
jah128 0:d6269d17c8cf 711 // MOTOR REQUESTS
jah128 0:d6269d17c8cf 712 case 40:
jah128 0:d6269d17c8cf 713 strcpy(command,"GET LEFT MOTOR SPEED");
jah128 0:d6269d17c8cf 714 sprintf(ret_message,"%1.5f",motor_left_speed);
jah128 0:d6269d17c8cf 715 send_message = 1;
jah128 0:d6269d17c8cf 716 break;
jah128 0:d6269d17c8cf 717
jah128 0:d6269d17c8cf 718 case 41:
jah128 0:d6269d17c8cf 719 strcpy(command,"GET RIGHT MOTOR SPEED");
jah128 0:d6269d17c8cf 720 sprintf(ret_message,"%1.5f",motor_right_speed);
jah128 0:d6269d17c8cf 721 send_message = 1;
jah128 0:d6269d17c8cf 722 break;
jah128 0:d6269d17c8cf 723 case 42:
jah128 0:d6269d17c8cf 724 strcpy(command,"GET BRAKE STATES");
jah128 0:d6269d17c8cf 725 sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
jah128 0:d6269d17c8cf 726 send_message = 1;
jah128 0:d6269d17c8cf 727 break;
jah128 0:d6269d17c8cf 728 case 43:
jah128 0:d6269d17c8cf 729 strcpy(command,"GET MOTOR STATES");
jah128 0:d6269d17c8cf 730 //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
jah128 0:d6269d17c8cf 731 send_message = 1;
jah128 0:d6269d17c8cf 732 break;
jah128 0:d6269d17c8cf 733 case 44:
jah128 0:d6269d17c8cf 734 strcpy(command,"GET ENCODERS");
jah128 0:d6269d17c8cf 735 sprintf(ret_message,"%d,%d",left_encoder,right_encoder);
jah128 0:d6269d17c8cf 736 send_message = 1;
jah128 0:d6269d17c8cf 737 break;
jah128 0:d6269d17c8cf 738
jah128 0:d6269d17c8cf 739 // LED REQUESTS
jah128 0:d6269d17c8cf 740 case 50:
jah128 0:d6269d17c8cf 741 strcpy(command,"GET LED STATES");
jah128 0:d6269d17c8cf 742 sprintf(ret_message,"%04x",get_led_states());
jah128 0:d6269d17c8cf 743 send_message = 1;
jah128 0:d6269d17c8cf 744 break;
jah128 0:d6269d17c8cf 745
jah128 0:d6269d17c8cf 746 // GENERAL REQUESTS
jah128 0:d6269d17c8cf 747 case 60:
jah128 0:d6269d17c8cf 748 strcpy(command,"GET SOFTWARE VERSION");
jah128 0:d6269d17c8cf 749 sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
jah128 0:d6269d17c8cf 750 send_message = 1;
jah128 0:d6269d17c8cf 751 break;
jah128 0:d6269d17c8cf 752
jah128 0:d6269d17c8cf 753 case 61:
jah128 0:d6269d17c8cf 754 strcpy(command,"GET UPTIME");
jah128 0:d6269d17c8cf 755 sprintf(ret_message,"%6.2f",get_uptime());
jah128 0:d6269d17c8cf 756 send_message = 1;
jah128 0:d6269d17c8cf 757 break;
jah128 0:d6269d17c8cf 758
jah128 0:d6269d17c8cf 759 case 62:
jah128 0:d6269d17c8cf 760 strcpy(command,"GET ID");
jah128 0:d6269d17c8cf 761 sprintf(ret_message,"%d",robot_id);
jah128 0:d6269d17c8cf 762 send_message = 1;
jah128 0:d6269d17c8cf 763 break;
jah128 0:d6269d17c8cf 764
jah128 0:d6269d17c8cf 765 case 63:
jah128 0:d6269d17c8cf 766 strcpy(command,"GET SWITCH BYTE");
jah128 0:d6269d17c8cf 767 sprintf(ret_message,"%02x",switch_byte);
jah128 0:d6269d17c8cf 768 send_message = 1;
jah128 0:d6269d17c8cf 769 break;
jah128 0:d6269d17c8cf 770 case 64:
jah128 0:d6269d17c8cf 771 strcpy(command,"GET USER CODE");
jah128 0:d6269d17c8cf 772 sprintf(ret_message,"%d",user_code_running);
jah128 0:d6269d17c8cf 773 send_message = 1;
jah128 0:d6269d17c8cf 774 break;
jah128 0:d6269d17c8cf 775 case 65:
jah128 0:d6269d17c8cf 776 strcpy(command,"GET RESPONSE STRING");
jah128 0:d6269d17c8cf 777 sprintf(ret_message,"PSI");
jah128 0:d6269d17c8cf 778 send_message = 1;
jah128 0:d6269d17c8cf 779 break;
jah128 0:d6269d17c8cf 780 case 66:
jah128 0:d6269d17c8cf 781 strcpy(command,"GET PROGRAM NAME");
jah128 0:d6269d17c8cf 782 sprintf(ret_message,"%s",program_name);
jah128 0:d6269d17c8cf 783 send_message = 1;
jah128 0:d6269d17c8cf 784 break;
jah128 0:d6269d17c8cf 785 case 67:
jah128 0:d6269d17c8cf 786 strcpy(command,"GET AUTHOR NAME");
jah128 0:d6269d17c8cf 787 sprintf(ret_message,"%s",author_name);
jah128 0:d6269d17c8cf 788 send_message = 1;
jah128 0:d6269d17c8cf 789 break;
jah128 0:d6269d17c8cf 790 case 68:
jah128 0:d6269d17c8cf 791 strcpy(command,"GET DEBUG MODE");
jah128 0:d6269d17c8cf 792 sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
jah128 0:d6269d17c8cf 793 send_message = 1;
jah128 0:d6269d17c8cf 794 break;
jah128 0:d6269d17c8cf 795 case 69:
jah128 0:d6269d17c8cf 796 strcpy(command,"GET SYSTEM WARNINGS");
jah128 0:d6269d17c8cf 797 sprintf(ret_message,"%d",system_warnings);
jah128 0:d6269d17c8cf 798 send_message = 1;
jah128 0:d6269d17c8cf 799 break;
jah128 0:d6269d17c8cf 800
jah128 0:d6269d17c8cf 801
jah128 0:d6269d17c8cf 802 // Sensors
jah128 0:d6269d17c8cf 803 case 80:
jah128 0:d6269d17c8cf 804 strcpy(command,"STORE BG. IR VALUES");
jah128 0:d6269d17c8cf 805 if(allow_commands) {
jah128 0:d6269d17c8cf 806 command_status = 1;
jah128 0:d6269d17c8cf 807 store_background_raw_ir_values();
jah128 0:d6269d17c8cf 808 } else command_status = 2;
jah128 0:d6269d17c8cf 809 break;
jah128 0:d6269d17c8cf 810 case 81:
jah128 0:d6269d17c8cf 811 strcpy(command,"STORE IL. IR VALUES");
jah128 0:d6269d17c8cf 812 if(allow_commands) {
jah128 0:d6269d17c8cf 813 command_status = 1;
jah128 0:d6269d17c8cf 814 store_illuminated_raw_ir_values();
jah128 0:d6269d17c8cf 815 } else command_status = 2;
jah128 0:d6269d17c8cf 816 break;
jah128 0:d6269d17c8cf 817 case 82:
jah128 0:d6269d17c8cf 818 strcpy(command,"STORE IR VALUES");
jah128 0:d6269d17c8cf 819 if(allow_commands) {
jah128 0:d6269d17c8cf 820 command_status = 1;
jah128 0:d6269d17c8cf 821 store_ir_values();
jah128 0:d6269d17c8cf 822 } else command_status = 2;
jah128 0:d6269d17c8cf 823 break;
jah128 0:d6269d17c8cf 824 case 83:
jah128 0:d6269d17c8cf 825 strcpy(command,"STORE BG BASE IR VALUES");
jah128 0:d6269d17c8cf 826 if(allow_commands) {
jah128 0:d6269d17c8cf 827 command_status = 1;
jah128 0:d6269d17c8cf 828 store_background_base_ir_values();
jah128 0:d6269d17c8cf 829 } else command_status = 2;
jah128 0:d6269d17c8cf 830 break;
jah128 0:d6269d17c8cf 831 case 84:
jah128 0:d6269d17c8cf 832 strcpy(command,"STORE IL. BASE IR VALUES");
jah128 0:d6269d17c8cf 833 if(allow_commands) {
jah128 0:d6269d17c8cf 834 command_status = 1;
jah128 0:d6269d17c8cf 835 store_illuminated_base_ir_values();
jah128 0:d6269d17c8cf 836 } else command_status = 2;
jah128 0:d6269d17c8cf 837 break;
jah128 0:d6269d17c8cf 838 case 85:
jah128 0:d6269d17c8cf 839 strcpy(command,"STORE BASE IR VALUES");
jah128 0:d6269d17c8cf 840 if(allow_commands) {
jah128 0:d6269d17c8cf 841 command_status = 1;
jah128 0:d6269d17c8cf 842 store_base_ir_values();
jah128 0:d6269d17c8cf 843 } else command_status = 2;
jah128 0:d6269d17c8cf 844 break;
jah128 0:d6269d17c8cf 845 case 86:
jah128 0:d6269d17c8cf 846 strcpy(command,"STORE ALL IR VALUES");
jah128 0:d6269d17c8cf 847 if(allow_commands) {
jah128 0:d6269d17c8cf 848 command_status = 1;
jah128 0:d6269d17c8cf 849 store_ir_values();
jah128 0:d6269d17c8cf 850 store_base_ir_values();
jah128 0:d6269d17c8cf 851 } else command_status = 2;
jah128 0:d6269d17c8cf 852 break;
jah128 0:d6269d17c8cf 853 case 90:
jah128 0:d6269d17c8cf 854 sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
jah128 0:d6269d17c8cf 855 sprintf(ret_message,"%d",get_background_raw_ir_value(message[1]));
jah128 0:d6269d17c8cf 856 send_message = 1;
jah128 0:d6269d17c8cf 857 break;
jah128 0:d6269d17c8cf 858 case 91:
jah128 0:d6269d17c8cf 859 sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
jah128 0:d6269d17c8cf 860 sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1]));
jah128 0:d6269d17c8cf 861 send_message = 1;
jah128 0:d6269d17c8cf 862 break;
jah128 0:d6269d17c8cf 863 case 92:
jah128 0:d6269d17c8cf 864 strcpy(command,"GET BG IR VALUES");
jah128 0:d6269d17c8cf 865 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:d6269d17c8cf 866 send_message = 1;
jah128 0:d6269d17c8cf 867 break;
jah128 0:d6269d17c8cf 868 case 93:
jah128 0:d6269d17c8cf 869 strcpy(command,"GET ILLUMINATED IR VALUES");
jah128 0:d6269d17c8cf 870 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:d6269d17c8cf 871 send_message = 1;
jah128 0:d6269d17c8cf 872 break;
jah128 0:d6269d17c8cf 873 case 94:
jah128 0:d6269d17c8cf 874 sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
jah128 0:d6269d17c8cf 875 sprintf(ret_message,"%d",get_background_base_ir_value(message[1]));
jah128 0:d6269d17c8cf 876 send_message = 1;
jah128 0:d6269d17c8cf 877 break;
jah128 0:d6269d17c8cf 878 case 95:
jah128 0:d6269d17c8cf 879 sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
jah128 0:d6269d17c8cf 880 sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1]));
jah128 0:d6269d17c8cf 881 send_message = 1;
jah128 0:d6269d17c8cf 882 break;
jah128 0:d6269d17c8cf 883 case 96:
jah128 0:d6269d17c8cf 884 strcpy(command,"GET BG BASE IR VALUES");
jah128 0:d6269d17c8cf 885 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:d6269d17c8cf 886 send_message = 1;
jah128 0:d6269d17c8cf 887 break;
jah128 0:d6269d17c8cf 888 case 97:
jah128 0:d6269d17c8cf 889 strcpy(command,"GET IL BASE IR VALUES");
jah128 0:d6269d17c8cf 890 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:d6269d17c8cf 891 send_message = 1;
jah128 0:d6269d17c8cf 892 break;
jah128 2:c6986ee3c7c5 893 case 98:
jah128 2:c6986ee3c7c5 894 strcpy(command,"CALCULATE BASE IR VALUES");
jah128 2:c6986ee3c7c5 895 sprintf(ret_message,"%03X%03X%03X%03X%03X",calculate_base_ir_value(0),calculate_base_ir_value(1),calculate_base_ir_value(2),calculate_base_ir_value(3),calculate_base_ir_value(4));
jah128 2:c6986ee3c7c5 896 send_message = 1;
jah128 2:c6986ee3c7c5 897 break;
jah128 2:c6986ee3c7c5 898 case 99:
jah128 2:c6986ee3c7c5 899 strcpy(command,"CALCULATE SIDE IR VALUES");
jah128 2:c6986ee3c7c5 900 sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",calculate_side_ir_value(0),calculate_side_ir_value(1),calculate_side_ir_value(2),calculate_side_ir_value(3),calculate_side_ir_value(4),calculate_side_ir_value(5),calculate_side_ir_value(6),calculate_side_ir_value(7));
jah128 2:c6986ee3c7c5 901 send_message = 1;
jah128 2:c6986ee3c7c5 902 break;
jah128 0:d6269d17c8cf 903 case 100:
jah128 0:d6269d17c8cf 904 strcpy(command,"START FILE TRANSFER MODE");
jah128 0:d6269d17c8cf 905 if(allow_commands) {
jah128 0:d6269d17c8cf 906 command_status = 1;
jah128 1:060690a934a9 907 IF_start_file_transfer_mode();
jah128 0:d6269d17c8cf 908 sprintf(ret_message,"OK");
jah128 0:d6269d17c8cf 909 send_message = 1;
jah128 0:d6269d17c8cf 910 } else command_status = 2;
jah128 0:d6269d17c8cf 911 break;
jah128 0:d6269d17c8cf 912 }
jah128 0:d6269d17c8cf 913
jah128 0:d6269d17c8cf 914
jah128 0:d6269d17c8cf 915 if(send_message) {
jah128 0:d6269d17c8cf 916 char message_length = strlen(ret_message);
jah128 0:d6269d17c8cf 917 switch(interface) {
jah128 0:d6269d17c8cf 918 case 0:
jah128 0:d6269d17c8cf 919 pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 0:d6269d17c8cf 920 break;
jah128 0:d6269d17c8cf 921 case 1:
jah128 0:d6269d17c8cf 922 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 0:d6269d17c8cf 923 break;
jah128 0:d6269d17c8cf 924 }
jah128 0:d6269d17c8cf 925 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:d6269d17c8cf 926 } else {
jah128 0:d6269d17c8cf 927 switch(interface) {
jah128 0:d6269d17c8cf 928 case 0:
jah128 0:d6269d17c8cf 929 pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 0:d6269d17c8cf 930 break;
jah128 0:d6269d17c8cf 931 case 1:
jah128 0:d6269d17c8cf 932 bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 0:d6269d17c8cf 933 break;
jah128 0:d6269d17c8cf 934 }
jah128 0:d6269d17c8cf 935 switch(command_status) {
jah128 0:d6269d17c8cf 936 case 0:
jah128 0:d6269d17c8cf 937 debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 938 break;
jah128 0:d6269d17c8cf 939 case 1:
jah128 0:d6269d17c8cf 940 debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 941 break;
jah128 0:d6269d17c8cf 942 case 2:
jah128 0:d6269d17c8cf 943 debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 944 break;
jah128 0:d6269d17c8cf 945 case 3:
jah128 0:d6269d17c8cf 946 debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 0:d6269d17c8cf 947 break;
jah128 0:d6269d17c8cf 948 }
jah128 0:d6269d17c8cf 949 }
jah128 0:d6269d17c8cf 950 }
jah128 0:d6269d17c8cf 951
jah128 0:d6269d17c8cf 952 char * IF_nibble_to_binary_char(char in)
jah128 0:d6269d17c8cf 953 {
jah128 0:d6269d17c8cf 954 char * ret = (char*)malloc(sizeof(char)*5);
jah128 0:d6269d17c8cf 955 for(int i=0; i<4; i++) {
jah128 0:d6269d17c8cf 956 if(in & (128 >> i)) ret[i]='1';
jah128 0:d6269d17c8cf 957 else ret[i]='0';
jah128 0:d6269d17c8cf 958 }
jah128 0:d6269d17c8cf 959 ret[4]=0;
jah128 0:d6269d17c8cf 960 return ret;
jah128 0:d6269d17c8cf 961 }
jah128 0:d6269d17c8cf 962
jah128 0:d6269d17c8cf 963 char * IF_char_to_binary_char(char in)
jah128 0:d6269d17c8cf 964 {
jah128 0:d6269d17c8cf 965 char * ret = (char*)malloc(sizeof(char)*9);
jah128 0:d6269d17c8cf 966 for(int i=0; i<8; i++) {
jah128 0:d6269d17c8cf 967 if(in & (128 >> i)) ret[i]='1';
jah128 0:d6269d17c8cf 968 else ret[i]='0';
jah128 0:d6269d17c8cf 969 }
jah128 0:d6269d17c8cf 970 ret[8]=0;
jah128 0:d6269d17c8cf 971 return ret;
jah128 0:d6269d17c8cf 972 }
jah128 0:d6269d17c8cf 973
jah128 0:d6269d17c8cf 974 float IF_decode_unsigned_float(char byte0, char byte1)
jah128 0:d6269d17c8cf 975 {
jah128 0:d6269d17c8cf 976 unsigned short sval = (byte0) << 8;
jah128 0:d6269d17c8cf 977 sval += byte1;
jah128 0:d6269d17c8cf 978 float scaled = sval / 65535.0f;
jah128 0:d6269d17c8cf 979 return scaled;
jah128 0:d6269d17c8cf 980 }
jah128 0:d6269d17c8cf 981
jah128 0:d6269d17c8cf 982 float IF_decode_float(char byte0, char byte1)
jah128 0:d6269d17c8cf 983 {
jah128 0:d6269d17c8cf 984 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 0:d6269d17c8cf 985 char sign = byte0 / 128;
jah128 0:d6269d17c8cf 986 short sval = (byte0 % 128) << 8;
jah128 0:d6269d17c8cf 987 sval += byte1;
jah128 0:d6269d17c8cf 988 float scaled = sval / 32767.0f;
jah128 0:d6269d17c8cf 989 if(sign == 0) scaled = 0-scaled;
jah128 0:d6269d17c8cf 990 return scaled;
jah128 0:d6269d17c8cf 991 }
jah128 0:d6269d17c8cf 992
jah128 0:d6269d17c8cf 993 float IF_decode_unsigned_float(char byte0)
jah128 0:d6269d17c8cf 994 {
jah128 0:d6269d17c8cf 995 unsigned short sval = (byte0);
jah128 0:d6269d17c8cf 996 float scaled = sval / 255.0f;
jah128 0:d6269d17c8cf 997 return scaled;
jah128 0:d6269d17c8cf 998 }
jah128 0:d6269d17c8cf 999
jah128 0:d6269d17c8cf 1000 float IF_decode_float(char byte0)
jah128 0:d6269d17c8cf 1001 {
jah128 0:d6269d17c8cf 1002 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 0:d6269d17c8cf 1003 char sign = byte0 / 128;
jah128 0:d6269d17c8cf 1004 short sval = (byte0 % 128);
jah128 0:d6269d17c8cf 1005 float scaled = sval / 127.0f;
jah128 0:d6269d17c8cf 1006 if(sign == 0) scaled = 0-scaled;
jah128 0:d6269d17c8cf 1007 return scaled;
jah128 0:d6269d17c8cf 1008 }
jah128 0:d6269d17c8cf 1009
jah128 0:d6269d17c8cf 1010 void IF_setup_serial_interfaces()
jah128 0:d6269d17c8cf 1011 {
jah128 0:d6269d17c8cf 1012 if(ENABLE_PC_SERIAL) {
jah128 0:d6269d17c8cf 1013 pc.baud(PC_BAUD);
jah128 0:d6269d17c8cf 1014 pc.attach(&IF_pc_rx_callback, Serial::RxIrq);
jah128 0:d6269d17c8cf 1015 }
jah128 0:d6269d17c8cf 1016 if(ENABLE_BLUETOOTH) {
jah128 0:d6269d17c8cf 1017 bt.baud(BLUETOOTH_BAUD);
jah128 0:d6269d17c8cf 1018 bt.attach(&IF_bt_rx_callback, Serial::RxIrq);
jah128 0:d6269d17c8cf 1019 }
jah128 0:d6269d17c8cf 1020 }
jah128 0:d6269d17c8cf 1021
jah128 0:d6269d17c8cf 1022 void IF_pc_rx_command_timeout()
jah128 0:d6269d17c8cf 1023 {
jah128 0:d6269d17c8cf 1024 char message_array[6];
jah128 0:d6269d17c8cf 1025 char length = 1 + pc_command_message_byte;
jah128 0:d6269d17c8cf 1026 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 1027 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1028 for(int k=0; k<pc_command_message_byte; k++) {
jah128 0:d6269d17c8cf 1029 message_array[k+1] = pc_command_message[k];
jah128 0:d6269d17c8cf 1030 }
jah128 0:d6269d17c8cf 1031 IF_handle_user_serial_message(message_array, length, 0);
jah128 0:d6269d17c8cf 1032 }
jah128 0:d6269d17c8cf 1033
jah128 0:d6269d17c8cf 1034 void IF_bt_rx_command_timeout()
jah128 0:d6269d17c8cf 1035 {
jah128 0:d6269d17c8cf 1036 char message_array[6];
jah128 0:d6269d17c8cf 1037 char length = 1 + bt_command_message_byte;
jah128 0:d6269d17c8cf 1038 bt_command_message_started = 0;
jah128 0:d6269d17c8cf 1039 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1040 for(int k=0; k<bt_command_message_byte; k++) {
jah128 0:d6269d17c8cf 1041 message_array[k+1] = bt_command_message[k];
jah128 0:d6269d17c8cf 1042 }
jah128 0:d6269d17c8cf 1043 IF_handle_user_serial_message(message_array, length, 1);
jah128 0:d6269d17c8cf 1044 }
jah128 0:d6269d17c8cf 1045
jah128 0:d6269d17c8cf 1046 void IF_pc_rx_callback()
jah128 0:d6269d17c8cf 1047 {
jah128 0:d6269d17c8cf 1048 int count = 0;
jah128 0:d6269d17c8cf 1049 char message_array[255];
jah128 0:d6269d17c8cf 1050
jah128 0:d6269d17c8cf 1051 while(pc.readable()) {
jah128 0:d6269d17c8cf 1052 char tc = pc.getc();
jah128 0:d6269d17c8cf 1053 message_array[count] = tc;
jah128 0:d6269d17c8cf 1054 count ++;
jah128 0:d6269d17c8cf 1055 if(pc_command_message_started == 1) {
jah128 0:d6269d17c8cf 1056 if(pc_command_message_byte == 3) {
jah128 0:d6269d17c8cf 1057 pc_command_timeout.detach();
jah128 0:d6269d17c8cf 1058 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1059 // A complete command message succesfully received, call handler
jah128 0:d6269d17c8cf 1060 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 1061 count = 0;
jah128 0:d6269d17c8cf 1062 IF_handle_command_serial_message(pc_command_message , 0);
jah128 0:d6269d17c8cf 1063 } else {
jah128 0:d6269d17c8cf 1064 // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
jah128 0:d6269d17c8cf 1065 pc_command_message_started = 0;
jah128 0:d6269d17c8cf 1066 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1067 message_array[1] = pc_command_message[0];
jah128 0:d6269d17c8cf 1068 message_array[2] = pc_command_message[1];
jah128 0:d6269d17c8cf 1069 message_array[3] = pc_command_message[2];
jah128 0:d6269d17c8cf 1070 message_array[4] = tc;
jah128 0:d6269d17c8cf 1071 count = 5;
jah128 0:d6269d17c8cf 1072 }
jah128 0:d6269d17c8cf 1073 } else {
jah128 0:d6269d17c8cf 1074 pc_command_message[pc_command_message_byte] = tc;
jah128 0:d6269d17c8cf 1075 pc_command_message_byte ++;
jah128 0:d6269d17c8cf 1076 }
jah128 0:d6269d17c8cf 1077 } else {
jah128 0:d6269d17c8cf 1078 if(count == 1) {
jah128 0:d6269d17c8cf 1079 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1080 pc_command_timeout.attach(&IF_pc_rx_command_timeout,command_timeout_period);
jah128 0:d6269d17c8cf 1081 pc_command_message_started = 1;
jah128 0:d6269d17c8cf 1082 pc_command_message_byte = 0;
jah128 0:d6269d17c8cf 1083
jah128 0:d6269d17c8cf 1084 }
jah128 0:d6269d17c8cf 1085 }
jah128 0:d6269d17c8cf 1086 }
jah128 0:d6269d17c8cf 1087 }
jah128 0:d6269d17c8cf 1088 if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0);
jah128 0:d6269d17c8cf 1089 }
jah128 0:d6269d17c8cf 1090
jah128 0:d6269d17c8cf 1091 Timeout bt_message_timeout;
jah128 0:d6269d17c8cf 1092 static float bt_message_timeout_period = 0.001; // 1 millisecond
jah128 0:d6269d17c8cf 1093 char bt_buffer[255];
jah128 0:d6269d17c8cf 1094 int bt_buffer_index = 0;
jah128 0:d6269d17c8cf 1095
jah128 0:d6269d17c8cf 1096 void IF_bt_message_timeout()
jah128 0:d6269d17c8cf 1097 {
jah128 0:d6269d17c8cf 1098 char buffer[255];
jah128 0:d6269d17c8cf 1099
jah128 0:d6269d17c8cf 1100 sprintf(buffer, bt_buffer, bt_buffer_index);
jah128 0:d6269d17c8cf 1101 buffer[bt_buffer_index] = 0;
jah128 0:d6269d17c8cf 1102 if(file_transfer_mode == 1) {
jah128 0:d6269d17c8cf 1103 IF_handle_file_transfer_serial_message(bt_buffer, bt_buffer_index, 1);
jah128 0:d6269d17c8cf 1104 } else {
jah128 0:d6269d17c8cf 1105 // debug("BT message timeout: %s [%d chars]\n", buffer, bt_buffer_index);
jah128 0:d6269d17c8cf 1106 if(bt_buffer_index == 5 && buffer[0] == COMMAND_MESSAGE_BYTE && buffer[4] == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1107 bt_command_message[0] = buffer[1];
jah128 0:d6269d17c8cf 1108 bt_command_message[1] = buffer[2];
jah128 0:d6269d17c8cf 1109 bt_command_message[2] = buffer[3];
jah128 0:d6269d17c8cf 1110 IF_handle_command_serial_message(bt_command_message , 1);
jah128 0:d6269d17c8cf 1111 } else IF_handle_user_serial_message(bt_buffer, bt_buffer_index, 1);
jah128 0:d6269d17c8cf 1112 }
jah128 0:d6269d17c8cf 1113 bt_buffer_index = 0;
jah128 0:d6269d17c8cf 1114 }
jah128 0:d6269d17c8cf 1115
jah128 0:d6269d17c8cf 1116 void IF_bt_rx_callback()
jah128 0:d6269d17c8cf 1117 {
jah128 0:d6269d17c8cf 1118 while(bt.readable()) {
jah128 0:d6269d17c8cf 1119 char byte = bt.getc();
jah128 0:d6269d17c8cf 1120
jah128 0:d6269d17c8cf 1121 bt_buffer[bt_buffer_index] = byte;
jah128 0:d6269d17c8cf 1122 bt_buffer_index++;
jah128 0:d6269d17c8cf 1123 }
jah128 0:d6269d17c8cf 1124
jah128 0:d6269d17c8cf 1125 bt_message_timeout.attach(&IF_bt_message_timeout, bt_message_timeout_period);
jah128 0:d6269d17c8cf 1126 }
jah128 0:d6269d17c8cf 1127
jah128 2:c6986ee3c7c5 1128 void IF_set_filename(char * filename_in){
jah128 2:c6986ee3c7c5 1129 strcpy(filename,filename_in);
jah128 2:c6986ee3c7c5 1130 }
jah128 2:c6986ee3c7c5 1131
jah128 2:c6986ee3c7c5 1132 unsigned short IF_calculateCRC16(int file_length){
jah128 2:c6986ee3c7c5 1133 unsigned short crc16table[256] = {
jah128 2:c6986ee3c7c5 1134 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
jah128 2:c6986ee3c7c5 1135 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
jah128 2:c6986ee3c7c5 1136 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
jah128 2:c6986ee3c7c5 1137 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
jah128 2:c6986ee3c7c5 1138 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
jah128 2:c6986ee3c7c5 1139 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
jah128 2:c6986ee3c7c5 1140 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
jah128 2:c6986ee3c7c5 1141 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
jah128 2:c6986ee3c7c5 1142 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
jah128 2:c6986ee3c7c5 1143 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
jah128 2:c6986ee3c7c5 1144 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
jah128 2:c6986ee3c7c5 1145 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
jah128 2:c6986ee3c7c5 1146 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
jah128 2:c6986ee3c7c5 1147 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
jah128 2:c6986ee3c7c5 1148 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
jah128 2:c6986ee3c7c5 1149 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
jah128 2:c6986ee3c7c5 1150 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
jah128 2:c6986ee3c7c5 1151 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
jah128 2:c6986ee3c7c5 1152 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
jah128 2:c6986ee3c7c5 1153 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
jah128 2:c6986ee3c7c5 1154 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
jah128 2:c6986ee3c7c5 1155 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
jah128 2:c6986ee3c7c5 1156 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
jah128 2:c6986ee3c7c5 1157 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
jah128 2:c6986ee3c7c5 1158 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
jah128 2:c6986ee3c7c5 1159 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
jah128 2:c6986ee3c7c5 1160 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
jah128 2:c6986ee3c7c5 1161 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
jah128 2:c6986ee3c7c5 1162 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
jah128 2:c6986ee3c7c5 1163 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
jah128 2:c6986ee3c7c5 1164 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
jah128 2:c6986ee3c7c5 1165 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};
jah128 2:c6986ee3c7c5 1166
jah128 2:c6986ee3c7c5 1167 //Opens, reads and calculates the CRC16 value for file pointed to by filename
jah128 2:c6986ee3c7c5 1168 unsigned short crc_value = 0;
jah128 2:c6986ee3c7c5 1169 FILE *fp = fopen(filename,"r");
jah128 2:c6986ee3c7c5 1170 char * buffer;
jah128 2:c6986ee3c7c5 1171 int limit = 512;
jah128 2:c6986ee3c7c5 1172 if(file_length < 512) limit = file_length;
jah128 2:c6986ee3c7c5 1173 buffer = (char*) malloc (sizeof(char)*limit);
jah128 2:c6986ee3c7c5 1174 int blocks = 1;
jah128 2:c6986ee3c7c5 1175 if(file_length > limit) blocks += file_length / limit;
jah128 2:c6986ee3c7c5 1176 for(int i=0;i<blocks;i++){
jah128 2:c6986ee3c7c5 1177 //Determine size of this block
jah128 2:c6986ee3c7c5 1178 int blocksize = limit;
jah128 2:c6986ee3c7c5 1179 if(i == blocks-1){
jah128 2:c6986ee3c7c5 1180 if((file_length % limit) != 0) blocksize = file_length % limit;
jah128 2:c6986ee3c7c5 1181 }
jah128 2:c6986ee3c7c5 1182 debug("Calculating %d bytes of CRC data...\n",blocksize);
jah128 2:c6986ee3c7c5 1183 int result;
jah128 2:c6986ee3c7c5 1184 result = fread(buffer,1,blocksize,fp);
jah128 2:c6986ee3c7c5 1185 debug("Data read: %d\n",result);
jah128 2:c6986ee3c7c5 1186 for(int j=0;j<blocksize;j++){
jah128 2:c6986ee3c7c5 1187 int subindex = ((crc_value>>8)^*(char *)(buffer[j]))&0x00FF;
jah128 2:c6986ee3c7c5 1188 //debug("J:%d Subindex:%d\n",j,subindex);
jah128 2:c6986ee3c7c5 1189 unsigned short table_value = crc16table[subindex];
jah128 2:c6986ee3c7c5 1190 crc_value=(crc_value<<8)^table_value;
jah128 2:c6986ee3c7c5 1191 }
jah128 2:c6986ee3c7c5 1192 }
jah128 2:c6986ee3c7c5 1193 fclose(fp);
jah128 2:c6986ee3c7c5 1194 debug("CRC Calculated: %x\n",crc_value);
jah128 2:c6986ee3c7c5 1195 return crc_value;
jah128 2:c6986ee3c7c5 1196 }
jah128 2:c6986ee3c7c5 1197
jah128 0:d6269d17c8cf 1198 //void IF_bt_rx_callback()
jah128 0:d6269d17c8cf 1199 //{
jah128 0:d6269d17c8cf 1200 // int count = 0;
jah128 0:d6269d17c8cf 1201 // char message_array[255];
jah128 0:d6269d17c8cf 1202 //
jah128 0:d6269d17c8cf 1203 // wait_ms(500); // Wait 0.5ms to allow a complete message to arrive before atttempting to process it
jah128 0:d6269d17c8cf 1204 //
jah128 0:d6269d17c8cf 1205 // while(bt.readable()) {
jah128 0:d6269d17c8cf 1206 // char tc = bt.getc();
jah128 0:d6269d17c8cf 1207 // message_array[count] = tc;
jah128 0:d6269d17c8cf 1208 // count ++;
jah128 0:d6269d17c8cf 1209 // if(bt_command_message_started == 1) {
jah128 0:d6269d17c8cf 1210 // if(bt_command_message_byte == 3) {
jah128 0:d6269d17c8cf 1211 // bt_command_timeout.detach();
jah128 0:d6269d17c8cf 1212 // if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1213 // // A complete command message succesfully received, call handler
jah128 0:d6269d17c8cf 1214 // bt_command_message_started = 0;
jah128 0:d6269d17c8cf 1215 // count = 0;
jah128 0:d6269d17c8cf 1216 // IF_handle_command_serial_message(bt_command_message , 1);
jah128 0:d6269d17c8cf 1217 // } else {
jah128 0:d6269d17c8cf 1218 // // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
jah128 0:d6269d17c8cf 1219 // bt_command_message_started = 0;
jah128 0:d6269d17c8cf 1220 // message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 0:d6269d17c8cf 1221 // message_array[1] = bt_command_message[0];
jah128 0:d6269d17c8cf 1222 // message_array[2] = bt_command_message[1];
jah128 0:d6269d17c8cf 1223 // message_array[3] = bt_command_message[2];
jah128 0:d6269d17c8cf 1224 // message_array[4] = tc;
jah128 0:d6269d17c8cf 1225 // count = 5;
jah128 0:d6269d17c8cf 1226 // }
jah128 0:d6269d17c8cf 1227 // } else {
jah128 0:d6269d17c8cf 1228 // bt_command_timeout.attach(&IF_bt_rx_command_timeout,command_timeout_period);
jah128 0:d6269d17c8cf 1229 // bt_command_message[bt_command_message_byte] = tc;
jah128 0:d6269d17c8cf 1230 // bt_command_message_byte ++;
jah128 0:d6269d17c8cf 1231 // }
jah128 0:d6269d17c8cf 1232 // } else {
jah128 0:d6269d17c8cf 1233 // if(count == 1) {
jah128 0:d6269d17c8cf 1234 // if(tc == COMMAND_MESSAGE_BYTE) {
jah128 0:d6269d17c8cf 1235 // bt_command_message_started = 1;
jah128 0:d6269d17c8cf 1236 // bt_command_message_byte = 0;
jah128 0:d6269d17c8cf 1237 //
jah128 0:d6269d17c8cf 1238 // }
jah128 0:d6269d17c8cf 1239 // }
jah128 0:d6269d17c8cf 1240 // }
jah128 0:d6269d17c8cf 1241 // }
jah128 0:d6269d17c8cf 1242 // if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1);
jah128 0:d6269d17c8cf 1243 //}