Diff: main.cpp

Revision:

44:b4a00fbab06b

Parent:

43:0e985a58f174

Child:

45:81dd782bc0b4

--- a/main.cpp	Sat Jul 27 22:52:19 2019 +0000
+++ b/main.cpp	Sun Jul 28 01:22:01 2019 +0000
@@ -60,9 +60,9 @@
     //testL3g();
     //testL3gAndShowAverage();
     //testTurnSensor();
-    testReckoner();
+    //testReckoner();
     //testButtons();
-    //testDriveHome();
+    testDriveHome();
     //testFinalSettleIn();
     //testCalibrate();
     //testLineFollowing();
@@ -148,13 +148,14 @@
 
 void updateReckoner()
 {
+    turnSensor.update();
+    reckoner.setTurnAngle(turnSensor.getAngle());
+
     if (!encoderBuffer.hasEvents())
     {
         return;
     }
     
-    reckoner.setTurnAngle(turnSensor.getAngle());
-    
     while(encoderBuffer.hasEvents())
     {
         PololuEncoderEvent event = encoderBuffer.readEvent();
@@ -354,6 +355,37 @@
             speedRight = reduceSpeed(speedRight, reduction);
         }
         
+        // tmphax
+        if (0) {
+        if (det != det)
+        {
+            // NaN
+            setLeds(1, 0, 0, 1);
+        }
+        else if (det > 0.5)
+        {
+            setLeds(0, 0, 1, 1);
+        }
+        else if (det > 0.1)
+        {
+            setLeds(0, 0, 0, 1);
+        }
+        else if (det < -0.5)
+        {
+            setLeds(1, 1, 0, 0);
+        }
+        else if (det < -0.1)
+        {
+            setLeds(1, 0, 0, 0);
+        }
+        else
+        {
+            // Heading basically the right direction.
+            setLeds(1, 1, 1, 1);
+        }
+        speedLeft = speedRight = 0;
+        }
+        
         if (det > 0)
         {
             speedLeft = reduceSpeed(speedLeft, det * 1000);