Bryce Williams / PID Featured

Basic but robust PID library

Dependents:   ESP8266_pid_mtrPos_webserver_SDcard_v2 ESP8266_pid_mtrSpeed_Webserver_SDcard ESP8266_pid_spd_and_pos_webserver_SDcard ESP8266_pid_redbot_webserver ... more

PID.cpp@0:9a6f7aafe531, 2015-11-23 (annotated)

Committer:
electromotivated
Date:
Mon Nov 23 02:42:53 2015 +0000
Revision:
0:9a6f7aafe531
Child:
1:c307cd559154
Fairly Robust PID controller library;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
electromotivated 0:9a6f7aafe531 1 #include "PID.h"
electromotivated 0:9a6f7aafe531 2 /*
electromotivated 0:9a6f7aafe531 3 Bryce Williams 11/19/2015
electromotivated 0:9a6f7aafe531 4 See PID.h for references as well as method descriptions
electromotivated 0:9a6f7aafe531 5 */
electromotivated 0:9a6f7aafe531 6
electromotivated 0:9a6f7aafe531 7 PID::PID(float* setpoint, float* feedback, float* output,
electromotivated 0:9a6f7aafe531 8 float output_lower, float output_upper,
electromotivated 0:9a6f7aafe531 9 float kp, float ki, float kd, float Ts){
electromotivated 0:9a6f7aafe531 10 _Ts = Ts; // Init params
electromotivated 0:9a6f7aafe531 11 _kp = kp;
electromotivated 0:9a6f7aafe531 12 _ki = ki*Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 13 _kd = kd / Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 14
electromotivated 0:9a6f7aafe531 15 _setpoint = setpoint;
electromotivated 0:9a6f7aafe531 16 _feedback = feedback;
electromotivated 0:9a6f7aafe531 17 _output = output;
electromotivated 0:9a6f7aafe531 18
electromotivated 0:9a6f7aafe531 19 _output_lower = output_lower;
electromotivated 0:9a6f7aafe531 20 _output_upper = output_upper;
electromotivated 0:9a6f7aafe531 21 }
electromotivated 0:9a6f7aafe531 22
electromotivated 0:9a6f7aafe531 23 void PID::start(){
electromotivated 0:9a6f7aafe531 24 // Start up such we avoid bumps... (see "Initialization" section in
electromotivated 0:9a6f7aafe531 25 // the reference link found in the header file).
electromotivated 0:9a6f7aafe531 26 last_feedback = *_feedback; // Eliminate derivative kick at start/restart
electromotivated 0:9a6f7aafe531 27 i_accumulator = clip(*_output, _output_lower,
electromotivated 0:9a6f7aafe531 28 _output_upper); // P and D terms are zero, thus
electromotivated 0:9a6f7aafe531 29 // i term is used to keep output unchanged
electromotivated 0:9a6f7aafe531 30
electromotivated 0:9a6f7aafe531 31 sample_timer.attach(this, &PID::sample, _Ts);
electromotivated 0:9a6f7aafe531 32 }
electromotivated 0:9a6f7aafe531 33
electromotivated 0:9a6f7aafe531 34 void PID::stop(){
electromotivated 0:9a6f7aafe531 35 sample_timer.detach();
electromotivated 0:9a6f7aafe531 36 }
electromotivated 0:9a6f7aafe531 37
electromotivated 0:9a6f7aafe531 38 float PID::getError(){
electromotivated 0:9a6f7aafe531 39 return error;
electromotivated 0:9a6f7aafe531 40 }
electromotivated 0:9a6f7aafe531 41
electromotivated 0:9a6f7aafe531 42 void PID::set_parameters(float kp, float ki, float kd, float Ts){
electromotivated 0:9a6f7aafe531 43 stop(); // Disable Sample Interrupt... stop()
electromotivated 0:9a6f7aafe531 44 _Ts = Ts; // Set New Sample Time
electromotivated 0:9a6f7aafe531 45 _kp = kp; // Seet New Kp
electromotivated 0:9a6f7aafe531 46 _ki = ki*Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 47 _kd = kd / Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 48 start(); // Enable Sample Interrupt... start()
electromotivated 0:9a6f7aafe531 49 }
electromotivated 0:9a6f7aafe531 50
electromotivated 0:9a6f7aafe531 51 void PID::sample(){
electromotivated 0:9a6f7aafe531 52 error = *_setpoint - *_feedback;
electromotivated 0:9a6f7aafe531 53
electromotivated 0:9a6f7aafe531 54 // Accumulate Integral Term such ki is applied to current error
electromotivated 0:9a6f7aafe531 55 // before adding to pool; avoids bumps if ki gain value is changed.
electromotivated 0:9a6f7aafe531 56 i_accumulator += _ki * error;
electromotivated 0:9a6f7aafe531 57 // Avoid "Windup" by clamping intergral term to output limits;
electromotivated 0:9a6f7aafe531 58 // essentially we stop integrating when we reach an upper or
electromotivated 0:9a6f7aafe531 59 // lower bound.
electromotivated 0:9a6f7aafe531 60 i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
electromotivated 0:9a6f7aafe531 61
electromotivated 0:9a6f7aafe531 62 // Run it!
electromotivated 0:9a6f7aafe531 63 *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
electromotivated 0:9a6f7aafe531 64 last_feedback = *_feedback;
electromotivated 0:9a6f7aafe531 65 // Clamp Output
electromotivated 0:9a6f7aafe531 66 *_output = clip(*_output, _output_lower, _output_upper);
electromotivated 0:9a6f7aafe531 67 }
electromotivated 0:9a6f7aafe531 68
electromotivated 0:9a6f7aafe531 69 float PID::clip(float value, float lower, float upper){
electromotivated 0:9a6f7aafe531 70 return std::max(lower, std::min(value, upper));
electromotivated 0:9a6f7aafe531 71 }