Mark Vandermeulen / FYDP_Final2

This is for our FYDP project. 2 MPU6050s are used

Dependencies:   Servo mbed

bt_shell/machine_interface/ros_machine_interface.h@4:05484073a641, 2015-03-22 (annotated)

Committer:
majik
Date:
Sun Mar 22 06:34:30 2015 +0000
Revision:
4:05484073a641
Parent:
0:21019d94ad33 
BOTH IMUs WORK NOW. Put them in separate threads. Servo is included.

Who changed what in which revision?

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