Diff: PositionSensor/PositionSensor.cpp

Revision:

119:ad7a6af6fba3

Parent:

28:ed9c1ca386fd

Child:

120:57b6f3b1356b

--- a/PositionSensor/PositionSensor.cpp	Mon Apr 24 01:51:16 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp	Tue Apr 25 04:49:46 2017 +0000
@@ -12,7 +12,10 @@
 PositionSensorEncoder::PositionSensorEncoder(int cpr, float offset) {
     _cpr = cpr;
     _offset = offset;
+    _lobes = (int) (RESOLVER_LOBES);
+    
     _valid = false;
+    _rotations = 0;
     
     __GPIOA_CLK_ENABLE();
     __GPIOB_CLK_ENABLE();
@@ -78,11 +81,30 @@
     }
 }
 
+/*
+ * Return the electrical position in radians
+ */
+ 
+float PositionSensorEncoder::GetUnlimitedElecPosition() {
+    int raw = TIM2->CNT;
+    float ep = POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset);
+    return ep + _rotations * 2 * PI;
+}
+
 void PositionSensorEncoder::ZeroEncoderCount(void){
     if (ZSense->read() == 1){
         if (ZSense->read() == 1){
             TIM2->CNT = 0;
-            _valid = true;
+            int dir = TIM2->CR1 & (1 << 4);
+            if (!_valid) {
+                _valid = true;
+                return;
+            }
+            if (dir == 0) {//upcounting
+                _rotations++;
+            } else {//downcounting
+                _rotations--;
+            }
         }
     }
 }