Diff: meta/pidcontroller.cpp

Revision:

11:dccbaa9274c5

Child:

21:da41c08e51bc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/meta/pidcontroller.cpp	Sun Mar 08 08:37:38 2015 +0000
@@ -0,0 +1,25 @@
+#include "includes.h"
+#include "filters.h"
+
+PidController::PidController(float ki, float kp, float kd, float out_max, float out_min) {
+    _ki = ki;
+    _kp = kp;
+    _kd = kd;
+    _last_in = 0.0f;
+    _integral = 0.0f;
+    _out_max = out_max;
+    _out_min = out_min;
+}
+
+float PidController::Update(float ref, float in) {
+    float error = ref - in;
+    _integral += error;
+    if (_integral * _ki > _out_max) _integral = _out_max / _ki;
+    if (_integral * _ki < _out_min) _integral = _out_min / _ki;
+    float deriv = _last_in - in;
+    _last_in = in;
+    float tmp = _ki * _integral + _kp * error + _kd * deriv;
+    if (tmp > _out_max) tmp = _out_max;
+    if (tmp < _out_min) tmp = _out_min;
+    return tmp;
+}
\ No newline at end of file