![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
PsiSwarm/serial.cpp
- Committer:
- jah128
- Date:
- 2015-10-03
- Revision:
- 0:8a5497a2e366
- Child:
- 6:ff3c66f7372b
File content as of revision 0:8a5497a2e366:
/* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File * * File: serial.cpp * * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York * * PsiSwarm Library Version: 0.2 * * September 2015 * */ #include "psiswarm.h" static float command_timeout_period = 0.1f; //If a complete command message is not received in 0.1s then consider it a user message char pc_command_message_started = 0; char pc_command_message_byte = 0; char pc_command_message[3]; char bt_command_message_started = 0; char bt_command_message_byte = 0; char bt_command_message[3]; char allow_commands = 1; char allow_requests = 1; Timeout pc_command_timeout; Timeout bt_command_timeout; // A predefined message structure for command messages is as follows: // [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4] // Byte 0 and Byte 4 must be equal to COMMAND_MESSAGE_BYTE [in psiswarm.h] or message is treated as a user message void handle_user_serial_message(char * message, char length, char interface) { //This is where user code for handling a (non-system) serial message should go //By default, nothing is done // //message = pointer to message char array //length = length of message //interface = 0 for PC serial connection, 1 for Bluetooth } void IF_handle_user_serial_message(char * message, char length, char interface) { char buffer[255]; sprintf(buffer,message,length); for(int i=0; i<length; i++) { buffer[i]=message[i]; } buffer[length]=0; if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length); else debug("Received USB message:%s [%d chars]\n",buffer,length); handle_user_serial_message(message,length,interface); } void IF_handle_command_serial_message(char message[3], char interface) { char iface [4]; if(interface) strcpy(iface,"BT"); else strcpy(iface,"USB"); char command [26]; char subcommand[30]; float dec; float l_dec; float r_dec; int irp_delay; char colour_string[7]; char ret_message[50]; char send_message = 0; char command_status = 0; // command_status values: // 0 - unrecognised command // 1 - command actioned // 2 - command blocked // 3 - invalid parameters subcommand[0]=0; command[0]=0; switch(message[0]) { // MOTOR COMMANDS case 1: strcpy(command,"SET LEFT MOTOR"); dec = IF_decode_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { command_status = 1; set_left_motor_speed(dec); } else command_status = 2; break; case 2: strcpy(command,"SET RIGHT MOTOR"); dec = IF_decode_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { set_right_motor_speed(dec); command_status = 1; } else command_status = 2; break; case 3: strcpy(command,"SET BOTH MOTORS"); dec = IF_decode_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { command_status = 1; forward(dec); } else command_status = 2; break; case 4: strcpy(command,"BRAKE LEFT MOTOR"); sprintf(subcommand,""); if(allow_commands) { command_status = 1; brake_left_motor(); } else command_status = 2; break; case 5: strcpy(command,"BRAKE RIGHT MOTOR"); sprintf(subcommand,""); if(allow_commands) { command_status = 1; brake_right_motor(); } else command_status = 2; break; case 6: strcpy(command,"BRAKE BOTH MOTORS"); sprintf(subcommand,""); if(allow_commands) { command_status = 1; brake(); } else command_status = 2; break; case 7: strcpy(command,"STOP BOTH MOTORS"); sprintf(subcommand,""); if(allow_commands) { command_status = 1; stop(); } else command_status = 2; break; case 8: strcpy(command,"TURN ON SPOT"); dec = IF_decode_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { command_status = 1; turn(dec); } else command_status = 2; break; case 9: strcpy(command,"SET EACH MOTOR"); l_dec = IF_decode_float(message[1]); r_dec = IF_decode_float(message[2]); sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec); if(allow_commands) { command_status = 1; set_left_motor_speed(l_dec); set_right_motor_speed(r_dec); } else command_status = 2; break; // LED COMMANDS case 10: strcpy(command,"SET LED STATES"); sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2])); if(allow_commands) { command_status = 1; set_leds(message[1],message[2]); } else command_status = 2; break; case 11: strcpy(command,"SET RED LED STATES"); sprintf(subcommand,"%s",IF_char_to_binary_char(message[1])); if(allow_commands) { command_status = 1; set_red_leds(message[1]); } else command_status = 2; break; case 12: strcpy(command,"SET GREEN LED STATES"); sprintf(subcommand,"%s",IF_char_to_binary_char(message[1])); if(allow_commands) { command_status = 1; set_green_leds(message[1]); } else command_status = 2; break; case 13: strcpy(command,"SET LED"); switch(message[2]) { case 1: strcpy(colour_string,"RED"); break; case 2: strcpy(colour_string,"GREEN"); break; case 3: strcpy(colour_string,"BOTH"); break; case 0: strcpy(colour_string,"OFF"); break; } if(message[1] < 8 && message[2] < 4) { sprintf(subcommand,"%d %s",message[1],colour_string); if(allow_commands) { command_status = 1; set_led(message[1],message[2]); } else command_status = 2; } else { sprintf(subcommand,"[INVALID CODE]"); command_status = 3; } break; case 14: strcpy(command,"SET CENTER LED STATE"); switch(message[1]) { case 1: strcpy(colour_string,"RED"); break; case 2: strcpy(colour_string,"GREEN"); break; case 3: strcpy(colour_string,"BOTH"); break; case 0: strcpy(colour_string,"OFF"); break; } if(message[1] < 4) { sprintf(subcommand,"%s",colour_string); if(allow_commands) { command_status = 1; set_center_led(message[1]); } else command_status = 2; } else { sprintf(subcommand,"[INVALID CODE]"); command_status = 3; } break; case 15: strcpy(command,"SET C.LED BRIGHTNESS"); dec = IF_decode_unsigned_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { command_status = 1; set_center_led_brightness(dec); } else command_status = 2; break; case 16: strcpy(command,"SET MBED LEDS"); sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1])); if(allow_commands) { command_status = 1; mbed_led1 = (message[1] & 128) >> 7; mbed_led2 = (message[1] & 64) >> 6; mbed_led3 = (message[1] & 32) >> 5; mbed_led4 = (message[1] & 16) >> 4; } else command_status = 2; break; case 17: strcpy(command,"BLINK OUTER LEDS"); dec = IF_decode_unsigned_float(message[1],message[2]); sprintf(subcommand,"FOR %1.5fS",dec); if(allow_commands) { command_status = 1; blink_leds(dec); } else command_status = 2; break; case 18: strcpy(command,"SET BASE LED STATE"); switch(message[1]) { case 1: strcpy(subcommand,"ON"); break; case 0: strcpy(subcommand,"OFF"); break; } //Function not yet implemented break; case 19: strcpy(command,"SET CENTER LED "); switch(message[1]) { case 1: strcpy(colour_string,"RED"); break; case 2: strcpy(colour_string,"GREEN"); break; case 3: strcpy(colour_string,"BOTH"); break; case 0: strcpy(colour_string,"OFF"); break; } dec = IF_decode_unsigned_float(message[2]); sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec); if(allow_commands) { command_status = 1; set_center_led(message[1],dec); } else command_status = 2; break; // DISPLAY COMMANDS case 20: strcpy(command,"SET DISPLAY "); switch(message[1]) { case 0: strcpy(subcommand,"CLEAR"); if(allow_commands) { command_status = 1; display.clear_display(); } else command_status = 2; break; case 1: strcpy(subcommand,"MESSAGE 1"); if(allow_commands) { command_status = 1; display.clear_display(); display.home(); display.write_string("PC CONNECTION"); display.set_position(1,0); display.write_string("STARTED"); } else command_status = 2; break; case 2: strcpy(subcommand,"MESSAGE 2"); if(allow_commands) { command_status = 1; display.clear_display(); display.home(); display.write_string("PC CONNECTION"); display.set_position(1,0); display.write_string("TERMINATED"); } else command_status = 2; break; case 3: strcpy(subcommand,"MESSAGE 3"); if(allow_commands) { command_status = 1; display.clear_display(); display.home(); display.write_string("ANDROID DEVICE"); display.set_position(1,0); display.write_string("CONNECTED"); } else command_status = 2; break; case 4: strcpy(subcommand,"MESSAGE 4"); if(allow_commands) { command_status = 1; display.clear_display(); display.home(); display.write_string("ANDROID DEVICE"); display.set_position(1,0); display.write_string("DISCONNECTED"); } else command_status = 2; break; } break; case 21: strcpy(command,"SET CURSOR "); if(message[1] < 2 && message[2] < 16) { sprintf(subcommand,"[%d,%d]",message[1],message[2]); if(allow_commands) { display.set_position(message[1],message[2]); } else command_status = 2; } else { sprintf(subcommand,"[INVALID]"); command_status = 3; } break; case 22: strcpy(command,"PRINT CHARACTERS "); char print_message[2]; print_message[0]=message[1]; print_message[1]=message[2]; sprintf(subcommand,"[%c,%c]",message[1],message[2]); if(allow_commands) { display.write_string(print_message,2); } else command_status = 2; break; case 23: strcpy(command,"SET DISPLAY B.NESS"); dec = IF_decode_unsigned_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { command_status = 1; display.set_backlight_brightness(dec); } else command_status = 2; break; case 30: strcpy(command,"SET DEBUG MODE"); switch(message[1]) { case 1: strcpy(subcommand,"ON"); break; case 0: strcpy(subcommand,"OFF"); break; } if(message[2] & 1) strcat (subcommand,"-PC"); if(message[2] & 2) strcat (subcommand,"-BT"); if(message[2] & 4) strcat (subcommand,"-DISP"); if(allow_commands) { command_status = 1; debug_mode = message[1]; debug_output = message[2]; } else command_status = 2; break; case 31: strcpy(command,"SET DEMO MODE"); switch(message[1] % 2) { case 1: strcpy(subcommand,"ON"); break; case 0: strcpy(subcommand,"OFF"); break; } if(allow_commands) { command_status = 1; demo_on = message[1] % 2; if(demo_on == 1) { user_code_restore_mode = user_code_running; user_code_running = 0; } else { user_code_running = user_code_restore_mode; } } else command_status = 2; break; case 32: strcpy(command,"SET USER CODE"); switch(message[1] % 2) { case 1: strcpy(subcommand,"ON"); break; case 0: strcpy(subcommand,"OFF"); break; } if(allow_commands) { command_status = 1; user_code_running = message[1] % 2; } else command_status = 2; break; case 33: strcpy(command,"PAUSE USER CODE"); dec = IF_decode_unsigned_float(message[1],message[2]) * 10; sprintf(subcommand,"FOR %2.3fS",dec); if(allow_commands) { command_status = 1; pause_user_code(dec); } else command_status = 2; break; case 34: strcpy(command,"RESET ENCODERS"); if(allow_commands) { command_status = 1; reset_encoders(); } else command_status = 2; break; case 35: strcpy(command,"SET ALLOW COMMANDS"); switch(message[1] % 2) { case 1: strcpy(subcommand,"ON"); break; case 0: strcpy(subcommand,"OFF"); break; } allow_commands = message[1] % 2; command_status = 1; break; case 36: irp_delay = (message[1] << 8) + message[2]; sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay); if(allow_commands) { command_status = 1; ir_pulse_delay = irp_delay; } else command_status = 2; break; case 37: irp_delay = (message[1] << 8) + message[2]; sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay); if(allow_commands) { command_status = 1; base_ir_pulse_delay = irp_delay; } else command_status = 2; break; // MOTOR REQUESTS case 40: strcpy(command,"GET LEFT MOTOR SPEED"); sprintf(ret_message,"%1.5f",motor_left_speed); send_message = 1; break; case 41: strcpy(command,"GET RIGHT MOTOR SPEED"); sprintf(ret_message,"%1.5f",motor_right_speed); send_message = 1; break; case 42: strcpy(command,"GET BRAKE STATES"); sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake); send_message = 1; break; case 43: strcpy(command,"GET MOTOR STATES"); //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake); send_message = 1; break; case 44: strcpy(command,"GET ENCODERS"); sprintf(ret_message,"%d,%d",left_encoder,right_encoder); send_message = 1; break; // LED REQUESTS case 50: strcpy(command,"GET LED STATES"); sprintf(ret_message,"%04x",get_led_states()); send_message = 1; break; // GENERAL REQUESTS case 60: strcpy(command,"GET SOFTWARE VERSION"); sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE); send_message = 1; break; case 61: strcpy(command,"GET UPTIME"); sprintf(ret_message,"%6.2f",get_uptime()); send_message = 1; break; case 62: strcpy(command,"GET ID"); sprintf(ret_message,"%d",robot_id); send_message = 1; break; case 63: strcpy(command,"GET SWITCH BYTE"); sprintf(ret_message,"%02x",switch_byte); send_message = 1; break; case 64: strcpy(command,"GET USER CODE"); sprintf(ret_message,"%d",user_code_running); send_message = 1; break; case 65: strcpy(command,"GET RESPONSE STRING"); sprintf(ret_message,"PSI"); send_message = 1; break; case 66: strcpy(command,"GET PROGRAM NAME"); sprintf(ret_message,"%s",program_name); send_message = 1; break; case 67: strcpy(command,"GET AUTHOR NAME"); sprintf(ret_message,"%s",author_name); send_message = 1; break; case 68: strcpy(command,"GET DEBUG MODE"); sprintf(ret_message,"%1d%1d",debug_mode,debug_output); send_message = 1; break; case 69: strcpy(command,"GET SYSTEM WARNINGS"); sprintf(ret_message,"%d",system_warnings); send_message = 1; break; // Sensors case 80: strcpy(command,"STORE BG. IR VALUES"); if(allow_commands) { command_status = 1; store_background_raw_ir_values(); } else command_status = 2; break; case 81: strcpy(command,"STORE IL. IR VALUES"); if(allow_commands) { command_status = 1; store_illuminated_raw_ir_values(); } else command_status = 2; break; case 82: strcpy(command,"STORE IR VALUES"); if(allow_commands) { command_status = 1; store_ir_values(); } else command_status = 2; break; case 83: strcpy(command,"STORE BG BASE IR VALUES"); if(allow_commands) { command_status = 1; store_background_base_ir_values(); } else command_status = 2; break; case 84: strcpy(command,"STORE IL. BASE IR VALUES"); if(allow_commands) { command_status = 1; store_illuminated_base_ir_values(); } else command_status = 2; break; case 85: strcpy(command,"STORE BASE IR VALUES"); if(allow_commands) { command_status = 1; store_base_ir_values(); } else command_status = 2; break; case 86: strcpy(command,"STORE ALL IR VALUES"); if(allow_commands) { command_status = 1; store_ir_values(); store_base_ir_values(); } else command_status = 2; break; case 90: sprintf(command,"%s %d","GET BG IR VALUE",message[1]); sprintf(ret_message,"%d",get_background_raw_ir_value(message[1])); send_message = 1; break; case 91: sprintf(command,"%s %d","GET IL IR VALUE",message[1]); sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1])); send_message = 1; break; case 92: strcpy(command,"GET BG IR VALUES"); 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)); send_message = 1; break; case 93: strcpy(command,"GET ILLUMINATED IR VALUES"); 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)); send_message = 1; break; case 94: sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]); sprintf(ret_message,"%d",get_background_base_ir_value(message[1])); send_message = 1; break; case 95: sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]); sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1])); send_message = 1; break; case 96: strcpy(command,"GET BG BASE IR VALUES"); 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)); send_message = 1; break; case 97: strcpy(command,"GET IL BASE IR VALUES"); 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)); send_message = 1; break; } if(send_message) { char message_length = strlen(ret_message); switch(interface) { case 0: pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message); break; case 1: bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message); break; } 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); } else { switch(interface) { case 0: pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status); break; case 1: bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status); break; } switch(command_status) { case 0: debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]); break; case 1: debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]); break; case 2: debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]); break; case 3: debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]); break; } } } char * IF_nibble_to_binary_char(char in) { char * ret = (char*)malloc(sizeof(char)*5); for(int i=0; i<4; i++) { if(in & (128 >> i)) ret[i]='1'; else ret[i]='0'; } ret[4]=0; return ret; } char * IF_char_to_binary_char(char in) { char * ret = (char*)malloc(sizeof(char)*9); for(int i=0; i<8; i++) { if(in & (128 >> i)) ret[i]='1'; else ret[i]='0'; } ret[8]=0; return ret; } float IF_decode_unsigned_float(char byte0, char byte1) { unsigned short sval = (byte0) << 8; sval += byte1; float scaled = sval / 65535.0f; return scaled; } float IF_decode_float(char byte0, char byte1) { // MSB is byte 0 is sign, rest is linear spread between 0 and 1 char sign = byte0 / 128; short sval = (byte0 % 128) << 8; sval += byte1; float scaled = sval / 32767.0f; if(sign == 0) scaled = 0-scaled; return scaled; } float IF_decode_unsigned_float(char byte0) { unsigned short sval = (byte0); float scaled = sval / 255.0f; return scaled; } float IF_decode_float(char byte0) { // MSB is byte 0 is sign, rest is linear spread between 0 and 1 char sign = byte0 / 128; short sval = (byte0 % 128); float scaled = sval / 127.0f; if(sign == 0) scaled = 0-scaled; return scaled; } void IF_setup_serial_interfaces() { if(ENABLE_PC_SERIAL) { pc.baud(PC_BAUD); pc.attach(&IF_pc_rx_callback, Serial::RxIrq); } if(ENABLE_BLUETOOTH) { bt.baud(BLUETOOTH_BAUD); bt.attach(&IF_bt_rx_callback, Serial::RxIrq); } } void IF_pc_rx_command_timeout() { char message_array[6]; char length = 1 + pc_command_message_byte; pc_command_message_started = 0; message_array[0] = COMMAND_MESSAGE_BYTE; for(int k=0; k<pc_command_message_byte; k++) { message_array[k+1] = pc_command_message[k]; } IF_handle_user_serial_message(message_array, length, 0); } void IF_bt_rx_command_timeout() { char message_array[6]; char length = 1 + bt_command_message_byte; bt_command_message_started = 0; message_array[0] = COMMAND_MESSAGE_BYTE; for(int k=0; k<bt_command_message_byte; k++) { message_array[k+1] = bt_command_message[k]; } IF_handle_user_serial_message(message_array, length, 1); } void IF_pc_rx_callback() { int count = 0; char message_array[255]; while(pc.readable()) { char tc = pc.getc(); message_array[count] = tc; count ++; if(pc_command_message_started == 1) { if(pc_command_message_byte == 3) { pc_command_timeout.detach(); if(tc == COMMAND_MESSAGE_BYTE) { // A complete command message succesfully received, call handler pc_command_message_started = 0; count = 0; IF_handle_command_serial_message(pc_command_message , 0); } else { // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message pc_command_message_started = 0; message_array[0] = COMMAND_MESSAGE_BYTE; message_array[1] = pc_command_message[0]; message_array[2] = pc_command_message[1]; message_array[3] = pc_command_message[2]; message_array[4] = tc; count = 5; } } else { pc_command_message[pc_command_message_byte] = tc; pc_command_message_byte ++; } } else { if(count == 1) { if(tc == COMMAND_MESSAGE_BYTE) { pc_command_timeout.attach(&IF_pc_rx_command_timeout,command_timeout_period); pc_command_message_started = 1; pc_command_message_byte = 0; } } } } if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0); } void IF_bt_rx_callback() { int count = 0; char message_array[255]; while(bt.readable()) { char tc = bt.getc(); message_array[count] = tc; count ++; if(bt_command_message_started == 1) { if(bt_command_message_byte == 3) { bt_command_timeout.detach(); if(tc == COMMAND_MESSAGE_BYTE) { // A complete command message succesfully received, call handler bt_command_message_started = 0; count = 0; IF_handle_command_serial_message(bt_command_message , 1); } else { // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message bt_command_message_started = 0; message_array[0] = COMMAND_MESSAGE_BYTE; message_array[1] = bt_command_message[0]; message_array[2] = bt_command_message[1]; message_array[3] = bt_command_message[2]; message_array[4] = tc; count = 5; } } else { bt_command_timeout.attach(&IF_bt_rx_command_timeout,command_timeout_period); bt_command_message[bt_command_message_byte] = tc; bt_command_message_byte ++; } } else { if(count == 1) { if(tc == COMMAND_MESSAGE_BYTE) { bt_command_message_started = 1; bt_command_message_byte = 0; } } } } if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1); }