Mark Vandermeulen / FYDP_Final2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Servo mbed

bt_shell/shell/bt_shell.cpp@0:21019d94ad33, 2015-03-21 (annotated)

Committer:
majik
Date:
Sat Mar 21 21:31:29 2015 +0000
Revision:
0:21019d94ad33
Child:
2:7b3822eacad8
Both IMUs work now

Who changed what in which revision?

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