N K / Mbed 2 deprecated analoghalls5_5

working-est copy with class-based code. still open loop

Dependencies:   mbed

Fork of analoghalls6 by N K

pidcontroller.cpp@10:b4abecccec7a, 2015-03-08 (annotated)

Committer:
nki
Date:
Sun Mar 08 00:45:28 2015 +0000
Revision:
10:b4abecccec7a
Parent:
9:d3b70c15baa9 
uguu;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 0:54cf32d35f4d 1 #include "includes.h"
bwang 1:1f58bdcf2956 2 #include "meta.h"
bwang 0:54cf32d35f4d 3
bwang 0:54cf32d35f4d 4 PidController::PidController(float ki, float kp, float kd, float out_max, float out_min) {
bwang 0:54cf32d35f4d 5 _ki = ki;
bwang 0:54cf32d35f4d 6 _kp = kp;
bwang 0:54cf32d35f4d 7 _kd = kd;
bwang 0:54cf32d35f4d 8 _last_in = 0.0f;
bwang 0:54cf32d35f4d 9 _integral = 0.0f;
bwang 0:54cf32d35f4d 10 _out_max = out_max;
bwang 0:54cf32d35f4d 11 _out_min = out_min;
bwang 0:54cf32d35f4d 12 }
bwang 0:54cf32d35f4d 13
nki 10:b4abecccec7a 14 float PidController::Update(float ref, float in) { //starts positive, integrator is adding up the error to get there. cranking throttle makes it go negative. that makes sense.
nki 10:b4abecccec7a 15 float error = ref - in;
nki 10:b4abecccec7a 16 //_integral += error;
nki 10:b4abecccec7a 17 _integral = 0.0f;
nki 9:d3b70c15baa9 18 if (_integral*_ki > _out_max) _integral = _out_max/_ki;
nki 9:d3b70c15baa9 19 if (_integral*_ki < _out_min) _integral = _out_min/_ki;
bwang 0:54cf32d35f4d 20 float deriv = _last_in - in;
bwang 0:54cf32d35f4d 21 _last_in = in;
bwang 0:54cf32d35f4d 22 float tmp = _ki * _integral + _kp * error + _kd * deriv;
bwang 0:54cf32d35f4d 23 if (tmp > _out_max) tmp = _out_max;
bwang 0:54cf32d35f4d 24 if (tmp < _out_min) tmp = _out_min;
bwang 0:54cf32d35f4d 25 return tmp;
bwang 0:54cf32d35f4d 26 }