Diff: PositionSensor/PositionSensor.cpp

Revision:

37:c0f352d6e8e3

Parent:

36:d88fd41f60a6

Child:

38:67e4e1453a4b

diff -r d88fd41f60a6 -r c0f352d6e8e3 PositionSensor/PositionSensor.cpp
--- a/PositionSensor/PositionSensor.cpp	Fri Mar 02 15:24:00 2018 +0000
+++ b/PositionSensor/PositionSensor.cpp	Fri Apr 13 13:50:54 2018 +0000
@@ -13,10 +13,11 @@
     spi = new SPI(PC_12, PC_11, PC_10);
     spi->format(16, 1);                                                          // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
     spi->frequency(25000000);
+    
     cs = new DigitalOut(PA_15);
     cs->write(1);
     readAngleCmd = 0xffff;   
-    MechOffset = 0;
+    MechOffset = offset;
     modPosition = 0;
     oldModPosition = 0;
     oldVel = 0;
@@ -24,10 +25,10 @@
     }
     
 void PositionSensorAM5147::Sample(){
-    cs->write(0);
+    GPIOA->ODR &= ~(1 << 15);
     raw = spi->write(readAngleCmd);
     raw &= 0x3FFF;                                                              //Extract last 14 bits
-    cs->write(1);
+    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
@@ -84,6 +85,10 @@
     return ElecPosition;
     }
 
+float PositionSensorAM5147::GetElecVelocity(){
+    return ElecVelocity;
+    }
+
 float PositionSensorAM5147::GetMechVelocity(){
     return MechVelocity;
     }
@@ -98,6 +103,9 @@
 void PositionSensorAM5147::SetElecOffset(float offset){
     ElecOffset = offset;
     }
+void PositionSensorAM5147::SetMechOffset(float offset){
+    MechOffset = offset;
+    }
 
 int PositionSensorAM5147::GetCPR(){
     return _CPR;
@@ -109,6 +117,7 @@
     }
     
 
+
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
     _ppairs = ppairs;
     _CPR = CPR;