Diff: PositionSensor/PositionSensor.cpp

Revision:

53:89565c1d9115

Parent:

52:8fff6f1a3f50

Child:

54:4ce8f97be6ae

--- a/PositionSensor/PositionSensor.cpp	Mon Dec 20 01:24:06 2021 +0000
+++ b/PositionSensor/PositionSensor.cpp	Mon Dec 27 02:47:12 2021 +0000
@@ -2,6 +2,8 @@
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "math_ops.h"
+
+#include "user_config.h"
 //#include "offset_lut.h"
 //#include <math.h>
 
@@ -43,17 +45,20 @@
     else if (angle - old_counts < -_CPR/2){
         rotations += 1;
         }
-    
+        //////
+    aa=rotations;
+    /////
     old_counts = angle;
     oldModPosition = modPosition;
     modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
     position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
-    MechPosition = position - MechOffset;
+    MechPosition = position - MechOffset; 
     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){
 
@@ -106,7 +111,9 @@
 
 void PositionSensorAM5147::ZeroPosition(){
     rotations = 0;
+    //rotations= quanshu;
     MechOffset = 0;
+    //MechOffset = GetMechPosition();
     Sample(.00025f);
     MechOffset = GetMechPosition();
     }