Diff: PositionSensor/PositionSensor.cpp

Revision:

24:5e18a87a0e95

Parent:

15:b583cd30b063

Child:

26:955a1dfc2705

--- a/PositionSensor/PositionSensor.cpp	Sat Nov 05 11:10:37 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sat Nov 05 14:02:22 2016 +0000
@@ -57,11 +57,11 @@
     int raw = TIM2->CNT;
     if (raw < 0) raw += _cpr;
     if (raw >= _cpr) raw -= _cpr;
-    float signed_mech = fmod(6.28318530718f * (raw) / (float)_cpr + _offset, 6.28318530718f);
-    if (signed_mech < 0){
-        return signed_mech + 6.28318530718f;
+    float mp = fmod(1 / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset), 2 * PI);
+    if (mp < 0){
+        return mp + 2 * PI;
     } else {
-        return signed_mech;
+        return mp;
     }  
 }
 
@@ -73,18 +73,18 @@
     int raw = TIM2->CNT;
     if (raw < 0) raw += _cpr;
     if (raw >= _cpr) raw -= _cpr;
-    float signed_elec = fmod((POLE_PAIRS / RESOLVER_LOBES * (6.28318530718f * (raw) / (float)_cpr + _offset)), 6.28318530718f);
-    if (signed_elec < 0) {
-        return signed_elec + 6.28318530718f;
+    float ep = fmod((POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset)), 2 * PI);
+    if (ep < 0) {
+        return ep + 2 * PI;
     } else {
-        return signed_elec;
+        return ep;
     }
 }
 
 void PositionSensorEncoder::ZeroEncoderCount(void){
     if (ZSense->read() == 1){
         if (ZSense->read() == 1){
-            TIM2->CNT=0;
+            TIM2->CNT = 0;
         }
     }
 }
\ No newline at end of file