Diff: pid.cpp

Revision:

0:009e84d7af32

Child:

3:f1067b5bb5af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.cpp	Fri Oct 19 10:57:39 2018 +0000
@@ -0,0 +1,49 @@
+#include "pid.h"
+
+PID::PID() {
+    Kp = 0;
+    Ki = 0;
+    Kd = 0;
+    error_integral = 0;
+    error_previous = 0;
+    pid_period = 0;
+    first_update = true;
+    
+    // Setup the low pass filter.
+    // The values are from the slides (PES, lecture 5)
+    low_pass_filter = BiQuad(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+}
+
+void PID::set_period(double period) {
+    pid_period = period;
+}
+
+void PID::set_k_values(double Kp, double Ki, double Kd) {
+    this->Kp = Kp;
+    this->Ki = Ki;
+    this->Kd = Kd;
+}
+
+double PID::update(double error) {
+    if (first_update) {
+        // Make sure the previous error has a sensible value (not 0).
+        error_previous = error;
+        first_update = false;
+    }
+    
+    // Proportional part
+    double u_p = Kp * error;
+    
+    // Integral part
+    error_integral += error * pid_period;
+    double u_i = Ki * error_integral;
+    
+    // Derivative part
+    double error_der = (error-error_previous)/pid_period;
+    double filtered_error_der = low_pass_filter.step(error_der);
+    double u_d = Kd * filtered_error_der;
+    error_previous = error;
+    
+    // Add all components and return.
+    return u_p + u_d + u_i;
+}
\ No newline at end of file