Revision 0:ba0a5e86259f, committed 2017-05-30

Comitter:

yxyang

Date:

Tue May 30 06:55:26 2017 +0000

Commit message:

Changed in this revision

diff -r 000000000000 -r ba0a5e86259f MPU6050IMU.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050IMU.lib	Tue May 30 06:55:26 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/yxyang/code/MPU6050IMU_1/#d23cb6fd82b7

diff -r 000000000000 -r ba0a5e86259f TSL1401CL.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TSL1401CL.lib	Tue May 30 06:55:26 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/yxyang/code/TSL1401CL_1/#4e108bee7994

diff -r 000000000000 -r ba0a5e86259f main.cpp
--- /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

diff -r 000000000000 -r ba0a5e86259f mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 30 06:55:26 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/ad3be0349dc5
\ No newline at end of file