Diff: PositionSensor/PositionSensor.cpp

Revision:

57:0795d2add37e

Parent:

56:fe5056ac6740

Child:

58:32e8927fe39f

--- a/PositionSensor/PositionSensor.cpp	Thu Oct 10 15:03:12 2019 +0000
+++ b/PositionSensor/PositionSensor.cpp	Thu Nov 07 18:14:51 2019 +0000
@@ -139,6 +139,153 @@
     }
     
 
+PositionSensoriCMU::PositionSensoriCMU(int CPR, float offset, int ppairs){
+    //_CPR = CPR;
+    _CPR = CPR;
+    _ppairs = ppairs;
+    ElecOffset = offset;
+    rotations = 0;
+    spi = new SPI(PC_12, PC_11, PC_10);
+    spi->format(8, 0);                                                          // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
+    spi->frequency(10000000);
+    
+    cs = new DigitalOut(PA_15);
+    cs->write(1);
+    readAngleCmd = 0xffff;   
+    MechOffset = offset;
+    modPosition = 0;
+    oldModPosition = 0;
+    oldVel = 0;
+    raw = 0;
+    first_sample = 0;
+    for(int i = 0; i<100; i++)              // Initial measurement is really noisy
+    {
+        cs->write(0);
+        spi->write(0xA6);
+        spi->write(0);
+        spi->write(0);
+        spi->write(0);
+        cs->write(1);
+        wait_us(100);
+    }
+
+    }
+    
+void PositionSensoriCMU::Sample(float dt){
+    //GPIOA->ODR &= ~(1 << 15);
+    //raw = spi->write(readAngleCmd);
+    //raw &= 0x3FFF;   
+    cs->write(0);
+    raw_bytes[0] = spi->write(0xA6);       // Read Register
+    raw_bytes[1] = spi->write(0);
+    raw_bytes[2] = spi->write(0);
+    raw_bytes[3] = spi->write(0);
+    cs->write(1);
+    
+    //for(int i=0; i<8; i++){printf("%d ", raw_bytes[i]);}
+    //printf("\n\r");
+    
+    raw = raw_bytes[1]<<16 | raw_bytes[2]<<8 | raw_bytes[3];
+    //raw = raw>>2;                                                             //Extract last 14 bits
+    //GPIOA->ODR |= (1 << 15);
+    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(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;
+    modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
+    position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
+    MechPosition = position - MechOffset;
+    float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
+    if(elec < 0) elec += 2.0f*PI;
+    else if(elec > 2.0f*PI) elec -= 2.0f*PI ; 
+    ElecPosition = elec;
+    
+    float vel;
+    //if(modPosition<.1f && oldModPosition>6.1f){
+
+    if((modPosition-oldModPosition) < -3.0f){
+        vel = (modPosition - oldModPosition + 2.0f*PI)/dt;
+        }
+    //else if(modPosition>6.1f && oldModPosition<0.1f){
+    else if((modPosition - oldModPosition) > 3.0f){
+        vel = (modPosition - oldModPosition - 2.0f*PI)/dt;
+        }
+    else{
+        vel = (modPosition-oldModPosition)/dt;
+    }    
+    
+    int n = 40;
+    float sum = vel;
+    for (int i = 1; i < (n); i++){
+        velVec[n - i] = velVec[n-i-1];
+        sum += velVec[n-i];
+        }
+    velVec[0] = vel;
+    MechVelocity =  sum/((float)n);
+    ElecVelocity = MechVelocity*_ppairs;
+    ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+    }
+
+int PositionSensoriCMU::GetRawPosition(){
+    return raw;
+    }
+
+float PositionSensoriCMU::GetMechPositionFixed(){
+    return MechPosition+MechOffset;
+    }
+    
+float PositionSensoriCMU::GetMechPosition(){
+    return MechPosition;
+    }
+
+float PositionSensoriCMU::GetElecPosition(){
+    return ElecPosition;
+    }
+
+float PositionSensoriCMU::GetElecVelocity(){
+    return ElecVelocity;
+    }
+
+float PositionSensoriCMU::GetMechVelocity(){
+    return MechVelocity;
+    }
+
+void PositionSensoriCMU::ZeroPosition(){
+    rotations = 0;
+    MechOffset = 0;
+    Sample(.00025f);
+    MechOffset = GetMechPosition();
+    }
+    
+void PositionSensoriCMU::SetElecOffset(float offset){
+    ElecOffset = offset;
+    }
+void PositionSensoriCMU::SetMechOffset(float offset){
+    MechOffset = offset;
+    first_sample = 0;
+    }
+
+int PositionSensoriCMU::GetCPR(){
+    return _CPR;
+    }
+
+
+void PositionSensoriCMU::WriteLUT(int new_lut[128]){
+    memcpy(offset_lut, new_lut, sizeof(offset_lut));
+    }
 
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
     _ppairs = ppairs;