Diff: PositionSensor/PositionSensor.cpp

Revision:

25:f5741040c4bb

Parent:

23:2adf23ee0305

Child:

26:2b865c00d7e9

--- a/PositionSensor/PositionSensor.cpp	Fri Apr 07 16:23:39 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sun Apr 09 03:05:52 2017 +0000
@@ -26,9 +26,12 @@
 void PositionSensorAM5147::Sample(){
     cs->write(0);
     raw = spi->write(readAngleCmd);
-    raw &= 0x3FFF;    //Extract last 14 bits
+    raw &= 0x3FFF;                                                              //Extract last 14 bits
     cs->write(1);
-    int angle = raw + offset_lut[raw>>7];
+    int off_1 = offset_lut[raw>>7];
+    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;
         }