Yuxiang Yang
ctrl
Revision 0:f13ea6a4e622, committed 2018-02-24
- Comitter:
- yxyang
- Date:
- Sat Feb 24 01:25:40 2018 +0000
- Commit message:
- ctrl
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TSL1401CL.lib Sat Feb 24 01:25:40 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/yxyang/code/TSL1401CLL/#9c6145a1d114
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Feb 24 01:25:40 2018 +0000
@@ -0,0 +1,212 @@
+/*
+ * 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 "mbed.h"
+#include "TSL1401CL.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 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
+double SPEED_PWM = 0.2;
+#define TURN_SENS_INNER 1.5F
+#define TURN_SENS_OUTER 0.5F
+
+// Defines data
+#define LINE_HIST_SIZE 12000
+#define LINE_END_TIME 25
+
+// 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];
+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;
+ int line_width = 30;
+
+ hist_index = 0;
+
+ // Read garbage data
+ while(!cam.integrationReady());
+ cam.read();
+
+ int avg_pixel = 0;
+ while(1) {
+ /***** Read line sensor *****/
+ while(!cam.integrationReady()); // Wait for integration
+ cam.read();
+
+ /***** Line following loop *****/
+
+ avg_pixel = cam.findPixelIntensity();
+ if (avg_pixel > 40000) {
+ setLEDs(1, 1, 1, 1);
+ m1_fwd.write(0.3);
+ m2_fwd.write(0.3);
+ //SPEED_PWM += 0.02;
+ } else {
+ setLEDs(0, 0, 0, 0);
+ m1_fwd.write(0);
+ m2_fwd.write(0);
+ //SPEED_PWM = 0.25;
+ }
+ }
+}
+
+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); // 0.5-1.5 SPEED_PWM
+ float pwm_inner = SPEED_PWM*(1.0F - TURN_SENS_INNER*abs(line_pos)*2.0F/(float)TSL1401CL_PIXEL_COUNT); // -0.5-2.5 SPEED_PWM
+
+ // 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Feb 24 01:25:40 2018 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/856d2700e60b \ No newline at end of file