Diff: PositionSensor/PositionSensor.cpp

Revision:

52:d4d5e3414865

Parent:

48:1b51771c3647

--- a/PositionSensor/PositionSensor.cpp	Thu Aug 06 07:25:51 2020 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sat Aug 08 13:43:20 2020 +0000
@@ -23,6 +23,13 @@
     oldModPosition = 0;
     oldVel = 0;
     raw = 0;
+    first_sample = 0;
+    for(int i = 0; i<100; i++)              // Initial measurement is really noisy
+    {
+        spi->write(0);
+        wait_us(100);
+    }
+    
     }
     
 void PositionSensorAM5147::Sample(float dt){
@@ -37,12 +44,16 @@
     int off_2 = offset_lut[((raw>>7)+1)%128];
     int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7);        // Interpolate between lookup table entries
     int angle = raw + off_interp;                                               // Correct for nonlinearity with lookup table from calibration
-    if(angle - old_counts > _CPR/2){
-        rotations -= 1;
-        }
-    else if (angle - old_counts < -_CPR/2){
-        rotations += 1;
-        }
+    
+    if(first_sample){
+        if(angle - old_counts > _CPR/2){
+            rotations -= 1;
+            }
+        else if (angle - old_counts < -_CPR/2){
+            rotations += 1;
+            }
+    }
+    if(!first_sample){first_sample = 1;}
     
     old_counts = angle;
     oldModPosition = modPosition;
@@ -116,6 +127,7 @@
     }
 void PositionSensorAM5147::SetMechOffset(float offset){
     MechOffset = offset;
+    first_sample = 0;
     }
 
 int PositionSensorAM5147::GetCPR(){