Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

Files at this revision

API Documentation at this revision

Revision 20:559db13dc85a, committed 2017-02-10

Comitter:
benson516
Date:
Fri Feb 10 18:25:10 2017 +0000
Parent:
19:24605ba48462
Commit message:
Add a LPF to wheel-speed

Changed in this revision

CURRENT_CONTROL.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 24605ba48462 -r 559db13dc85a CURRENT_CONTROL.cpp
--- a/CURRENT_CONTROL.cpp	Tue Feb 07 09:52:56 2017 +0000
+++ b/CURRENT_CONTROL.cpp	Fri Feb 10 18:25:10 2017 +0000
@@ -53,7 +53,7 @@
     wheelSpeed(QEI_A, QEI_B, NC, pulsesPerRev, arraysize, samplingTime, QEI::X4_ENCODING), //(pin1, pin2, pin3, pulsesPerRev, arraysize, sampletime, pulses)
     pid(0.0,0.0,0.0,samplingTime),
     lpf_current(samplingTime, 400.0), // 1.5915 Hz = 10 rad/s
-    lpf_wheelSpeed(samplingTime, 200.0, 2) // 2nd-order LPF, fc = 200.0 Hz
+    lpf_wheelSpeed(samplingTime, 300.0, 2) // 2nd-order LPF, fc = 200.0 Hz
 
 
 {
@@ -202,12 +202,21 @@
     // Speed
     wheelSpeed.Calculate();
 
+    /*
     //
     if (reverse_rotationalSpeed)
         angularSpeed = -wheelSpeed.getAngularSpeed();
     else
         angularSpeed = wheelSpeed.getAngularSpeed();
     //
+    */
+
+    //
+    if (reverse_rotationalSpeed)
+        angularSpeed = lpf_wheelSpeed.filter( -wheelSpeed.getAngularSpeed() );
+    else
+        angularSpeed = lpf_wheelSpeed.filter( wheelSpeed.getAngularSpeed() );
+    //
 
     // Flag
     Flag_SpeedCal_Iterated = true;