Diff: main.cpp

Revision:

48:c84b7b3ab0e8

Parent:

46:f11cb4f93aac

Child:

49:eaa6fd514f4f

--- a/main.cpp	Wed Apr 15 21:19:52 2015 +0000
+++ b/main.cpp	Wed Apr 15 21:42:52 2015 +0000
@@ -17,9 +17,14 @@
 
 Reckoner reckoner;
 LineTracker lineTracker;
+TurnSensor turnSensor;
 Logger logger;
 Pacer loggerPacer(50000);
 
+uint32_t totalEncoderCounts = 0;
+uint32_t nextLogEncoderCount = 0;
+const uint32_t logSpacing = 200;
+
 void setLeds(bool v1, bool v2, bool v3, bool v4)
 {
    led1 = v1;
@@ -78,9 +83,16 @@
 
 void loggerService()
 {
-    if (loggerPacer.pace())
+    // loggerPacer.pace()
+    if (totalEncoderCounts > nextLogEncoderCount)
     {
-        logger.log();   
+        nextLogEncoderCount += logSpacing;
+        
+        struct LogEntry entry;
+        entry.turnAngle = turnSensor.getAngle() >> 16;
+        entry.x = reckoner.x >> 16;
+        entry.y = reckoner.y >> 16;
+        logger.log(&entry);
     }
 }
 
@@ -125,15 +137,19 @@
         {
         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
             reckoner.handleTickLeftForward();
+            totalEncoderCounts++;
             break;
         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
             reckoner.handleTickLeftBackward();
+            totalEncoderCounts--;
             break;
         case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
             reckoner.handleTickRightForward();
+            totalEncoderCounts++;
             break;
         case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
             reckoner.handleTickRightBackward();
+            totalEncoderCounts--;
             break;
         }
     }
@@ -155,11 +171,13 @@
         {
         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
         case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
+            totalEncoderCounts++;
             reckoner.handleForward();
             break;
         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
         case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
             reckoner.handleBackward();
+            totalEncoderCounts--;
             break;
         }
     }
@@ -237,7 +255,7 @@
 
 void updateMotorsToFollowLineFast()
 {
-    const int16_t drivingSpeed = 1100;
+    const int16_t drivingSpeed = 1000;
     const int32_t followLineStrength = drivingSpeed * 5 / 4;
     static int16_t lastPosition = 1000;
 
@@ -261,14 +279,14 @@
 }
 
 void followLineFast()
-{   
+{
+    totalEncoderCounts = 0;
     Pacer reportPacer(200000);
     
     loadCalibration();
     uint32_t loopCount = 0;
     Timer timer;
     timer.start();
-    TurnSensor turnSensor;
     turnSensor.start();
     while(1)
     {