Practical Robotics Modular Robot Library
serialcomms.cpp
- Committer:
- jah128
- Date:
- 2017-01-02
- Revision:
- 4:c2e933d53bea
- Child:
- 5:6da8daaeb9f7
File content as of revision 4:c2e933d53bea:
#include "robot.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 allow_commands = 1; Timeout pc_command_timeout; void SerialComms::setup_serial_interfaces() { if(ENABLE_PC_SERIAL) { pc.baud(PC_BAUD); pc.attach(this,&SerialComms::_pc_rx_callback, Serial::RxIrq); } if(ENABLE_BLUETOOTH) { bt.baud(BLUETOOTH_BAUD); bt.attach(this,&SerialComms::_bt_rx_callback, Serial::RxIrq); } } void SerialComms::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 = decode_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { command_status = 1; motors.set_left_motor_speed(dec); } else command_status = 2; break; case 2: strcpy(command,"SET RIGHT MOTOR"); dec = decode_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { motors.set_right_motor_speed(dec); command_status = 1; } else command_status = 2; break; case 3: strcpy(command,"SET BOTH MOTORS"); dec = decode_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { command_status = 1; motors.forwards(dec); } else command_status = 2; break; case 4: strcpy(command,"BRAKE LEFT MOTOR"); sprintf(subcommand,""); if(allow_commands) { command_status = 1; motors.brake_left(); } else command_status = 2; break; case 5: strcpy(command,"BRAKE RIGHT MOTOR"); sprintf(subcommand,""); if(allow_commands) { command_status = 1; motors.brake_right(); } else command_status = 2; break; case 6: strcpy(command,"BRAKE BOTH MOTORS"); sprintf(subcommand,""); if(allow_commands) { command_status = 1; motors.brake(); } else command_status = 2; break; case 7: strcpy(command,"COAST BOTH MOTORS"); sprintf(subcommand,""); if(allow_commands) { command_status = 1; motors.coast(); } else command_status = 2; break; case 8: strcpy(command,"TURN ON SPOT"); dec = decode_float(message[1],message[2]); sprintf(subcommand,"%1.5f",dec); if(allow_commands) { command_status = 1; motors.turn(dec); } else command_status = 2; break; case 9: strcpy(command,"SET EACH MOTOR"); l_dec = decode_float(message[1]); r_dec = decode_float(message[2]); sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec); if(allow_commands) { command_status = 1; motors.set_left_motor_speed(l_dec); motors.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",_char_to_binary_char(message[1]), _char_to_binary_char(message[2])); if(allow_commands) { command_status = 1; // led.set_leds(message[1],message[2]); } else command_status = 2; break; case 11: strcpy(command,"SET RED LED STATES"); sprintf(subcommand,"%s",_char_to_binary_char(message[1])); if(allow_commands) { command_status = 1; // led.set_red_leds(message[1]); } else command_status = 2; break; case 12: strcpy(command,"SET GREEN LED STATES"); sprintf(subcommand,"%s",_char_to_binary_char(message[1])); if(allow_commands) { command_status = 1; // led.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; // led.set_led(message[1],message[2]); } else command_status = 2; } else { sprintf(subcommand,"[INVALID CODE]"); command_status = 3; } break; case 14: strcpy(command,"SET MBED LEDS"); sprintf(subcommand,"%s",_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 15: strcpy(command,"SET CASE LED"); dec = decode_unsigned_float(message[1],message[2]); sprintf(subcommand,"FOR %1.5fS",dec); if(allow_commands) { command_status = 1; // led.blink_leds(dec); } else command_status = 2; break; case 20: 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 21: 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; // MOTOR REQUESTS case 100: strcpy(command,"GET LEFT MOTOR SPEED"); sprintf(ret_message,"%1.5f",motors.get_left_motor_speed()); send_message = 1; break; case 101: strcpy(command,"GET RIGHT MOTOR SPEED"); sprintf(ret_message,"%1.5f",motors.get_right_motor_speed()); send_message = 1; break; // GENERAL REQUESTS case 102: strcpy(command,"GET SOFTWARE VERSION"); sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE); send_message = 1; break; case 103: strcpy(command,"GET UPTIME"); sprintf(ret_message,"%6.2f",robot.get_uptime()); send_message = 1; break; case 104: strcpy(command,"GET DEBUG MODE"); sprintf(ret_message,"%1d%1d",debug_mode,debug_output); send_message = 1; break; case 105: strcpy(command,"GET HEX IR VALUES"); sprintf(ret_message,"%02X%02X%02X%02X%02X%02X%02X%02X",sensors.get_adc_value(0),sensors.get_adc_value(1),sensors.get_adc_value(2),sensors.get_adc_value(3),sensors.get_adc_value(4),sensors.get_adc_value(5),sensors.get_adc_value(6),sensors.get_adc_value(7)); send_message = 1; break; case 106: strcpy(command,"GET HEX STATUS VALUES"); robot.update_status_message(); sprintf(ret_message,"%02X%02X%02X%02X%02X%02X%02X%02X",status_message[0],status_message[1],status_message[2],status_message[3],status_message[4],status_message[5],status_message[6],status_message[7]); send_message = 1; break; case 107: strcpy(command,"GET IR VALUES"); //Special case: characters may have zero values so use printf directly pc.printf("%c%c%c%c%c%c%c%c%c",IR_MESSAGE_BYTE,sensors.get_adc_value(0),sensors.get_adc_value(1),sensors.get_adc_value(2),sensors.get_adc_value(3),sensors.get_adc_value(4),sensors.get_adc_value(5),sensors.get_adc_value(6),sensors.get_adc_value(7)); robot.debug("IR Message sent\n"); send_message = 2; break; case 108: strcpy(command,"GET STATUS VALUES"); robot.update_status_message(); //Special case: characters may have zero values so use printf directly pc.printf("%c%c%c%c%c%c%c%c%c",STATUS_MESSAGE_BYTE,status_message[0],status_message[1],status_message[2],status_message[3],status_message[4],status_message[5],status_message[6],status_message[7]); robot.debug("Status Message sent\n"); send_message = 2; break; } if(send_message > 0) { if(send_message == 1){ 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; } robot.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: robot.debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]); break; case 1: robot.debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]); break; case 2: robot.debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]); break; case 3: robot.debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]); break; } } } void SerialComms::handle_user_serial_message(char * message, char length, char interface) { robot.debug("U:[L=%d] ",length); for(int i=0; i<length; i++) { robot.debug("%2X",message[i]); } robot.debug("\n"); } // Private functions void SerialComms::_pc_rx_callback() { int count = 0; char message_array[128]; 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; 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(this,&SerialComms::_pc_rx_command_timeout,command_timeout_period); pc_command_message_started = 1; pc_command_message_byte = 0; } } } } if(!pc_command_message_started && count>0) handle_user_serial_message(message_array, count, 0); } void SerialComms::_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]; } handle_user_serial_message(message_array, length, 0); } void SerialComms::_bt_rx_callback() { } float SerialComms::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 SerialComms::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; } float SerialComms::decode_unsigned_float(char byte0, char byte1) { unsigned short sval = (byte0) << 8; sval += byte1; float scaled = sval / 65535.0f; return scaled; } float SerialComms::decode_unsigned_float(char byte0) { unsigned short sval = (byte0); float scaled = sval / 255.0f; return scaled; } char * SerialComms::_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 * SerialComms::_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; }