Yuxiang Yang / Mbed 2 deprecated controller_with_autostop

mbed controller for openroach

Dependencies:   MPU6050IMU QEI RPCInterface TSL1401CL mbed

Fork of controller_with_imu by Yuxiang Yang

main.cpp@0:4856445a8db9, 2017-05-30 (annotated)

Committer:
yxyang
Date:
Tue May 30 06:43:31 2017 +0000
Revision:
0:4856445a8db9
Child:
1:150bafe1bd61
Make public

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yxyang 0:4856445a8db9 1 /*
yxyang 0:4856445a8db9 2 * A sample path following program for the Zumy board.
yxyang 0:4856445a8db9 3 * Author: Galen Savidge
yxyang 0:4856445a8db9 4 *
yxyang 0:4856445a8db9 5 * Behavior:
yxyang 0:4856445a8db9 6 * Zumy follows the edge of the line until no line is found for LINE_END_TIME
yxyang 0:4856445a8db9 7 * cycles. Line position is recorded every frame as an 8 bit unsigned int. The
yxyang 0:4856445a8db9 8 * general area of the line is shown on the 4 LEDs on the mbed board. If a line
yxyang 0:4856445a8db9 9 * is not found all 4 LEDs are turned on. The Zumy stops and ine position data
yxyang 0:4856445a8db9 10 * is output to serial once the end of the track is reached.
yxyang 0:4856445a8db9 11 */
yxyang 0:4856445a8db9 12
yxyang 0:4856445a8db9 13 #include "MPU6050.h"
yxyang 0:4856445a8db9 14 #include "TSL1401CL.h"
yxyang 0:4856445a8db9 15 #include "mbed.h"
yxyang 0:4856445a8db9 16
yxyang 0:4856445a8db9 17 /***** Constants ******/
yxyang 0:4856445a8db9 18
yxyang 0:4856445a8db9 19 MPU6050 mpu6050;
yxyang 0:4856445a8db9 20
yxyang 0:4856445a8db9 21 #define CAM_INTEGRATION_TIME 80
yxyang 0:4856445a8db9 22
yxyang 0:4856445a8db9 23 // Higher line threshold -> the sensor will only recognize larger changes in
yxyang 0:4856445a8db9 24 // brightness as a line edge
yxyang 0:4856445a8db9 25 #define LINE_THRESHOLD 3000
yxyang 0:4856445a8db9 26 #define LINE_PRECISION 2
yxyang 0:4856445a8db9 27 #define LINE_CROP_AMOUNT 4
yxyang 0:4856445a8db9 28
yxyang 0:4856445a8db9 29 // These constants define the base pwm across the motors and how much the
yxyang 0:4856445a8db9 30 // controller
yxyang 0:4856445a8db9 31 // adjusts based on position of the line relative to the sensor
yxyang 0:4856445a8db9 32 #define SPEED_PWM 0.23
yxyang 0:4856445a8db9 33 #define TURN_SENS_INNER 1.5F
yxyang 0:4856445a8db9 34 #define TURN_SENS_OUTER 0.5F
yxyang 0:4856445a8db9 35
yxyang 0:4856445a8db9 36 // Defines data
yxyang 0:4856445a8db9 37 #define LINE_HIST_SIZE 1000
yxyang 0:4856445a8db9 38 #define LINE_END_TIME 2500
yxyang 0:4856445a8db9 39
yxyang 0:4856445a8db9 40 // Sensor pins
yxyang 0:4856445a8db9 41 #define clk p16
yxyang 0:4856445a8db9 42 #define si p17
yxyang 0:4856445a8db9 43 #define adc p18
yxyang 0:4856445a8db9 44
yxyang 0:4856445a8db9 45 // Motors -- As labelled on the Zumy mbed board
yxyang 0:4856445a8db9 46 PwmOut m1_fwd (p21);
yxyang 0:4856445a8db9 47 PwmOut m1_back (p22);
yxyang 0:4856445a8db9 48
yxyang 0:4856445a8db9 49 PwmOut m2_fwd (p23);
yxyang 0:4856445a8db9 50 PwmOut m2_back (p24);
yxyang 0:4856445a8db9 51
yxyang 0:4856445a8db9 52 // LEDs
yxyang 0:4856445a8db9 53 DigitalOut led1 (LED1);
yxyang 0:4856445a8db9 54 DigitalOut led2 (LED2);
yxyang 0:4856445a8db9 55 DigitalOut led3 (LED3);
yxyang 0:4856445a8db9 56 DigitalOut led4 (LED4);
yxyang 0:4856445a8db9 57
yxyang 0:4856445a8db9 58 // USB serial
yxyang 0:4856445a8db9 59 Serial pc (USBTX, USBRX);
yxyang 0:4856445a8db9 60
yxyang 0:4856445a8db9 61 // Data storage
yxyang 0:4856445a8db9 62 uint8_t line_hist[LINE_HIST_SIZE];
yxyang 0:4856445a8db9 63 int16_t accel_x[LINE_HIST_SIZE];
yxyang 0:4856445a8db9 64 int16_t accel_y[LINE_HIST_SIZE];
yxyang 0:4856445a8db9 65 int16_t accel_z[LINE_HIST_SIZE];
yxyang 0:4856445a8db9 66 int16_t gyro_x[LINE_HIST_SIZE];
yxyang 0:4856445a8db9 67 int16_t gyro_y[LINE_HIST_SIZE];
yxyang 0:4856445a8db9 68 int16_t gyro_z[LINE_HIST_SIZE];
yxyang 0:4856445a8db9 69
yxyang 0:4856445a8db9 70 uint16_t hist_index;
yxyang 0:4856445a8db9 71
yxyang 0:4856445a8db9 72 /***** Helper functions *****/
yxyang 0:4856445a8db9 73 float max (float a, float b);
yxyang 0:4856445a8db9 74
yxyang 0:4856445a8db9 75 // Sets the 4 LEDS on the mbed board to the four given binary values
yxyang 0:4856445a8db9 76 void setLEDs (uint8_t a, uint8_t b, uint8_t c, uint8_t d);
yxyang 0:4856445a8db9 77
yxyang 0:4856445a8db9 78 // Steers by linearly decreasing the inner wheel speed as the line moves from
yxyang 0:4856445a8db9 79 // the center (unused in this program)
yxyang 0:4856445a8db9 80 void steerStupid (int8_t line_pos);
yxyang 0:4856445a8db9 81
yxyang 0:4856445a8db9 82 // Based on steerStupid; adds the ability for the inner wheel to rotate in
yxyang 0:4856445a8db9 83 // reverse (unused in this program)
yxyang 0:4856445a8db9 84 void steerPointTurns (int8_t line_pos);
yxyang 0:4856445a8db9 85
yxyang 0:4856445a8db9 86 // Based on steerPointTurns; the outer wheel moves faster the farther the line
yxyang 0:4856445a8db9 87 // is from center
yxyang 0:4856445a8db9 88 void steerImprovedPointTurns (int8_t line_pos);
yxyang 0:4856445a8db9 89
yxyang 0:4856445a8db9 90 int
yxyang 0:4856445a8db9 91 main ()
yxyang 0:4856445a8db9 92 {
yxyang 0:4856445a8db9 93 /********** SENSOR SETUP **********/
yxyang 0:4856445a8db9 94 TSL1401CL cam (clk, si, adc);
yxyang 0:4856445a8db9 95 cam.setIntegrationTime (CAM_INTEGRATION_TIME);
yxyang 0:4856445a8db9 96
yxyang 0:4856445a8db9 97 int8_t line_pos = -1;
yxyang 0:4856445a8db9 98 int8_t line_pos_previous = -1;
yxyang 0:4856445a8db9 99 uint8_t line_lost_time = 0;
yxyang 0:4856445a8db9 100
yxyang 0:4856445a8db9 101 hist_index = 0;
yxyang 0:4856445a8db9 102
yxyang 0:4856445a8db9 103 i2c.frequency (400000);
yxyang 0:4856445a8db9 104
yxyang 0:4856445a8db9 105 volatile bool imu_ready = false;
yxyang 0:4856445a8db9 106
yxyang 0:4856445a8db9 107 wait_ms (100);
yxyang 0:4856445a8db9 108 uint8_t whoami = mpu6050.readByte (MPU6050_ADDRESS, WHO_AM_I_MPU6050);
yxyang 0:4856445a8db9 109
yxyang 0:4856445a8db9 110 if (whoami == 0x68) // WHO_AM_I should always be 0x68
yxyang 0:4856445a8db9 111 {
yxyang 0:4856445a8db9 112 mpu6050.MPU6050SelfTest (SelfTest);
yxyang 0:4856445a8db9 113 if (SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f
yxyang 0:4856445a8db9 114 && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f)
yxyang 0:4856445a8db9 115 {
yxyang 0:4856445a8db9 116 mpu6050.resetMPU6050 ();
yxyang 0:4856445a8db9 117 mpu6050.calibrateMPU6050 (gyroBias, accelBias);
yxyang 0:4856445a8db9 118 mpu6050.initMPU6050 ();
yxyang 0:4856445a8db9 119 mpu6050.getAres ();
yxyang 0:4856445a8db9 120 mpu6050.getGres ();
yxyang 0:4856445a8db9 121 imu_ready = true;
yxyang 0:4856445a8db9 122 }
yxyang 0:4856445a8db9 123 }
yxyang 0:4856445a8db9 124 else
yxyang 0:4856445a8db9 125 {
yxyang 0:4856445a8db9 126 setLEDs (1, 0, 1, 0);
yxyang 0:4856445a8db9 127 wait_ms (1000);
yxyang 0:4856445a8db9 128 }
yxyang 0:4856445a8db9 129 // Read garbage data
yxyang 0:4856445a8db9 130 while (!cam.integrationReady ())
yxyang 0:4856445a8db9 131 ;
yxyang 0:4856445a8db9 132 cam.read ();
yxyang 0:4856445a8db9 133
yxyang 0:4856445a8db9 134 while (1)
yxyang 0:4856445a8db9 135 {
yxyang 0:4856445a8db9 136 /***** Read line sensor *****/
yxyang 0:4856445a8db9 137 while (!cam.integrationReady ())
yxyang 0:4856445a8db9 138 ; // Wait for integration
yxyang 0:4856445a8db9 139 cam.read ();
yxyang 0:4856445a8db9 140 /***** Line following loop *****/
yxyang 0:4856445a8db9 141
yxyang 0:4856445a8db9 142 line_pos = cam.findLineEdge (LINE_THRESHOLD, LINE_PRECISION,
yxyang 0:4856445a8db9 143 LINE_CROP_AMOUNT);
yxyang 0:4856445a8db9 144
yxyang 0:4856445a8db9 145 if (line_pos != -1)
yxyang 0:4856445a8db9 146 { // On the line
yxyang 0:4856445a8db9 147 // Set LEDs based on the position of the line
yxyang 0:4856445a8db9 148 if (line_pos < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 4)
yxyang 0:4856445a8db9 149 {
yxyang 0:4856445a8db9 150 setLEDs (1, 0, 0, 0);
yxyang 0:4856445a8db9 151 }
yxyang 0:4856445a8db9 152 else if (line_pos
yxyang 0:4856445a8db9 153 < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 2)
yxyang 0:4856445a8db9 154 {
yxyang 0:4856445a8db9 155 setLEDs (0, 1, 0, 0);
yxyang 0:4856445a8db9 156 }
yxyang 0:4856445a8db9 157 else if (line_pos
yxyang 0:4856445a8db9 158 < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) * 3 / 4)
yxyang 0:4856445a8db9 159 {
yxyang 0:4856445a8db9 160 setLEDs (0, 0, 1, 0);
yxyang 0:4856445a8db9 161 }
yxyang 0:4856445a8db9 162 else
yxyang 0:4856445a8db9 163 {
yxyang 0:4856445a8db9 164 setLEDs (0, 0, 0, 1);
yxyang 0:4856445a8db9 165 }
yxyang 0:4856445a8db9 166
yxyang 0:4856445a8db9 167 // Record the line position
yxyang 0:4856445a8db9 168 line_hist[hist_index] = line_pos;
yxyang 0:4856445a8db9 169 line_lost_time = 0;
yxyang 0:4856445a8db9 170 if (imu_ready)
yxyang 0:4856445a8db9 171 {
yxyang 0:4856445a8db9 172
yxyang 0:4856445a8db9 173 if (mpu6050.readByte (MPU6050_ADDRESS, INT_STATUS) & 0x01)
yxyang 0:4856445a8db9 174 { // check if data ready interrupt
yxyang 0:4856445a8db9 175 mpu6050.readAccelData (accelCount);
yxyang 0:4856445a8db9 176 // Read the x/y/z adc values
yxyang 0:4856445a8db9 177
yxyang 0:4856445a8db9 178 accel_x[hist_index] = accelCount[0];
yxyang 0:4856445a8db9 179 accel_y[hist_index] = accelCount[1];
yxyang 0:4856445a8db9 180 accel_z[hist_index] = accelCount[2];
yxyang 0:4856445a8db9 181
yxyang 0:4856445a8db9 182 // Calculate the gyro value into actual degrees per
yxyang 0:4856445a8db9 183 // second
yxyang 0:4856445a8db9 184
yxyang 0:4856445a8db9 185 mpu6050.readGyroData (gyroCount);
yxyang 0:4856445a8db9 186
yxyang 0:4856445a8db9 187 gyro_x[hist_index] = gyroCount[0];
yxyang 0:4856445a8db9 188 gyro_y[hist_index] = gyroCount[1];
yxyang 0:4856445a8db9 189 gyro_z[hist_index] = gyroCount[2];
yxyang 0:4856445a8db9 190 // Read the x/y/z ad values
yxyang 0:4856445a8db9 191 // gyro_x = (float)gyroCount[0] * gRes
yxyang 0:4856445a8db9 192 // - gyroBias[0]; // get actual gyro value, this
yxyang 0:4856445a8db9 193 // // depends on scale being set
yxyang 0:4856445a8db9 194 // gyro_y = (float)gyroCount[1] * gRes - gyroBias[1];
yxyang 0:4856445a8db9 195 // gyro_z = (float)gyroCount[2] * gRes - gyroBias[2];
yxyang 0:4856445a8db9 196 // pc.printf ("Gyro,%d,%d,%d\n", gyroCount[0],
yxyang 0:4856445a8db9 197 // gyroCount[1],
yxyang 0:4856445a8db9 198 // gyroCount[2]);
yxyang 0:4856445a8db9 199 //
yxyang 0:4856445a8db9 200 }
yxyang 0:4856445a8db9 201 }
yxyang 0:4856445a8db9 202 hist_index++;
yxyang 0:4856445a8db9 203
yxyang 0:4856445a8db9 204 // Steer the vehicle
yxyang 0:4856445a8db9 205 steerImprovedPointTurns (line_pos);
yxyang 0:4856445a8db9 206
yxyang 0:4856445a8db9 207 line_pos_previous = line_pos;
yxyang 0:4856445a8db9 208 }
yxyang 0:4856445a8db9 209 else
yxyang 0:4856445a8db9 210 { // Lost the line
yxyang 0:4856445a8db9 211 // setLEDs (1, 1, 1, 1);
yxyang 0:4856445a8db9 212 if (abs (line_pos_previous - TSL1401CL_PIXEL_COUNT / 2)
yxyang 0:4856445a8db9 213 > TSL1401CL_PIXEL_COUNT / 4)
yxyang 0:4856445a8db9 214 {
yxyang 0:4856445a8db9 215 // Steer at maximum turn angle towards the last known line
yxyang 0:4856445a8db9 216 // direction
yxyang 0:4856445a8db9 217 if (line_pos_previous >= TSL1401CL_PIXEL_COUNT / 2)
yxyang 0:4856445a8db9 218 {
yxyang 0:4856445a8db9 219 steerImprovedPointTurns (TSL1401CL_PIXEL_COUNT - 1
yxyang 0:4856445a8db9 220 - LINE_CROP_AMOUNT);
yxyang 0:4856445a8db9 221 // line_hist[hist_index] = TSL1401CL_PIXEL_COUNT - 1 -
yxyang 0:4856445a8db9 222 // LINE_CROP_AMOUNT;
yxyang 0:4856445a8db9 223 }
yxyang 0:4856445a8db9 224 else
yxyang 0:4856445a8db9 225 {
yxyang 0:4856445a8db9 226 steerImprovedPointTurns (LINE_CROP_AMOUNT);
yxyang 0:4856445a8db9 227 // line_hist[hist_index] = LINE_CROP_AMOUNT;
yxyang 0:4856445a8db9 228 }
yxyang 0:4856445a8db9 229 }
yxyang 0:4856445a8db9 230 else
yxyang 0:4856445a8db9 231 {
yxyang 0:4856445a8db9 232 line_lost_time++;
yxyang 0:4856445a8db9 233 if (line_lost_time >= LINE_END_TIME)
yxyang 0:4856445a8db9 234 {
yxyang 0:4856445a8db9 235 setLEDs (1, 0, 1, 0);
yxyang 0:4856445a8db9 236 // Line end; transmit data to PC
yxyang 0:4856445a8db9 237 m1_fwd.write (0);
yxyang 0:4856445a8db9 238 m2_fwd.write (0);
yxyang 0:4856445a8db9 239 // while (!pc.readable ())
yxyang 0:4856445a8db9 240 // ;
yxyang 0:4856445a8db9 241 pc.printf ("----- Start line data -----\n");
yxyang 0:4856445a8db9 242 for (int i = 0; i <= hist_index; i++)
yxyang 0:4856445a8db9 243 {
yxyang 0:4856445a8db9 244 pc.printf ("Pos: %d\n", line_hist[i]);
yxyang 0:4856445a8db9 245 float x, y, z;
yxyang 0:4856445a8db9 246 x = (float)accel_x[i] * aRes - accelBias[0];
yxyang 0:4856445a8db9 247 y = (float)accel_y[i] * aRes - accelBias[1];
yxyang 0:4856445a8db9 248 z = (float)accel_z[i] * aRes - accelBias[2];
yxyang 0:4856445a8db9 249 pc.printf ("Acceleration,%f,%f,%f\n", x, y, z);
yxyang 0:4856445a8db9 250 x = (float)gyro_x[i] * gRes - gyroBias[0];
yxyang 0:4856445a8db9 251 y = (float)gyro_y[i] * gRes - gyroBias[1];
yxyang 0:4856445a8db9 252 z = (float)gyro_z[i] * gRes - gyroBias[2];
yxyang 0:4856445a8db9 253 pc.printf ("Gyroscope,%f,%f,%f\n", x, y, z);
yxyang 0:4856445a8db9 254 }
yxyang 0:4856445a8db9 255 while (1)
yxyang 0:4856445a8db9 256 ;
yxyang 0:4856445a8db9 257 }
yxyang 0:4856445a8db9 258 }
yxyang 0:4856445a8db9 259 }
yxyang 0:4856445a8db9 260 if (hist_index > LINE_HIST_SIZE)
yxyang 0:4856445a8db9 261 {
yxyang 0:4856445a8db9 262 setLEDs (1, 0, 0, 1);
yxyang 0:4856445a8db9 263
yxyang 0:4856445a8db9 264 // Line end; transmit data to PC
yxyang 0:4856445a8db9 265 m1_fwd.write (0);
yxyang 0:4856445a8db9 266 m2_fwd.write (0);
yxyang 0:4856445a8db9 267 // while (!pc.readable ())
yxyang 0:4856445a8db9 268 // ;
yxyang 0:4856445a8db9 269 pc.printf ("----- Start line data -----\n");
yxyang 0:4856445a8db9 270 for (int i = 0; i <= hist_index; i++)
yxyang 0:4856445a8db9 271 {
yxyang 0:4856445a8db9 272 pc.printf ("Pos: %d\n", line_hist[i]);
yxyang 0:4856445a8db9 273 float x, y, z;
yxyang 0:4856445a8db9 274 x = (float)accel_x[i] * aRes - accelBias[0];
yxyang 0:4856445a8db9 275 y = (float)accel_y[i] * aRes - accelBias[1];
yxyang 0:4856445a8db9 276 z = (float)accel_z[i] * aRes - accelBias[2];
yxyang 0:4856445a8db9 277 pc.printf ("Acceleration,%f,%f,%f\n", x, y, z);
yxyang 0:4856445a8db9 278 x = (float)gyro_x[i] * gRes - gyroBias[0];
yxyang 0:4856445a8db9 279 y = (float)gyro_y[i] * gRes - gyroBias[1];
yxyang 0:4856445a8db9 280 z = (float)gyro_z[i] * gRes - gyroBias[2];
yxyang 0:4856445a8db9 281 pc.printf ("Gyroscope,%f,%f,%f\n", x, y, z);
yxyang 0:4856445a8db9 282 }
yxyang 0:4856445a8db9 283 while (1)
yxyang 0:4856445a8db9 284 ;
yxyang 0:4856445a8db9 285 }
yxyang 0:4856445a8db9 286 }
yxyang 0:4856445a8db9 287 }
yxyang 0:4856445a8db9 288
yxyang 0:4856445a8db9 289 float
yxyang 0:4856445a8db9 290 max (float a, float b)
yxyang 0:4856445a8db9 291 {
yxyang 0:4856445a8db9 292 if (a >= b)
yxyang 0:4856445a8db9 293 return a;
yxyang 0:4856445a8db9 294 return b;
yxyang 0:4856445a8db9 295 }
yxyang 0:4856445a8db9 296
yxyang 0:4856445a8db9 297 void
yxyang 0:4856445a8db9 298 setLEDs (uint8_t a, uint8_t b, uint8_t c, uint8_t d)
yxyang 0:4856445a8db9 299 {
yxyang 0:4856445a8db9 300 led1 = a;
yxyang 0:4856445a8db9 301 led2 = b;
yxyang 0:4856445a8db9 302 led3 = c;
yxyang 0:4856445a8db9 303 led4 = d;
yxyang 0:4856445a8db9 304 }
yxyang 0:4856445a8db9 305
yxyang 0:4856445a8db9 306 void
yxyang 0:4856445a8db9 307 steerStupid (int8_t line_pos)
yxyang 0:4856445a8db9 308 {
yxyang 0:4856445a8db9 309 line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
yxyang 0:4856445a8db9 310
yxyang 0:4856445a8db9 311 if (line_pos < 0)
yxyang 0:4856445a8db9 312 {
yxyang 0:4856445a8db9 313 m1_fwd.write (SPEED_PWM);
yxyang 0:4856445a8db9 314 m1_back.write (0);
yxyang 0:4856445a8db9 315 m2_fwd.write (max (SPEED_PWM * (1.0F
yxyang 0:4856445a8db9 316 + TURN_SENS_INNER * line_pos * 2.0F
yxyang 0:4856445a8db9 317 / (float)TSL1401CL_PIXEL_COUNT),
yxyang 0:4856445a8db9 318 0));
yxyang 0:4856445a8db9 319 m2_back.write (0);
yxyang 0:4856445a8db9 320 }
yxyang 0:4856445a8db9 321
yxyang 0:4856445a8db9 322 if (line_pos >= 0)
yxyang 0:4856445a8db9 323 {
yxyang 0:4856445a8db9 324 m2_fwd.write (SPEED_PWM);
yxyang 0:4856445a8db9 325 m2_back.write (0);
yxyang 0:4856445a8db9 326 m1_fwd.write (max (SPEED_PWM * (1.0F
yxyang 0:4856445a8db9 327 - TURN_SENS_INNER * line_pos * 2.0F
yxyang 0:4856445a8db9 328 / (float)TSL1401CL_PIXEL_COUNT),
yxyang 0:4856445a8db9 329 0));
yxyang 0:4856445a8db9 330 m1_back.write (0);
yxyang 0:4856445a8db9 331 }
yxyang 0:4856445a8db9 332 }
yxyang 0:4856445a8db9 333
yxyang 0:4856445a8db9 334 void
yxyang 0:4856445a8db9 335 steerPointTurns (int8_t line_pos)
yxyang 0:4856445a8db9 336 {
yxyang 0:4856445a8db9 337 line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
yxyang 0:4856445a8db9 338
yxyang 0:4856445a8db9 339 float pwm_outer = SPEED_PWM;
yxyang 0:4856445a8db9 340 float pwm_inner = SPEED_PWM * (1.0F
yxyang 0:4856445a8db9 341 - TURN_SENS_INNER * abs (line_pos) * 2.0F
yxyang 0:4856445a8db9 342 / (float)TSL1401CL_PIXEL_COUNT);
yxyang 0:4856445a8db9 343
yxyang 0:4856445a8db9 344 if (line_pos < 0)
yxyang 0:4856445a8db9 345 {
yxyang 0:4856445a8db9 346 m1_fwd.write (pwm_outer);
yxyang 0:4856445a8db9 347 m1_back.write (0);
yxyang 0:4856445a8db9 348 if (pwm_inner >= 0)
yxyang 0:4856445a8db9 349 {
yxyang 0:4856445a8db9 350 m2_fwd.write (pwm_inner);
yxyang 0:4856445a8db9 351 m2_back.write (0);
yxyang 0:4856445a8db9 352 }
yxyang 0:4856445a8db9 353 else
yxyang 0:4856445a8db9 354 {
yxyang 0:4856445a8db9 355 m2_fwd.write (0);
yxyang 0:4856445a8db9 356 m2_back.write (pwm_inner * -1.0F);
yxyang 0:4856445a8db9 357 }
yxyang 0:4856445a8db9 358 }
yxyang 0:4856445a8db9 359
yxyang 0:4856445a8db9 360 if (line_pos >= 0)
yxyang 0:4856445a8db9 361 {
yxyang 0:4856445a8db9 362 m2_fwd.write (pwm_outer);
yxyang 0:4856445a8db9 363 m2_back.write (0);
yxyang 0:4856445a8db9 364 if (pwm_inner >= 0)
yxyang 0:4856445a8db9 365 {
yxyang 0:4856445a8db9 366 m1_fwd.write (pwm_inner);
yxyang 0:4856445a8db9 367 m1_back.write (0);
yxyang 0:4856445a8db9 368 }
yxyang 0:4856445a8db9 369 else
yxyang 0:4856445a8db9 370 {
yxyang 0:4856445a8db9 371 m1_fwd.write (0);
yxyang 0:4856445a8db9 372 m1_back.write (pwm_inner * -1.0F);
yxyang 0:4856445a8db9 373 }
yxyang 0:4856445a8db9 374 }
yxyang 0:4856445a8db9 375 }
yxyang 0:4856445a8db9 376
yxyang 0:4856445a8db9 377 void
yxyang 0:4856445a8db9 378 steerImprovedPointTurns (int8_t line_pos)
yxyang 0:4856445a8db9 379 {
yxyang 0:4856445a8db9 380 line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
yxyang 0:4856445a8db9 381
yxyang 0:4856445a8db9 382 // Find desired motor voltages based on the controller
yxyang 0:4856445a8db9 383 float pwm_outer = SPEED_PWM * (1.0F
yxyang 0:4856445a8db9 384 + TURN_SENS_OUTER * abs (line_pos) * 2.0F
yxyang 0:4856445a8db9 385 / (float)TSL1401CL_PIXEL_COUNT);
yxyang 0:4856445a8db9 386 float pwm_inner = SPEED_PWM * (1.0F
yxyang 0:4856445a8db9 387 - TURN_SENS_INNER * abs (line_pos) * 2.0F
yxyang 0:4856445a8db9 388 / (float)TSL1401CL_PIXEL_COUNT);
yxyang 0:4856445a8db9 389
yxyang 0:4856445a8db9 390 // Write to the appropriate PWM pins
yxyang 0:4856445a8db9 391 if (line_pos < 0)
yxyang 0:4856445a8db9 392 {
yxyang 0:4856445a8db9 393 m1_fwd.write (pwm_outer);
yxyang 0:4856445a8db9 394 m1_back.write (0);
yxyang 0:4856445a8db9 395 if (pwm_inner >= 0)
yxyang 0:4856445a8db9 396 {
yxyang 0:4856445a8db9 397 m2_fwd.write (pwm_inner);
yxyang 0:4856445a8db9 398 m2_back.write (0);
yxyang 0:4856445a8db9 399 }
yxyang 0:4856445a8db9 400 else
yxyang 0:4856445a8db9 401 {
yxyang 0:4856445a8db9 402 m2_fwd.write (0);
yxyang 0:4856445a8db9 403 m2_back.write (pwm_inner * -1.0F);
yxyang 0:4856445a8db9 404 }
yxyang 0:4856445a8db9 405 }
yxyang 0:4856445a8db9 406
yxyang 0:4856445a8db9 407 if (line_pos >= 0)
yxyang 0:4856445a8db9 408 {
yxyang 0:4856445a8db9 409 m2_fwd.write (pwm_outer);
yxyang 0:4856445a8db9 410 m2_back.write (0);
yxyang 0:4856445a8db9 411 if (pwm_inner >= 0)
yxyang 0:4856445a8db9 412 {
yxyang 0:4856445a8db9 413 m1_fwd.write (pwm_inner);
yxyang 0:4856445a8db9 414 m1_back.write (0);
yxyang 0:4856445a8db9 415 }
yxyang 0:4856445a8db9 416 else
yxyang 0:4856445a8db9 417 {
yxyang 0:4856445a8db9 418 m1_fwd.write (0);
yxyang 0:4856445a8db9 419 m1_back.write (pwm_inner * -1.0F);
yxyang 0:4856445a8db9 420 }
yxyang 0:4856445a8db9 421 }
yxyang 0:4856445a8db9 422 }