Code for our FYDP -only one IMU works right now -RTOS is working
Embed:
(wiki syntax)
Show/hide line numbers
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 = false; //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::yield(); 00140 } 00141 } 00142 else 00143 { 00144 tinysh_char_in(c); 00145 } 00146 00147 if ( c == '$')//c == '\n' || c == '\r') 00148 py_machine_input = false; 00149 }
Generated on Wed Jul 13 2022 10:32:23 by 1.7.2