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