Diff: PID.cpp

Revision:

0:c15430f1895f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,38 @@
+// Nicked this code from https://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/
+
+#include "PID.h"
+
+void pid_zeroize(PID* pid) {
+    // set prev and integrated error to zero
+    pid->prev_error = 0;
+    pid->int_error = 0;
+}
+ 
+void pid_update(PID* pid, double curr_error, double dt) 
+{
+    double diff;
+    double p_term;
+    double i_term;
+    double d_term;
+ 
+    // integration with windup guarding
+    pid->int_error += (curr_error * dt);
+    if (pid->int_error < -(pid->windup_guard))
+        pid->int_error = -(pid->windup_guard);
+    else if (pid->int_error > pid->windup_guard)
+        pid->int_error = pid->windup_guard;
+ 
+    // differentiation
+    diff = ((curr_error - pid->prev_error) / dt);
+ 
+    // scaling
+    p_term = (pid->proportional_gain * curr_error);
+    i_term = (pid->integral_gain     * pid->int_error);
+    d_term = (pid->derivative_gain   * diff);
+ 
+    // summation of terms
+    pid->control = p_term + i_term + d_term;
+ 
+    // save current error as previous error for next iteration
+    pid->prev_error = curr_error;
+}
\ No newline at end of file