Initial Commit
Dependencies: mbed HC05 QEI MODSERIAL SWSPI mbed-rtos
shell/bt_shell.cpp@6:5ab1735265a9, 2014-10-21 (annotated)
- Committer:
- harshb
- Date:
- Tue Oct 21 21:59:15 2014 +0000
- Revision:
- 6:5ab1735265a9
- Parent:
- 4:81b0de07841f
Initial Commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Throwbot | 2:0f80c8a1c236 | 1 | #include "bt_shell.h" |
Throwbot | 2:0f80c8a1c236 | 2 | //#include "keybindings.h" |
Throwbot | 2:0f80c8a1c236 | 3 | //the following functions are for the python interface |
Throwbot | 2:0f80c8a1c236 | 4 | |
Throwbot | 2:0f80c8a1c236 | 5 | //save a struct that indicates which data is to be returned |
Throwbot | 2:0f80c8a1c236 | 6 | Timer tt; |
Throwbot | 2:0f80c8a1c236 | 7 | interface iface; |
Throwbot | 2:0f80c8a1c236 | 8 | int linear_velocity_value ; |
Throwbot | 2:0f80c8a1c236 | 9 | int linear_velocity_direction; |
Throwbot | 2:0f80c8a1c236 | 10 | int rotational_velocity_value ; |
Throwbot | 2:0f80c8a1c236 | 11 | int rotational_velocity_direction; |
Throwbot | 2:0f80c8a1c236 | 12 | int bit_size=20; |
Throwbot | 2:0f80c8a1c236 | 13 | int thetha1=300; |
Throwbot | 2:0f80c8a1c236 | 14 | int thetha=0; |
Throwbot | 2:0f80c8a1c236 | 15 | int stall=0; |
Throwbot | 2:0f80c8a1c236 | 16 | int bump=1; |
Throwbot | 2:0f80c8a1c236 | 17 | int Lspeed=1; |
Throwbot | 2:0f80c8a1c236 | 18 | int Rspeed=1; |
Throwbot | 2:0f80c8a1c236 | 19 | int k=0; |
Throwbot | 3:d1197b5ea68a | 20 | int send_thetha=0; |
Throwbot | 2:0f80c8a1c236 | 21 | int char_received=0; |
Throwbot | 2:0f80c8a1c236 | 22 | DigitalOut myreset(PTA20); |
Throwbot | 2:0f80c8a1c236 | 23 | Timeout reset_pgm; |
Throwbot | 2:0f80c8a1c236 | 24 | //mandatory tiny shell output function |
Throwbot | 2:0f80c8a1c236 | 25 | void print_all() |
Throwbot | 2:0f80c8a1c236 | 26 | { |
Throwbot | 2:0f80c8a1c236 | 27 | bt.lock(); |
Throwbot | 2:0f80c8a1c236 | 28 | stdio_mutex.lock(); |
Throwbot | 3:d1197b5ea68a | 29 | send_thetha=heading*11.375; |
Throwbot | 2:0f80c8a1c236 | 30 | bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", |
Throwbot | 2:0f80c8a1c236 | 31 | dx/256,dx%256,dy/256,dy%256,\ |
Throwbot | 3:d1197b5ea68a | 32 | (send_thetha)/256,(send_thetha)%256,\ |
Throwbot | 2:0f80c8a1c236 | 33 | stall, bump,\ |
Throwbot | 2:0f80c8a1c236 | 34 | UL_rR/256,UL_rR%256,\ |
Throwbot | 2:0f80c8a1c236 | 35 | UL_R/256,UL_R%256, \ |
Throwbot | 2:0f80c8a1c236 | 36 | UL_F/256,UL_F%256,\ |
Throwbot | 2:0f80c8a1c236 | 37 | UL_L/256,UL_L%256,\ |
Throwbot | 2:0f80c8a1c236 | 38 | UL_rL/256,UL_rL%256,\ |
Throwbot | 2:0f80c8a1c236 | 39 | UL_B/256,UL_B%256); |
Throwbot | 2:0f80c8a1c236 | 40 | dx=0; |
Throwbot | 2:0f80c8a1c236 | 41 | dy=0; |
Throwbot | 2:0f80c8a1c236 | 42 | stdio_mutex.unlock(); |
Throwbot | 2:0f80c8a1c236 | 43 | bt.unlock(); |
Throwbot | 2:0f80c8a1c236 | 44 | } |
Throwbot | 2:0f80c8a1c236 | 45 | void bt_shell_run() |
Throwbot | 2:0f80c8a1c236 | 46 | { |
Throwbot | 2:0f80c8a1c236 | 47 | char buffer[12]; |
Throwbot | 2:0f80c8a1c236 | 48 | bt.lock(); |
Throwbot | 2:0f80c8a1c236 | 49 | if(bt.readable()) { |
Throwbot | 2:0f80c8a1c236 | 50 | bt.gets(buffer,5); |
Throwbot | 2:0f80c8a1c236 | 51 | char_received=buffer[2]; |
Throwbot | 2:0f80c8a1c236 | 52 | bt.unlock(); |
Throwbot | 2:0f80c8a1c236 | 53 | if(buffer[3]=='R') { |
Throwbot | 2:0f80c8a1c236 | 54 | bt.gets(buffer,char_received); |
Throwbot | 2:0f80c8a1c236 | 55 | linear_velocity_value = buffer[0]<<8|buffer[1]; |
Throwbot | 2:0f80c8a1c236 | 56 | linear_velocity_direction= buffer[2]; |
Throwbot | 2:0f80c8a1c236 | 57 | rotational_velocity_value = buffer[3]<<8|buffer[4]; |
Throwbot | 2:0f80c8a1c236 | 58 | rotational_velocity_direction= buffer[5]; |
Throwbot | 4:81b0de07841f | 59 | |
Throwbot | 3:d1197b5ea68a | 60 | if( linear_velocity_direction==0x01) { |
Throwbot | 3:d1197b5ea68a | 61 | linear_velocity_value=linear_velocity_value; |
Throwbot | 3:d1197b5ea68a | 62 | linear_velocity_value=linear_velocity_value; |
Throwbot | 2:0f80c8a1c236 | 63 | } else if( linear_velocity_direction==0x10) { |
Throwbot | 3:d1197b5ea68a | 64 | linear_velocity_value=-linear_velocity_value; |
Throwbot | 3:d1197b5ea68a | 65 | linear_velocity_value=-linear_velocity_value; |
Throwbot | 2:0f80c8a1c236 | 66 | } |
Throwbot | 2:0f80c8a1c236 | 67 | if( rotational_velocity_direction==0x01 && rotational_velocity_value !=0) { |
Throwbot | 3:d1197b5ea68a | 68 | rotational_velocity_value=rotational_velocity_value*M_PI*50/180; |
Throwbot | 3:d1197b5ea68a | 69 | Rmotor_speed=linear_velocity_value+rotational_velocity_value; |
Throwbot | 3:d1197b5ea68a | 70 | Lmotor_speed=linear_velocity_value-rotational_velocity_value; |
Throwbot | 2:0f80c8a1c236 | 71 | } else if( rotational_velocity_direction==0x10 && rotational_velocity_value !=0) { |
Throwbot | 3:d1197b5ea68a | 72 | rotational_velocity_value=rotational_velocity_value*M_PI*50/180; |
Throwbot | 3:d1197b5ea68a | 73 | Rmotor_speed=linear_velocity_value-rotational_velocity_value; |
Throwbot | 3:d1197b5ea68a | 74 | Lmotor_speed=linear_velocity_value+rotational_velocity_value; |
Throwbot | 3:d1197b5ea68a | 75 | } else { |
Throwbot | 3:d1197b5ea68a | 76 | Rmotor_speed=linear_velocity_value; |
Throwbot | 3:d1197b5ea68a | 77 | Lmotor_speed=linear_velocity_value; |
Throwbot | 3:d1197b5ea68a | 78 | } |
Throwbot | 2:0f80c8a1c236 | 79 | } else if(buffer[0] == 'P' && buffer[1]== 'O') { |
Throwbot | 3:d1197b5ea68a | 80 | software_interuupt=1; |
Throwbot | 2:0f80c8a1c236 | 81 | myreset=0; |
Throwbot | 4:81b0de07841f | 82 | } else if(buffer[3] == 'E') { |
Throwbot | 4:81b0de07841f | 83 | bt.printf(">>1B"); |
Throwbot | 4:81b0de07841f | 84 | bt.gets(buffer, 5); |
Throwbot | 4:81b0de07841f | 85 | if (buffer[3] =='O') { |
Throwbot | 4:81b0de07841f | 86 | bt.gets(buffer, 2); |
Throwbot | 4:81b0de07841f | 87 | bt_connected=true; |
Throwbot | 4:81b0de07841f | 88 | if(buffer[0]=='A') { |
Throwbot | 4:81b0de07841f | 89 | Selected_robot='A'; |
Throwbot | 4:81b0de07841f | 90 | imperial_march(); |
Throwbot | 4:81b0de07841f | 91 | } else if(buffer[0]=='F') { |
Throwbot | 4:81b0de07841f | 92 | Selected_robot='F'; |
Throwbot | 4:81b0de07841f | 93 | Led_on(); |
Throwbot | 4:81b0de07841f | 94 | } |
Throwbot | 4:81b0de07841f | 95 | } |
Throwbot | 4:81b0de07841f | 96 | } else if(buffer[3] == '?') { |
Throwbot | 4:81b0de07841f | 97 | bt.printf("eBot#2"); |
Throwbot | 4:81b0de07841f | 98 | } |
Throwbot | 4:81b0de07841f | 99 | else if(buffer[3] == 'X') { // Harsh please send <<1X when you are disconnecting |
Throwbot | 4:81b0de07841f | 100 | bt_connected=false; // from Amigobot it will realise the connection is stooped |
Throwbot | 4:81b0de07841f | 101 | } |
Throwbot | 3:d1197b5ea68a | 102 | else { |
Throwbot | 2:0f80c8a1c236 | 103 | bt.rxBufferFlush(); |
Throwbot | 2:0f80c8a1c236 | 104 | } |
Throwbot | 2:0f80c8a1c236 | 105 | } |
Throwbot | 2:0f80c8a1c236 | 106 | bt.unlock(); |
Throwbot | 4:81b0de07841f | 107 | if(bt_connected) |
Throwbot | 4:81b0de07841f | 108 | print_all(); |
Throwbot | 2:0f80c8a1c236 | 109 | } |