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@0:37123f30e8b2, 2020-01-14 (annotated)

Committer:
pmmccorkell
Date:
Tue Jan 14 19:52:12 2020 +0000
Revision:
0:37123f30e8b2
Child:
4:e89234ca9d4e
asdf

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