Diff: main.cpp

Revision:

44:edcacba44760

Parent:

43:6cb32548c1b4

Child:

45:e16e74bbbf8c

--- a/main.cpp	Tue Apr 14 01:56:59 2015 +0000
+++ b/main.cpp	Wed Apr 15 19:14:39 2015 +0000
@@ -12,6 +12,8 @@
 #include "reckoner.h"
 #include "buttons.h"
 #include "line_tracker.h"
+#include "l3g.h"
+#include "turn_sensor.h"
 
 Reckoner reckoner;
 LineTracker lineTracker;
@@ -31,6 +33,13 @@
     pc.baud(115200);
     
     // Enable pull-ups on encoder pins and give them a chance to settle.
+    if (l3gInit())
+    {
+        // Error initializing the gyro.
+        setLeds(0, 0, 1, 1);
+        while(1);
+    }
+
     encodersInit();
     motorsInit();
     buttonsInit();
@@ -50,7 +59,8 @@
     //testTurnInPlace();
     //testCloseness();
     //testLogger();
-    
+    testL3g();
+    //testTurnSensor();
 
     // Real routines for the contest.
     loadCalibration();
@@ -243,7 +253,13 @@
         }
         
         lineTracker.read();
-        updateMotorsToFollowLineFast();        
+        updateMotorsToFollowLineFast();
+        
+        if (button1DefinitelyPressed())
+        {
+            break;
+        }
     }
+    motorsSpeedSet(0, 0);
 }