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@1:c307cd559154, 2015-11-24 (annotated)

Committer:
electromotivated
Date:
Tue Nov 24 00:15:01 2015 +0000
Revision:
1:c307cd559154
Parent:
0:9a6f7aafe531
Child:
3:6c2c985408df
Added methods to return currently set params;

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 sample_timer.attach(this, &PID::sample, _Ts);
electromotivated 0:9a6f7aafe531 31 }
electromotivated 0:9a6f7aafe531 32
electromotivated 0:9a6f7aafe531 33 void PID::stop(){
electromotivated 0:9a6f7aafe531 34 sample_timer.detach();
electromotivated 0:9a6f7aafe531 35 }
electromotivated 0:9a6f7aafe531 36
electromotivated 0:9a6f7aafe531 37 float PID::getError(){
electromotivated 0:9a6f7aafe531 38 return error;
electromotivated 0:9a6f7aafe531 39 }
electromotivated 0:9a6f7aafe531 40
electromotivated 0:9a6f7aafe531 41 void PID::set_parameters(float kp, float ki, float kd, float Ts){
electromotivated 0:9a6f7aafe531 42 stop(); // Disable Sample Interrupt... stop()
electromotivated 0:9a6f7aafe531 43 _Ts = Ts; // Set New Sample Time
electromotivated 0:9a6f7aafe531 44 _kp = kp; // Seet New Kp
electromotivated 0:9a6f7aafe531 45 _ki = ki*Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 46 _kd = kd / Ts; // Roll sample time into gain
electromotivated 0:9a6f7aafe531 47 start(); // Enable Sample Interrupt... start()
electromotivated 0:9a6f7aafe531 48 }
electromotivated 0:9a6f7aafe531 49
electromotivated 1:c307cd559154 50 float PID::getKp(){
electromotivated 1:c307cd559154 51 return _kp;
electromotivated 1:c307cd559154 52 }
electromotivated 1:c307cd559154 53
electromotivated 1:c307cd559154 54 float PID::getKi(){
electromotivated 1:c307cd559154 55 return _ki;
electromotivated 1:c307cd559154 56 }
electromotivated 1:c307cd559154 57
electromotivated 1:c307cd559154 58 float PID::getKd(){
electromotivated 1:c307cd559154 59 return _kd;
electromotivated 1:c307cd559154 60 }
electromotivated 1:c307cd559154 61
electromotivated 1:c307cd559154 62 float PID::getTs(){
electromotivated 1:c307cd559154 63 return _Ts;
electromotivated 1:c307cd559154 64 }
electromotivated 1:c307cd559154 65
electromotivated 0:9a6f7aafe531 66 void PID::sample(){
electromotivated 0:9a6f7aafe531 67 error = *_setpoint - *_feedback;
electromotivated 0:9a6f7aafe531 68
electromotivated 0:9a6f7aafe531 69 // Accumulate Integral Term such ki is applied to current error
electromotivated 0:9a6f7aafe531 70 // before adding to pool; avoids bumps if ki gain value is changed.
electromotivated 0:9a6f7aafe531 71 i_accumulator += _ki * error;
electromotivated 0:9a6f7aafe531 72 // Avoid "Windup" by clamping intergral term to output limits;
electromotivated 0:9a6f7aafe531 73 // essentially we stop integrating when we reach an upper or
electromotivated 0:9a6f7aafe531 74 // lower bound.
electromotivated 0:9a6f7aafe531 75 i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
electromotivated 0:9a6f7aafe531 76
electromotivated 0:9a6f7aafe531 77 // Run it!
electromotivated 0:9a6f7aafe531 78 *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
electromotivated 0:9a6f7aafe531 79 last_feedback = *_feedback;
electromotivated 0:9a6f7aafe531 80 // Clamp Output
electromotivated 0:9a6f7aafe531 81 *_output = clip(*_output, _output_lower, _output_upper);
electromotivated 0:9a6f7aafe531 82 }
electromotivated 0:9a6f7aafe531 83
electromotivated 0:9a6f7aafe531 84 float PID::clip(float value, float lower, float upper){
electromotivated 0:9a6f7aafe531 85 return std::max(lower, std::min(value, upper));
electromotivated 0:9a6f7aafe531 86 }