Code for our FYDP -only one IMU works right now -RTOS is working
bt_shell/shell/bt_shell.cpp
- Committer:
- majik
- Date:
- 2015-03-18
- Revision:
- 0:964eb6a2ef00
File content as of revision 0:964eb6a2ef00:
//you could save some memory by disabling the bt_shell (the shell is not used with the PYTHON interface) #include "bt_shell.h" #include "tinysh.h" #include "motor_fnt.h" #include "remotecontrol_fnt.h" #include "recon_fnt.h" #include "calibrate_fnt.h" #include "py_machine_interface.h" #include "ros_machine_interface.h" //#include "keybindings.h" tinysh_cmd_t movecmd= {0,"move","command motor movements","[L_speed] [R_speed] [duration_ms](optional)",move_fnt,0,0,0}; tinysh_cmd_t reconpositioncmd= {0,"resetposition","reset recon position",0,reset_position_fnt,0,0,0}; tinysh_cmd_t flipmotorscmd= {0,"flipmotors","left right motors swap",0,flip_motors_fnt,0,0,0}; tinysh_cmd_t remotecontrolcmd= {0,"rc","control using keyboard","[speed](optional)",remotecontrol_fnt,0,0,0}; tinysh_cmd_t stopmotorscmd= {0,"stop","stop motors",0,stop_motors_fnt,0,0,0}; tinysh_cmd_t gotocoordcmd= {0,"gotocoord","goto coordinates",0,goto_coord_fnt,0,0,0}; tinysh_cmd_t calibratecmd= {0,"calibrate","calibration settings","[module] [setting] [value]",calibrate_fnt,0,0,0}; tinysh_cmd_t scalemotorscmd= {0,"scale","scale motor output","[value]",motor_scale_fnt,0,0,0}; //the following functions are for the python interface //save a struct that indicates which data is to be returned interface iface; bool send_prev = 0; //mandatory tiny shell output function void tinysh_char_out(unsigned char c) { bt.lock(); bt.putc(c); bt.unlock(); } //mandatory tiny shell output function void bt_shell_init() { //set prompt tinysh_set_prompt("$ "); t.start(); //add custom commands here tinysh_add_command(&movecmd); tinysh_add_command(&reconpositioncmd); tinysh_add_command(&flipmotorscmd); tinysh_add_command(&remotecontrolcmd); tinysh_add_command(&stopmotorscmd); tinysh_add_command(&gotocoordcmd); tinysh_add_command(&calibratecmd); tinysh_add_command(&scalemotorscmd); bt.lock(); while(bt.readable()) bt.getc(); bt.unlock(); tinysh_char_in('\r'); tinysh_char_in('\n'); } void bt_shell_run(){ static bool py_machine_input = false; //this is for the python and brain file connection (not finished) static bool ros_machine_input = false; //this is for the connection to ROS send_flag=1; //always send stuff bt.lock(); if(!bt.readable()){ bt.unlock(); return; } char c = bt.getc(); bt.unlock(); if( c == '#' ) py_machine_input = true; else if(c== '&'){ ros_machine_input = true; bt.lock(); bt.printf("&\n\r"); bt.unlock(); } if(py_machine_input) { bt.lock(); bt.baud(BT_BAUD_RATE); bt.unlock(); //This is for the python interface while(py_machine_input) { bt.lock(); if(bt.readable()) { c = bt.getc(); bt.unlock(); //if(c!= '?') //interface_send_data(iface); py_machine_input = python_shell_connection(c); } bt.unlock(); Thread::wait(10); } } else if(ros_machine_input) { while(ros_machine_input) { bt.lock(); if(bt.readable()) { c= bt.getc(); bt.unlock(); ros_machine_input = ros_shell_connection(c); } bt.unlock(); if(send_flag) { print_all(); send_prev = 1; }else if(send_prev) { bt.lock(); bt.printf("&;STOPPED;\n\r"); bt.unlock(); send_prev = 0; } if(obstacle_flag && fwd_flag && !calibrate_flag) { moveMotors(0,0,0); obstacle_flag = 0; send_flag = 0; send_prev = 0; fwd_flag = 0; bt.lock(); bt.printf("&;OBSTACLE;\n\r"); bt.unlock(); } Thread::yield(); } } else { tinysh_char_in(c); } if ( c == '$')//c == '\n' || c == '\r') py_machine_input = false; }