Code for our FYDP -only one IMU works right now -RTOS is working
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?
User | Revision | Line number | New 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 |