Chun Feng Huang / PID_modified

PID modified by C.-F. Huang

Fork of PID by LDSC_Robotics_TAs

PID.cpp@6:0d1e877c7f60, 2016-12-15 (annotated)

Committer:
benson516
Date:
Thu Dec 15 20:17:49 2016 +0000
Revision:
6:0d1e877c7f60
Parent:
5:016c99bb877f 
Turn the default values for eanabling output saturation and anti-windup to false.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
weisnail 0:7f9b4ca968ae 1 #include "PID.h"
weisnail 0:7f9b4ca968ae 2
benson516 2:b9610a2d2ea0 3 PID::PID(float Kp_in, float Ki_in, float Kd_in, float Sampletime_in){
weisnail 0:7f9b4ca968ae 4
benson516 2:b9610a2d2ea0 5 // Parameters
benson516 6:0d1e877c7f60 6 Outputlimit_bool = false; // true
weisnail 0:7f9b4ca968ae 7 Inputlimit_bool = false;
benson516 6:0d1e877c7f60 8 AntiWindUp_bool = false; // true
benson516 3:d8646d8c994f 9 outputLimits_H = 7.4;
benson516 3:d8646d8c994f 10 outputLimits_L = -7.4;
weisnail 0:7f9b4ca968ae 11 inputLimits_H = 0.0;
weisnail 0:7f9b4ca968ae 12 inputLimits_L = 0.0;
benson516 2:b9610a2d2ea0 13 feedbackvalue = 0.0;
benson516 2:b9610a2d2ea0 14
benson516 2:b9610a2d2ea0 15 // Feedback gain
benson516 2:b9610a2d2ea0 16 Kp = Kp_in;
benson516 2:b9610a2d2ea0 17 Ki = Ki_in;
benson516 2:b9610a2d2ea0 18 Kd = Kd_in;
benson516 4:e3c9cb64be44 19 // Ka = 0.1; // Volt.-sec./deg.
benson516 4:e3c9cb64be44 20 Ka = 0.0017;// Volt.-sec./rad
benson516 2:b9610a2d2ea0 21
benson516 2:b9610a2d2ea0 22 // Sampling time
benson516 2:b9610a2d2ea0 23 Ts = Sampletime_in;
weisnail 0:7f9b4ca968ae 24
benson516 2:b9610a2d2ea0 25 // Variables
benson516 2:b9610a2d2ea0 26 error[0] = 0.0;
benson516 2:b9610a2d2ea0 27 error[1] = 0.0;
benson516 2:b9610a2d2ea0 28 error_I = 0.0;
benson516 2:b9610a2d2ea0 29 //
benson516 2:b9610a2d2ea0 30 reference = 0.0;
benson516 2:b9610a2d2ea0 31 output = 0.0;
benson516 2:b9610a2d2ea0 32
benson516 2:b9610a2d2ea0 33
benson516 2:b9610a2d2ea0 34
weisnail 0:7f9b4ca968ae 35 }
weisnail 0:7f9b4ca968ae 36
weisnail 0:7f9b4ca968ae 37 void PID::SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L){
weisnail 0:7f9b4ca968ae 38 Outputlimit_bool = true;
weisnail 0:7f9b4ca968ae 39 outputLimits_H = setoutputLimits_H;
weisnail 0:7f9b4ca968ae 40 outputLimits_L = setoutputLimits_L;
weisnail 0:7f9b4ca968ae 41 }
weisnail 0:7f9b4ca968ae 42
weisnail 0:7f9b4ca968ae 43 void PID::SetInputLimits(float setinputLimits_H, float setinputLimits_L){
weisnail 0:7f9b4ca968ae 44 Inputlimit_bool = true;
weisnail 0:7f9b4ca968ae 45 inputLimits_H = setinputLimits_H;
weisnail 0:7f9b4ca968ae 46 inputLimits_L = setinputLimits_L;
weisnail 0:7f9b4ca968ae 47 }
weisnail 0:7f9b4ca968ae 48
benson516 3:d8646d8c994f 49 void PID::EnableAntiWindUp(float Ka_in)
adam_z 1:4df4895863cd 50 {
adam_z 1:4df4895863cd 51 AntiWindUp_bool = true;
benson516 3:d8646d8c994f 52 Ka = Ka_in;
adam_z 1:4df4895863cd 53 }
adam_z 1:4df4895863cd 54
benson516 2:b9610a2d2ea0 55 void PID::Compute(float reference_in, float feedbackvalue_in){
weisnail 0:7f9b4ca968ae 56
weisnail 0:7f9b4ca968ae 57 if(Inputlimit_bool == true){
benson516 2:b9610a2d2ea0 58 if( reference_in > inputLimits_H){
weisnail 0:7f9b4ca968ae 59 reference = inputLimits_H;
benson516 2:b9610a2d2ea0 60 }else if( reference_in < inputLimits_L){
weisnail 0:7f9b4ca968ae 61 reference = inputLimits_L;
benson516 2:b9610a2d2ea0 62 }else{
benson516 2:b9610a2d2ea0 63 reference = reference_in;
weisnail 0:7f9b4ca968ae 64 }
benson516 2:b9610a2d2ea0 65
weisnail 0:7f9b4ca968ae 66 }else{
benson516 2:b9610a2d2ea0 67 reference = reference_in;
weisnail 0:7f9b4ca968ae 68 }
weisnail 0:7f9b4ca968ae 69
benson516 2:b9610a2d2ea0 70 // bypass
benson516 2:b9610a2d2ea0 71 feedbackvalue = feedbackvalue_in;
weisnail 0:7f9b4ca968ae 72
weisnail 0:7f9b4ca968ae 73 error[0] = reference - feedbackvalue;
benson516 2:b9610a2d2ea0 74 // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
benson516 2:b9610a2d2ea0 75 output = Kp*error[0] + Ki*error_I;
benson516 2:b9610a2d2ea0 76
benson516 2:b9610a2d2ea0 77 // Delay 1
benson516 2:b9610a2d2ea0 78 error[1] = error[0];
weisnail 0:7f9b4ca968ae 79
benson516 2:b9610a2d2ea0 80 // Integration
benson516 2:b9610a2d2ea0 81 error_I += Ts*error[0];
benson516 2:b9610a2d2ea0 82
benson516 2:b9610a2d2ea0 83 // Output satuation
benson516 2:b9610a2d2ea0 84 if(Outputlimit_bool && AntiWindUp_bool){
weisnail 0:7f9b4ca968ae 85 if( output >= outputLimits_H){
benson516 3:d8646d8c994f 86 // output = output - (output - outputLimits_H)*Ka;
benson516 3:d8646d8c994f 87 error_I -= Ka*(output - outputLimits_H); // Anti-windup
benson516 3:d8646d8c994f 88 output = outputLimits_H;
weisnail 0:7f9b4ca968ae 89 }else if( output <= outputLimits_L){
benson516 3:d8646d8c994f 90 // output = output - (output - outputLimits_L)*Ka;
benson516 3:d8646d8c994f 91 error_I -= Ka*(output - outputLimits_L); // Anti-windup
benson516 3:d8646d8c994f 92 output = outputLimits_L;
weisnail 0:7f9b4ca968ae 93 }
weisnail 0:7f9b4ca968ae 94 }else{
benson516 2:b9610a2d2ea0 95 // output = output;
weisnail 0:7f9b4ca968ae 96 }
weisnail 0:7f9b4ca968ae 97
adam_z 1:4df4895863cd 98 }
benson516 4:e3c9cb64be44 99 void PID::Compute_noWindUP(float reference_in, float feedbackvalue_in){
adam_z 1:4df4895863cd 100
benson516 4:e3c9cb64be44 101 if(Inputlimit_bool == true){
benson516 4:e3c9cb64be44 102 if( reference_in > inputLimits_H){
benson516 4:e3c9cb64be44 103 reference = inputLimits_H;
benson516 4:e3c9cb64be44 104 }else if( reference_in < inputLimits_L){
benson516 4:e3c9cb64be44 105 reference = inputLimits_L;
benson516 4:e3c9cb64be44 106 }else{
benson516 4:e3c9cb64be44 107 reference = reference_in;
benson516 4:e3c9cb64be44 108 }
benson516 4:e3c9cb64be44 109
benson516 4:e3c9cb64be44 110 }else{
benson516 4:e3c9cb64be44 111 reference = reference_in;
benson516 4:e3c9cb64be44 112 }
benson516 4:e3c9cb64be44 113
benson516 4:e3c9cb64be44 114 // bypass
benson516 4:e3c9cb64be44 115 feedbackvalue = feedbackvalue_in;
benson516 4:e3c9cb64be44 116
benson516 4:e3c9cb64be44 117 error[0] = reference - feedbackvalue;
benson516 4:e3c9cb64be44 118 // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
benson516 4:e3c9cb64be44 119 output = Kp*error[0] + Ki*error_I;
benson516 4:e3c9cb64be44 120
benson516 4:e3c9cb64be44 121 // Delay 1
benson516 4:e3c9cb64be44 122 error[1] = error[0];
benson516 4:e3c9cb64be44 123
benson516 4:e3c9cb64be44 124 // Integration
benson516 4:e3c9cb64be44 125 error_I += Ts*error[0];
benson516 4:e3c9cb64be44 126
benson516 4:e3c9cb64be44 127 // Output satuation
benson516 4:e3c9cb64be44 128 /*
benson516 4:e3c9cb64be44 129 if(Outputlimit_bool && AntiWindUp_bool){
benson516 4:e3c9cb64be44 130 if( output >= outputLimits_H){
benson516 4:e3c9cb64be44 131 // output = output - (output - outputLimits_H)*Ka;
benson516 4:e3c9cb64be44 132 error_I -= Ka*(output - outputLimits_H); // Anti-windup
benson516 4:e3c9cb64be44 133 output = outputLimits_H;
benson516 4:e3c9cb64be44 134 }else if( output <= outputLimits_L){
benson516 4:e3c9cb64be44 135 // output = output - (output - outputLimits_L)*Ka;
benson516 4:e3c9cb64be44 136 error_I -= Ka*(output - outputLimits_L); // Anti-windup
benson516 4:e3c9cb64be44 137 output = outputLimits_L;
benson516 4:e3c9cb64be44 138 }
benson516 4:e3c9cb64be44 139 }else{
benson516 4:e3c9cb64be44 140 // output = output;
benson516 4:e3c9cb64be44 141 }
benson516 4:e3c9cb64be44 142 */
benson516 4:e3c9cb64be44 143
benson516 4:e3c9cb64be44 144 }
benson516 5:016c99bb877f 145
benson516 5:016c99bb877f 146 // Use separately
benson516 5:016c99bb877f 147 // Compute_noWindUP() -- [ Saturation_output(): optional, can be replaced by customized saturation function ] -- Anti_windup(delta) -- output
benson516 5:016c99bb877f 148 //////////////////////////////////////////////////
benson516 4:e3c9cb64be44 149 void PID::Saturation_output(){
benson516 4:e3c9cb64be44 150 if( output >= outputLimits_H){
benson516 4:e3c9cb64be44 151 delta_output = outputLimits_H - output;
benson516 4:e3c9cb64be44 152 output = outputLimits_H;
benson516 4:e3c9cb64be44 153 }else if( output <= outputLimits_L){
benson516 4:e3c9cb64be44 154 delta_output = outputLimits_L - output;
benson516 4:e3c9cb64be44 155 output = outputLimits_L;
benson516 4:e3c9cb64be44 156 }else{
benson516 4:e3c9cb64be44 157 delta_output = 0.0;
benson516 4:e3c9cb64be44 158 }
benson516 4:e3c9cb64be44 159 }
benson516 4:e3c9cb64be44 160 void PID::Anti_windup(float delta){ // delta_V = Vs - V
benson516 4:e3c9cb64be44 161 // Anti-windup compensation
benson516 4:e3c9cb64be44 162 error_I += Ka*delta; // Anti-windup
benson516 5:016c99bb877f 163 }
benson516 5:016c99bb877f 164 ////////////////////////////////////////////////// end Use separately
benson516 5:016c99bb877f 165
benson516 5:016c99bb877f 166 // Use alone
benson516 5:016c99bb877f 167 // Compute_noWindUP() -- Anti_windup() -- output
benson516 5:016c99bb877f 168 //////////////////////////////////////////////////
benson516 5:016c99bb877f 169 void PID::Anti_windup(){ // delta_V = Vs - V
benson516 5:016c99bb877f 170
benson516 5:016c99bb877f 171 PID::Saturation_output();
benson516 5:016c99bb877f 172 // Anti-windup compensation
benson516 5:016c99bb877f 173 error_I += Ka*PID::delta_output; // Anti-windup
benson516 5:016c99bb877f 174 }
benson516 5:016c99bb877f 175 ////////////////////////////////////////////////// end Use alone