Chun Feng Huang / PID_modified

PID modified by C.-F. Huang

Fork of PID by LDSC_Robotics_TAs

PID.cpp@7:52f66036f628, 2016-12-24 (annotated)

Committer:
benson516
Date:
Sat Dec 24 09:51:00 2016 +0000
Revision:
7:52f66036f628
Parent:
4:e3c9cb64be44 
Add the reset function

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){
benson516 7:52f66036f628 4
benson516 2:b9610a2d2ea0 5 // Parameters
benson516 7:52f66036f628 6 Outputlimit_bool = false; // true
weisnail 0:7f9b4ca968ae 7 Inputlimit_bool = false;
benson516 7:52f66036f628 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;
benson516 7:52f66036f628 12 inputLimits_L = 0.0;
benson516 7:52f66036f628 13 feedbackvalue = 0.0;
benson516 7:52f66036f628 14
benson516 7:52f66036f628 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 7:52f66036f628 21
benson516 2:b9610a2d2ea0 22 // Sampling time
benson516 7:52f66036f628 23 Ts = Sampletime_in;
benson516 7:52f66036f628 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 7:52f66036f628 32
benson516 2:b9610a2d2ea0 33
benson516 7:52f66036f628 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;
benson516 7:52f66036f628 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;
benson516 7:52f66036f628 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 }
benson516 7:52f66036f628 54 void PID::Reset(void){
benson516 7:52f66036f628 55 error[0] = 0.0;
benson516 7:52f66036f628 56 error[1] = 0.0;
benson516 7:52f66036f628 57 error_I = 0.0;
benson516 7:52f66036f628 58 output = 0.0;
benson516 7:52f66036f628 59 }
benson516 2:b9610a2d2ea0 60 void PID::Compute(float reference_in, float feedbackvalue_in){
weisnail 0:7f9b4ca968ae 61
weisnail 0:7f9b4ca968ae 62 if(Inputlimit_bool == true){
benson516 2:b9610a2d2ea0 63 if( reference_in > inputLimits_H){
weisnail 0:7f9b4ca968ae 64 reference = inputLimits_H;
benson516 2:b9610a2d2ea0 65 }else if( reference_in < inputLimits_L){
weisnail 0:7f9b4ca968ae 66 reference = inputLimits_L;
benson516 2:b9610a2d2ea0 67 }else{
benson516 2:b9610a2d2ea0 68 reference = reference_in;
weisnail 0:7f9b4ca968ae 69 }
benson516 7:52f66036f628 70
weisnail 0:7f9b4ca968ae 71 }else{
benson516 2:b9610a2d2ea0 72 reference = reference_in;
weisnail 0:7f9b4ca968ae 73 }
benson516 7:52f66036f628 74
benson516 2:b9610a2d2ea0 75 // bypass
benson516 2:b9610a2d2ea0 76 feedbackvalue = feedbackvalue_in;
benson516 7:52f66036f628 77
weisnail 0:7f9b4ca968ae 78 error[0] = reference - feedbackvalue;
benson516 2:b9610a2d2ea0 79 // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
benson516 2:b9610a2d2ea0 80 output = Kp*error[0] + Ki*error_I;
benson516 7:52f66036f628 81
benson516 2:b9610a2d2ea0 82 // Delay 1
benson516 7:52f66036f628 83 error[1] = error[0];
benson516 7:52f66036f628 84
benson516 2:b9610a2d2ea0 85 // Integration
benson516 2:b9610a2d2ea0 86 error_I += Ts*error[0];
benson516 7:52f66036f628 87
benson516 2:b9610a2d2ea0 88 // Output satuation
benson516 2:b9610a2d2ea0 89 if(Outputlimit_bool && AntiWindUp_bool){
weisnail 0:7f9b4ca968ae 90 if( output >= outputLimits_H){
benson516 3:d8646d8c994f 91 // output = output - (output - outputLimits_H)*Ka;
benson516 3:d8646d8c994f 92 error_I -= Ka*(output - outputLimits_H); // Anti-windup
benson516 3:d8646d8c994f 93 output = outputLimits_H;
weisnail 0:7f9b4ca968ae 94 }else if( output <= outputLimits_L){
benson516 3:d8646d8c994f 95 // output = output - (output - outputLimits_L)*Ka;
benson516 3:d8646d8c994f 96 error_I -= Ka*(output - outputLimits_L); // Anti-windup
benson516 3:d8646d8c994f 97 output = outputLimits_L;
weisnail 0:7f9b4ca968ae 98 }
weisnail 0:7f9b4ca968ae 99 }else{
benson516 2:b9610a2d2ea0 100 // output = output;
weisnail 0:7f9b4ca968ae 101 }
benson516 7:52f66036f628 102
adam_z 1:4df4895863cd 103 }
benson516 4:e3c9cb64be44 104 void PID::Compute_noWindUP(float reference_in, float feedbackvalue_in){
adam_z 1:4df4895863cd 105
benson516 4:e3c9cb64be44 106 if(Inputlimit_bool == true){
benson516 4:e3c9cb64be44 107 if( reference_in > inputLimits_H){
benson516 4:e3c9cb64be44 108 reference = inputLimits_H;
benson516 4:e3c9cb64be44 109 }else if( reference_in < inputLimits_L){
benson516 4:e3c9cb64be44 110 reference = inputLimits_L;
benson516 4:e3c9cb64be44 111 }else{
benson516 4:e3c9cb64be44 112 reference = reference_in;
benson516 4:e3c9cb64be44 113 }
benson516 7:52f66036f628 114
benson516 4:e3c9cb64be44 115 }else{
benson516 4:e3c9cb64be44 116 reference = reference_in;
benson516 4:e3c9cb64be44 117 }
benson516 7:52f66036f628 118
benson516 4:e3c9cb64be44 119 // bypass
benson516 4:e3c9cb64be44 120 feedbackvalue = feedbackvalue_in;
benson516 7:52f66036f628 121
benson516 4:e3c9cb64be44 122 error[0] = reference - feedbackvalue;
benson516 4:e3c9cb64be44 123 // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
benson516 4:e3c9cb64be44 124 output = Kp*error[0] + Ki*error_I;
benson516 7:52f66036f628 125
benson516 4:e3c9cb64be44 126 // Delay 1
benson516 7:52f66036f628 127 error[1] = error[0];
benson516 7:52f66036f628 128
benson516 4:e3c9cb64be44 129 // Integration
benson516 4:e3c9cb64be44 130 error_I += Ts*error[0];
benson516 7:52f66036f628 131
benson516 4:e3c9cb64be44 132 // Output satuation
benson516 4:e3c9cb64be44 133 /*
benson516 4:e3c9cb64be44 134 if(Outputlimit_bool && AntiWindUp_bool){
benson516 4:e3c9cb64be44 135 if( output >= outputLimits_H){
benson516 4:e3c9cb64be44 136 // output = output - (output - outputLimits_H)*Ka;
benson516 4:e3c9cb64be44 137 error_I -= Ka*(output - outputLimits_H); // Anti-windup
benson516 4:e3c9cb64be44 138 output = outputLimits_H;
benson516 4:e3c9cb64be44 139 }else if( output <= outputLimits_L){
benson516 4:e3c9cb64be44 140 // output = output - (output - outputLimits_L)*Ka;
benson516 4:e3c9cb64be44 141 error_I -= Ka*(output - outputLimits_L); // Anti-windup
benson516 4:e3c9cb64be44 142 output = outputLimits_L;
benson516 4:e3c9cb64be44 143 }
benson516 4:e3c9cb64be44 144 }else{
benson516 4:e3c9cb64be44 145 // output = output;
benson516 4:e3c9cb64be44 146 }
benson516 4:e3c9cb64be44 147 */
benson516 7:52f66036f628 148
benson516 4:e3c9cb64be44 149 }
benson516 7:52f66036f628 150
benson516 7:52f66036f628 151 // Use separately
benson516 7:52f66036f628 152 // Compute_noWindUP() -- [ Saturation_output(): optional, can be replaced by customized saturation function ] -- Anti_windup(delta) -- output
benson516 7:52f66036f628 153 //////////////////////////////////////////////////
benson516 4:e3c9cb64be44 154 void PID::Saturation_output(){
benson516 4:e3c9cb64be44 155 if( output >= outputLimits_H){
benson516 4:e3c9cb64be44 156 delta_output = outputLimits_H - output;
benson516 4:e3c9cb64be44 157 output = outputLimits_H;
benson516 4:e3c9cb64be44 158 }else if( output <= outputLimits_L){
benson516 4:e3c9cb64be44 159 delta_output = outputLimits_L - output;
benson516 4:e3c9cb64be44 160 output = outputLimits_L;
benson516 4:e3c9cb64be44 161 }else{
benson516 4:e3c9cb64be44 162 delta_output = 0.0;
benson516 4:e3c9cb64be44 163 }
benson516 4:e3c9cb64be44 164 }
benson516 4:e3c9cb64be44 165 void PID::Anti_windup(float delta){ // delta_V = Vs - V
benson516 4:e3c9cb64be44 166 // Anti-windup compensation
benson516 4:e3c9cb64be44 167 error_I += Ka*delta; // Anti-windup
benson516 7:52f66036f628 168 }
benson516 7:52f66036f628 169 ////////////////////////////////////////////////// end Use separately
benson516 7:52f66036f628 170
benson516 7:52f66036f628 171 // Use alone
benson516 7:52f66036f628 172 // Compute_noWindUP() -- Anti_windup() -- output
benson516 7:52f66036f628 173 //////////////////////////////////////////////////
benson516 7:52f66036f628 174 void PID::Anti_windup(){ // delta_V = Vs - V
benson516 7:52f66036f628 175
benson516 7:52f66036f628 176 PID::Saturation_output();
benson516 7:52f66036f628 177 // Anti-windup compensation
benson516 7:52f66036f628 178 error_I += Ka*PID::delta_output; // Anti-windup
benson516 7:52f66036f628 179 }
benson516 7:52f66036f628 180 ////////////////////////////////////////////////// end Use alone