Revision 21:c279c6a83671, committed 2014-02-27

Comitter:

DavidEGrayson

Date:

Thu Feb 27 23:20:34 2014 +0000

Parent:

20:dbec34f0e76b

Child:

22:44c032e59ff5

Commit message:

Wrote a whole bunch of code that could theoretically allow the robot to compete, but it has not been tested at all yet.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/line_tracker.cpp	Thu Feb 27 23:20:34 2014 +0000
@@ -0,0 +1,133 @@
+#include "line_tracker.h"
+
+static uint16_t readSensor(uint8_t index)
+{
+    return lineSensorsAnalog[index].read_u16();
+}
+
+LineTracker::LineTracker()
+{
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        calibratedMaximum[s] = 0;   
+        calibratedMinimum[s] = 0xFFFF;   
+    }
+}
+
+void LineTracker::updateCalibration()
+{
+    const int sampleCount = 10;
+
+    uint16_t maxValues[LINE_SENSOR_COUNT];
+    uint16_t minValues[LINE_SENSOR_COUNT];
+    
+    for(uint8_t sample = 0; sample < sampleCount; sample++)
+    {
+        for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+        {
+            uint16_t reading = readSensor(s);
+            if (reading > maxValues[s]) { maxValues[s] = reading; }
+            if (reading < minValues[s]) { minValues[s] = reading; }
+        }
+    }
+    
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        if (minValues[s] > calibratedMaximum[s]) { calibratedMaximum[s] = minValues[s]; }
+        if (maxValues[s] > calibratedMinimum[s]) { calibratedMinimum[s] = maxValues[s]; }
+    }
+}
+
+void LineTracker::read()
+{
+    readRawValues();
+    updateCalibratedValues();
+    updateLineStatus();
+}
+
+void LineTracker::readRawValues()
+{
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        rawValues[s] = readSensor(s);
+    }
+}
+
+void LineTracker::updateCalibratedValues()
+{
+    // Update the calibrated values.
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        uint16_t calmin = calibratedMinimum[s];
+        uint16_t calmax = calibratedMaximum[s];
+        uint16_t denominator = calmax - calmin;
+        int16_t x = 0;
+        if(denominator != 0)
+        {
+            x = ((int32_t)rawValues[s] - calmin) * 1000 / denominator;
+            if(x < 0)
+            {
+                x = 0;
+            }
+            else if(x > 1000)
+            {
+                x = 1000;
+            }
+        }
+        calibratedValues[s] = x;
+    }
+}
+
+void LineTracker::updateLineStatus()
+{
+    uint32_t avg = 0;
+    uint32_t sum = 0;
+    
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        // keep track of whether we see the line at all
+        uint16_t value = calibratedValues[s];
+        if (value > 200)
+        {
+            lineVisible = true;
+        }
+        
+        // only average in values that are above a noise threshold
+        if (value > 50)
+        {
+            avg += (uint32_t)(value) * s * 1000;
+            sum += value;
+        }
+    }
+    
+    if (lineVisible)
+    {
+        linePosition = avg/sum;   
+    }
+    else
+    {
+        // We cannot see the line, so just snap the position to the left-most or right-most
+        // depending on what we saw previousl.
+        
+        const uint32_t max = (LINE_SENSOR_COUNT-1)*1000;
+        if(linePosition < max/2)
+        {
+            linePosition = 0;
+        }
+        else
+        {
+            linePosition = max;
+        }
+    }
+}
+
+// The return value of this should only be heeded if the calibration seems to be OK.
+bool LineTracker::getLineVisible()
+{
+    return lineVisible;
+}
+
+uint16_t LineTracker::getLinePosition()
+{
+    return linePosition;   
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/line_tracker.h	Thu Feb 27 23:20:34 2014 +0000
@@ -0,0 +1,28 @@
+#pragma once
+
+#include "line_sensors.h"
+
+class LineTracker
+{
+    public:
+    LineTracker();
+    
+    void updateCalibration();
+    
+    void read();    
+    bool getLineVisible();
+    uint16_t getLinePosition();
+    
+    uint16_t rawValues[LINE_SENSOR_COUNT];
+    uint16_t calibratedValues[LINE_SENSOR_COUNT];
+    uint16_t calibratedMaximum[LINE_SENSOR_COUNT];
+    uint16_t calibratedMinimum[LINE_SENSOR_COUNT];
+    
+    private:
+    void readRawValues();
+    void updateCalibratedValues();
+    void updateLineStatus();
+    
+    bool lineVisible;
+    uint16_t linePosition;
+};

--- a/main.cpp	Thu Feb 27 19:46:35 2014 +0000
+++ b/main.cpp	Thu Feb 27 23:20:34 2014 +0000
@@ -1,7 +1,9 @@
 #include <mbed.h>
 #include <Pacer.h>
+#include <GeneralDebouncer.h>
 #include <math.h>
 
+#include "main.h"
 #include "motors.h"
 #include "encoders.h"
 #include "leds.h"
@@ -9,6 +11,20 @@
 #include "test.h"
 #include "reckoner.h"
 #include "buttons.h"
+#include "line_tracker.h"
+
+Reckoner reckoner;
+LineTracker lineTracker;
+
+const int16_t drivingSpeed = 300;
+
+void setLeds(bool v1, bool v2, bool v3, bool v4)
+{
+   led1 = v1;
+   led2 = v2;
+   led3 = v3;
+   led4 = v4;
+}
 
 int __attribute__((noreturn)) main()
 {
@@ -26,12 +42,23 @@
     //testReckoner();
     //testButtons();
     //testDriveHome();
-    testFinalSettleIn();
+    //testFinalSettleIn();
 
-    while(1)
-    {
-
-    }
+    // Real routines for the contest.
+    setLeds(1, 0, 0, 0);
+    waitForSignalToStart();
+    setLeds(0, 1, 0, 0);
+    findLineAndCalibrate();
+    setLeds(1, 1, 0, 0);
+    turnRightToFindLine();
+    setLeds(0, 0, 1, 0);
+    followLineToEnd();
+    setLeds(1, 0, 1, 0);
+    driveHomeAlmost();
+    setLeds(0, 1, 1, 0);
+    finalSettleIn();
+    setLeds(1, 1, 1, 1);
+    while(1){}
 }
 
 void updateReckonerFromEncoders()
@@ -81,7 +108,7 @@
     return (reckoner.x * s - reckoner.y * c) / magnitude();
 }
 
