Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
shell/bt_shell.cpp
- Committer:
- Throwbot
- Date:
- 2014-10-14
- Revision:
- 4:81b0de07841f
- Parent:
- 3:d1197b5ea68a
File content as of revision 4:81b0de07841f:
#include "bt_shell.h" //#include "keybindings.h" //the following functions are for the python interface //save a struct that indicates which data is to be returned Timer tt; interface iface; int linear_velocity_value ; int linear_velocity_direction; int rotational_velocity_value ; int rotational_velocity_direction; int bit_size=20; int thetha1=300; int thetha=0; int stall=0; int bump=1; int Lspeed=1; int Rspeed=1; int k=0; int send_thetha=0; int char_received=0; DigitalOut myreset(PTA20); Timeout reset_pgm; //mandatory tiny shell output function void print_all() { bt.lock(); stdio_mutex.lock(); send_thetha=heading*11.375; bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", dx/256,dx%256,dy/256,dy%256,\ (send_thetha)/256,(send_thetha)%256,\ stall, bump,\ UL_rR/256,UL_rR%256,\ UL_R/256,UL_R%256, \ UL_F/256,UL_F%256,\ UL_L/256,UL_L%256,\ UL_rL/256,UL_rL%256,\ UL_B/256,UL_B%256); dx=0; dy=0; stdio_mutex.unlock(); bt.unlock(); } void bt_shell_run() { char buffer[12]; bt.lock(); if(bt.readable()) { bt.gets(buffer,5); char_received=buffer[2]; bt.unlock(); if(buffer[3]=='R') { bt.gets(buffer,char_received); linear_velocity_value = buffer[0]<<8|buffer[1]; linear_velocity_direction= buffer[2]; rotational_velocity_value = buffer[3]<<8|buffer[4]; rotational_velocity_direction= buffer[5]; if( linear_velocity_direction==0x01) { linear_velocity_value=linear_velocity_value; linear_velocity_value=linear_velocity_value; } else if( linear_velocity_direction==0x10) { linear_velocity_value=-linear_velocity_value; linear_velocity_value=-linear_velocity_value; } if( rotational_velocity_direction==0x01 && rotational_velocity_value !=0) { rotational_velocity_value=rotational_velocity_value*M_PI*50/180; Rmotor_speed=linear_velocity_value+rotational_velocity_value; Lmotor_speed=linear_velocity_value-rotational_velocity_value; } else if( rotational_velocity_direction==0x10 && rotational_velocity_value !=0) { rotational_velocity_value=rotational_velocity_value*M_PI*50/180; Rmotor_speed=linear_velocity_value-rotational_velocity_value; Lmotor_speed=linear_velocity_value+rotational_velocity_value; } else { Rmotor_speed=linear_velocity_value; Lmotor_speed=linear_velocity_value; } } else if(buffer[0] == 'P' && buffer[1]== 'O') { software_interuupt=1; myreset=0; } else if(buffer[3] == 'E') { bt.printf(">>1B"); bt.gets(buffer, 5); if (buffer[3] =='O') { bt.gets(buffer, 2); bt_connected=true; if(buffer[0]=='A') { Selected_robot='A'; imperial_march(); } else if(buffer[0]=='F') { Selected_robot='F'; Led_on(); } } } else if(buffer[3] == '?') { bt.printf("eBot#2"); } else if(buffer[3] == 'X') { // Harsh please send <<1X when you are disconnecting bt_connected=false; // from Amigobot it will realise the connection is stooped } else { bt.rxBufferFlush(); } } bt.unlock(); if(bt_connected) print_all(); }