Diff: PositionSensor/PositionSensor.cpp

Revision:

45:aadebe074af6

Parent:

38:67e4e1453a4b

Child:

47:55bdc4d5096b

--- a/PositionSensor/PositionSensor.cpp	Sat Jul 14 22:03:52 2018 +0000
+++ b/PositionSensor/PositionSensor.cpp	Mon Jul 30 20:25:24 2018 +0000
@@ -1,6 +1,7 @@
 
 #include "mbed.h"
 #include "PositionSensor.h"
+#include "math_ops.h"
 //#include "offset_lut.h"
 //#include <math.h>
 
@@ -24,7 +25,7 @@
     raw = 0;
     }
     
-void PositionSensorAM5147::Sample(){
+void PositionSensorAM5147::Sample(float dt){
     GPIOA->ODR &= ~(1 << 15);
     raw = spi->write(readAngleCmd);
     raw &= 0x3FFF;                                                              //Extract last 14 bits
@@ -42,23 +43,26 @@
     
     old_counts = angle;
     oldModPosition = modPosition;
-    modPosition = ((6.28318530718f * ((float) angle))/ (float)_CPR);
-    position = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
+    modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
+    position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
     MechPosition = position - MechOffset;
-    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
-    if(elec < 0) elec += 6.28318530718f;
-    else if(elec > 6.28318530718f) elec -= 6.28318530718f ; 
+    float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
+    if(elec < 0) elec += 2.0f*PI;
+    else if(elec > 2.0f*PI) elec -= 2.0f*PI ; 
     ElecPosition = elec;
     
     float vel;
-    if(modPosition<.1f && oldModPosition>6.1f){
-        vel = (modPosition - oldModPosition + 6.28318530718f)*40000.0f;
+    //if(modPosition<.1f && oldModPosition>6.1f){
+
+    if((modPosition-oldModPosition) < -3.0f){
+        vel = (modPosition - oldModPosition + 2.0f*PI)/dt;
         }
-    else if(modPosition>6.1f && oldModPosition<0.1f){
-        vel = (modPosition - oldModPosition - 6.28318530718f)*40000.0f;
+    //else if(modPosition>6.1f && oldModPosition<0.1f){
+    else if((modPosition - oldModPosition) > 3.0f){
+        vel = (modPosition - oldModPosition - 2.0f*PI)/dt;
         }
     else{
-        vel = (modPosition-oldModPosition)*40000.0f;
+        vel = (modPosition-oldModPosition)/dt;
     }    
     
     int n = 40;
@@ -68,7 +72,7 @@
         sum += velVec[n-i];
         }
     velVec[0] = vel;
-    MechVelocity =  sum/(float)n;
+    MechVelocity =  sum/((float)n);
     ElecVelocity = MechVelocity*_ppairs;
     ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
     }
@@ -100,7 +104,7 @@
 void PositionSensorAM5147::ZeroPosition(){
     rotations = 0;
     MechOffset = 0;
-    Sample();
+    Sample(.00025f);
     MechOffset = GetMechPosition();
     }
     
@@ -189,7 +193,7 @@
     //ZTest->write(1);
     }
     
-void PositionSensorEncoder::Sample(){
+void PositionSensorEncoder::Sample(float dt){
     
     }