Yuxiang Yang
mbed controller for openroach
Dependencies: MPU6050IMU QEI RPCInterface TSL1401CL mbed
Fork of
controller_with_imu
by 
Yuxiang Yang
Diff: main.cpp
- Revision:
- 0:4856445a8db9
- Child:
- 1:150bafe1bd61
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue May 30 06:43:31 2017 +0000
@@ -0,0 +1,422 @@
+/*
+ * A sample path following program for the Zumy board.
+ * Author: Galen Savidge
+ *
+ * Behavior:
+ * Zumy follows the edge of the line until no line is found for LINE_END_TIME
+ * cycles. Line position is recorded every frame as an 8 bit unsigned int. The
+ * general area of the line is shown on the 4 LEDs on the mbed board. If a line
+ * is not found all 4 LEDs are turned on. The Zumy stops and ine position data
+ * is output to serial once the end of the track is reached.
+ */
+
+#include "MPU6050.h"
+#include "TSL1401CL.h"
+#include "mbed.h"
+
+/***** Constants ******/
+
+MPU6050 mpu6050;
+
+#define CAM_INTEGRATION_TIME 80
+
+// Higher line threshold -> the sensor will only recognize larger changes in
+// brightness as a line edge
+#define LINE_THRESHOLD 3000
+#define LINE_PRECISION 2
+#define LINE_CROP_AMOUNT 4
+
+// These constants define the base pwm across the motors and how much the
+// controller
+// adjusts based on position of the line relative to the sensor
+#define SPEED_PWM 0.23
+#define TURN_SENS_INNER 1.5F
+#define TURN_SENS_OUTER 0.5F
+
+// Defines data
+#define LINE_HIST_SIZE 1000
+#define LINE_END_TIME 2500
+
+// Sensor pins
+#define clk p16
+#define si p17
+#define adc p18
+
+// Motors -- As labelled on the Zumy mbed board
+PwmOut m1_fwd (p21);
+PwmOut m1_back (p22);
+
+PwmOut m2_fwd (p23);
+PwmOut m2_back (p24);
+
+// LEDs
+DigitalOut led1 (LED1);
+DigitalOut led2 (LED2);
+DigitalOut led3 (LED3);
+DigitalOut led4 (LED4);
+
+// USB serial
+Serial pc (USBTX, USBRX);
+
+// Data storage
+uint8_t line_hist[LINE_HIST_SIZE];
+int16_t accel_x[LINE_HIST_SIZE];
+int16_t accel_y[LINE_HIST_SIZE];
+int16_t accel_z[LINE_HIST_SIZE];
+int16_t gyro_x[LINE_HIST_SIZE];
+int16_t gyro_y[LINE_HIST_SIZE];
+int16_t gyro_z[LINE_HIST_SIZE];
+
+uint16_t hist_index;
+
+/***** Helper functions *****/
+float max (float a, float b);
+
+// Sets the 4 LEDS on the mbed board to the four given binary values
+void setLEDs (uint8_t a, uint8_t b, uint8_t c, uint8_t d);
+
+// Steers by linearly decreasing the inner wheel speed as the line moves from
+// the center (unused in this program)
+void steerStupid (int8_t line_pos);
+
+// Based on steerStupid; adds the ability for the inner wheel to rotate in
+// reverse (unused in this program)
+void steerPointTurns (int8_t line_pos);
+
+// Based on steerPointTurns; the outer wheel moves faster the farther the line
+// is from center
+void steerImprovedPointTurns (int8_t line_pos);
+
+int
+main ()
+{
+ /********** SENSOR SETUP **********/
+ TSL1401CL cam (clk, si, adc);
+ cam.setIntegrationTime (CAM_INTEGRATION_TIME);
+
+ int8_t line_pos = -1;
+ int8_t line_pos_previous = -1;
+ uint8_t line_lost_time = 0;
+
+ hist_index = 0;
+
+ i2c.frequency (400000);
+
+ volatile bool imu_ready = false;
+
+ wait_ms (100);
+ uint8_t whoami = mpu6050.readByte (MPU6050_ADDRESS, WHO_AM_I_MPU6050);
+
+ if (whoami == 0x68) // WHO_AM_I should always be 0x68
+ {
+ mpu6050.MPU6050SelfTest (SelfTest);
+ if (SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f
+ && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f)
+ {
+ mpu6050.resetMPU6050 ();
+ mpu6050.calibrateMPU6050 (gyroBias, accelBias);
+ mpu6050.initMPU6050 ();
+ mpu6050.getAres ();
+ mpu6050.getGres ();
+ imu_ready = true;
+ }
+ }
+ else
+ {
+ setLEDs (1, 0, 1, 0);
+ wait_ms (1000);
+ }
+ // Read garbage data
+ while (!cam.integrationReady ())
+ ;
+ cam.read ();
+
+ while (1)
+ {
+ /***** Read line sensor *****/
+ while (!cam.integrationReady ())
+ ; // Wait for integration
+ cam.read ();
+ /***** Line following loop *****/
+
+ line_pos = cam.findLineEdge (LINE_THRESHOLD, LINE_PRECISION,
+ LINE_CROP_AMOUNT);
+
+ if (line_pos != -1)
+ { // On the line
+ // Set LEDs based on the position of the line
+ if (line_pos < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 4)
+ {
+ setLEDs (1, 0, 0, 0);
+ }
+ else if (line_pos
+ < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 2)
+ {
+ setLEDs (0, 1, 0, 0);
+ }
+ else if (line_pos
+ < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) * 3 / 4)
+ {
+ setLEDs (0, 0, 1, 0);
+ }
+ else
+ {
+ setLEDs (0, 0, 0, 1);
+ }
+
+ // Record the line position
+ line_hist[hist_index] = line_pos;
+ line_lost_time = 0;
+ if (imu_ready)
+ {
+
+ if (mpu6050.readByte (MPU6050_ADDRESS, INT_STATUS) & 0x01)
+ { // check if data ready interrupt
+ mpu6050.readAccelData (accelCount);
+ // Read the x/y/z adc values
+
+ accel_x[hist_index] = accelCount[0];
+ accel_y[hist_index] = accelCount[1];
+ accel_z[hist_index] = accelCount[2];
+
+ // Calculate the gyro value into actual degrees per
+ // second
+
+ mpu6050.readGyroData (gyroCount);
+
+ gyro_x[hist_index] = gyroCount[0];
+ gyro_y[hist_index] = gyroCount[1];
+ gyro_z[hist_index] = gyroCount[2];
+ // Read the x/y/z ad values
+ // gyro_x = (float)gyroCount[0] * gRes
+ // - gyroBias[0]; // get actual gyro value, this
+ // // depends on scale being set
+ // gyro_y = (float)gyroCount[1] * gRes - gyroBias[1];
+ // gyro_z = (float)gyroCount[2] * gRes - gyroBias[2];
+ // pc.printf ("Gyro,%d,%d,%d\n", gyroCount[0],
+ // gyroCount[1],
+ // gyroCount[2]);
+ //
+ }
+ }
+ hist_index++;
+
+ // Steer the vehicle
+ steerImprovedPointTurns (line_pos);
+
+ line_pos_previous = line_pos;
+ }
+ else
+ { // Lost the line
+ // setLEDs (1, 1, 1, 1);
+ if (abs (line_pos_previous - TSL1401CL_PIXEL_COUNT / 2)
+ > TSL1401CL_PIXEL_COUNT / 4)
+ {
+ // Steer at maximum turn angle towards the last known line
+ // direction
+ if (line_pos_previous >= TSL1401CL_PIXEL_COUNT / 2)
+ {
+ steerImprovedPointTurns (TSL1401CL_PIXEL_COUNT - 1
+ - LINE_CROP_AMOUNT);
+ // line_hist[hist_index] = TSL1401CL_PIXEL_COUNT - 1 -
+ // LINE_CROP_AMOUNT;
+ }
+ else
+ {
+ steerImprovedPointTurns (LINE_CROP_AMOUNT);
+ // line_hist[hist_index] = LINE_CROP_AMOUNT;
+ }
+ }
+ else
+ {
+ line_lost_time++;
+ if (line_lost_time >= LINE_END_TIME)
+ {
+ setLEDs (1, 0, 1, 0);
+ // Line end; transmit data to PC
+ m1_fwd.write (0);
+ m2_fwd.write (0);
+ // while (!pc.readable ())
+ // ;
+ pc.printf ("----- Start line data -----\n");
+ for (int i = 0; i <= hist_index; i++)
+ {
+ pc.printf ("Pos: %d\n", line_hist[i]);
+ float x, y, z;
+ x = (float)accel_x[i] * aRes - accelBias[0];
+ y = (float)accel_y[i] * aRes - accelBias[1];
+ z = (float)accel_z[i] * aRes - accelBias[2];
+ pc.printf ("Acceleration,%f,%f,%f\n", x, y, z);
+ x = (float)gyro_x[i] * gRes - gyroBias[0];
+ y = (float)gyro_y[i] * gRes - gyroBias[1];
+ z = (float)gyro_z[i] * gRes - gyroBias[2];
+ pc.printf ("Gyroscope,%f,%f,%f\n", x, y, z);
+ }
+ while (1)
+ ;
+ }
+ }
+ }
+ if (hist_index > LINE_HIST_SIZE)
+ {
+ setLEDs (1, 0, 0, 1);
+
+ // Line end; transmit data to PC
+ m1_fwd.write (0);
+ m2_fwd.write (0);
+ // while (!pc.readable ())
+ // ;
+ pc.printf ("----- Start line data -----\n");
+ for (int i = 0; i <= hist_index; i++)
+ {
+ pc.printf ("Pos: %d\n", line_hist[i]);
+ float x, y, z;
+ x = (float)accel_x[i] * aRes - accelBias[0];
+ y = (float)accel_y[i] * aRes - accelBias[1];
+ z = (float)accel_z[i] * aRes - accelBias[2];
+ pc.printf ("Acceleration,%f,%f,%f\n", x, y, z);
+ x = (float)gyro_x[i] * gRes - gyroBias[0];
+ y = (float)gyro_y[i] * gRes - gyroBias[1];
+ z = (float)gyro_z[i] * gRes - gyroBias[2];
+ pc.printf ("Gyroscope,%f,%f,%f\n", x, y, z);
+ }
+ while (1)
+ ;
+ }
+ }
+}
+
+float
+max (float a, float b)
+{
+ if (a >= b)
+ return a;
+ return b;
+}
+
+void
+setLEDs (uint8_t a, uint8_t b, uint8_t c, uint8_t d)
+{
+ led1 = a;
+ led2 = b;
+ led3 = c;
+ led4 = d;
+}
+
+void
+steerStupid (int8_t line_pos)
+{
+ line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
+
+ if (line_pos < 0)
+ {
+ m1_fwd.write (SPEED_PWM);
+ m1_back.write (0);
+ m2_fwd.write (max (SPEED_PWM * (1.0F
+ + TURN_SENS_INNER * line_pos * 2.0F
+ / (float)TSL1401CL_PIXEL_COUNT),
+ 0));
+ m2_back.write (0);
+ }
+
+ if (line_pos >= 0)
+ {
+ m2_fwd.write (SPEED_PWM);
+ m2_back.write (0);
+ m1_fwd.write (max (SPEED_PWM * (1.0F
+ - TURN_SENS_INNER * line_pos * 2.0F
+ / (float)TSL1401CL_PIXEL_COUNT),
+ 0));
+ m1_back.write (0);
+ }
+}
+
+void
+steerPointTurns (int8_t line_pos)
+{
+ line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
+
+ float pwm_outer = SPEED_PWM;
+ float pwm_inner = SPEED_PWM * (1.0F
+ - TURN_SENS_INNER * abs (line_pos) * 2.0F
+ / (float)TSL1401CL_PIXEL_COUNT);
+
+ if (line_pos < 0)
+ {
+ m1_fwd.write (pwm_outer);
+ m1_back.write (0);
+ if (pwm_inner >= 0)
+ {
+ m2_fwd.write (pwm_inner);
+ m2_back.write (0);
+ }
+ else
+ {
+ m2_fwd.write (0);
+ m2_back.write (pwm_inner * -1.0F);
+ }
+ }
+
+ if (line_pos >= 0)
+ {
+ m2_fwd.write (pwm_outer);
+ m2_back.write (0);
+ if (pwm_inner >= 0)
+ {
+ m1_fwd.write (pwm_inner);
+ m1_back.write (0);
+ }
+ else
+ {
+ m1_fwd.write (0);
+ m1_back.write (pwm_inner * -1.0F);
+ }
+ }
+}
+
+void
+steerImprovedPointTurns (int8_t line_pos)
+{
+ line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
+
+ // Find desired motor voltages based on the controller
+ float pwm_outer = SPEED_PWM * (1.0F
+ + TURN_SENS_OUTER * abs (line_pos) * 2.0F
+ / (float)TSL1401CL_PIXEL_COUNT);
+ float pwm_inner = SPEED_PWM * (1.0F
+ - TURN_SENS_INNER * abs (line_pos) * 2.0F
+ / (float)TSL1401CL_PIXEL_COUNT);
+
+ // Write to the appropriate PWM pins
+ if (line_pos < 0)
+ {
+ m1_fwd.write (pwm_outer);
+ m1_back.write (0);
+ if (pwm_inner >= 0)
+ {
+ m2_fwd.write (pwm_inner);
+ m2_back.write (0);
+ }
+ else
+ {
+ m2_fwd.write (0);
+ m2_back.write (pwm_inner * -1.0F);
+ }
+ }
+
+ if (line_pos >= 0)
+ {
+ m2_fwd.write (pwm_outer);
+ m2_back.write (0);
+ if (pwm_inner >= 0)
+ {
+ m1_fwd.write (pwm_inner);
+ m1_back.write (0);
+ }
+ else
+ {
+ m1_fwd.write (0);
+ m1_back.write (pwm_inner * -1.0F);
+ }
+ }
+}
\ No newline at end of file