bt_shell.cpp

00001 //you could save some memory by disabling the bt_shell (the shell is not used with the PYTHON interface)
00002 
00003 #include "bt_shell.h"
00004 #include "tinysh.h"
00005 #include "motor_fnt.h"
00006 #include "remotecontrol_fnt.h"
00007 #include "recon_fnt.h"
00008 #include "calibrate_fnt.h"
00009 #include "py_machine_interface.h"
00010 #include "ros_machine_interface.h"
00011 //#include "keybindings.h"
00012 
00013 tinysh_cmd_t movecmd= {0,"move","command motor movements","[L_speed] [R_speed] [duration_ms](optional)",move_fnt,0,0,0};
00014 tinysh_cmd_t reconpositioncmd= {0,"resetposition","reset recon position",0,reset_position_fnt,0,0,0};
00015 tinysh_cmd_t flipmotorscmd= {0,"flipmotors","left right motors swap",0,flip_motors_fnt,0,0,0};
00016 tinysh_cmd_t remotecontrolcmd= {0,"rc","control using keyboard","[speed](optional)",remotecontrol_fnt,0,0,0};
00017 tinysh_cmd_t stopmotorscmd= {0,"stop","stop motors",0,stop_motors_fnt,0,0,0};
00018 tinysh_cmd_t gotocoordcmd= {0,"gotocoord","goto coordinates",0,goto_coord_fnt,0,0,0};
00019 tinysh_cmd_t calibratecmd= {0,"calibrate","calibration settings","[module] [setting] [value]",calibrate_fnt,0,0,0};
00020 tinysh_cmd_t scalemotorscmd= {0,"scale","scale motor output","[value]",motor_scale_fnt,0,0,0};
00021 //the following functions are for the python interface
00022 
00023 //save a struct that indicates which data is to be returned
00024 
00025 interface iface;
00026 bool send_prev = 0;
00027 
00028 //mandatory tiny shell output function
00029 void tinysh_char_out(unsigned char c)
00030 {
00031     bt.lock();
00032     bt.putc(c);
00033     bt.unlock();
00034 }
00035 
00036 //mandatory tiny shell output function
00037 void bt_shell_init()
00038 { 
00039     //set prompt
00040     tinysh_set_prompt("$ ");
00041 t.start();
00042     //add custom commands here
00043     tinysh_add_command(&movecmd);
00044     tinysh_add_command(&reconpositioncmd);
00045     tinysh_add_command(&flipmotorscmd);
00046     tinysh_add_command(&remotecontrolcmd);
00047     tinysh_add_command(&stopmotorscmd);
00048     tinysh_add_command(&gotocoordcmd);
00049     tinysh_add_command(&calibratecmd);
00050     tinysh_add_command(&scalemotorscmd);
00051  
00052     bt.lock();
00053     while(bt.readable())
00054         bt.getc();
00055     bt.unlock();
00056         
00057     tinysh_char_in('\r');
00058     tinysh_char_in('\n');
00059 }
00060 
00061 void bt_shell_run(){
00062 
00063     static bool py_machine_input = false;   //this is for the python and brain file connection (not finished)
00064     static bool ros_machine_input = true;  //this is for the connection to ROS
00065 send_flag=1; //always send stuff
00066     bt.lock();
00067     
00068     if(!bt.readable()){
00069         bt.unlock();
00070         return;
00071     }
00072         
00073     char c = bt.getc();
00074     bt.unlock();
00075        
00076     if( c == '#' )
00077         py_machine_input = true;
00078     else if(c== '&'){
00079         ros_machine_input = true;
00080         bt.lock();
00081         bt.printf("&\n\r");
00082         bt.unlock();
00083     }
00084     if(py_machine_input)
00085     {
00086         bt.lock();
00087         bt.baud(BT_BAUD_RATE);
00088         bt.unlock();
00089         //This is for the python interface
00090         while(py_machine_input)
00091         {
00092             bt.lock();
00093             if(bt.readable()) 
00094             {
00095                 c = bt.getc();
00096                 bt.unlock();
00097                 //if(c!= '?')
00098                 //interface_send_data(iface);
00099                 py_machine_input = python_shell_connection(c);
00100             }
00101             bt.unlock();
00102             Thread::wait(10);
00103         }
00104     }
00105     else if(ros_machine_input)
00106     {
00107         while(ros_machine_input)
00108         {
00109             bt.lock();
00110             if(bt.readable())
00111             {
00112                 c= bt.getc();
00113                 bt.unlock();
00114                 ros_machine_input = ros_shell_connection(c);
00115             }
00116             bt.unlock();
00117             if(send_flag)
00118             {
00119                 print_all();
00120                 send_prev = 1;
00121             }else if(send_prev)
00122             {   
00123                 bt.lock();
00124                 bt.printf("&;STOPPED;\n\r");
00125                 bt.unlock();
00126                 send_prev = 0;
00127             }
00128             if(obstacle_flag && fwd_flag && !calibrate_flag) 
00129             {
00130                 //moveMotors(0,0,0);
00131                 obstacle_flag = 0;
00132                 send_flag = 0;
00133                 send_prev = 0;
00134                 fwd_flag = 0;
00135                 bt.lock();
00136                 bt.printf("&;OBSTACLE;\n\r");
00137                 bt.unlock();
00138             }
00139             Thread::wait(20);
00140 //            Thread::yield();
00141         }
00142     }
00143     else
00144     {
00145         tinysh_char_in(c);
00146     }
00147           
00148     if ( c == '$')//c == '\n' || c == '\r')
00149         py_machine_input = false;
00150 }