Biorobotics 7 / BioroboticsMotorControl

PID motor controll for the biorobotics project.

Dependencies:   FastPWM QEI

Dependents:   PID_example Motor_calibration Demo_mode Demo_mode ... more

Diff: pid.cpp

Revision:
4:5353c5d0d2ed
Parent:
3:f1067b5bb5af
Child:
14:e5fc69651b1d
diff -r f1067b5bb5af -r 5353c5d0d2ed pid.cpp
--- a/pid.cpp	Mon Oct 29 15:27:56 2018 +0000
+++ b/pid.cpp	Wed Oct 31 06:13:15 2018 +0000
@@ -29,6 +29,16 @@
     this->Kd = Kd;
 }
 
+void PID::clear_state() {
+    first_update = true;
+    error_previous = 0;
+    error_integral = 0;
+    
+    // Cleart the low pass filter.
+    wz[0] = 0;
+    wz[1] = 0;
+}
+
 double PID::update(double error) {
     if (first_update) {
         // Make sure the previous error has a sensible value (not 0).