Diff: main.cpp

Revision:

39:a5e25fd52ff8

Parent:

38:5e93a479c244

Child:

40:e79cefc241f8

--- a/main.cpp	Thu Mar 13 17:49:43 2014 +0000
+++ b/main.cpp	Tue Apr 14 01:06:41 2015 +0000
@@ -18,8 +18,6 @@
 Logger logger;
 Pacer loggerPacer(50000);
 
-const int16_t drivingSpeed = 400;
-
 void setLeds(bool v1, bool v2, bool v3, bool v4)
 {
    led1 = v1;
@@ -59,21 +57,9 @@
     
     setLeds(1, 0, 0, 0);
     waitForSignalToStart();
-    
+        
     setLeds(0, 1, 0, 0);    
-    findLine();
-    
-    //setLeds(1, 1, 0, 0);
-    //turnRightToFindLine();
-    
-    setLeds(0, 0, 1, 0);
-    followLineToEnd();
-    
-    setLeds(1, 0, 1, 0);
-    driveHomeAlmost();
-    
-    //setLeds(0, 1, 1, 0);
-    //finalSettleIn();
+    followLineFast();
     
     setLeds(1, 1, 1, 1);
     loggerReportLoop();
@@ -102,11 +88,11 @@
 void loadCalibration()
 {
     /** QTR-3RC **/
-    lineTracker.calibratedMinimum[0] =  100;
-    lineTracker.calibratedMinimum[1] =   94;
-    lineTracker.calibratedMinimum[2] =  103;
-    lineTracker.calibratedMaximum[0] =  792;
-    lineTracker.calibratedMaximum[1] =  807;
+    lineTracker.calibratedMinimum[0] =  137;
+    lineTracker.calibratedMinimum[1] =  132;
+    lineTracker.calibratedMinimum[2] =  154;
+    lineTracker.calibratedMaximum[0] =  644;
+    lineTracker.calibratedMaximum[1] =  779;
     lineTracker.calibratedMaximum[2] = 1000;    
     
     /** QTR-3A
@@ -192,27 +178,9 @@
     wait(0.2);
 }
 
-// Keep the robot pointing the in the right direction (1, 0).
-// This should basically keep it going straight.
-void updateMotorsToDriveStraight()
-{
-    const int32_t straightDriveStrength = 1000;
-    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);   
-}
-
 void updateMotorsToFollowLine()
 {
+    const int16_t drivingSpeed = 400;
     const int followLineStrength = drivingSpeed * 5 / 4;
 
     int16_t speedLeft = drivingSpeed;
@@ -227,201 +195,46 @@
         speedRight = reduceSpeed(speedRight, reduction);
     }
     
-    motorsSpeedSet(speedLeft, speedRight);      
-}
-
-void findLine()
-{   
-    GeneralDebouncer lineStatus(10000);
-    while(1)
-    {
-        lineTracker.read();
-        lineTracker.updateCalibration();       
-        updateReckonerFromEncoders();
-        loggerService();
-        updateMotorsToDriveStraight();
-        lineStatus.update(lineTracker.getLineVisible());       
-
-        if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 20000)
-        {
-            break;
-        }
-    }
+    motorsSpeedSet(speedLeft, speedRight);
 }
 
-/**
-void turnRightToFindLine()
-{   
-    while(1)
-    {
-        lineTracker.read();
-        lineTracker.updateCalibration();
-        updateReckonerFromEncoders();
-        
-        if(lineTracker.getLineVisible())
-        {
-            break;   
-        }
-        
-        motorsSpeedSet(300, 100);        
-    }
-}**/
-
-void followLineToEnd()
+void updateMotorsToFollowLineFast()
 {
-    Timer timer;
-    timer.start();
-    
-    GeneralDebouncer lineStatus(10000);
-    const uint32_t lineDebounceTime = 1000000;
-    
-    while(1)
-    {
-        lineTracker.read();
-        updateReckonerFromEncoders();
-        loggerService();
+    const int16_t drivingSpeed = 600;
+    const int followLineStrength = drivingSpeed * 5 / 4;
+    static int16_t lastPosition = 0;
+
+    int16_t position = lineTracker.getLinePosition();
 
-        lineStatus.update(lineTracker.getLineVisible());
-        
-        bool lostLine = lineStatus.getState() == false &&
-          lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime;        
-        if(lostLine && timer.read_us() >= 2000000)
-        {
-            break;   
-        }
-        
-        updateMotorsToFollowLine();     
+    int16_t speedLeft = drivingSpeed;
+    int16_t speedRight = drivingSpeed;
+    int16_t reduction = (position - 1000) * followLineStrength / 1000;
+    if(reduction < 0)
+    {
+        speedLeft = reduceSpeed(speedLeft, -reduction);   
     }
+    else
+    {
+        speedRight = reduceSpeed(speedRight, reduction);
+    }
+    
+    motorsSpeedSet(speedLeft, speedRight);
+    
+    lastPosition = position;
 }
 
-void driveHomeAlmost()
-{
-    Timer timer;
-    timer.start();
+void followLineFast()
+{   
+    Pacer reportPacer(200000);
     
+    loadCalibration();
     while(1)
     {
         updateReckonerFromEncoders();
         loggerService();
 
-        float magn = magnitude();
-        
-        if (magn < (1<<(14+7)))  
-        {
-            // We are within 128 encoder ticks, so go to the next step.
-            break;
-        }
-        
-        float det = determinant();
-        
-        int16_t speedLeft = drivingSpeed;
-        int16_t speedRight = drivingSpeed;
-        if (magn < (1<<(14+9))) // Within 512 encoder ticks of the origin, so slow down.
-        {
-            int16_t reduction = (1 - magn/(1<<(14+8))) * (drivingSpeed/2);
-            speedLeft = reduceSpeed(speedLeft, reduction);
-            speedRight = reduceSpeed(speedRight, reduction);
-        }
-        
-        if (det > 0)
-        {
-            speedLeft = reduceSpeed(speedLeft, det * 1000);
-        }
-        else
-        {
-            speedRight = reduceSpeed(speedRight, -det * 1000);               
-        }
-        motorsSpeedSet(speedLeft, speedRight);
+        lineTracker.read();
+        updateMotorsToFollowLineFast();        
     }
-    
-    motorsSpeedSet(0, 0);
 }
 
-void finalSettleIn()
-{
-    const int16_t settleSpeed = 300;
-    const int16_t settleModificationStrength = 150;
-    
-    Timer timer;
-    timer.start();
-    
-    // State 0: rotating
-    // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
-    uint8_t state = 0;
-    
-    Pacer reportPacer(200000);
-    Pacer motorUpdatePacer(10000);   
-
-    float integral = 0;
-    
-    motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling
-    
-    while(1)
-    {
-        led1 = (state == 1);
-        
-        updateReckonerFromEncoders();
-        loggerService();
-
-        float dot = dotProduct();
-        int16_t speedModification = -dot * settleModificationStrength;
-        if (speedModification > settleModificationStrength)
-        {
-            speedModification = settleModificationStrength;    
-        }
-        else if (speedModification < -settleModificationStrength)
-        {
-            speedModification = -settleModificationStrength;
-        }
-        
-        if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29))
-        {
-            // Stop turning and start trying to maintain the right position.
-            state = 1;
-        }
-        
-        if (state == 1 && timer.read_ms() >= 5000)
-        {
-            // Stop moving.
-            break;   
-        }
-        
-        if (motorUpdatePacer.pace())
-        {
-            int16_t rotationSpeed;
-            if (state == 1)
-            {
-                float s = (float)reckoner.sin / (1 << 30);
-                integral += s;
-                rotationSpeed = -(s * 2400 + integral * 20);
-                
-                if (rotationSpeed > 300)
-                {
-                    rotationSpeed = 300; 
-                }
-                if (rotationSpeed < -300)
-                {
-                    rotationSpeed = -300; 
-                }
-            }
-            else
-            {
-                rotationSpeed = settleSpeed;
-            }
-            
-            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());
-        }
-    }
-    
-    // Done!  Stop moving.
-    motorsSpeedSet(0, 0);
-}