Mark Vandermeulen / FYDP_Final

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

Dependencies:   mbed

bt_shell/machine_interface/ros_machine_interface.h@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 #ifndef ROS_MACHINE_INTERFACE_H
majik 0:964eb6a2ef00 2 #define ROS_MACHINE_INTERFACE_H
majik 0:964eb6a2ef00 3 //THIS FILE IS INCOMPLETE///
majik 0:964eb6a2ef00 4
majik 0:964eb6a2ef00 5
majik 0:964eb6a2ef00 6 /**This file contains the implementations for the controls through a python interface
majik 0:964eb6a2ef00 7 //The brain file uses ';' to separate lines
majik 0:964eb6a2ef00 8 Read IMU [acceleration and gyros]
majik 0:964eb6a2ef00 9 Read Gyro
majik 0:964eb6a2ef00 10 Read Angle
majik 0:964eb6a2ef00 11 Read Acceleration
majik 0:964eb6a2ef00 12 Read IR sensors
majik 0:964eb6a2ef00 13 Read Optical flow sensors
majik 0:964eb6a2ef00 14 Read current sensor
majik 0:964eb6a2ef00 15 Read voltage sensor
majik 0:964eb6a2ef00 16
majik 0:964eb6a2ef00 17 Drive to [x,y] (x,y)
majik 0:964eb6a2ef00 18 Drive fwd (speed, time)
majik 0:964eb6a2ef00 19 Run R motor (speed,time)
majik 0:964eb6a2ef00 20 Run L motor (speed,time)
majik 0:964eb6a2ef00 21 Set Direction (direction)
majik 0:964eb6a2ef00 22 Enable IMU control (bool) //if this is enabled, IMU will keep robot straight
majik 0:964eb6a2ef00 23 */
majik 0:964eb6a2ef00 24 #include "keybindings.h"
majik 0:964eb6a2ef00 25
majik 0:964eb6a2ef00 26
majik 0:964eb6a2ef00 27 int ros_get_cmd(); //this reads from the bt connection
majik 0:964eb6a2ef00 28 int q_pow10(int n); //this calculates 10^n quickly (only up to 10^6)
majik 0:964eb6a2ef00 29 void print_all();
majik 0:964eb6a2ef00 30 int ros_get_cmd(char s[16], int n);
majik 0:964eb6a2ef00 31 //char C;
majik 0:964eb6a2ef00 32 //char buffer[10];
majik 0:964eb6a2ef00 33 int IR_mm(float x);
majik 0:964eb6a2ef00 34
majik 0:964eb6a2ef00 35 bool ros_shell_connection(char c)
majik 0:964eb6a2ef00 36 {
majik 0:964eb6a2ef00 37 char S[16];
majik 0:964eb6a2ef00 38 float dum;
majik 0:964eb6a2ef00 39 float param[5];
majik 0:964eb6a2ef00 40 int arg_in[10];
majik 0:964eb6a2ef00 41 char run_time;
majik 0:964eb6a2ef00 42 int m_offset = 100; //use this so that the MATLAB can send motor values between 0 and 200 instead of -100 and 100
majik 0:964eb6a2ef00 43 //bt.printf("&%f;",t.read());
majik 0:964eb6a2ef00 44
majik 0:964eb6a2ef00 45 switch (c)
majik 0:964eb6a2ef00 46 {
majik 0:964eb6a2ef00 47 case CONNECTION: //Return query if connection is ready
majik 0:964eb6a2ef00 48 bt.lock();
majik 0:964eb6a2ef00 49 bt.printf("\nEBOTDEMO\n");
majik 0:964eb6a2ef00 50 bt.unlock();
majik 0:964eb6a2ef00 51 reckon.reset();
majik 0:964eb6a2ef00 52 break;
majik 0:964eb6a2ef00 53 case ALL_SENSOR: //returns ALL sensor data //while robot is moving, send things every 50milliseconds
majik 0:964eb6a2ef00 54 print_all();
majik 0:964eb6a2ef00 55 break;
majik 0:964eb6a2ef00 56 case CURRENT_SENSE:
majik 0:964eb6a2ef00 57 break;
majik 0:964eb6a2ef00 58 case IMU_READ:
majik 0:964eb6a2ef00 59 send_flag = 1;
majik 0:964eb6a2ef00 60 //bt.lock();
majik 0:964eb6a2ef00 61 //bt.printf("i;%3.3f;%3.3f;%3.3f;%3.3f;%3.3f;%3.3f;",imu_data.acc[0],imu_data.acc[1],imu_data.acc[2],imu_data.ypr[0],imu_data.ypr[1],imu_data.ypr[2]);
majik 0:964eb6a2ef00 62 //bt.unlock();
majik 0:964eb6a2ef00 63 break;
majik 0:964eb6a2ef00 64 case IMU_YAW:
majik 0:964eb6a2ef00 65 dum = imu_data.ypr[0]*180/PI;
majik 0:964eb6a2ef00 66 bt.lock();
majik 0:964eb6a2ef00 67 bt.printf("y;%3.3f;",dum);
majik 0:964eb6a2ef00 68 bt.unlock();
majik 0:964eb6a2ef00 69 break;
majik 0:964eb6a2ef00 70 //optical flow
majik 0:964eb6a2ef00 71 case OPT_READ:
majik 0:964eb6a2ef00 72 //return optical flow (pixels/second for left and right)
majik 0:964eb6a2ef00 73 //TODO: include second optical flow sensor
majik 0:964eb6a2ef00 74 //bt.printf("O;%3.3f;%3.3f;",opt_flow.dx_px,opt_flow.dy_px);
majik 0:964eb6a2ef00 75 bt.lock();
majik 0:964eb6a2ef00 76 bt.printf("%c;%d;%d;%d;%d;",OPT_READ,reckon.flow_dx_L,reckon.flow_dy_L,reckon.flow_dx_R,reckon.flow_dy_R);
majik 0:964eb6a2ef00 77 bt.unlock();
majik 0:964eb6a2ef00 78 reckon.reset();
majik 0:964eb6a2ef00 79 break;
majik 0:964eb6a2ef00 80 case IR_READ:
majik 0:964eb6a2ef00 81 //return 5 IR sensors clockwise (Front, right_front, right_rear, left_rear, left_front)
majik 0:964eb6a2ef00 82 //ask Wen to fill out this function
majik 0:964eb6a2ef00 83 extern AnalogIn irL;
majik 0:964eb6a2ef00 84 extern AnalogIn irC;
majik 0:964eb6a2ef00 85 extern AnalogIn irR;
majik 0:964eb6a2ef00 86 extern DigitalOut irBack;
majik 0:964eb6a2ef00 87 if(irBack){
majik 0:964eb6a2ef00 88 irBack = 0; //read from IR
majik 0:964eb6a2ef00 89 Thread::wait(50);
majik 0:964eb6a2ef00 90 }
majik 0:964eb6a2ef00 91 param[0] = irL.read();
majik 0:964eb6a2ef00 92 Thread::wait(10);
majik 0:964eb6a2ef00 93 param[1] = irC.read();
majik 0:964eb6a2ef00 94 Thread::wait(10);
majik 0:964eb6a2ef00 95 param[2] = irR.read();
majik 0:964eb6a2ef00 96 param[3] = 0;
majik 0:964eb6a2ef00 97 param[4]=0;
majik 0:964eb6a2ef00 98 //irBack = 1; //read back IR
majik 0:964eb6a2ef00 99 /*Thread::wait(50);
majik 0:964eb6a2ef00 100 param[3] = irL.read();
majik 0:964eb6a2ef00 101 Thread::wait(10);
majik 0:964eb6a2ef00 102 param[4] = irR.read();
majik 0:964eb6a2ef00 103 irBack = 0;*/
majik 0:964eb6a2ef00 104 //bt.printf("%f;%f;%f;%f;%f;",param[0],param[1],param[2],param[3],param[4]);
majik 0:964eb6a2ef00 105 for(int i = 0; i<5; i++)
majik 0:964eb6a2ef00 106 arg_in[i]=IR_mm(param[i]);
majik 0:964eb6a2ef00 107 bt.lock();
majik 0:964eb6a2ef00 108 bt.printf("%d;%d;%d;%d;%d;",arg_in[0],arg_in[1],arg_in[2],arg_in[3],arg_in[4]);
majik 0:964eb6a2ef00 109 bt.unlock();
majik 0:964eb6a2ef00 110 //Thread::wait(40);
majik 0:964eb6a2ef00 111 break;
majik 0:964eb6a2ef00 112 /*case IR_1:
majik 0:964eb6a2ef00 113 if(irBack){
majik 0:964eb6a2ef00 114 irBack = 0;
majik 0:964eb6a2ef00 115 Thread::wait(50);
majik 0:964eb6a2ef00 116 }
majik 0:964eb6a2ef00 117 param[0] = irL.read();
majik 0:964eb6a2ef00 118 bt.printf("%c;%f;",IR_1,param[0]);
majik 0:964eb6a2ef00 119 break;
majik 0:964eb6a2ef00 120 case IR_2:
majik 0:964eb6a2ef00 121 if(irBack){
majik 0:964eb6a2ef00 122 irBack = 0;
majik 0:964eb6a2ef00 123 Thread::wait(50);
majik 0:964eb6a2ef00 124 }
majik 0:964eb6a2ef00 125 param[0] = irC.read();
majik 0:964eb6a2ef00 126 bt.printf("%c;%f;",IR_2,param[0]);
majik 0:964eb6a2ef00 127 break;
majik 0:964eb6a2ef00 128 case IR_3:
majik 0:964eb6a2ef00 129 if(irBack){
majik 0:964eb6a2ef00 130 irBack = 0;
majik 0:964eb6a2ef00 131 Thread::wait(50);
majik 0:964eb6a2ef00 132 }
majik 0:964eb6a2ef00 133 param[0] = irR.read();
majik 0:964eb6a2ef00 134 bt.printf("%c;%f;",IR_3,param[0]);
majik 0:964eb6a2ef00 135 break;
majik 0:964eb6a2ef00 136 case IR_4:
majik 0:964eb6a2ef00 137 if(!irBack){
majik 0:964eb6a2ef00 138 irBack = 1;
majik 0:964eb6a2ef00 139 Thread::wait(50);
majik 0:964eb6a2ef00 140 }
majik 0:964eb6a2ef00 141 param[0] = irR.read();
majik 0:964eb6a2ef00 142 bt.printf("%c;%f;",IR_4,param[0]);
majik 0:964eb6a2ef00 143 break;
majik 0:964eb6a2ef00 144 case IR_5:
majik 0:964eb6a2ef00 145 if(!irBack){
majik 0:964eb6a2ef00 146 irBack = 1;
majik 0:964eb6a2ef00 147 Thread::wait(50);
majik 0:964eb6a2ef00 148 }
majik 0:964eb6a2ef00 149 param[0] = irL.read();
majik 0:964eb6a2ef00 150 bt.printf("%c;%f;",IR_5,param[0]);
majik 0:964eb6a2ef00 151 break;*/
majik 0:964eb6a2ef00 152 case LOCATION:
majik 0:964eb6a2ef00 153 //return x,y,z,theta (maybe not z)
majik 0:964eb6a2ef00 154 break;
majik 0:964eb6a2ef00 155
majik 0:964eb6a2ef00 156 case MOTOR: //command: 'w[L_speed],[R_speed] //I should write an actual function to receive data from serial
majik 0:964eb6a2ef00 157 //Another argument should be included: time to run motors.
majik 0:964eb6a2ef00 158 arg_in[0] = ros_get_cmd();
majik 0:964eb6a2ef00 159 arg_in[1] = ros_get_cmd();
majik 0:964eb6a2ef00 160 arg_in[2] = ros_get_cmd();
majik 0:964eb6a2ef00 161 if(arg_in[0] == arg_in[1] && arg_in[0] > 100){
majik 0:964eb6a2ef00 162 if(IR_mm(irC.read()) < 260 && IR_mm(irC.read()) > 0) //if there is an obstacle, don't drive fwd
majik 0:964eb6a2ef00 163 {
majik 0:964eb6a2ef00 164 bt.lock();
majik 0:964eb6a2ef00 165 bt.printf("&;OBSTACLE;");
majik 0:964eb6a2ef00 166 bt.unlock();
majik 0:964eb6a2ef00 167 break;
majik 0:964eb6a2ef00 168 }
majik 0:964eb6a2ef00 169 fwd_flag = 1;
majik 0:964eb6a2ef00 170 }
majik 0:964eb6a2ef00 171 moveMotors(arg_in[0]-m_offset,arg_in[1]-m_offset,arg_in[2]);
majik 0:964eb6a2ef00 172 bt.lock();
majik 0:964eb6a2ef00 173 bt.printf("L=%d;R=%d;t=%d",arg_in[0],arg_in[1],arg_in[2]); //don't need to respond. Maybe have a confirmation?
majik 0:964eb6a2ef00 174 bt.unlock();
majik 0:964eb6a2ef00 175 send_flag = 1;
majik 0:964eb6a2ef00 176 break;
majik 0:964eb6a2ef00 177
majik 0:964eb6a2ef00 178 case (MOTOR_STOP):
majik 0:964eb6a2ef00 179 *motors.Right = 0;
majik 0:964eb6a2ef00 180 *motors.Left = 0;
majik 0:964eb6a2ef00 181 send_flag = 0; //
majik 0:964eb6a2ef00 182 break;
majik 0:964eb6a2ef00 183
majik 0:964eb6a2ef00 184
majik 0:964eb6a2ef00 185
majik 0:964eb6a2ef00 186
majik 0:964eb6a2ef00 187 case (IMU_CALIBRATE):
majik 0:964eb6a2ef00 188 calibrate_flag = 1;
majik 0:964eb6a2ef00 189 break;
majik 0:964eb6a2ef00 190 case (OPT_CALIBRATE):
majik 0:964eb6a2ef00 191 calibrate_optFlow_flag=1;
majik 0:964eb6a2ef00 192 break;
majik 0:964eb6a2ef00 193 case END_CONNECTION:
majik 0:964eb6a2ef00 194 bt.lock();
majik 0:964eb6a2ef00 195 bt.printf("End Connection\n");
majik 0:964eb6a2ef00 196 bt.unlock();
majik 0:964eb6a2ef00 197 return 0;
majik 0:964eb6a2ef00 198 ///break;
majik 0:964eb6a2ef00 199 default:
majik 0:964eb6a2ef00 200 bt.lock();
majik 0:964eb6a2ef00 201 bt.putc(c); //echo command
majik 0:964eb6a2ef00 202 bt.unlock();
majik 0:964eb6a2ef00 203 break;
majik 0:964eb6a2ef00 204 }
majik 0:964eb6a2ef00 205 bt.lock();
majik 0:964eb6a2ef00 206 bt.printf("\n\r");
majik 0:964eb6a2ef00 207 bt.unlock();
majik 0:964eb6a2ef00 208 return 1;
majik 0:964eb6a2ef00 209 }
majik 0:964eb6a2ef00 210 int ros_get_cmd(char s[16], int n)
majik 0:964eb6a2ef00 211 {
majik 0:964eb6a2ef00 212 int i= 0, j = 0;
majik 0:964eb6a2ef00 213 char S[16];
majik 0:964eb6a2ef00 214 int count = 0;
majik 0:964eb6a2ef00 215 while(count < n && i<13)
majik 0:964eb6a2ef00 216 {
majik 0:964eb6a2ef00 217 if(s[i] == ';')
majik 0:964eb6a2ef00 218 count++;
majik 0:964eb6a2ef00 219 i++;
majik 0:964eb6a2ef00 220 }
majik 0:964eb6a2ef00 221 count = 0;
majik 0:964eb6a2ef00 222 while(i<13)
majik 0:964eb6a2ef00 223 {
majik 0:964eb6a2ef00 224 if(s[i] == ';')
majik 0:964eb6a2ef00 225 break;
majik 0:964eb6a2ef00 226 S[j] = s[i];
majik 0:964eb6a2ef00 227 j++;
majik 0:964eb6a2ef00 228 i++;
majik 0:964eb6a2ef00 229 }
majik 0:964eb6a2ef00 230 for(i=0;i<j;i++)
majik 0:964eb6a2ef00 231 {
majik 0:964eb6a2ef00 232 count += ((S[i])-48)* q_pow10(j-i-1);
majik 0:964eb6a2ef00 233 }
majik 0:964eb6a2ef00 234 return count;
majik 0:964eb6a2ef00 235 }
majik 0:964eb6a2ef00 236 void ros_interface_send_data()
majik 0:964eb6a2ef00 237 {
majik 0:964eb6a2ef00 238 //int mseconds = (int)time(NULL)+(int)t.read_ms();
majik 0:964eb6a2ef00 239 bt.lock();
majik 0:964eb6a2ef00 240 bt.printf("&%d;",t.read_ms());//indicate start of line, output system time
majik 0:964eb6a2ef00 241 bt.unlock();
majik 0:964eb6a2ef00 242 //send all the requested data
majik 0:964eb6a2ef00 243 //if (x.list[0]){ //send all imu data
majik 0:964eb6a2ef00 244 //bt.printf("A%3.3f,%3.3f,%3.3f,G%3.3f,%3.3f,%3.3f",mpu.ax,mpu.ay,mpu.az,mpu.gx,mpu.gy,mpu.gz);
majik 0:964eb6a2ef00 245 //}
majik 0:964eb6a2ef00 246
majik 0:964eb6a2ef00 247 //0 - IMU_all
majik 0:964eb6a2ef00 248 //1 - IMU_acc
majik 0:964eb6a2ef00 249 //2 - IMU_gyr
majik 0:964eb6a2ef00 250 //3 - IMU_rotation
majik 0:964eb6a2ef00 251 //4 - IR_sensors
majik 0:964eb6a2ef00 252 //5 - Optical_flow_sensors
majik 0:964eb6a2ef00 253 //6 - Current_sensor
majik 0:964eb6a2ef00 254 //7 - Voltage_sensor
majik 0:964eb6a2ef00 255
majik 0:964eb6a2ef00 256
majik 0:964eb6a2ef00 257 }
majik 0:964eb6a2ef00 258
majik 0:964eb6a2ef00 259 int ros_get_cmd() //this requires a ',' or ';' or <enter> at the end of every command
majik 0:964eb6a2ef00 260 {
majik 0:964eb6a2ef00 261 char c = '0';
majik 0:964eb6a2ef00 262 char C[10]; //stores the array of chars read
majik 0:964eb6a2ef00 263 int n=0; //counter
majik 0:964eb6a2ef00 264 int timeout = 0; //timeout counter
majik 0:964eb6a2ef00 265 int accum=0; //accumulates value of array
majik 0:964eb6a2ef00 266
majik 0:964eb6a2ef00 267 while(c != ',' && c != 13 && c !=';' && c!='x'){ //keep looping until a comma/semicolon is read (or enter is pressed)
majik 0:964eb6a2ef00 268 bt.lock();
majik 0:964eb6a2ef00 269 while(! bt.readable()){
majik 0:964eb6a2ef00 270 bt.unlock();
majik 0:964eb6a2ef00 271 Thread::yield(); //wait for char input
majik 0:964eb6a2ef00 272 timeout++;
majik 0:964eb6a2ef00 273 if(timeout > 10000) //if one second has passed, give up on this command
majik 0:964eb6a2ef00 274 return -1;
majik 0:964eb6a2ef00 275 bt.lock();
majik 0:964eb6a2ef00 276 }
majik 0:964eb6a2ef00 277 c= bt.getc(); //read the next char in the buffer
majik 0:964eb6a2ef00 278 bt.unlock();
majik 0:964eb6a2ef00 279
majik 0:964eb6a2ef00 280 //bt.printf("%c",c); //REMOVE THIS. DEBUG ONLY
majik 0:964eb6a2ef00 281 //if(c == '.' || n >=9) break; //concatenate decimals. I'm not handling decimals. No reason.
majik 0:964eb6a2ef00 282 C[n]=c;
majik 0:964eb6a2ef00 283 n++;
majik 0:964eb6a2ef00 284 }
majik 0:964eb6a2ef00 285
majik 0:964eb6a2ef00 286 for(int i = 0; i<n-1; i++){
majik 0:964eb6a2ef00 287 //need to subtract 2: 1 b/c n is incremented at the end of each iteration, 2 b/c comma is read.
majik 0:964eb6a2ef00 288 accum += ((C[i])-48)* q_pow10(n-i-2);
majik 0:964eb6a2ef00 289 }
majik 0:964eb6a2ef00 290 // memset(buffer, 0 , sizeof buffer);
majik 0:964eb6a2ef00 291 return accum;
majik 0:964eb6a2ef00 292 }
majik 0:964eb6a2ef00 293
majik 0:964eb6a2ef00 294 int q_pow10(int n){ //returns 10^n, up to 10^7 [8 sig figs]
majik 0:964eb6a2ef00 295 static int pow10[8] = {1,10,100,1000,10000,100000,1000000,10000000};
majik 0:964eb6a2ef00 296 return pow10[n];
majik 0:964eb6a2ef00 297 }
majik 0:964eb6a2ef00 298 void print_all(){ //call this function to print all sensor data
majik 0:964eb6a2ef00 299 float data[13];
majik 0:964eb6a2ef00 300 data[0]= getTime();
majik 0:964eb6a2ef00 301 data[1]= imu_data.ypr[0];
majik 0:964eb6a2ef00 302 data[2]= imu_data.ypr[1];
majik 0:964eb6a2ef00 303 data[3]= imu_data.ypr[2];
majik 0:964eb6a2ef00 304
majik 0:964eb6a2ef00 305 data[4]= imu2_data.ypr[0];
majik 0:964eb6a2ef00 306 data[5]= imu2_data.ypr[1];
majik 0:964eb6a2ef00 307 data[6]= imu2_data.ypr[2];
majik 0:964eb6a2ef00 308
majik 0:964eb6a2ef00 309
majik 0:964eb6a2ef00 310 bt.lock();
majik 0:964eb6a2ef00 311 bt.printf("&;");
majik 0:964eb6a2ef00 312 bt.printf("%f;",data[0]); //print time as an integer (milliseconds)
majik 0:964eb6a2ef00 313 for(int i = 1; i<=6; i++){
majik 0:964eb6a2ef00 314 bt.printf("%.3f;",data[i]);
majik 0:964eb6a2ef00 315 }
majik 0:964eb6a2ef00 316 bt.printf("\n\r");
majik 0:964eb6a2ef00 317 bt.unlock();
majik 0:964eb6a2ef00 318 //commented out the below code because we want to ignore the rear IR sensors
majik 0:964eb6a2ef00 319 /*dt = t.read() - data[0];
majik 0:964eb6a2ef00 320 irBack = 1;
majik 0:964eb6a2ef00 321 if(dt < 50)
majik 0:964eb6a2ef00 322 Thread::wait(50-dt);
majik 0:964eb6a2ef00 323
majik 0:964eb6a2ef00 324 data[6]= irR.read();
majik 0:964eb6a2ef00 325 data[7]= irL.read();
majik 0:964eb6a2ef00 326 data[8]= imu_data.ypr[0]*180/PI;
majik 0:964eb6a2ef00 327 irBack = 0;
majik 0:964eb6a2ef00 328 */
majik 0:964eb6a2ef00 329
majik 0:964eb6a2ef00 330 }
majik 0:964eb6a2ef00 331 int IR_mm(float x) //converts IR sensor value to millimeters
majik 0:964eb6a2ef00 332 {
majik 0:964eb6a2ef00 333 //return 9462/(x*3.3/5*1024 - 16.92);
majik 0:964eb6a2ef00 334 if (x <= 0.24)
majik 0:964eb6a2ef00 335 return -1;
majik 0:964eb6a2ef00 336 return 170/x;
majik 0:964eb6a2ef00 337 }
majik 0:964eb6a2ef00 338
majik 0:964eb6a2ef00 339 #endif