Diff: PositionSensor/PositionSensor.cpp

Revision:

48:a74e401a6d84

Parent:

47:f4ecf3e0576a

--- a/PositionSensor/PositionSensor.cpp	Wed May 13 09:53:27 2020 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Apr 07 10:12:43 2021 +0000
@@ -24,6 +24,10 @@
     oldVel = 0;
     raw = 0;
     flag_first_time = true;
+    for(int i=0;i<40;i++){
+        velVec[i] = 0;
+        // MechPositionVec[i] = 0;
+        }
     }
     
 void PositionSensorAM5147::Sample(float dt){
@@ -72,14 +76,20 @@
     
     int n = 40;
     float sum = vel;
+    //float sum2 = MechPosition;
     for (int i = 1; i < (n); i++){
         velVec[n - i] = velVec[n-i-1];
         sum += velVec[n-i];
+        //MechPositionVec[n-i] = MechPositionVec[n-i-1];
+        //sum2 += MechPositionVec[n-i];
         }
     velVec[0] = vel;
+    //MechPositionVec[0] = MechPosition;
+    //MechPosition = sum2 / ((float)n);
     MechVelocity =  sum/((float)n);
     ElecVelocity = MechVelocity*_ppairs;
-    ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+    // ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+    ElecVelocityFilt = 0.76094f*ElecVelocityFilt + 0.23906f*ElecVelocity;    // 2khz speed filter
     }
 
 int PositionSensorAM5147::GetRawPosition(){
@@ -99,7 +109,8 @@
     }
 
 float PositionSensorAM5147::GetElecVelocity(){
-    return ElecVelocity;
+    //return ElecVelocity;
+    return ElecVelocityFilt;
     }
 
 float PositionSensorAM5147::GetMechVelocity(){