Diff: main.cpp

Revision:

21:c279c6a83671

Parent:

20:dbec34f0e76b

Child:

22:44c032e59ff5

--- 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);
 }