bt_shell.cpp

00001 #include "bt_shell.h"
00002 //#include "keybindings.h"
00003 //the following functions are for the python interface
00004 
00005 //save a struct that indicates which data is to be returned
00006 Timer tt;
00007 interface iface;
00008 int linear_velocity_value ;
00009 int linear_velocity_direction;
00010 int rotational_velocity_value ;
00011 int rotational_velocity_direction;
00012 int bit_size=20;
00013 int thetha1=300;
00014 int thetha=0;
00015 int stall=0;
00016 int bump=1;
00017 int Lspeed=1;
00018 int Rspeed=1;
00019 int k=0;
00020 int send_thetha=0;
00021 int char_received=0;
00022 DigitalOut myreset(PTA20);
00023 Timeout reset_pgm;
00024 //mandatory tiny shell output function
00025 void print_all()
00026 {
00027     bt.lock();
00028     stdio_mutex.lock();
00029     send_thetha=heading*11.375;
00030     bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
00031               dx/256,dx%256,dy/256,dy%256,\
00032               (send_thetha)/256,(send_thetha)%256,\
00033               stall, bump,\
00034               UL_rR/256,UL_rR%256,\
00035               UL_R/256,UL_R%256, \
00036               UL_F/256,UL_F%256,\
00037               UL_L/256,UL_L%256,\
00038               UL_rL/256,UL_rL%256,\
00039               UL_B/256,UL_B%256);
00040     dx=0;
00041     dy=0;
00042     stdio_mutex.unlock();
00043     bt.unlock();
00044 }
00045 void bt_shell_run()
00046 {
00047     char buffer[12];
00048     bt.lock();
00049     if(bt.readable()) {
00050         bt.gets(buffer,5);
00051         char_received=buffer[2];
00052         bt.unlock();
00053         if(buffer[3]=='R') {
00054             bt.gets(buffer,char_received);
00055             linear_velocity_value = buffer[0]<<8|buffer[1];
00056             linear_velocity_direction= buffer[2];
00057             rotational_velocity_value = buffer[3]<<8|buffer[4];
00058             rotational_velocity_direction= buffer[5];
00059 
00060             if( linear_velocity_direction==0x01) {
00061                 linear_velocity_value=linear_velocity_value;
00062                 linear_velocity_value=linear_velocity_value;
00063             } else if( linear_velocity_direction==0x10) {
00064                 linear_velocity_value=-linear_velocity_value;
00065                 linear_velocity_value=-linear_velocity_value;
00066             }
00067             if( rotational_velocity_direction==0x01 && rotational_velocity_value !=0) {
00068                 rotational_velocity_value=rotational_velocity_value*M_PI*50/180;
00069                 Rmotor_speed=linear_velocity_value+rotational_velocity_value;
00070                 Lmotor_speed=linear_velocity_value-rotational_velocity_value;
00071             } else if( rotational_velocity_direction==0x10 && rotational_velocity_value !=0) {
00072                 rotational_velocity_value=rotational_velocity_value*M_PI*50/180;
00073                 Rmotor_speed=linear_velocity_value-rotational_velocity_value;
00074                 Lmotor_speed=linear_velocity_value+rotational_velocity_value;
00075             } else {
00076                 Rmotor_speed=linear_velocity_value;
00077                 Lmotor_speed=linear_velocity_value;
00078             }
00079         } else if(buffer[0] == 'P'  && buffer[1]== 'O') {
00080             software_interuupt=1;
00081             myreset=0;
00082         } else if(buffer[3] == 'E') {
00083             bt.printf(">>1B");
00084             bt.gets(buffer, 5);
00085             if (buffer[3] =='O') {
00086                 bt.gets(buffer, 2);
00087                 bt_connected=true;
00088                 if(buffer[0]=='A') {
00089                     Selected_robot='A';
00090                     imperial_march();
00091                 } else if(buffer[0]=='F') {
00092                     Selected_robot='F';
00093                     Led_on();
00094                 }
00095             }
00096         } else if(buffer[3] == '?') {
00097             bt.printf("eBot#2");
00098         }
00099         else if(buffer[3] == 'X') {         // Harsh please send <<1X when you are disconnecting 
00100             bt_connected=false;             // from Amigobot it will realise the connection is stooped
00101         }
00102         else {
00103             bt.rxBufferFlush();
00104         }
00105     }
00106     bt.unlock();
00107     if(bt_connected)
00108         print_all();
00109 }