David Grayson / Mbed 2 deprecated DeadReckoning

David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.

Dependencies:   PololuEncoder Pacer mbed GeneralDebouncer

Files at this revision

API Documentation at this revision

Revision 44:b4a00fbab06b, committed 2019-07-28

Comitter:
DavidEGrayson
Date:
Sun Jul 28 01:22:01 2019 +0000
Parent:
43:0e985a58f174
Child:
45:81dd782bc0b4
Commit message:
Get testDriveHome to work: the robot got to within about 6 inches of where it was in one test I did.

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
test.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Sat Jul 27 22:52:19 2019 +0000
+++ b/main.cpp	Sun Jul 28 01:22:01 2019 +0000
@@ -60,9 +60,9 @@
     //testL3g();
     //testL3gAndShowAverage();
     //testTurnSensor();
-    testReckoner();
+    //testReckoner();
     //testButtons();
-    //testDriveHome();
+    testDriveHome();
     //testFinalSettleIn();
     //testCalibrate();
     //testLineFollowing();
@@ -148,13 +148,14 @@
 
 void updateReckoner()
 {
+    turnSensor.update();
+    reckoner.setTurnAngle(turnSensor.getAngle());
+
     if (!encoderBuffer.hasEvents())
     {
         return;
     }
     
-    reckoner.setTurnAngle(turnSensor.getAngle());
-    
     while(encoderBuffer.hasEvents())
     {
         PololuEncoderEvent event = encoderBuffer.readEvent();
@@ -354,6 +355,37 @@
             speedRight = reduceSpeed(speedRight, reduction);
         }
         
+        // tmphax
+        if (0) {
+        if (det != det)
+        {
+            // NaN
+            setLeds(1, 0, 0, 1);
+        }
+        else if (det > 0.5)
+        {
+            setLeds(0, 0, 1, 1);
+        }
+        else if (det > 0.1)
+        {
+            setLeds(0, 0, 0, 1);
+        }
+        else if (det < -0.5)
+        {
+            setLeds(1, 1, 0, 0);
+        }
+        else if (det < -0.1)
+        {
+            setLeds(1, 0, 0, 0);
+        }
+        else
+        {
+            // Heading basically the right direction.
+            setLeds(1, 1, 1, 1);
+        }
+        speedLeft = speedRight = 0;
+        }
+        
         if (det > 0)
         {
             speedLeft = reduceSpeed(speedLeft, det * 1000);

--- a/test.cpp	Sat Jul 27 22:52:19 2019 +0000
+++ b/test.cpp	Sun Jul 28 01:22:01 2019 +0000
@@ -35,6 +35,7 @@
 void testCloseness()
 {
     doGyroCalibration();
+    turnSensor.start();
     led1 = 1;
     while(1)
     {
@@ -55,6 +56,8 @@
 void testTurnInPlace()
 {
     doGyroCalibration();
+    turnSensor.start();
+
     led1 = 1;
     while(!button1DefinitelyPressed())
     {
@@ -105,6 +108,9 @@
 // This also tests the LineTracker by printing out a lot of data from it.
 void testLineFollowing()
 {
+    doGyroCalibration();
+    turnSensor.start();
+
     led1 = 1;   
     while(!button1DefinitelyPressed())
     {
@@ -148,6 +154,7 @@
 void testDriveHome()
 {
     doGyroCalibration();
+    turnSensor.start();
     led1 = 1;   
     while(!button1DefinitelyPressed())
     {
@@ -167,6 +174,7 @@
 void testFinalSettleIn()
 {
     doGyroCalibration();
+    turnSensor.start();
     led1 = 1;
     while(!button1DefinitelyPressed())
     {
@@ -199,12 +207,11 @@
 void testReckoner()
 {
     doGyroCalibration();
+    turnSensor.start();
     led1 = 1;
-    turnSensor.start();
     Pacer reportPacer(100000);
     while(1)
     {
-        turnSensor.update();
         updateReckoner();
         
         led1 = reckoner.x > 0;