Diff: main.cpp

Revision:

40:6fa672be85ec

Parent:

39:b19dfc5d4d4b

Child:

41:3ead1dd2cc3a

--- a/main.cpp	Thu Jul 25 02:11:25 2019 +0000
+++ b/main.cpp	Thu Jul 25 02:53:34 2019 +0000
@@ -12,11 +12,17 @@
 #include "reckoner.h"
 #include "buttons.h"
 #include "line_tracker.h"
+#include "l3g.h"
+#include "turn_sensor.h"
 
 Reckoner reckoner;
 LineTracker lineTracker;
+TurnSensor turnSensor;
 Logger logger;
-Pacer loggerPacer(50000);
+
+uint32_t totalEncoderCounts = 0;
+uint32_t nextLogEncoderCount = 0;
+const uint32_t logSpacing = 100;
 
 const int16_t drivingSpeed = 400;
 
@@ -32,6 +38,13 @@
 {
     pc.baud(115200);
     
+    if (l3gInit())
+    {
+        // Error initializing the gyro.
+        setLeds(0, 0, 1, 1);
+        while(1);
+    }
+    
     // Enable pull-ups on encoder pins and give them a chance to settle.
     encodersInit();
     motorsInit();
@@ -42,6 +55,8 @@
     //testEncoders();
     //testMotorSpeed();
     //testLineSensors();
+    //testL3g();
+    testTurnSensor();
     //testReckoner();
     //testButtons();
     //testDriveHome();
@@ -82,9 +97,15 @@
 
 void loggerService()
 {
-    if (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);
     }
 }
 
@@ -129,15 +150,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;
         }
     }