Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

serialcomms.cpp@4:c2e933d53bea, 2017-01-02 (annotated)

Committer:
jah128
Date:
Mon Jan 02 15:17:22 2017 +0000
Revision:
4:c2e933d53bea
Child:
5:6da8daaeb9f7
Added serialcomms library (adapted from Psi) and LED animations

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 4:c2e933d53bea 1 #include "robot.h"
jah128 4:c2e933d53bea 2
jah128 4:c2e933d53bea 3
jah128 4:c2e933d53bea 4 static float command_timeout_period = 0.1f; //If a complete command message is not received in 0.1s then consider it a user message
jah128 4:c2e933d53bea 5 char pc_command_message_started = 0;
jah128 4:c2e933d53bea 6 char pc_command_message_byte = 0;
jah128 4:c2e933d53bea 7 char pc_command_message[3];
jah128 4:c2e933d53bea 8 char allow_commands = 1;
jah128 4:c2e933d53bea 9 Timeout pc_command_timeout;
jah128 4:c2e933d53bea 10
jah128 4:c2e933d53bea 11 void SerialComms::setup_serial_interfaces()
jah128 4:c2e933d53bea 12 {
jah128 4:c2e933d53bea 13 if(ENABLE_PC_SERIAL) {
jah128 4:c2e933d53bea 14 pc.baud(PC_BAUD);
jah128 4:c2e933d53bea 15 pc.attach(this,&SerialComms::_pc_rx_callback, Serial::RxIrq);
jah128 4:c2e933d53bea 16 }
jah128 4:c2e933d53bea 17 if(ENABLE_BLUETOOTH) {
jah128 4:c2e933d53bea 18 bt.baud(BLUETOOTH_BAUD);
jah128 4:c2e933d53bea 19 bt.attach(this,&SerialComms::_bt_rx_callback, Serial::RxIrq);
jah128 4:c2e933d53bea 20 }
jah128 4:c2e933d53bea 21 }
jah128 4:c2e933d53bea 22
jah128 4:c2e933d53bea 23 void SerialComms::handle_command_serial_message(char message [3], char interface)
jah128 4:c2e933d53bea 24 {
jah128 4:c2e933d53bea 25 char iface [4];
jah128 4:c2e933d53bea 26 if(interface) strcpy(iface,"BT");
jah128 4:c2e933d53bea 27 else strcpy(iface,"USB");
jah128 4:c2e933d53bea 28 char command [26];
jah128 4:c2e933d53bea 29 char subcommand[30];
jah128 4:c2e933d53bea 30 float dec;
jah128 4:c2e933d53bea 31 float l_dec;
jah128 4:c2e933d53bea 32 float r_dec;
jah128 4:c2e933d53bea 33 int irp_delay;
jah128 4:c2e933d53bea 34 char colour_string[7];
jah128 4:c2e933d53bea 35 char ret_message[50];
jah128 4:c2e933d53bea 36 char send_message = 0;
jah128 4:c2e933d53bea 37 char command_status = 0;
jah128 4:c2e933d53bea 38 // command_status values:
jah128 4:c2e933d53bea 39 // 0 - unrecognised command
jah128 4:c2e933d53bea 40 // 1 - command actioned
jah128 4:c2e933d53bea 41 // 2 - command blocked
jah128 4:c2e933d53bea 42 // 3 - invalid parameters
jah128 4:c2e933d53bea 43
jah128 4:c2e933d53bea 44 subcommand[0]=0;
jah128 4:c2e933d53bea 45 command[0]=0;
jah128 4:c2e933d53bea 46 switch(message[0]) {
jah128 4:c2e933d53bea 47 // MOTOR COMMANDS
jah128 4:c2e933d53bea 48 case 1:
jah128 4:c2e933d53bea 49 strcpy(command,"SET LEFT MOTOR");
jah128 4:c2e933d53bea 50 dec = decode_float(message[1],message[2]);
jah128 4:c2e933d53bea 51 sprintf(subcommand,"%1.5f",dec);
jah128 4:c2e933d53bea 52 if(allow_commands) {
jah128 4:c2e933d53bea 53 command_status = 1;
jah128 4:c2e933d53bea 54 motors.set_left_motor_speed(dec);
jah128 4:c2e933d53bea 55 } else command_status = 2;
jah128 4:c2e933d53bea 56 break;
jah128 4:c2e933d53bea 57 case 2:
jah128 4:c2e933d53bea 58 strcpy(command,"SET RIGHT MOTOR");
jah128 4:c2e933d53bea 59 dec = decode_float(message[1],message[2]);
jah128 4:c2e933d53bea 60 sprintf(subcommand,"%1.5f",dec);
jah128 4:c2e933d53bea 61 if(allow_commands) {
jah128 4:c2e933d53bea 62 motors.set_right_motor_speed(dec);
jah128 4:c2e933d53bea 63 command_status = 1;
jah128 4:c2e933d53bea 64 } else command_status = 2;
jah128 4:c2e933d53bea 65 break;
jah128 4:c2e933d53bea 66 case 3:
jah128 4:c2e933d53bea 67 strcpy(command,"SET BOTH MOTORS");
jah128 4:c2e933d53bea 68 dec = decode_float(message[1],message[2]);
jah128 4:c2e933d53bea 69 sprintf(subcommand,"%1.5f",dec);
jah128 4:c2e933d53bea 70 if(allow_commands) {
jah128 4:c2e933d53bea 71 command_status = 1;
jah128 4:c2e933d53bea 72 motors.forwards(dec);
jah128 4:c2e933d53bea 73 } else command_status = 2;
jah128 4:c2e933d53bea 74 break;
jah128 4:c2e933d53bea 75 case 4:
jah128 4:c2e933d53bea 76 strcpy(command,"BRAKE LEFT MOTOR");
jah128 4:c2e933d53bea 77 sprintf(subcommand,"");
jah128 4:c2e933d53bea 78 if(allow_commands) {
jah128 4:c2e933d53bea 79 command_status = 1;
jah128 4:c2e933d53bea 80 motors.brake_left();
jah128 4:c2e933d53bea 81 } else command_status = 2;
jah128 4:c2e933d53bea 82 break;
jah128 4:c2e933d53bea 83 case 5:
jah128 4:c2e933d53bea 84 strcpy(command,"BRAKE RIGHT MOTOR");
jah128 4:c2e933d53bea 85 sprintf(subcommand,"");
jah128 4:c2e933d53bea 86 if(allow_commands) {
jah128 4:c2e933d53bea 87 command_status = 1;
jah128 4:c2e933d53bea 88 motors.brake_right();
jah128 4:c2e933d53bea 89 } else command_status = 2;
jah128 4:c2e933d53bea 90 break;
jah128 4:c2e933d53bea 91 case 6:
jah128 4:c2e933d53bea 92 strcpy(command,"BRAKE BOTH MOTORS");
jah128 4:c2e933d53bea 93 sprintf(subcommand,"");
jah128 4:c2e933d53bea 94 if(allow_commands) {
jah128 4:c2e933d53bea 95 command_status = 1;
jah128 4:c2e933d53bea 96 motors.brake();
jah128 4:c2e933d53bea 97 } else command_status = 2;
jah128 4:c2e933d53bea 98 break;
jah128 4:c2e933d53bea 99 case 7:
jah128 4:c2e933d53bea 100 strcpy(command,"COAST BOTH MOTORS");
jah128 4:c2e933d53bea 101 sprintf(subcommand,"");
jah128 4:c2e933d53bea 102 if(allow_commands) {
jah128 4:c2e933d53bea 103 command_status = 1;
jah128 4:c2e933d53bea 104 motors.coast();
jah128 4:c2e933d53bea 105 } else command_status = 2;
jah128 4:c2e933d53bea 106 break;
jah128 4:c2e933d53bea 107 case 8:
jah128 4:c2e933d53bea 108 strcpy(command,"TURN ON SPOT");
jah128 4:c2e933d53bea 109 dec = decode_float(message[1],message[2]);
jah128 4:c2e933d53bea 110 sprintf(subcommand,"%1.5f",dec);
jah128 4:c2e933d53bea 111 if(allow_commands) {
jah128 4:c2e933d53bea 112 command_status = 1;
jah128 4:c2e933d53bea 113 motors.turn(dec);
jah128 4:c2e933d53bea 114 } else command_status = 2;
jah128 4:c2e933d53bea 115 break;
jah128 4:c2e933d53bea 116 case 9:
jah128 4:c2e933d53bea 117 strcpy(command,"SET EACH MOTOR");
jah128 4:c2e933d53bea 118 l_dec = decode_float(message[1]);
jah128 4:c2e933d53bea 119 r_dec = decode_float(message[2]);
jah128 4:c2e933d53bea 120 sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
jah128 4:c2e933d53bea 121 if(allow_commands) {
jah128 4:c2e933d53bea 122 command_status = 1;
jah128 4:c2e933d53bea 123 motors.set_left_motor_speed(l_dec);
jah128 4:c2e933d53bea 124 motors.set_right_motor_speed(r_dec);
jah128 4:c2e933d53bea 125 } else command_status = 2;
jah128 4:c2e933d53bea 126 break;
jah128 4:c2e933d53bea 127
jah128 4:c2e933d53bea 128 // LED COMMANDS
jah128 4:c2e933d53bea 129 case 10:
jah128 4:c2e933d53bea 130 strcpy(command,"SET LED STATES");
jah128 4:c2e933d53bea 131 sprintf(subcommand,"G:%s R:%s",_char_to_binary_char(message[1]), _char_to_binary_char(message[2]));
jah128 4:c2e933d53bea 132 if(allow_commands) {
jah128 4:c2e933d53bea 133 command_status = 1;
jah128 4:c2e933d53bea 134 // led.set_leds(message[1],message[2]);
jah128 4:c2e933d53bea 135 } else command_status = 2;
jah128 4:c2e933d53bea 136 break;
jah128 4:c2e933d53bea 137 case 11:
jah128 4:c2e933d53bea 138 strcpy(command,"SET RED LED STATES");
jah128 4:c2e933d53bea 139 sprintf(subcommand,"%s",_char_to_binary_char(message[1]));
jah128 4:c2e933d53bea 140 if(allow_commands) {
jah128 4:c2e933d53bea 141 command_status = 1;
jah128 4:c2e933d53bea 142 // led.set_red_leds(message[1]);
jah128 4:c2e933d53bea 143 } else command_status = 2;
jah128 4:c2e933d53bea 144 break;
jah128 4:c2e933d53bea 145 case 12:
jah128 4:c2e933d53bea 146 strcpy(command,"SET GREEN LED STATES");
jah128 4:c2e933d53bea 147 sprintf(subcommand,"%s",_char_to_binary_char(message[1]));
jah128 4:c2e933d53bea 148 if(allow_commands) {
jah128 4:c2e933d53bea 149 command_status = 1;
jah128 4:c2e933d53bea 150 // led.set_green_leds(message[1]);
jah128 4:c2e933d53bea 151 } else command_status = 2;
jah128 4:c2e933d53bea 152 break;
jah128 4:c2e933d53bea 153 case 13:
jah128 4:c2e933d53bea 154 strcpy(command,"SET LED");
jah128 4:c2e933d53bea 155 switch(message[2]) {
jah128 4:c2e933d53bea 156 case 1:
jah128 4:c2e933d53bea 157 strcpy(colour_string,"RED");
jah128 4:c2e933d53bea 158 break;
jah128 4:c2e933d53bea 159 case 2:
jah128 4:c2e933d53bea 160 strcpy(colour_string,"GREEN");
jah128 4:c2e933d53bea 161 break;
jah128 4:c2e933d53bea 162 case 3:
jah128 4:c2e933d53bea 163 strcpy(colour_string,"BOTH");
jah128 4:c2e933d53bea 164 break;
jah128 4:c2e933d53bea 165 case 0:
jah128 4:c2e933d53bea 166 strcpy(colour_string,"OFF");
jah128 4:c2e933d53bea 167 break;
jah128 4:c2e933d53bea 168 }
jah128 4:c2e933d53bea 169 if(message[1] < 8 && message[2] < 4) {
jah128 4:c2e933d53bea 170 sprintf(subcommand,"%d %s",message[1],colour_string);
jah128 4:c2e933d53bea 171 if(allow_commands) {
jah128 4:c2e933d53bea 172 command_status = 1;
jah128 4:c2e933d53bea 173 // led.set_led(message[1],message[2]);
jah128 4:c2e933d53bea 174 } else command_status = 2;
jah128 4:c2e933d53bea 175 } else {
jah128 4:c2e933d53bea 176 sprintf(subcommand,"[INVALID CODE]");
jah128 4:c2e933d53bea 177 command_status = 3;
jah128 4:c2e933d53bea 178 }
jah128 4:c2e933d53bea 179 break;
jah128 4:c2e933d53bea 180 case 14:
jah128 4:c2e933d53bea 181 strcpy(command,"SET MBED LEDS");
jah128 4:c2e933d53bea 182 sprintf(subcommand,"%s",_nibble_to_binary_char(message[1]));
jah128 4:c2e933d53bea 183 if(allow_commands) {
jah128 4:c2e933d53bea 184 command_status = 1;
jah128 4:c2e933d53bea 185 mbed_led1 = (message[1] & 128) >> 7;
jah128 4:c2e933d53bea 186 mbed_led2 = (message[1] & 64) >> 6;
jah128 4:c2e933d53bea 187 mbed_led3 = (message[1] & 32) >> 5;
jah128 4:c2e933d53bea 188 mbed_led4 = (message[1] & 16) >> 4;
jah128 4:c2e933d53bea 189 } else command_status = 2;
jah128 4:c2e933d53bea 190 break;
jah128 4:c2e933d53bea 191 case 15:
jah128 4:c2e933d53bea 192 strcpy(command,"SET CASE LED");
jah128 4:c2e933d53bea 193 dec = decode_unsigned_float(message[1],message[2]);
jah128 4:c2e933d53bea 194 sprintf(subcommand,"FOR %1.5fS",dec);
jah128 4:c2e933d53bea 195 if(allow_commands) {
jah128 4:c2e933d53bea 196 command_status = 1;
jah128 4:c2e933d53bea 197 // led.blink_leds(dec);
jah128 4:c2e933d53bea 198 } else command_status = 2;
jah128 4:c2e933d53bea 199 break;
jah128 4:c2e933d53bea 200 case 20:
jah128 4:c2e933d53bea 201 strcpy(command,"SET DEBUG MODE");
jah128 4:c2e933d53bea 202 switch(message[1]) {
jah128 4:c2e933d53bea 203 case 1:
jah128 4:c2e933d53bea 204 strcpy(subcommand,"ON");
jah128 4:c2e933d53bea 205 break;
jah128 4:c2e933d53bea 206 case 0:
jah128 4:c2e933d53bea 207 strcpy(subcommand,"OFF");
jah128 4:c2e933d53bea 208 break;
jah128 4:c2e933d53bea 209 }
jah128 4:c2e933d53bea 210 if(message[2] & 1) strcat (subcommand,"-PC");
jah128 4:c2e933d53bea 211 if(message[2] & 2) strcat (subcommand,"-BT");
jah128 4:c2e933d53bea 212 if(message[2] & 4) strcat (subcommand,"-DISP");
jah128 4:c2e933d53bea 213 if(allow_commands) {
jah128 4:c2e933d53bea 214 command_status = 1;
jah128 4:c2e933d53bea 215 debug_mode = message[1];
jah128 4:c2e933d53bea 216 debug_output = message[2];
jah128 4:c2e933d53bea 217 } else command_status = 2;
jah128 4:c2e933d53bea 218 break;
jah128 4:c2e933d53bea 219
jah128 4:c2e933d53bea 220 case 21:
jah128 4:c2e933d53bea 221 strcpy(command,"SET ALLOW COMMANDS");
jah128 4:c2e933d53bea 222 switch(message[1] % 2) {
jah128 4:c2e933d53bea 223 case 1:
jah128 4:c2e933d53bea 224 strcpy(subcommand,"ON");
jah128 4:c2e933d53bea 225 break;
jah128 4:c2e933d53bea 226 case 0:
jah128 4:c2e933d53bea 227 strcpy(subcommand,"OFF");
jah128 4:c2e933d53bea 228 break;
jah128 4:c2e933d53bea 229 }
jah128 4:c2e933d53bea 230 allow_commands = message[1] % 2;
jah128 4:c2e933d53bea 231 command_status = 1;
jah128 4:c2e933d53bea 232 break;
jah128 4:c2e933d53bea 233
jah128 4:c2e933d53bea 234 // MOTOR REQUESTS
jah128 4:c2e933d53bea 235 case 100:
jah128 4:c2e933d53bea 236 strcpy(command,"GET LEFT MOTOR SPEED");
jah128 4:c2e933d53bea 237 sprintf(ret_message,"%1.5f",motors.get_left_motor_speed());
jah128 4:c2e933d53bea 238 send_message = 1;
jah128 4:c2e933d53bea 239 break;
jah128 4:c2e933d53bea 240
jah128 4:c2e933d53bea 241 case 101:
jah128 4:c2e933d53bea 242 strcpy(command,"GET RIGHT MOTOR SPEED");
jah128 4:c2e933d53bea 243 sprintf(ret_message,"%1.5f",motors.get_right_motor_speed());
jah128 4:c2e933d53bea 244 send_message = 1;
jah128 4:c2e933d53bea 245 break;
jah128 4:c2e933d53bea 246
jah128 4:c2e933d53bea 247 // GENERAL REQUESTS
jah128 4:c2e933d53bea 248 case 102:
jah128 4:c2e933d53bea 249 strcpy(command,"GET SOFTWARE VERSION");
jah128 4:c2e933d53bea 250 sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
jah128 4:c2e933d53bea 251 send_message = 1;
jah128 4:c2e933d53bea 252 break;
jah128 4:c2e933d53bea 253
jah128 4:c2e933d53bea 254 case 103:
jah128 4:c2e933d53bea 255 strcpy(command,"GET UPTIME");
jah128 4:c2e933d53bea 256 sprintf(ret_message,"%6.2f",robot.get_uptime());
jah128 4:c2e933d53bea 257 send_message = 1;
jah128 4:c2e933d53bea 258 break;
jah128 4:c2e933d53bea 259
jah128 4:c2e933d53bea 260 case 104:
jah128 4:c2e933d53bea 261 strcpy(command,"GET DEBUG MODE");
jah128 4:c2e933d53bea 262 sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
jah128 4:c2e933d53bea 263 send_message = 1;
jah128 4:c2e933d53bea 264 break;
jah128 4:c2e933d53bea 265 case 105:
jah128 4:c2e933d53bea 266 strcpy(command,"GET HEX IR VALUES");
jah128 4:c2e933d53bea 267 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));
jah128 4:c2e933d53bea 268 send_message = 1;
jah128 4:c2e933d53bea 269 break;
jah128 4:c2e933d53bea 270 case 106:
jah128 4:c2e933d53bea 271 strcpy(command,"GET HEX STATUS VALUES");
jah128 4:c2e933d53bea 272 robot.update_status_message();
jah128 4:c2e933d53bea 273 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]);
jah128 4:c2e933d53bea 274 send_message = 1;
jah128 4:c2e933d53bea 275 break;
jah128 4:c2e933d53bea 276 case 107:
jah128 4:c2e933d53bea 277 strcpy(command,"GET IR VALUES");
jah128 4:c2e933d53bea 278 //Special case: characters may have zero values so use printf directly
jah128 4:c2e933d53bea 279 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));
jah128 4:c2e933d53bea 280 robot.debug("IR Message sent\n");
jah128 4:c2e933d53bea 281 send_message = 2;
jah128 4:c2e933d53bea 282 break;
jah128 4:c2e933d53bea 283 case 108:
jah128 4:c2e933d53bea 284 strcpy(command,"GET STATUS VALUES");
jah128 4:c2e933d53bea 285 robot.update_status_message();
jah128 4:c2e933d53bea 286 //Special case: characters may have zero values so use printf directly
jah128 4:c2e933d53bea 287 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]);
jah128 4:c2e933d53bea 288 robot.debug("Status Message sent\n");
jah128 4:c2e933d53bea 289 send_message = 2;
jah128 4:c2e933d53bea 290 break;
jah128 4:c2e933d53bea 291 }
jah128 4:c2e933d53bea 292
jah128 4:c2e933d53bea 293
jah128 4:c2e933d53bea 294 if(send_message > 0) {
jah128 4:c2e933d53bea 295 if(send_message == 1){
jah128 4:c2e933d53bea 296 char message_length = strlen(ret_message);
jah128 4:c2e933d53bea 297 switch(interface) {
jah128 4:c2e933d53bea 298 case 0:
jah128 4:c2e933d53bea 299 pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 4:c2e933d53bea 300 break;
jah128 4:c2e933d53bea 301 case 1:
jah128 4:c2e933d53bea 302 bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
jah128 4:c2e933d53bea 303 break;
jah128 4:c2e933d53bea 304 }
jah128 4:c2e933d53bea 305 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);
jah128 4:c2e933d53bea 306 }
jah128 4:c2e933d53bea 307 } else {
jah128 4:c2e933d53bea 308 switch(interface) {
jah128 4:c2e933d53bea 309 case 0:
jah128 4:c2e933d53bea 310 pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 4:c2e933d53bea 311 break;
jah128 4:c2e933d53bea 312 case 1:
jah128 4:c2e933d53bea 313 bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
jah128 4:c2e933d53bea 314 break;
jah128 4:c2e933d53bea 315 }
jah128 4:c2e933d53bea 316 switch(command_status) {
jah128 4:c2e933d53bea 317 case 0:
jah128 4:c2e933d53bea 318 robot.debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
jah128 4:c2e933d53bea 319 break;
jah128 4:c2e933d53bea 320 case 1:
jah128 4:c2e933d53bea 321 robot.debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 4:c2e933d53bea 322 break;
jah128 4:c2e933d53bea 323 case 2:
jah128 4:c2e933d53bea 324 robot.debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 4:c2e933d53bea 325 break;
jah128 4:c2e933d53bea 326 case 3:
jah128 4:c2e933d53bea 327 robot.debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
jah128 4:c2e933d53bea 328 break;
jah128 4:c2e933d53bea 329 }
jah128 4:c2e933d53bea 330 }
jah128 4:c2e933d53bea 331 }
jah128 4:c2e933d53bea 332
jah128 4:c2e933d53bea 333
jah128 4:c2e933d53bea 334
jah128 4:c2e933d53bea 335 void SerialComms::handle_user_serial_message(char * message, char length, char interface)
jah128 4:c2e933d53bea 336 {
jah128 4:c2e933d53bea 337 robot.debug("U:[L=%d] ",length);
jah128 4:c2e933d53bea 338 for(int i=0; i<length; i++) {
jah128 4:c2e933d53bea 339 robot.debug("%2X",message[i]);
jah128 4:c2e933d53bea 340 }
jah128 4:c2e933d53bea 341 robot.debug("\n");
jah128 4:c2e933d53bea 342 }
jah128 4:c2e933d53bea 343
jah128 4:c2e933d53bea 344
jah128 4:c2e933d53bea 345 // Private functions
jah128 4:c2e933d53bea 346
jah128 4:c2e933d53bea 347 void SerialComms::_pc_rx_callback()
jah128 4:c2e933d53bea 348 {
jah128 4:c2e933d53bea 349 int count = 0;
jah128 4:c2e933d53bea 350 char message_array[128];
jah128 4:c2e933d53bea 351 while(pc.readable()) {
jah128 4:c2e933d53bea 352 char tc = pc.getc();
jah128 4:c2e933d53bea 353 message_array[count] = tc;
jah128 4:c2e933d53bea 354 count ++;
jah128 4:c2e933d53bea 355 if(pc_command_message_started == 1) {
jah128 4:c2e933d53bea 356 if(pc_command_message_byte == 3) {
jah128 4:c2e933d53bea 357 pc_command_timeout.detach();
jah128 4:c2e933d53bea 358 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 4:c2e933d53bea 359 // A complete command message succesfully received, call handler
jah128 4:c2e933d53bea 360 pc_command_message_started = 0;
jah128 4:c2e933d53bea 361 count = 0;
jah128 4:c2e933d53bea 362 handle_command_serial_message(pc_command_message , 0);
jah128 4:c2e933d53bea 363 } else {
jah128 4:c2e933d53bea 364 // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
jah128 4:c2e933d53bea 365 pc_command_message_started = 0;
jah128 4:c2e933d53bea 366 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 4:c2e933d53bea 367 message_array[1] = pc_command_message[0];
jah128 4:c2e933d53bea 368 message_array[2] = pc_command_message[1];
jah128 4:c2e933d53bea 369 message_array[3] = pc_command_message[2];
jah128 4:c2e933d53bea 370 message_array[4] = tc;
jah128 4:c2e933d53bea 371 count = 5;
jah128 4:c2e933d53bea 372 }
jah128 4:c2e933d53bea 373 } else {
jah128 4:c2e933d53bea 374 pc_command_message[pc_command_message_byte] = tc;
jah128 4:c2e933d53bea 375 pc_command_message_byte ++;
jah128 4:c2e933d53bea 376 }
jah128 4:c2e933d53bea 377 } else {
jah128 4:c2e933d53bea 378 if(count == 1) {
jah128 4:c2e933d53bea 379 if(tc == COMMAND_MESSAGE_BYTE) {
jah128 4:c2e933d53bea 380 pc_command_timeout.attach(this,&SerialComms::_pc_rx_command_timeout,command_timeout_period);
jah128 4:c2e933d53bea 381 pc_command_message_started = 1;
jah128 4:c2e933d53bea 382 pc_command_message_byte = 0;
jah128 4:c2e933d53bea 383
jah128 4:c2e933d53bea 384 }
jah128 4:c2e933d53bea 385 }
jah128 4:c2e933d53bea 386 }
jah128 4:c2e933d53bea 387 }
jah128 4:c2e933d53bea 388 if(!pc_command_message_started && count>0) handle_user_serial_message(message_array, count, 0);
jah128 4:c2e933d53bea 389 }
jah128 4:c2e933d53bea 390
jah128 4:c2e933d53bea 391
jah128 4:c2e933d53bea 392 void SerialComms::_pc_rx_command_timeout()
jah128 4:c2e933d53bea 393 {
jah128 4:c2e933d53bea 394 char message_array[6];
jah128 4:c2e933d53bea 395 char length = 1 + pc_command_message_byte;
jah128 4:c2e933d53bea 396 pc_command_message_started = 0;
jah128 4:c2e933d53bea 397 message_array[0] = COMMAND_MESSAGE_BYTE;
jah128 4:c2e933d53bea 398 for(int k=0; k<pc_command_message_byte; k++) {
jah128 4:c2e933d53bea 399 message_array[k+1] = pc_command_message[k];
jah128 4:c2e933d53bea 400 }
jah128 4:c2e933d53bea 401 handle_user_serial_message(message_array, length, 0);
jah128 4:c2e933d53bea 402 }
jah128 4:c2e933d53bea 403
jah128 4:c2e933d53bea 404
jah128 4:c2e933d53bea 405 void SerialComms::_bt_rx_callback()
jah128 4:c2e933d53bea 406 {
jah128 4:c2e933d53bea 407
jah128 4:c2e933d53bea 408 }
jah128 4:c2e933d53bea 409
jah128 4:c2e933d53bea 410
jah128 4:c2e933d53bea 411 float SerialComms::decode_float(char byte0, char byte1)
jah128 4:c2e933d53bea 412 {
jah128 4:c2e933d53bea 413 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 4:c2e933d53bea 414 char sign = byte0 / 128;
jah128 4:c2e933d53bea 415 short sval = (byte0 % 128) << 8;
jah128 4:c2e933d53bea 416 sval += byte1;
jah128 4:c2e933d53bea 417 float scaled = sval / 32767.0f;
jah128 4:c2e933d53bea 418 if(sign == 0) scaled = 0-scaled;
jah128 4:c2e933d53bea 419 return scaled;
jah128 4:c2e933d53bea 420 }
jah128 4:c2e933d53bea 421
jah128 4:c2e933d53bea 422 float SerialComms::decode_float(char byte0)
jah128 4:c2e933d53bea 423 {
jah128 4:c2e933d53bea 424 // MSB is byte 0 is sign, rest is linear spread between 0 and 1
jah128 4:c2e933d53bea 425 char sign = byte0 / 128;
jah128 4:c2e933d53bea 426 short sval = (byte0 % 128);
jah128 4:c2e933d53bea 427 float scaled = sval / 127.0f;
jah128 4:c2e933d53bea 428 if(sign == 0) scaled = 0-scaled;
jah128 4:c2e933d53bea 429 return scaled;
jah128 4:c2e933d53bea 430 }
jah128 4:c2e933d53bea 431
jah128 4:c2e933d53bea 432 float SerialComms::decode_unsigned_float(char byte0, char byte1)
jah128 4:c2e933d53bea 433 {
jah128 4:c2e933d53bea 434 unsigned short sval = (byte0) << 8;
jah128 4:c2e933d53bea 435 sval += byte1;
jah128 4:c2e933d53bea 436 float scaled = sval / 65535.0f;
jah128 4:c2e933d53bea 437 return scaled;
jah128 4:c2e933d53bea 438 }
jah128 4:c2e933d53bea 439
jah128 4:c2e933d53bea 440 float SerialComms::decode_unsigned_float(char byte0)
jah128 4:c2e933d53bea 441 {
jah128 4:c2e933d53bea 442 unsigned short sval = (byte0);
jah128 4:c2e933d53bea 443 float scaled = sval / 255.0f;
jah128 4:c2e933d53bea 444 return scaled;
jah128 4:c2e933d53bea 445 }
jah128 4:c2e933d53bea 446
jah128 4:c2e933d53bea 447
jah128 4:c2e933d53bea 448 char * SerialComms::_nibble_to_binary_char(char in)
jah128 4:c2e933d53bea 449 {
jah128 4:c2e933d53bea 450 char * ret = (char*)malloc(sizeof(char)*5);
jah128 4:c2e933d53bea 451 for(int i=0; i<4; i++) {
jah128 4:c2e933d53bea 452 if(in & (128 >> i)) ret[i]='1';
jah128 4:c2e933d53bea 453 else ret[i]='0';
jah128 4:c2e933d53bea 454 }
jah128 4:c2e933d53bea 455 ret[4]=0;
jah128 4:c2e933d53bea 456 return ret;
jah128 4:c2e933d53bea 457 }
jah128 4:c2e933d53bea 458
jah128 4:c2e933d53bea 459 char * SerialComms::_char_to_binary_char(char in)
jah128 4:c2e933d53bea 460 {
jah128 4:c2e933d53bea 461 char * ret = (char*)malloc(sizeof(char)*9);
jah128 4:c2e933d53bea 462 for(int i=0; i<8; i++) {
jah128 4:c2e933d53bea 463 if(in & (128 >> i)) ret[i]='1';
jah128 4:c2e933d53bea 464 else ret[i]='0';
jah128 4:c2e933d53bea 465 }
jah128 4:c2e933d53bea 466 ret[8]=0;
jah128 4:c2e933d53bea 467 return ret;
jah128 4:c2e933d53bea 468 }