Library for the PsiSwarm Robot - Version 0.4
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?
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 | 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 | //} |