Pat McC / Mbed 2 deprecated AUV_PIDusna

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed BNO055_fusion_AUV

PIDusna/PID.cpp@4:e89234ca9d4e, 2020-11-12 (annotated)

Committer:
pmmccorkell
Date:
Thu Nov 12 20:25:07 2020 +0000
Revision:
4:e89234ca9d4e
Parent:
0:37123f30e8b2 
asf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmmccorkell 0:37123f30e8b2 1 /*
pmmccorkell 0:37123f30e8b2 2 * PID class for mbed
pmmccorkell 0:37123f30e8b2 3 * US Naval Academy
pmmccorkell 0:37123f30e8b2 4 * Robotics and Control TSD
pmmccorkell 0:37123f30e8b2 5 * Patrick McCorkell
pmmccorkell 0:37123f30e8b2 6 *
pmmccorkell 0:37123f30e8b2 7 * Created: 2019 Dec 11
pmmccorkell 0:37123f30e8b2 8 *
pmmccorkell 0:37123f30e8b2 9 */
pmmccorkell 0:37123f30e8b2 10
pmmccorkell 0:37123f30e8b2 11 #include "mbed.h"
pmmccorkell 0:37123f30e8b2 12 #include "PID.h"
pmmccorkell 0:37123f30e8b2 13
pmmccorkell 0:37123f30e8b2 14 // K values as floats.
pmmccorkell 0:37123f30e8b2 15 // dt should be set to same as Ticker that uses this class.
pmmccorkell 0:37123f30e8b2 16 // deadzone represents the acceptable error, defaults to 0.
pmmccorkell 4:e89234ca9d4e 17 PID::PID (float Kp, float Ki, float Kd, float dt, float deadzone)
pmmccorkell 0:37123f30e8b2 18 {
pmmccorkell 0:37123f30e8b2 19 set_dt(dt);
pmmccorkell 0:37123f30e8b2 20 set_K(Kp, Ki, Kd);
pmmccorkell 0:37123f30e8b2 21 set_deadzone(deadzone);
pmmccorkell 4:e89234ca9d4e 22 clear();
pmmccorkell 0:37123f30e8b2 23 }
pmmccorkell 0:37123f30e8b2 24
pmmccorkell 0:37123f30e8b2 25 /*
pmmccorkell 0:37123f30e8b2 26 Notes from Levi DeVries, PhD:
pmmccorkell 0:37123f30e8b2 27 The method goes like this:
pmmccorkell 0:37123f30e8b2 28 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.
pmmccorkell 0:37123f30e8b2 29 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.
pmmccorkell 0:37123f30e8b2 30 */
pmmccorkell 0:37123f30e8b2 31 void PID::calculate_K(float Tu)
pmmccorkell 0:37123f30e8b2 32 {
pmmccorkell 0:37123f30e8b2 33 float calc_Kp = _Kp/5;
pmmccorkell 0:37123f30e8b2 34 float calc_Ki = (2*calc_Kp)/Tu;
pmmccorkell 0:37123f30e8b2 35 float calc_Kd = (calc_Kp * Tu)/3;
pmmccorkell 0:37123f30e8b2 36 set_K(calc_Kp,calc_Ki,calc_Kd);
pmmccorkell 0:37123f30e8b2 37 }
pmmccorkell 0:37123f30e8b2 38
pmmccorkell 0:37123f30e8b2 39 // PID::PID_calculate_K(float max_error, float output_range, float P_max, float timeframe, float time_factor, int dt, float deadzone)
pmmccorkell 0:37123f30e8b2 40 // {
pmmccorkell 0:37123f30e8b2 41 // //Initialize dt and deadzone values.
pmmccorkell 0:37123f30e8b2 42 // set_dt(dt);
pmmccorkell 0:37123f30e8b2 43 // set_deadzone(deadzone);
pmmccorkell 0:37123f30e8b2 44
pmmccorkell 0:37123f30e8b2 45 // //Calculate the proportion to reach Max P value at Max Error value.
pmmccorkell 0:37123f30e8b2 46 // float Kp=(P_max/max_error);
pmmccorkell 0:37123f30e8b2 47
pmmccorkell 0:37123f30e8b2 48 // //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).
pmmccorkell 0:37123f30e8b2 49 // float iterations=time_factor*(timeframe/_dt);
pmmccorkell 0:37123f30e8b2 50 // float error_delta=(max_error/iterations);
pmmccorkell 0:37123f30e8b2 51 // float integral = 0;
pmmccorkell 0:37123f30e8b2 52 // float error=max_error;
pmmccorkell 0:37123f30e8b2 53 // //Find the total integral at (factor * target) time in the max distance scenario.
pmmccorkell 0:37123f30e8b2 54 // int i=0;
pmmccorkell 0:37123f30e8b2 55 // while (i<iterations) {
pmmccorkell 0:37123f30e8b2 56 // integral+=error;
pmmccorkell 0:37123f30e8b2 57 // //error starts off at max, and slowly declines to 0 as approaching target.
pmmccorkell 0:37123f30e8b2 58 // error-=error_delta;
pmmccorkell 0:37123f30e8b2 59 // i++;
pmmccorkell 0:37123f30e8b2 60 // }
pmmccorkell 0:37123f30e8b2 61 // float Ki=(output_range - P_max)/integral;
pmmccorkell 0:37123f30e8b2 62
pmmccorkell 0:37123f30e8b2 63 // //Calculate Kd to cancel out Ki if we're on schedule to reach target under timeframe.
pmmccorkell 0:37123f30e8b2 64 // //Start by rerunning error from Max to 0, but within the specified timeframe.
pmmccorkell 0:37123f30e8b2 65 // iterations=(timeframe/_dt);
pmmccorkell 0:37123f30e8b2 66 // error_delta=(max_error/iterations);
pmmccorkell 0:37123f30e8b2 67 // integral=0;
pmmccorkell 0:37123f30e8b2 68 // error=max_error;
pmmccorkell 0:37123f30e8b2 69 // i=0;
pmmccorkell 0:37123f30e8b2 70 // while (i<iterations) {
pmmccorkell 0:37123f30e8b2 71 // integral+=error;
pmmccorkell 0:37123f30e8b2 72 // error-=error_delta;
pmmccorkell 0:37123f30e8b2 73 // i++;
pmmccorkell 0:37123f30e8b2 74 // }
pmmccorkell 0:37123f30e8b2 75 // //Having reran the integral, calculate the integral gain to offset.
pmmccorkell 0:37123f30e8b2 76 // float gainI=Ki*integral;
pmmccorkell 0:37123f30e8b2 77 // }
pmmccorkell 0:37123f30e8b2 78
pmmccorkell 0:37123f30e8b2 79 // Change dt.
pmmccorkell 4:e89234ca9d4e 80 void PID::set_dt(float dt)
pmmccorkell 0:37123f30e8b2 81 {
pmmccorkell 0:37123f30e8b2 82 _dt=dt;
pmmccorkell 0:37123f30e8b2 83 }
pmmccorkell 0:37123f30e8b2 84
pmmccorkell 0:37123f30e8b2 85 // For instances when the Integral should be zeroed out.
pmmccorkell 0:37123f30e8b2 86 void PID::clear_integral()
pmmccorkell 0:37123f30e8b2 87 {
pmmccorkell 0:37123f30e8b2 88 _integral=0;
pmmccorkell 0:37123f30e8b2 89 }
pmmccorkell 0:37123f30e8b2 90
pmmccorkell 0:37123f30e8b2 91 // For instances when the last error should be zeroed out.
pmmccorkell 0:37123f30e8b2 92 void PID::clear_error_previous()
pmmccorkell 0:37123f30e8b2 93 {
pmmccorkell 0:37123f30e8b2 94 _error_previous=0;
pmmccorkell 0:37123f30e8b2 95 }
pmmccorkell 0:37123f30e8b2 96
pmmccorkell 0:37123f30e8b2 97 void PID::clear()
pmmccorkell 0:37123f30e8b2 98 {
pmmccorkell 0:37123f30e8b2 99 clear_integral();
pmmccorkell 0:37123f30e8b2 100 clear_error_previous();
pmmccorkell 0:37123f30e8b2 101 }
pmmccorkell 0:37123f30e8b2 102
pmmccorkell 0:37123f30e8b2 103 // Sets K values for all 3 terms.
pmmccorkell 0:37123f30e8b2 104 void PID::set_K(float Kp, float Ki, float Kd)
pmmccorkell 0:37123f30e8b2 105 {
pmmccorkell 0:37123f30e8b2 106 // Setup proportional value.
pmmccorkell 0:37123f30e8b2 107 _Kp=Kp;
pmmccorkell 0:37123f30e8b2 108
pmmccorkell 0:37123f30e8b2 109 // Setup integral values.
pmmccorkell 0:37123f30e8b2 110 clear_integral();
pmmccorkell 0:37123f30e8b2 111 _Ki=Ki;
pmmccorkell 0:37123f30e8b2 112
pmmccorkell 0:37123f30e8b2 113 // Setup derivative values.
pmmccorkell 0:37123f30e8b2 114 clear_error_previous();
pmmccorkell 0:37123f30e8b2 115 _Kd=Kd;
pmmccorkell 0:37123f30e8b2 116 }
pmmccorkell 0:37123f30e8b2 117
pmmccorkell 0:37123f30e8b2 118 // Sets deadzone, if applicable.
pmmccorkell 0:37123f30e8b2 119 void PID::set_deadzone(float deadzone)
pmmccorkell 0:37123f30e8b2 120 {
pmmccorkell 0:37123f30e8b2 121 _deadzone=deadzone;
pmmccorkell 0:37123f30e8b2 122 }
pmmccorkell 0:37123f30e8b2 123
pmmccorkell 0:37123f30e8b2 124 // Calculate the PID from setpoint and measured.
pmmccorkell 0:37123f30e8b2 125 // Returns the gain.
pmmccorkell 0:37123f30e8b2 126 float PID::process(float setpoint, float measured)
pmmccorkell 0:37123f30e8b2 127 {
pmmccorkell 0:37123f30e8b2 128 // Calculate the error.
pmmccorkell 0:37123f30e8b2 129 float error=setpoint-measured;
pmmccorkell 0:37123f30e8b2 130
pmmccorkell 0:37123f30e8b2 131 // If abs value of error is smaller than the deadzone,
pmmccorkell 0:37123f30e8b2 132 // cause all the PID gains to zeroize.
pmmccorkell 0:37123f30e8b2 133 if (abs(error)<_deadzone) {
pmmccorkell 0:37123f30e8b2 134 clear_integral();
pmmccorkell 0:37123f30e8b2 135 clear_error_previous();
pmmccorkell 0:37123f30e8b2 136 error=0;
pmmccorkell 0:37123f30e8b2 137 }
pmmccorkell 0:37123f30e8b2 138
pmmccorkell 0:37123f30e8b2 139 // Proportional = Kp * e
pmmccorkell 0:37123f30e8b2 140 float k_term = (_Kp*error);
pmmccorkell 0:37123f30e8b2 141
pmmccorkell 0:37123f30e8b2 142 // Integral = Ki * e dt
pmmccorkell 0:37123f30e8b2 143 _integral+=(error*_dt);
pmmccorkell 0:37123f30e8b2 144 float i_term = (_Ki*_integral);
pmmccorkell 0:37123f30e8b2 145
pmmccorkell 0:37123f30e8b2 146 // Derivative = Kd * (de/dt)
pmmccorkell 0:37123f30e8b2 147 float d_term = (_Kd* ((error-_error_previous)/_dt) );
pmmccorkell 0:37123f30e8b2 148
pmmccorkell 0:37123f30e8b2 149 // PID = P + I + D
pmmccorkell 0:37123f30e8b2 150 float PID_calc = k_term+i_term+d_term;
pmmccorkell 0:37123f30e8b2 151
pmmccorkell 0:37123f30e8b2 152 // Update last error for next Derivative calculation.
pmmccorkell 0:37123f30e8b2 153 _error_previous=error;
pmmccorkell 0:37123f30e8b2 154
pmmccorkell 0:37123f30e8b2 155 // Return the calculated PID gain.
pmmccorkell 0:37123f30e8b2 156 return PID_calc;
pmmccorkell 0:37123f30e8b2 157 }
pmmccorkell 0:37123f30e8b2 158
pmmccorkell 0:37123f30e8b2 159 // Overloaded version to give function the error directly.
pmmccorkell 0:37123f30e8b2 160 float PID::process(float error)
pmmccorkell 0:37123f30e8b2 161 {
pmmccorkell 4:e89234ca9d4e 162 float k_term=0;
pmmccorkell 4:e89234ca9d4e 163 float i_term=0;
pmmccorkell 4:e89234ca9d4e 164 float d_term=0;
pmmccorkell 4:e89234ca9d4e 165 float out_PID=0;
pmmccorkell 0:37123f30e8b2 166 // If abs value of error is smaller than the deadzone,
pmmccorkell 0:37123f30e8b2 167 // cause all the PID gains to zeroize.
pmmccorkell 4:e89234ca9d4e 168 if ((abs(error))<_deadzone) {
pmmccorkell 0:37123f30e8b2 169 clear_integral();
pmmccorkell 0:37123f30e8b2 170 clear_error_previous();
pmmccorkell 0:37123f30e8b2 171 error=0;
pmmccorkell 0:37123f30e8b2 172 }
pmmccorkell 0:37123f30e8b2 173
pmmccorkell 0:37123f30e8b2 174 // Proportional = Kp * e
pmmccorkell 4:e89234ca9d4e 175 k_term = (_Kp*error);
pmmccorkell 0:37123f30e8b2 176
pmmccorkell 0:37123f30e8b2 177 // Integral = Ki * e dt
pmmccorkell 0:37123f30e8b2 178 _integral+=(error*_dt);
pmmccorkell 4:e89234ca9d4e 179 i_term = (_Ki*_integral);
pmmccorkell 0:37123f30e8b2 180
pmmccorkell 0:37123f30e8b2 181 // Derivative = Kd * (de/dt)
pmmccorkell 4:e89234ca9d4e 182 d_term = (_Kd* ((error-_error_previous)/_dt) );
pmmccorkell 0:37123f30e8b2 183
pmmccorkell 0:37123f30e8b2 184 // PID = P + I + D
pmmccorkell 4:e89234ca9d4e 185 out_PID = k_term+i_term+d_term;
pmmccorkell 0:37123f30e8b2 186
pmmccorkell 0:37123f30e8b2 187 // Update last error for next Derivative calculation.
pmmccorkell 0:37123f30e8b2 188 _error_previous=error;
pmmccorkell 0:37123f30e8b2 189 return out_PID;
pmmccorkell 0:37123f30e8b2 190 }
pmmccorkell 0:37123f30e8b2 191
pmmccorkell 0:37123f30e8b2 192 void PID::get_gain_values(PID_GAINS_TypeDef *gains) {
pmmccorkell 0:37123f30e8b2 193 gains->Kp = _Kp;
pmmccorkell 0:37123f30e8b2 194 gains->Ki = _Ki;
pmmccorkell 0:37123f30e8b2 195 gains->Kd = _Kd;
pmmccorkell 0:37123f30e8b2 196 }
pmmccorkell 0:37123f30e8b2 197