EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

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?

UserRevisionLine numberNew 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 }