Diff: test.cpp

Revision:

46:f11cb4f93aac

Parent:

45:e16e74bbbf8c

Child:

52:05a8e919ddb0

--- a/test.cpp	Wed Apr 15 19:19:19 2015 +0000
+++ b/test.cpp	Wed Apr 15 21:19:22 2015 +0000
@@ -47,8 +47,8 @@
 
 void showOrientationWithLeds34()
 {
-    led3 = reckoner.cos > 0;
-    led4 = reckoner.sin > 0;
+    led3 = reckoner.cosv > 0;
+    led4 = reckoner.sinv > 0;
 }
 
 void testTurnInPlace()
@@ -78,7 +78,7 @@
         if (motorUpdatePacer.pace())
         {
             int16_t rotationSpeed;
-            float s = (float)reckoner.sin / (1 << 30);
+            float s = (float)reckoner.sinv / (1 << 30);
             integral += s;
             rotationSpeed = -(s * 2400 + integral * 20);
             
@@ -272,7 +272,7 @@
         if (reportPacer.pace())
         {
             pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
-              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
               encoderLeft.getCount(), encoderRight.getCount(), determinant());
         }
     }
@@ -439,7 +439,7 @@
         if(reportPacer.pace())
         {
             pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
-              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
               determinant(), dotProduct());
        }
     }
@@ -507,4 +507,32 @@
             pc.printf("%d\r\n", turnSensor.getAngleDegrees());
         }
     }
+}
+
+void testReckoningWithGyro()
+{   
+    setLeds(0, 0, 0, 1);
+    waitForSignalToStart();
+    setLeds(1, 0, 0, 1);
+
+    loadCalibration();
+    Timer timer;
+    timer.start();
+    TurnSensor turnSensor;
+    turnSensor.start();
+    while(1)
+    {
+        turnSensor.update();
+        updateReckoner(turnSensor);
+        loggerService();
+        
+        if (button1DefinitelyPressed())
+        {
+            break;
+        }
+    }
+    motorsSpeedSet(0, 0);
+    
+    setLeds(1, 1, 1, 1);
+    loggerReportLoop();
 }
\ No newline at end of file