Diff: test.cpp

Revision:

43:0e985a58f174

Parent:

42:96671b71aac5

Child:

44:b4a00fbab06b

diff -r 96671b71aac5 -r 0e985a58f174 test.cpp
--- a/test.cpp	Sat Jul 27 20:58:46 2019 +0000
+++ b/test.cpp	Sat Jul 27 22:52:19 2019 +0000
@@ -25,7 +25,7 @@
     {
         led3 = logger.isFull();
     
-        updateReckonerFromEncoders();
+        updateReckoner();
         loggerService();
     }
     led2 = 1;
@@ -34,29 +34,31 @@
 
 void testCloseness()
 {
+    doGyroCalibration();
     led1 = 1;
     while(1)
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
         float magn = magnitude();
         
-        led3 = (magn < (1<<(14+7)));
-        led4 = (magn < (1<<(14+9)));
+        led3 = magn < (1 << 5);
+        led4 = magn < (1 << 7);
     }
 }
 
 void showOrientationWithLeds34()
 {
-    led3 = reckoner.cos > 0;
-    led4 = reckoner.sin > 0;
+    led3 = reckoner.cosv > 0;
+    led4 = reckoner.sinv > 0;
 }
 
 void testTurnInPlace()
 {
+    doGyroCalibration();
     led1 = 1;
     while(!button1DefinitelyPressed())
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
         showOrientationWithLeds34();
     }
     led2 = 1;
@@ -67,7 +69,7 @@
     motorsSpeedSet(-300, 300);
     while(timer.read_ms() < 4000)
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
         showOrientationWithLeds34();
     }
     timer.reset();
@@ -78,7 +80,7 @@
         if (motorUpdatePacer.pace())
         {
             int16_t rotationSpeed;
-            float s = (float)reckoner.sin / (1 << 30);
+            float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT);
             integral += s;
             rotationSpeed = -(s * 2400 + integral * 20);
             
@@ -100,109 +102,13 @@
     infiniteReckonerReportLoop();      
 }
 
-
-void testSensorGlitches()
-{
-    AnalogIn testInput(p18);
-    Pacer reportPacer(1000000);
-    uint32_t badCount = 0, goodCount = 0;
-    pc.printf("hi\r\n");
-    
-    //uint16_t riseCount = 0;
-    uint16_t reading = 0xFF;
-    
-    while(1)
-    {
-        /** This digital filtering did not work
-        {
-            wait(0.01);
-            uint16_t raw = testInput.read_u16();
-            if (raw < reading)
-            {
-                riseCount = 0;
-                reading = raw;
-            }
-            else
-            {
-                riseCount++;
-                if (riseCount == 10)
-                {
-                    riseCount = 0;
-                    reading = raw;   
-                }
-            }
-        }
-        **/
-
-        uint16_t values[LINE_SENSOR_COUNT];        
-        readSensors(values);
-        reading = values[0];
-        
-        if(reading > 100)
-        {
-            badCount += 1;
-            //pc.printf("f %5d %11d %11d\r\n", reading, badCount, goodCount);
-        }
-        else
-        {
-            goodCount += 1;
-        }
-        
-        if (reportPacer.pace())
-        {
-            pc.printf("h %5d %11d %11d\r\n", reading, badCount, goodCount);
-        }
-    }
-}
-
-void testAnalog()
-{
-    AnalogIn testInput(p18);
-    
-    DigitalOut pin20(p20);
-    DigitalOut pin19(p19);
-    //DigitalOut pin18(p18);
-    DigitalOut pin17(p17);
-    DigitalOut pin16(p16);
-    DigitalOut pin15(p15);
-    
-    pin20 = 0;
-    pin19 = 0;
-    //pin18 = 0;
-    pin17 = 0;
-    pin16 = 0;
-    pin15 = 0;
-    
-    uint32_t badCount = 0, goodCount = 0;
-    
-    Pacer reportPacer(1000000);
-    while(1)
-    {
-        uint16_t reading = testInput.read_u16();
-        if(reading > 100)
-        {
-            badCount += 1;
-            pc.printf("%5d %11d %11d\r\n", reading, badCount, goodCount);   
-        }
-        else
-        {
-            goodCount += 1;   
-        }
-        
-        if (reportPacer.pace())
-        {
-            pc.printf("Hello\r\n");   
-        }
-    }
-}
-
 // This also tests the LineTracker by printing out a lot of data from it.
 void testLineFollowing()
 {
     led1 = 1;   
     while(!button1DefinitelyPressed())
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
     }
     led2 = 1;
     
@@ -212,7 +118,7 @@
     uint16_t loopCount = 0;
     while(1)
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
         bool lineVisiblePrevious = lineTracker.getLineVisible();
         lineTracker.read();
         updateMotorsToFollowLine();
@@ -241,10 +147,11 @@
 
 void testDriveHome()
 {
+    doGyroCalibration();
     led1 = 1;   
     while(!button1DefinitelyPressed())
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
     }
     
     setLeds(1, 0, 1, 0);
@@ -259,10 +166,11 @@
 
 void testFinalSettleIn()
 {
+    doGyroCalibration();
     led1 = 1;
     while(!button1DefinitelyPressed())
     {
-        updateReckonerFromEncoders();
+        updateReckoner();
     }   
     finalSettleIn();
     infiniteReckonerReportLoop();     
@@ -292,19 +200,21 @@
 {
     doGyroCalibration();
     led1 = 1;
+    turnSensor.start();
     Pacer reportPacer(100000);
     while(1)
     {
-        updateReckonerFromEncoders();
+        turnSensor.update();
+        updateReckoner();
         
-        led1 = (reckoner.x > 0);
-        led2 = (reckoner.y > 0);
+        led1 = reckoner.x > 0;
+        led2 = reckoner.y > 0;
         showOrientationWithLeds34();
         
         if (reportPacer.pace())
         {
-            pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
-              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+            pc.printf("%6d %6d %11d %11d | %8d %8d %10f\r\n",
+              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
               encoderLeft.getCount(), encoderRight.getCount(), determinant());
         }
     }
@@ -313,12 +223,11 @@
 void testTurnSensor()
 {
     pc.printf("Test turn sensor\r\n");
+    doGyroCalibration();
     led1 = 1;
-    doGyroCalibration();
     //Pacer reportPacer(200000);  // 0.2 s
     Pacer reportPacer(10000000);  // 10 s
     Timer timer;
-    led2 = 1;
     timer.start();
     turnSensor.start();
     while(1)
@@ -430,10 +339,6 @@
             }
         }
         
-        //values[0] = lineSensorsAnalog[0].read_u16();
-        //values[1] = lineSensorsAnalog[1].read_u16();
-        //values[2] = lineSensorsAnalog[2].read_u16();
-
         uint16_t values[3];
         readSensors(values);
         
@@ -589,8 +494,8 @@
         showOrientationWithLeds34();
         if(reportPacer.pace())
         {
-            pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
-              reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
+            pc.printf("%6d %6d %11d %11d | %11f %11f\r\n",
+              reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
               determinant(), dotProduct());
        }
     }