Revision 56:fe5056ac6740, committed 2019-10-10

Comitter:

benkatz

Date:

Thu Oct 10 15:03:12 2019 +0000

Parent:

55:c4c9fec8539c

Commit message:

fixed position-sensor turn-on weirdness; ; improved output zeroing to work independent of encoder rollover angle

Changed in this revision

--- a/PositionSensor/PositionSensor.cpp	Fri Oct 04 14:18:39 2019 +0000
+++ b/PositionSensor/PositionSensor.cpp	Thu Oct 10 15:03:12 2019 +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){
@@ -36,12 +43,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;
@@ -115,6 +126,7 @@
     }
 void PositionSensorAM5147::SetMechOffset(float offset){
     MechOffset = offset;
+    first_sample = 0;
     }
 
 int PositionSensorAM5147::GetCPR(){

--- a/PositionSensor/PositionSensor.h	Fri Oct 04 14:18:39 2019 +0000
+++ b/PositionSensor/PositionSensor.h	Thu Oct 10 15:03:12 2019 +0000
@@ -35,7 +35,7 @@
     //DigitalOut *ZTest;
     virtual void ZeroEncoderCount(void);
     virtual void ZeroEncoderCountDown(void);
-    int _CPR, flag, rotations, _ppairs, raw;
+    int _CPR, flag, rotations, _ppairs, raw, first_sample;
     //int state;
     float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40];
     int offset_lut[128];
@@ -58,7 +58,7 @@
     virtual void WriteLUT(int new_lut[128]);
 private:
     float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt;
-    int raw, _CPR, rotations, old_counts, _ppairs;
+    int raw, _CPR, rotations, old_counts, _ppairs, first_sample;
     SPI *spi;
     DigitalOut *cs;
     int readAngleCmd;

--- a/main.cpp	Fri Oct 04 14:18:39 2019 +0000
+++ b/main.cpp	Thu Oct 10 15:03:12 2019 +0000
@@ -183,6 +183,10 @@
         //for (delay = 0; delay < 55; delay++);
         
         spi.Sample(DT);                                                           // sample position sensor
+        /*
+        if(count < 10){printf("%d\n\r", spi.GetRawPosition());}
+        count ++;
+        */        
         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
@@ -486,6 +490,10 @@
     if(isnan(I_MAX_CONT) || I_MAX_CONT==-1){I_MAX_CONT = 14.0f;}
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
     spi.SetMechOffset(M_OFFSET);
+    spi.Sample(1.0f);
+    if(spi.GetMechPosition() > PI){spi.SetMechOffset(M_OFFSET+2.0f*PI);}        // now zeroes to +- 30 degrees about nominal, independent of rollover point
+    else if (spi.GetMechPosition() < -PI){spi.SetMechOffset(M_OFFSET-2.0f*PI);}
+    
     int lut[128] = {0};
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table