N K / Mbed 2 deprecated GaNtroller

a fork of priustroller

Dependencies:   mbed

Fork of priustroller_current by N K

meta/pidcontroller.cpp@24:f1ff9c7256b5, 2015-03-09 (annotated)

Committer:
bwang
Date:
Mon Mar 09 11:33:14 2015 +0000
Revision:
24:f1ff9c7256b5
Parent:
21:da41c08e51bc
Child:
25:0003b824dd7d
Added multi-channel debug logger in debug/, set up #channels and depth in main(), write in log() function. motor stops turning after trace is captured, requires reset to restart

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 11:dccbaa9274c5 1 #include "includes.h"
bwang 11:dccbaa9274c5 2 #include "filters.h"
bwang 11:dccbaa9274c5 3
bwang 11:dccbaa9274c5 4 PidController::PidController(float ki, float kp, float kd, float out_max, float out_min) {
bwang 11:dccbaa9274c5 5 _ki = ki;
bwang 11:dccbaa9274c5 6 _kp = kp;
bwang 11:dccbaa9274c5 7 _kd = kd;
bwang 11:dccbaa9274c5 8 _last_in = 0.0f;
bwang 11:dccbaa9274c5 9 _integral = 0.0f;
bwang 11:dccbaa9274c5 10 _out_max = out_max;
bwang 11:dccbaa9274c5 11 _out_min = out_min;
bwang 11:dccbaa9274c5 12 }
bwang 11:dccbaa9274c5 13
bwang 11:dccbaa9274c5 14 float PidController::Update(float ref, float in) {
bwang 11:dccbaa9274c5 15 float error = ref - in;
bwang 11:dccbaa9274c5 16 _integral += error;
bwang 11:dccbaa9274c5 17 if (_integral * _ki > _out_max) _integral = _out_max / _ki;
bwang 11:dccbaa9274c5 18 if (_integral * _ki < _out_min) _integral = _out_min / _ki;
bwang 11:dccbaa9274c5 19 float deriv = _last_in - in;
bwang 11:dccbaa9274c5 20 _last_in = in;
bwang 11:dccbaa9274c5 21 float tmp = _ki * _integral + _kp * error + _kd * deriv;
bwang 11:dccbaa9274c5 22 if (tmp > _out_max) tmp = _out_max;
bwang 11:dccbaa9274c5 23 if (tmp < _out_min) tmp = _out_min;
bwang 11:dccbaa9274c5 24 return tmp;
bwang 11:dccbaa9274c5 25 }