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