Diff: PositionSensor/PositionSensor.cpp

Revision:

9:074575151e4b

Parent:

2:eabe8feaaabb

Child:

15:b583cd30b063

--- a/PositionSensor/PositionSensor.cpp	Fri Mar 18 12:07:14 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Tue May 17 06:44:09 2016 +0000
@@ -1,6 +1,7 @@
 
 #include "mbed.h"
 #include "PositionSensor.h"
+#include "config.h"
 #include <math.h>
 
 /*
@@ -8,8 +9,8 @@
  * offset: mechanical position offset in radians
  */
     
-PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset) {
-    _CPR = CPR;
+PositionSensorEncoder::PositionSensorEncoder(int cpr, float offset) {
+    _cpr = cpr;
     _offset = offset;
     
     __GPIOA_CLK_ENABLE();
@@ -54,9 +55,9 @@
  
 float PositionSensorEncoder::GetMechPosition() {
     int raw = TIM2->CNT;
-    if (raw < 0) raw += _CPR;
-    if (raw >= _CPR) raw -= _CPR;
-    float signed_elec = fmod((1.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f);
+    if (raw < 0) raw += _cpr;
+    if (raw >= _cpr) raw -= _cpr;
+    float signed_elec = fmod(6.28318530718f * (raw) / (float)_cpr + _offset, 6.28318530718f);
     if (signed_elec < 0){
         return signed_elec + 6.28318530718f;
     }
@@ -71,9 +72,9 @@
  
 float PositionSensorEncoder::GetElecPosition() {
     int raw = TIM2->CNT;
-    if (raw < 0) raw += _CPR;
-    if (raw >= _CPR) raw -= _CPR;
-    float signed_elec = fmod((2.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f);
+    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;
     } else {