Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

serialcomms.cpp@6:732aa91eb555, 2017-01-13 (annotated)

Committer:
jah128
Date:
Fri Jan 13 23:16:23 2017 +0000
Revision:
6:732aa91eb555
Parent:
5:6da8daaeb9f7 
Updated;

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