-int16_t reduceSpeed(int16_t speed, int16_t reduction)
+int16_t reduceSpeed(int16_t speed, int32_t reduction)
 {
     if (reduction > speed)
     {
@@ -93,26 +120,146 @@
     }
 }
 
-void driveHomeAlmost();
-void finalSettleIn();
+// Returns true if each line sensor has one third of a volt of difference between the
+// maximum seen value and the minimum.
+bool calibrationLooksGood()
+{
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        uint16_t contrast = lineTracker.calibratedMaximum[s] - lineTracker.calibratedMinimum[s];
+        if (contrast < 9929)  // 0xFFFF*0.5/3.3 = 9929
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
+void waitForSignalToStart()
+{
+    while(!button1DefinitelyPressed())
+    {
+        updateReckonerFromEncoders();
+    }   
+    reckoner.reset();
+}
+
 
-void driveHome()
+void findLineAndCalibrate()
 {
-    driveHomeAlmost();
-    finalSettleIn();
+    const int32_t straightDriveStrength = 1000;
+    
+    Timer timer;
+    timer.start();
+    
+    Timer goodCalibrationTimer;
+    bool goodCalibration = false;
+    
+    while(1)
+    {
+        lineTracker.read();
+        lineTracker.updateCalibration();       
+        updateReckonerFromEncoders();
+        
+        // Keep the robot pointing the in the right direction.  This should basically keep it going straight.
+        int16_t speedLeft = drivingSpeed;
+        int16_t speedRight = drivingSpeed;
+        int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
+        if (reduction > 0)
+        {
+            speedRight = reduceSpeed(speedRight, -reduction);
+        }
+        else
+        {
+            speedLeft = reduceSpeed(speedLeft, reduction);               
+        }
+        motorsSpeedSet(speedLeft, speedRight);
+        
+        if (goodCalibration)
+        {
+            if(goodCalibrationTimer.read_us() >= 300000)
+            {
+                // The calibration was good and we traveled for a bit of time after that,
+                // so we must be a bit over the line.
+                break;
+            }
+        }
+        else
+        {
+            if(calibrationLooksGood())
+            {
+                goodCalibration = true;
+                goodCalibrationTimer.start();
+            }
+        }
+    }
+}
+
+void turnRightToFindLine()
+{   
+    while(1)
+    {
+        lineTracker.read();
+        lineTracker.updateCalibration();
+        updateReckonerFromEncoders();
+        
+        if(lineTracker.getLineVisible())
+        {
+            break;   
+        }
+        
+        motorsSpeedSet(300, 100);        
+    }
+}
+
+void followLineToEnd()
+{
+    GeneralDebouncer lineStatus(10000);
+    const uint32_t lineDebounceTime = 100000;
+    const int followLineStrength = 300;
+    
+    while(1)
+    {
+        lineTracker.read();
+        updateReckonerFromEncoders();
+
+        lineStatus.update(lineTracker.getLineVisible());
+        
+        bool lostLine = lineStatus.getState() == false &&
+          lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime;
+        
+        if(lostLine)
+        {
+            break;   
+        }
+        
+        if(lineTracker.getLineVisible())
+        {
+            break;   
+        }
+        
+        int16_t speedLeft = drivingSpeed;
+        int16_t speedRight = drivingSpeed;
+        int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500;
+        if(reduction < 0)
+        {
+            reduceSpeed(speedLeft, -reduction);   
+        }
+        else
+        {
+            reduceSpeed(speedRight, reduction);
+        }
+        
+        
+        motorsSpeedSet(speedLeft, speedRight);        
+    }
 }
 
 void driveHomeAlmost()
 {
-    led1 = 1; led2 = 1; led3 = 0; led4 = 0;
-    
-    bool dir = false;
-    uint16_t transitions = 0;
     Timer timer;
     timer.start();
     
-    const int16_t maxSpeed = 300;
-    
     while(1)
     {
         updateReckonerFromEncoders();
@@ -127,17 +274,11 @@
         
         float det = determinant();
         
-        {
-            bool nextDir = det > 0;
-            if (nextDir != dir) { transitions++; }
-            dir = nextDir;
-        }
-        
-        int16_t speedLeft = maxSpeed;
-        int16_t speedRight = maxSpeed;
+        int16_t speedLeft = drivingSpeed;
+        int16_t speedRight = drivingSpeed;
         if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down.
         {
-            int16_t reduction = (1 - magn/(1<<20)) * maxSpeed;
+            int16_t reduction = (1 - magn/(1<<20)) * drivingSpeed;
             speedLeft = reduceSpeed(speedLeft, reduction);
             speedRight = reduceSpeed(speedRight, reduction);
         }
@@ -158,8 +299,6 @@
 
 void finalSettleIn()
 {
-    led1 = 1; led2 = 1; led3 = 1; led4 = 0;
-    
     const int16_t settleSpeed = 200;
     const int16_t settleModificationStrength = 100;
     
@@ -170,6 +309,8 @@
     // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
     uint8_t state = 0;
     
+    Pacer reportPacer(200000);   
+
     while(1)
     {
         updateReckonerFromEncoders();
@@ -185,17 +326,23 @@
             speedModification = -settleModificationStrength;
         }
         
-        if (state == 0 && timer.read_ms() >= 6000 && reckoner.cos > (1 << 29))
+        if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29))
         {
-            led1 = 1; led2 = 0; led3 = 1; led4 = 0;
+            // Stop turning and start trying to maintain the right position.
             state = 1;
         }
         
+        if (state == 1 && timer.read_ms() >= 5000)
+        {
+            // Stop moving.
+            break;   
+        }
+        
         int16_t rotationSpeed;
         if (state == 1)
         {
             float s = (float)reckoner.sin / (1 << 30);
-            rotationSpeed = -s * 300;
+            rotationSpeed = -s * 600;
         }
         else
         {
@@ -205,8 +352,15 @@
         int16_t speedLeft = -rotationSpeed + speedModification;
         int16_t speedRight = rotationSpeed + speedModification;
         motorsSpeedSet(speedLeft, speedRight);
+        
+        if (state == 1 && reportPacer.pace())
+        {
+            pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
+              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+              determinant(), dotProduct());
+        }
     }
     
-    led1 = 1; led2 = 1; led3 = 1; led4 = 1;
+    // Done!  Stop moving.
     motorsSpeedSet(0, 0);
 }

