Diff: main.cpp

Revision:

46:f11cb4f93aac

Parent:

45:e16e74bbbf8c

Child:

48:c84b7b3ab0e8

--- a/main.cpp	Wed Apr 15 19:19:19 2015 +0000
+++ b/main.cpp	Wed Apr 15 21:19:22 2015 +0000
@@ -60,7 +60,8 @@
     //testCloseness();
     //testLogger();
     //testL3g();
-    testTurnSensor();
+    //testTurnSensor();
+    //testReckoningWithGyro();
 
     // Real routines for the contest.
     loadCalibration();
@@ -138,6 +139,32 @@
     }
 }
 
+void updateReckoner(TurnSensor & turnSensor)
+{
+    if (!encoderBuffer.hasEvents())
+    {
+        return;
+    }
+    
+    reckoner.setTurnAngle(turnSensor.getAngle());
+    
+    while(encoderBuffer.hasEvents())
+    {
+        PololuEncoderEvent event = encoderBuffer.readEvent();
+        switch(event)
+        {
+        case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
+        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
+            reckoner.handleForward();
+            break;
+        case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
+        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
+            reckoner.handleBackward();
+            break;
+        }
+    }
+}
+
 float magnitude()
 {
     return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);   
@@ -145,8 +172,8 @@
 
 float dotProduct()
 {
-    float s = (float)reckoner.sin / (1 << 30);
-    float c = (float)reckoner.cos / (1 << 30);
+    float s = (float)reckoner.sinv / (1 << 30);
+    float c = (float)reckoner.cosv / (1 << 30);
     float magn = magnitude();
     if (magn == 0){ return 0; }    
     return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
@@ -157,8 +184,8 @@
 float determinant()
 {
     // TODO: get rid of the magic numbers here (i.e. 30)
-    float s = (float)reckoner.sin / (1 << 30);
-    float c = (float)reckoner.cos / (1 << 30);
+    float s = (float)reckoner.sinv / (1 << 30);
+    float c = (float)reckoner.cosv / (1 << 30);
     return (reckoner.x * s - reckoner.y * c) / magnitude();
 }
 
@@ -241,10 +268,13 @@
     uint32_t loopCount = 0;
     Timer timer;
     timer.start();
+    TurnSensor turnSensor;
+    turnSensor.start();
     while(1)
     {
         loopCount += 1;
-        updateReckonerFromEncoders();
+        turnSensor.update();
+        updateReckoner(turnSensor);
         loggerService();
         
         if ((loopCount % 256) == 0)