Diff: main.cpp

Revision:

43:0e985a58f174

Parent:

42:96671b71aac5

Child:

44:b4a00fbab06b

--- a/main.cpp	Sat Jul 27 20:58:46 2019 +0000
+++ b/main.cpp	Sat Jul 27 22:52:19 2019 +0000
@@ -59,15 +59,13 @@
     //testLineSensors();
     //testL3g();
     //testL3gAndShowAverage();
-    testTurnSensor();
-    //testReckoner();
+    //testTurnSensor();
+    testReckoner();
     //testButtons();
     //testDriveHome();
     //testFinalSettleIn();
     //testCalibrate();
     //testLineFollowing();
-    //testAnalog();
-    //testSensorGlitches();
     //testTurnInPlace();
     //testCloseness();
     //testLogger();
@@ -148,27 +146,28 @@
     lineTracker.calibratedMaximum[2] = 1000;    
 }
 
-void updateReckonerFromEncoders()
+void updateReckoner()
 {
+    if (!encoderBuffer.hasEvents())
+    {
+        return;
+    }
+    
+    reckoner.setTurnAngle(turnSensor.getAngle());
+    
     while(encoderBuffer.hasEvents())
     {
         PololuEncoderEvent event = encoderBuffer.readEvent();
         switch(event)
         {
         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
-            reckoner.handleTickLeftForward();
+        case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
             totalEncoderCounts++;
+            reckoner.handleForward();
             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();
+            reckoner.handleBackward();
             totalEncoderCounts--;
             break;
         }
@@ -182,8 +181,8 @@
 
 float dotProduct()
 {
-    float s = (float)reckoner.sin / (1 << 30);
-    float c = (float)reckoner.cos / (1 << 30);
+    float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT);
+    float c = (float)reckoner.cosv / (1 << RECKONER_LOG_UNIT);
     float magn = magnitude();
     if (magn == 0){ return 0; }    
     return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
@@ -193,9 +192,8 @@
 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
 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 << RECKONER_LOG_UNIT);
+    float c = (float)reckoner.cosv / (1 << RECKONER_LOG_UNIT);
     return (reckoner.x * s - reckoner.y * c) / magnitude();
 }
 
@@ -215,12 +213,12 @@
 {
     while(!button1DefinitelyPressed())
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
     }
     reckoner.reset();
     while(button1DefinitelyPressed())
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
     }
     wait(0.2);
 }
@@ -232,7 +230,7 @@
     const int32_t straightDriveStrength = 1000;
     int16_t speedLeft = drivingSpeed;
     int16_t speedRight = drivingSpeed;
-    int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
+    int32_t reduction = reckoner.sinv * straightDriveStrength >> RECKONER_LOG_UNIT;
     if (reduction > 0)
     {
         speedRight = reduceSpeed(speedRight, reduction);
@@ -270,7 +268,7 @@
     {
         lineTracker.read();
         lineTracker.updateCalibration();       
-        updateReckonerFromEncoders();
+        updateReckoner();
         loggerService();
         updateMotorsToDriveStraight();
         lineStatus.update(lineTracker.getLineVisible());       
@@ -311,7 +309,7 @@
     while(1)
     {
         lineTracker.read();
-        updateReckonerFromEncoders();
+        updateReckoner();
         loggerService();
 
         lineStatus.update(lineTracker.getLineVisible());
@@ -334,7 +332,7 @@
     
     while(1)
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
         loggerService();
 
         float magn = magnitude();
@@ -393,7 +391,7 @@
     {
         led1 = (state == 1);
         
-        updateReckonerFromEncoders();
+        updateReckoner();
         loggerService();
 
         float dot = dotProduct();
@@ -407,7 +405,8 @@
             speedModification = -settleModificationStrength;
         }
         
-        if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29))
+        if (state == 0 && timer.read_ms() >= 2000 &&
+          reckoner.cosv > (1 << (RECKONER_LOG_UNIT - 1)))
         {
             // Stop turning and start trying to maintain the right position.
             state = 1;
@@ -424,7 +423,7 @@
             int16_t rotationSpeed;
             if (state == 1)
             {
-                float s = (float)reckoner.sin / (1 << 30);
+                float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT);
                 integral += s;
                 rotationSpeed = -(s * 2400 + integral * 20);
                 
@@ -449,8 +448,8 @@
         
         if (state == 1 && reportPacer.pace())
         {
-            pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
-              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+            pc.printf("%5d %5d %11d %11d | %11f %11f\r\n",
+              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
               determinant(), dotProduct());
         }
     }