--- a/main.h	Thu Feb 27 19:46:35 2014 +0000
+++ b/main.h	Thu Feb 27 23:20:34 2014 +0000
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "reckoner.h"
+#include "line_tracker.h"
+
+void waitForSignalToStart();
+void findLineAndCalibrate();
+void turnRightToFindLine();
+void followLineToEnd();
+void driveHomeAlmost();
+void finalSettleIn();
+
+void updateReckonerFromEncoders();
+float determinant();
+float dotProduct();
+
+extern Reckoner reckoner;
+extern LineTracker lineTracker;

--- a/reckoner.cpp	Thu Feb 27 19:46:35 2014 +0000
+++ b/reckoner.cpp	Thu Feb 27 23:20:34 2014 +0000
@@ -102,9 +102,12 @@
 
 #define LOG_COS_TO_X_CONVERSION  16    // 30 - 14
 
-Reckoner reckoner;
+Reckoner::Reckoner()
+{
+    reset();
+}
 
-Reckoner::Reckoner()
+void Reckoner::reset()
 {
   cos = 1 << LOG_UNIT_MAGNITUDE;
   sin = 0;

--- a/reckoner.h	Thu Feb 27 19:46:35 2014 +0000
+++ b/reckoner.h	Thu Feb 27 23:20:34 2014 +0000
@@ -1,3 +1,5 @@
+#pragma once
+
 class Reckoner
 {
     public:
@@ -16,6 +18,8 @@
     // or backwards due to a single encoder tick.
     int32_t x, y;
 
+    void reset();
+
     void handleTickLeftForward();
     void handleTickLeftBackward();
     void handleTickRightForward();
@@ -25,5 +29,3 @@
     void handleRight();
     void handleLeft();
 };
-
-extern Reckoner reckoner;
\ No newline at end of file

--- a/test.cpp	Thu Feb 27 19:46:35 2014 +0000
+++ b/test.cpp	Thu Feb 27 23:20:34 2014 +0000
@@ -4,6 +4,7 @@
 #include "motors.h"
 #include <Pacer.h>
 
+#include "main.h"
 #include "test.h"
 #include "leds.h"
 #include "encoders.h"
@@ -21,8 +22,9 @@
     while(!button1DefinitelyPressed())
     {
         updateReckonerFromEncoders();
-    }   
-    driveHome();
+    }
+    driveHomeAlmost();
+    finalSettleIn();
     infiniteReckonerReportLoop();     
 }
 

--- a/test.h	Thu Feb 27 19:46:35 2014 +0000
+++ b/test.h	Thu Feb 27 23:20:34 2014 +0000
@@ -7,14 +7,3 @@
 void __attribute__((noreturn)) testButtons();
 void __attribute__((noreturn)) testDriveHome();
 void __attribute__((noreturn)) testFinalSettleIn();
-
-// These are actually defined in main.h:
-
-void updateReckonerFromEncoders();
-
-void driveHome();
-void driveHomeAlmost();
-void finalSettleIn();
-
-float determinant();
-float dotProduct();
\ No newline at end of file