Diff: pidcontroller.cpp

Revision:

0:54cf32d35f4d

Child:

1:1f58bdcf2956

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pidcontroller.cpp	Sun Mar 01 06:16:01 2015 +0000
@@ -0,0 +1,25 @@
+#include "includes.h"
+#include "classes.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 > _out_max) _integral = _out_max;
+    if (_integral < _out_min) _integral = _out_min;
+    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