N K / Mbed 2 deprecated WangBoard_MotorScience

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM mbed

Fork of foc-ed_in_the_bot_compact by N K

Diff: PositionSensor/PositionSensor.cpp

Revision:
9:074575151e4b
Parent:
2:eabe8feaaabb
--- 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 {