Diff: PositionSensor/PositionSensor.cpp

Revision:

5:efd3838b79a6

Parent:

2:eabe8feaaabb

--- a/PositionSensor/PositionSensor.cpp	Fri Mar 18 12:07:14 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sun Mar 27 03:40:52 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();
@@ -73,7 +74,7 @@
     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);
+    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 {