AnimatLab  2
Test
StdPID.cpp
1 
7 #include "StdAfx.h"
8 #include "StdPID.h"
9 
10 namespace StdUtils
11 {
12 
13 #define DEGTORAD ((double)3.1415926535/180.0)
14 
15 CStdPID::CStdPID(void)
16 {
17  FullReset();
18 }
19 
20 CStdPID::CStdPID(float fltSetpoint, float fltGain, float fltIntegralAct, float fltDerivativeAct,
21  bool bComplexError, bool bAntiResetWindup, bool bRampLimit,
22  float fltRangeMax, float fltRangeMin, float fltARWBound, float fltRampGradient)
23 {
24  FullReset();
25 
26  Setpoint(fltSetpoint);
27  Gain(fltGain);
28  IntegralAct(fltIntegralAct);
29  DerivativeAct(fltDerivativeAct);
30  ComplexError(bComplexError);
31  AntiResetWindup(bAntiResetWindup);
32  RampLimit(bRampLimit);
33  RangeMax(fltRangeMax);
34  RangeMin(fltRangeMin);
35  ARWBound(fltARWBound);
36  RampGradient(fltRampGradient);
37 }
38 
39 
40 CStdPID::~CStdPID(void)
41 {
42 }
43 
44 
45 void CStdPID::FullReset()
46 {
47  m_fltError = 0;
48  m_fltSetpoint = 0;
49  m_bComplexError = true;
50  m_bAntiResetWindup = true;
51  m_bRampLimit = true;
52  m_fltErrorChange = 0;
53 
54  m_fltGain = 0;
55  m_fltIntegralAct = 0;
56 
57  m_fltProportional = 0;
58  m_fltIntegral = 0;
59  m_fltOldIntegral = 0;
60  m_fltDerivative = 0;
61  m_fltDerivativeAct = 0;
62 
63  m_fltOutput = 0;
64  m_fltOldOutput = 0;
65 
66  m_fltRangeMax = 0;
67  m_fltRangeMin = 0;
68  m_fltRange = 0;
69  m_fltARWBound = 0;
70  m_fltRampGradient = 0;
71 
72  //Initialize with 3 spots
73  m_aryOldErrors.Clear();
74  m_aryOldErrors.Add(0);
75  m_aryOldErrors.Add(0);
76  m_aryOldErrors.Add(0);
77 }
78 
79 void CStdPID::ResetVars()
80 {
81  m_fltError = 0;
82  m_fltSetpoint = 0;
83  m_fltErrorChange = 0;
84 
85  m_fltProportional = 0;
86  m_fltIntegral = 0;
87  m_fltOldIntegral = 0;
88  m_fltDerivative = 0;
89 
90  m_fltOutput = 0;
91  m_fltOldOutput = 0;
92 
93  //Initialize with 3 spots
94  m_aryOldErrors.Clear();
95  m_aryOldErrors.Add(0);
96  m_aryOldErrors.Add(0);
97  m_aryOldErrors.Add(0);
98 }
99 
100 float CStdPID::Error() {return m_fltError;}
101 
102 float CStdPID::ErrorChange() {return m_fltErrorChange;}
103 
104 void CStdPID::Setpoint(float fltVal) {m_fltSetpoint = fltVal;}
105 
106 float CStdPID::Setpoint() {return m_fltSetpoint;}
107 
108 void CStdPID::Gain(float fltVal)
109 {
110  if(fltVal >= 0)
111  m_fltGain = fltVal;
112 }
113 
114 float CStdPID::Gain() {return m_fltGain;}
115 
116 void CStdPID::IntegralAct(float fltVal)
117 {
118  if(fltVal >= 0)
119  m_fltIntegralAct = fltVal;
120 }
121 
122 float CStdPID::IntegralAct() {return m_fltIntegralAct;}
123 
124 void CStdPID::DerivativeAct(float fltVal)
125 {
126  if(fltVal >= 0)
127  m_fltDerivativeAct = fltVal;
128 }
129 
130 float CStdPID::DerivativeAct() {return m_fltDerivativeAct;}
131 
132 float CStdPID::Proportional() {return m_fltProportional;}
133 
134 float CStdPID::Integral() {return m_fltIntegral;}
135 
136 float CStdPID::OldIntegral() {return m_fltOldIntegral;}
137 
138 float CStdPID::Derivative() {return m_fltDerivative;}
139 
140 float CStdPID::Output() {return m_fltOutput;}
141 
142 float CStdPID::OldOutput() {return m_fltOldOutput;}
143 
144 void CStdPID::ComplexError(bool bVal) {m_bComplexError = bVal;}
145 
146 bool CStdPID::ComplexError() {return m_bComplexError;}
147 
148 void CStdPID::AntiResetWindup(bool bVal) {m_bAntiResetWindup = bVal;}
149 
150 bool CStdPID::AntiResetWindup() {return m_bAntiResetWindup;}
151 
152 void CStdPID::RampLimit(bool bVal) {m_bRampLimit = bVal;}
153 
154 bool CStdPID::RampLimit() {return m_bRampLimit;}
155 
156 void CStdPID::RangeMax(float fltVal)
157 {
158  if(fltVal >= m_fltRangeMin)
159  {
160  m_fltRangeMax = fltVal;
161  m_fltRange = m_fltRangeMax - m_fltRangeMin;
162  }
163 }
164 
165 float CStdPID::RangeMax() {return m_fltRangeMax;}
166 
167 void CStdPID::RangeMin(float fltVal)
168 {
169  if(fltVal <= m_fltRangeMax)
170  {
171  m_fltRangeMin = fltVal;
172  m_fltRange = m_fltRangeMax - m_fltRangeMin;
173  }
174 }
175 
176 float CStdPID::RangeMin() {return m_fltRangeMin;}
177 
178 float CStdPID::Range() {return m_fltRange;}
179 
190 void CStdPID::ARWBound(float fltVal)
191 {
192  if(fltVal >= 0 && fltVal <= 1)
193  m_fltARWBound = fltVal;
194 }
195 
196 float CStdPID::ARWBound() {return m_fltARWBound;}
197 
206 void CStdPID::RampGradient(float fltVal)
207 {
208  if(fltVal >= 0 && fltVal <= 90)
209  m_fltRampGradient = fltVal;
210 }
211 
212 float CStdPID::RampGradient() {return m_fltRampGradient;}
213 
214 
215 // Calculate PID
216 float CStdPID::Calculate(float fltDt, float fltInput)
217 {
218  float fltError1;
219  float fltError2;
220  float fltError3;
221  float fltChange;
222  float fltMaxChange;
223 
224  // Error
225  m_fltError = m_fltSetpoint-fltInput;
226 
227  // Use simple error change or more stable average error change
228  if(!m_bComplexError)
229  {
230  fltError1 = m_aryOldErrors.GetAt(2);
231  m_fltErrorChange = m_fltError-fltError1;
232  }
233  else
234  {
235  fltError1 = m_aryOldErrors.GetAt(2);
236  fltError2 = m_aryOldErrors.GetAt(1);
237  fltError3 = m_aryOldErrors.GetAt(0);
238  m_fltErrorChange = (m_fltError+(3*fltError1)-(3*fltError2)-fltError3)/6;
239  }
240 
241  // Proportional
242  m_fltProportional = m_fltGain * m_fltError;
243 
244  // Integral
245  m_fltIntegral = m_fltIntegral + (m_fltGain * (m_fltIntegralAct*fltDt) * m_fltError);
246 
247  // Derivative
248  m_fltDerivative = m_fltGain * (m_fltDerivativeAct/fltDt) * m_fltErrorChange;
249 
250  // Calculate Output
251  m_fltOutput = (m_fltProportional + m_fltIntegral + m_fltDerivative);
252 
253  // Perform Anti-reset windup?
254  if(m_bAntiResetWindup)
255  {
256  if(m_fltOutput >= m_fltRangeMax-(m_fltARWBound*m_fltRange))
257  {
258  m_fltOutput = m_fltRangeMax-(m_fltARWBound*m_fltRange);
259  m_fltIntegral = m_fltOldIntegral;
260  }
261  else if(m_fltOutput <= m_fltRangeMin+(m_fltARWBound*m_fltRange))
262  {
263  m_fltOutput = m_fltRangeMin+(m_fltARWBound*m_fltRange);
264  m_fltIntegral = m_fltOldIntegral;
265  }
266  }
267 
268  // Perform Ramp limiting?
269  if(m_bRampLimit)
270  {
271  fltMaxChange = (float)tan((((double)m_fltRampGradient)*DEGTORAD)/fltDt);
272  fltChange = m_fltOutput - m_fltOldOutput;
273  if(fltChange > fltMaxChange)
274  {
275  m_fltOutput = m_fltOldOutput + fltMaxChange;
276 
277  // limit integral?
278  fltChange = (float)fabs(m_fltIntegral - m_fltOldIntegral);
279  if(fltChange > fltMaxChange)
280  {
281  m_fltIntegral = m_fltOldIntegral + fltMaxChange;
282  }
283  }
284  else if(fltChange < (0-fltMaxChange))
285  {
286  m_fltOutput = m_fltOldOutput - fltMaxChange;
287 
288  // limit integral?
289  fltChange = (float)fabs(m_fltIntegral - m_fltOldIntegral);
290  if(fltChange > fltMaxChange)
291  {
292  m_fltIntegral = m_fltOldIntegral - fltMaxChange;
293  }
294  }
295  }
296 
297  m_aryOldErrors.AddEnd(m_fltError);
298 
299  m_fltOldIntegral = m_fltIntegral;
300 
301  m_fltOldOutput = m_fltOutput;
302 
303  return m_fltOutput;
304 }
305 
306 } //StdUtils
virtual void ARWBound(float fltVal)
Sets the anti-reset boundary. This is specified as a percentage of the entire output range...
Definition: StdPID.cpp:190
virtual void RampGradient(float fltVal)
Ramp gradient.
Definition: StdPID.cpp:206
Namespace for the standard utility objects.
Definition: MarkupSTL.cpp:19