Psi Swarm Robot
Practical Robotics Modular Robot Library
Diff: serialcomms.cpp
- Revision:
- 4:c2e933d53bea
- Child:
- 5:6da8daaeb9f7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/serialcomms.cpp Mon Jan 02 15:17:22 2017 +0000
@@ -0,0 +1,468 @@
+#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;
+}
\ No newline at end of file