Yuxiang Yang
Made code public
Dependencies: MPU6050IMU_1 TSL1401CL_1 mbed
Diff: main.cpp
- Revision:
- 0:ba0a5e86259f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue May 30 06:55:26 2017 +0000
@@ -0,0 +1,316 @@
+/*
+ * 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 "TSL1401CL.h"
+#include "mbed.h"
+
+/***** Constants ******/
+#define CAM_INTEGRATION_TIME 80
+
+// Higher line threshold -> the sensor will only recognize larger changes in
+// brightness as a line edge
+#define LINE_THRESHOLD 000
+#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.25
+#define TURN_SENS_INNER 1.5F
+#define TURN_SENS_OUTER 0.5F
+
+// Defines data
+#define LINE_HIST_SIZE 12000
+#define RETRACT_START_TIME 50
+#define LINE_END_TIME 1000
+#define SMOOTHING_FACTOR 0.5
+
+// 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
+uint16_t line_hist[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 (uint16_t a, uint16_t b, uint16_t c, uint16_t d);
+
+// Based on steerPointTurns; the outer wheel moves faster the farther the line
+// is from center
+void steerImprovedPointTurns (int16_t line_pos);
+
+void steerImprovedPointTurnsBackward (int16_t line_pos);
+
+int
+main ()
+{
+ /********** SENSOR SETUP **********/
+ TSL1401CL cam (clk, si, adc);
+ cam.setIntegrationTime (CAM_INTEGRATION_TIME);
+
+ int16_t line_pos = -1;
+ int16_t line_pos_previous = -1;
+ int16_t steer_pos = -1;
+ uint16_t line_lost_time = 0;
+
+ hist_index = 0;
+
+ // 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;
+ hist_index++;
+ line_lost_time = 0;
+
+ if (steer_pos == -1)
+ {
+ steer_pos = line_pos;
+ }
+ else
+ {
+ steer_pos = line_pos * (1 - SMOOTHING_FACTOR)
+ + steer_pos * SMOOTHING_FACTOR;
+ }
+
+ // Steer the vehicle
+ steerImprovedPointTurns (steer_pos);
+ line_pos_previous = line_pos;
+ }
+ else
+ { // Lost the line
+ line_lost_time++;
+ if (line_lost_time >= LINE_END_TIME)
+ {
+ // Line end; transmit data to PC
+ setLEDs (1, 1, 1, 1);
+ m1_fwd.write (0);
+ m2_fwd.write (0);
+ m1_back.write (0);
+ m2_back.write (0);
+ while (!pc.readable ())
+ ;
+ pc.printf ("----- Start line data -----\n");
+ for (int i = 0; i <= hist_index; i++)
+ {
+ pc.printf ("%d\n", line_hist[i]);
+ }
+ while (1)
+ ;
+ }
+ else if (abs (steer_pos - TSL1401CL_PIXEL_COUNT / 2)
+ > TSL1401CL_PIXEL_COUNT / 4)
+ {
+ setLEDs (1, 0, 0, 1);
+ // Steer at maximum turn angle towards the last known line
+ // direction
+ if (steer_pos >= 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 if (line_lost_time > RETRACT_START_TIME)
+ {
+ setLEDs (0, 1, 1, 0);
+ steerImprovedPointTurnsBackward (TSL1401CL_PIXEL_COUNT / 2);
+ wait_ms (50);
+ }
+ else
+ {
+ setLEDs (0, 0, 0, 0);
+ }
+ line_hist[hist_index] = 0;
+ hist_index++;
+ }
+ if (hist_index > LINE_HIST_SIZE)
+ {
+ hist_index = 0;
+ }
+ }
+}
+
+float
+max (float a, float b)
+{
+ if (a >= b)
+ return a;
+ return b;
+}
+
+void
+setLEDs (uint16_t a, uint16_t b, uint16_t c, uint16_t d)
+{
+ led1 = a;
+ led2 = b;
+ led3 = c;
+ led4 = d;
+}
+
+void
+steerImprovedPointTurns (int16_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);
+ }
+ }
+}
+
+void
+steerImprovedPointTurnsBackward (int16_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_back.write (pwm_outer);
+ m1_fwd.write (0);
+ if (pwm_inner >= 0)
+ {
+ m2_back.write (pwm_inner);
+ m2_fwd.write (0);
+ }
+ else
+ {
+ m2_back.write (0);
+ m2_fwd.write (pwm_inner * -1.0F);
+ }
+ }
+
+ if (line_pos >= 0)
+ {
+ m2_back.write (pwm_outer);
+ m2_fwd.write (0);
+ if (pwm_inner >= 0)
+ {
+ m1_back.write (pwm_inner);
+ m1_fwd.write (0);
+ }
+ else
+ {
+ m1_back.write (0);
+ m1_fwd.write (pwm_inner * -1.0F);
+ }
+ }
+}
\ No newline at end of file