PID.cpp

00001 /*
00002  * PID class for mbed
00003  * US Naval Academy
00004  * Robotics and Control TSD
00005  * Patrick McCorkell
00006  *
00007  * Created: 2019 Dec 11
00008  * 
00009  */
00010 
00011 #include "mbed.h"
00012 #include "PID.h"
00013 
00014 // K values as floats.
00015 // dt should be set to same as Ticker that uses this class.
00016 // deadzone represents the acceptable error, defaults to 0.
00017 PID::PID (float Kp, float Ki, float Kd, float dt, float deadzone)
00018 {
00019     set_dt(dt);
00020     set_K(Kp, Ki, Kd);
00021     set_deadzone(deadzone);
00022     clear();
00023 }
00024 
00025 /*
00026 Notes from Levi DeVries, PhD:
00027 The method goes like this:
00028 1) Set Ti and Td to zero. Turn Kp up until the response is borderline unstable (it oscillates without settling to a constant). Write down that number, call it Ku. Also, note the period of the oscillations in the marginally stable output, call that period Tu.
00029 2) I assume you are going for a response with little to no overshoot. To achieve this choose, Kp = Ku/5, Ti = Tu/2, and Td = Tu/3 (if you are choosing gains by time constants). If you choosing gains instead of time constants Ki = (2/5)*Ku/Tu, Kd = Ku*Tu/15.
00030 */
00031 void PID::calculate_K(float Tu)
00032 {
00033     float calc_Kp = _Kp/5;
00034     float calc_Ki = (2*calc_Kp)/Tu;
00035     float calc_Kd = (calc_Kp * Tu)/3;
00036     set_K(calc_Kp,calc_Ki,calc_Kd);
00037 }
00038 
00039 // PID::PID_calculate_K(float max_error, float output_range, float P_max, float timeframe, float time_factor, int dt, float deadzone)
00040 // {
00041     // //Initialize dt and deadzone values.
00042     // set_dt(dt);
00043     // set_deadzone(deadzone);
00044     
00045     // //Calculate the proportion to reach Max P value at Max Error value.
00046     // float Kp=(P_max/max_error);
00047     
00048     // //Calculate the integral to reach Max range at (factor * target) time (ie if target is 5s and factor is 2, Integral will drive to reach max PWM at 10s).
00049     // float iterations=time_factor*(timeframe/_dt);
00050     // float error_delta=(max_error/iterations);
00051     // float integral = 0;
00052     // float error=max_error;
00053     // //Find the total integral at (factor * target) time in the max distance scenario.
00054     // int i=0;
00055     // while (i<iterations) {
00056         // integral+=error;
00057         // //error starts off at max, and slowly declines to 0 as approaching target.
00058         // error-=error_delta;
00059         // i++;
00060     // }
00061     // float Ki=(output_range - P_max)/integral;
00062     
00063     // //Calculate Kd to cancel out Ki if we're on schedule to reach target under timeframe.
00064     // //Start by rerunning error from Max to 0, but within the specified timeframe.
00065     // iterations=(timeframe/_dt);
00066     // error_delta=(max_error/iterations);
00067     // integral=0;
00068     // error=max_error;
00069     // i=0;
00070     // while (i<iterations) {
00071         // integral+=error;
00072         // error-=error_delta;
00073         // i++;
00074     // }
00075     // //Having reran the integral, calculate the integral gain to offset.
00076     // float gainI=Ki*integral;
00077 // }
00078 
00079 // Change dt.
00080 void PID::set_dt(float dt) 
00081 {
00082     _dt=dt;
00083 }
00084 
00085 // For instances when the Integral should be zeroed out.
00086 void PID::clear_integral() 
00087 {
00088     _integral=0;
00089 }
00090 
00091 // For instances when the last error should be zeroed out.
00092 void PID::clear_error_previous() 
00093 {
00094     _error_previous=0;
00095 }
00096 
00097 void PID::clear()
00098 {
00099     clear_integral();
00100     clear_error_previous();
00101 }
00102 
00103 // Sets K values for all 3 terms.
00104 void PID::set_K(float Kp, float Ki, float Kd) 
00105 {
00106     // Setup proportional value.
00107     _Kp=Kp;
00108     
00109     // Setup integral values.
00110     clear_integral();
00111     _Ki=Ki;
00112     
00113     // Setup derivative values.
00114     clear_error_previous();
00115     _Kd=Kd;
00116 }
00117 
00118 // Sets deadzone, if applicable.
00119 void PID::set_deadzone(float deadzone) 
00120 {
00121     _deadzone=deadzone;
00122 }
00123 
00124 // Calculate the PID from setpoint and measured.
00125 // Returns the gain.
00126 float PID::process(float setpoint, float measured) 
00127 {
00128     // Calculate the error.
00129     float error=setpoint-measured;
00130     
00131     // If abs value of error is smaller than the deadzone,
00132     //  cause all the PID gains to zeroize.
00133     if (abs(error)<_deadzone) {
00134         clear_integral();
00135         clear_error_previous();
00136         error=0;
00137     }
00138     
00139     // Proportional = Kp * e
00140     float k_term = (_Kp*error);
00141     
00142     // Integral = Ki * e dt
00143     _integral+=(error*_dt);
00144     float i_term = (_Ki*_integral);
00145     
00146     // Derivative = Kd * (de/dt)
00147     float d_term = (_Kd* ((error-_error_previous)/_dt) );
00148     
00149     // PID = P + I + D
00150     float PID_calc = k_term+i_term+d_term;
00151 
00152     // Update last error for next Derivative calculation.
00153     _error_previous=error;
00154     
00155     // Return the calculated PID gain.
00156     return PID_calc;
00157 }
00158 
00159 // Overloaded version to give function the error directly.
00160 float PID::process(float error)
00161 {
00162     float k_term=0;
00163     float i_term=0;
00164     float d_term=0;
00165     float out_PID=0;
00166     // If abs value of error is smaller than the deadzone,
00167     //  cause all the PID gains to zeroize.
00168     if ((abs(error))<_deadzone) {
00169         clear_integral();
00170         clear_error_previous();
00171         error=0;
00172     }
00173 
00174     // Proportional = Kp * e
00175     k_term = (_Kp*error);
00176 
00177     // Integral = Ki * e dt
00178     _integral+=(error*_dt);
00179     i_term = (_Ki*_integral);
00180 
00181     // Derivative = Kd * (de/dt)
00182     d_term = (_Kd* ((error-_error_previous)/_dt) );
00183 
00184     // PID = P + I + D
00185     out_PID = k_term+i_term+d_term;
00186 
00187     // Update last error for next Derivative calculation.
00188     _error_previous=error;
00189     return out_PID;
00190 }
00191 
00192 void PID::get_gain_values(PID_GAINS_TypeDef *gains) {
00193     gains->Kp = _Kp;
00194     gains->Ki = _Ki;
00195     gains->Kd = _Kd;
00196 }
00197