Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

bt_shell/shell/bt_shell.cpp@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:964eb6a2ef00 1 //you could save some memory by disabling the bt_shell (the shell is not used with the PYTHON interface)
majik 0:964eb6a2ef00 2
majik 0:964eb6a2ef00 3 #include "bt_shell.h"
majik 0:964eb6a2ef00 4 #include "tinysh.h"
majik 0:964eb6a2ef00 5 #include "motor_fnt.h"
majik 0:964eb6a2ef00 6 #include "remotecontrol_fnt.h"
majik 0:964eb6a2ef00 7 #include "recon_fnt.h"
majik 0:964eb6a2ef00 8 #include "calibrate_fnt.h"
majik 0:964eb6a2ef00 9 #include "py_machine_interface.h"
majik 0:964eb6a2ef00 10 #include "ros_machine_interface.h"
majik 0:964eb6a2ef00 11 //#include "keybindings.h"
majik 0:964eb6a2ef00 12
majik 0:964eb6a2ef00 13 tinysh_cmd_t movecmd= {0,"move","command motor movements","[L_speed] [R_speed] [duration_ms](optional)",move_fnt,0,0,0};
majik 0:964eb6a2ef00 14 tinysh_cmd_t reconpositioncmd= {0,"resetposition","reset recon position",0,reset_position_fnt,0,0,0};
majik 0:964eb6a2ef00 15 tinysh_cmd_t flipmotorscmd= {0,"flipmotors","left right motors swap",0,flip_motors_fnt,0,0,0};
majik 0:964eb6a2ef00 16 tinysh_cmd_t remotecontrolcmd= {0,"rc","control using keyboard","[speed](optional)",remotecontrol_fnt,0,0,0};
majik 0:964eb6a2ef00 17 tinysh_cmd_t stopmotorscmd= {0,"stop","stop motors",0,stop_motors_fnt,0,0,0};
majik 0:964eb6a2ef00 18 tinysh_cmd_t gotocoordcmd= {0,"gotocoord","goto coordinates",0,goto_coord_fnt,0,0,0};
majik 0:964eb6a2ef00 19 tinysh_cmd_t calibratecmd= {0,"calibrate","calibration settings","[module] [setting] [value]",calibrate_fnt,0,0,0};
majik 0:964eb6a2ef00 20 tinysh_cmd_t scalemotorscmd= {0,"scale","scale motor output","[value]",motor_scale_fnt,0,0,0};
majik 0:964eb6a2ef00 21 //the following functions are for the python interface
majik 0:964eb6a2ef00 22
majik 0:964eb6a2ef00 23 //save a struct that indicates which data is to be returned
majik 0:964eb6a2ef00 24
majik 0:964eb6a2ef00 25 interface iface;
majik 0:964eb6a2ef00 26 bool send_prev = 0;
majik 0:964eb6a2ef00 27
majik 0:964eb6a2ef00 28 //mandatory tiny shell output function
majik 0:964eb6a2ef00 29 void tinysh_char_out(unsigned char c)
majik 0:964eb6a2ef00 30 {
majik 0:964eb6a2ef00 31 bt.lock();
majik 0:964eb6a2ef00 32 bt.putc(c);
majik 0:964eb6a2ef00 33 bt.unlock();
majik 0:964eb6a2ef00 34 }
majik 0:964eb6a2ef00 35
majik 0:964eb6a2ef00 36 //mandatory tiny shell output function
majik 0:964eb6a2ef00 37 void bt_shell_init()
majik 0:964eb6a2ef00 38 {
majik 0:964eb6a2ef00 39 //set prompt
majik 0:964eb6a2ef00 40 tinysh_set_prompt("$ ");
majik 0:964eb6a2ef00 41 t.start();
majik 0:964eb6a2ef00 42 //add custom commands here
majik 0:964eb6a2ef00 43 tinysh_add_command(&movecmd);
majik 0:964eb6a2ef00 44 tinysh_add_command(&reconpositioncmd);
majik 0:964eb6a2ef00 45 tinysh_add_command(&flipmotorscmd);
majik 0:964eb6a2ef00 46 tinysh_add_command(&remotecontrolcmd);
majik 0:964eb6a2ef00 47 tinysh_add_command(&stopmotorscmd);
majik 0:964eb6a2ef00 48 tinysh_add_command(&gotocoordcmd);
majik 0:964eb6a2ef00 49 tinysh_add_command(&calibratecmd);
majik 0:964eb6a2ef00 50 tinysh_add_command(&scalemotorscmd);
majik 0:964eb6a2ef00 51
majik 0:964eb6a2ef00 52 bt.lock();
majik 0:964eb6a2ef00 53 while(bt.readable())
majik 0:964eb6a2ef00 54 bt.getc();
majik 0:964eb6a2ef00 55 bt.unlock();
majik 0:964eb6a2ef00 56
majik 0:964eb6a2ef00 57 tinysh_char_in('\r');
majik 0:964eb6a2ef00 58 tinysh_char_in('\n');
majik 0:964eb6a2ef00 59 }
majik 0:964eb6a2ef00 60
majik 0:964eb6a2ef00 61 void bt_shell_run(){
majik 0:964eb6a2ef00 62
majik 0:964eb6a2ef00 63 static bool py_machine_input = false; //this is for the python and brain file connection (not finished)
majik 0:964eb6a2ef00 64 static bool ros_machine_input = false; //this is for the connection to ROS
majik 0:964eb6a2ef00 65 send_flag=1; //always send stuff
majik 0:964eb6a2ef00 66 bt.lock();
majik 0:964eb6a2ef00 67
majik 0:964eb6a2ef00 68 if(!bt.readable()){
majik 0:964eb6a2ef00 69 bt.unlock();
majik 0:964eb6a2ef00 70 return;
majik 0:964eb6a2ef00 71 }
majik 0:964eb6a2ef00 72
majik 0:964eb6a2ef00 73 char c = bt.getc();
majik 0:964eb6a2ef00 74 bt.unlock();
majik 0:964eb6a2ef00 75
majik 0:964eb6a2ef00 76 if( c == '#' )
majik 0:964eb6a2ef00 77 py_machine_input = true;
majik 0:964eb6a2ef00 78 else if(c== '&'){
majik 0:964eb6a2ef00 79 ros_machine_input = true;
majik 0:964eb6a2ef00 80 bt.lock();
majik 0:964eb6a2ef00 81 bt.printf("&\n\r");
majik 0:964eb6a2ef00 82 bt.unlock();
majik 0:964eb6a2ef00 83 }
majik 0:964eb6a2ef00 84 if(py_machine_input)
majik 0:964eb6a2ef00 85 {
majik 0:964eb6a2ef00 86 bt.lock();
majik 0:964eb6a2ef00 87 bt.baud(BT_BAUD_RATE);
majik 0:964eb6a2ef00 88 bt.unlock();
majik 0:964eb6a2ef00 89 //This is for the python interface
majik 0:964eb6a2ef00 90 while(py_machine_input)
majik 0:964eb6a2ef00 91 {
majik 0:964eb6a2ef00 92 bt.lock();
majik 0:964eb6a2ef00 93 if(bt.readable())
majik 0:964eb6a2ef00 94 {
majik 0:964eb6a2ef00 95 c = bt.getc();
majik 0:964eb6a2ef00 96 bt.unlock();
majik 0:964eb6a2ef00 97 //if(c!= '?')
majik 0:964eb6a2ef00 98 //interface_send_data(iface);
majik 0:964eb6a2ef00 99 py_machine_input = python_shell_connection(c);
majik 0:964eb6a2ef00 100 }
majik 0:964eb6a2ef00 101 bt.unlock();
majik 0:964eb6a2ef00 102 Thread::wait(10);
majik 0:964eb6a2ef00 103 }
majik 0:964eb6a2ef00 104 }
majik 0:964eb6a2ef00 105 else if(ros_machine_input)
majik 0:964eb6a2ef00 106 {
majik 0:964eb6a2ef00 107 while(ros_machine_input)
majik 0:964eb6a2ef00 108 {
majik 0:964eb6a2ef00 109 bt.lock();
majik 0:964eb6a2ef00 110 if(bt.readable())
majik 0:964eb6a2ef00 111 {
majik 0:964eb6a2ef00 112 c= bt.getc();
majik 0:964eb6a2ef00 113 bt.unlock();
majik 0:964eb6a2ef00 114 ros_machine_input = ros_shell_connection(c);
majik 0:964eb6a2ef00 115 }
majik 0:964eb6a2ef00 116 bt.unlock();
majik 0:964eb6a2ef00 117 if(send_flag)
majik 0:964eb6a2ef00 118 {
majik 0:964eb6a2ef00 119 print_all();
majik 0:964eb6a2ef00 120 send_prev = 1;
majik 0:964eb6a2ef00 121 }else if(send_prev)
majik 0:964eb6a2ef00 122 {
majik 0:964eb6a2ef00 123 bt.lock();
majik 0:964eb6a2ef00 124 bt.printf("&;STOPPED;\n\r");
majik 0:964eb6a2ef00 125 bt.unlock();
majik 0:964eb6a2ef00 126 send_prev = 0;
majik 0:964eb6a2ef00 127 }
majik 0:964eb6a2ef00 128 if(obstacle_flag && fwd_flag && !calibrate_flag)
majik 0:964eb6a2ef00 129 {
majik 0:964eb6a2ef00 130 moveMotors(0,0,0);
majik 0:964eb6a2ef00 131 obstacle_flag = 0;
majik 0:964eb6a2ef00 132 send_flag = 0;
majik 0:964eb6a2ef00 133 send_prev = 0;
majik 0:964eb6a2ef00 134 fwd_flag = 0;
majik 0:964eb6a2ef00 135 bt.lock();
majik 0:964eb6a2ef00 136 bt.printf("&;OBSTACLE;\n\r");
majik 0:964eb6a2ef00 137 bt.unlock();
majik 0:964eb6a2ef00 138 }
majik 0:964eb6a2ef00 139 Thread::yield();
majik 0:964eb6a2ef00 140 }
majik 0:964eb6a2ef00 141 }
majik 0:964eb6a2ef00 142 else
majik 0:964eb6a2ef00 143 {
majik 0:964eb6a2ef00 144 tinysh_char_in(c);
majik 0:964eb6a2ef00 145 }
majik 0:964eb6a2ef00 146
majik 0:964eb6a2ef00 147 if ( c == '$')//c == '\n' || c == '\r')
majik 0:964eb6a2ef00 148 py_machine_input = false;
majik 0:964eb6a2ef00 149 }