1 | /* Class PropIntDeriv
|
---|
2 | *
|
---|
3 | * This class contains the constructor to create an instance of
|
---|
4 | * a proportional plus integral plus Derivative (PID) controller and
|
---|
5 | * the methods needed to use this controller in control loops in the
|
---|
6 | * time domain, Laplace transform s domain or the z-transform z domain.
|
---|
7 | *
|
---|
8 | * This class is a subclass of the superclass BlackBox.
|
---|
9 | *
|
---|
10 | * Author: Michael Thomas Flanagan.
|
---|
11 | *
|
---|
12 | * Created: August 2002
|
---|
13 | * Updated: 17 April 2003, 3 May 2005, 2 July 2006, 27 February 2008, 6 April 2008, 7 November 2009
|
---|
14 | * 24 May 2010
|
---|
15 | *
|
---|
16 | * DOCUMENTATION:
|
---|
17 | * See Michael T Flanagan's JAVA library on-line web page:
|
---|
18 | * http://www.ee.ucl.ac.uk/~mflanaga/java/PropIntDeriv.html
|
---|
19 | * http://www.ee.ucl.ac.uk/~mflanaga/java/
|
---|
20 | *
|
---|
21 | * Copyright (c) 2002 - 2010 Michael Thomas Flanagan
|
---|
22 | *
|
---|
23 | * PERMISSION TO COPY:
|
---|
24 | * Permission to use, copy and modify this software and its documentation for
|
---|
25 | * NON-COMMERCIAL purposes is granted, without fee, provided that an acknowledgement
|
---|
26 | * to the author, Michael Thomas Flanagan at www.ee.ac.uk/~mflanaga, appears in all copies.
|
---|
27 | *
|
---|
28 | * Dr Michael Thomas Flanagan makes no representations about the suitability
|
---|
29 | * or fitness of the software for any or for a particular purpose.
|
---|
30 | * Michael Thomas Flanagan shall not be liable for any damages suffered
|
---|
31 | * as a result of using, modifying or distributing this software or its derivatives.
|
---|
32 | *
|
---|
33 | ***************************************************************************************/
|
---|
34 |
|
---|
35 |
|
---|
36 | package agents.anac.y2015.agentBuyogV2.flanagan.control;
|
---|
37 | import agents.anac.y2015.agentBuyogV2.flanagan.complex.Complex;
|
---|
38 | import agents.anac.y2015.agentBuyogV2.flanagan.complex.ComplexPoly;
|
---|
39 | import agents.anac.y2015.agentBuyogV2.flanagan.plot.Plot;
|
---|
40 | import agents.anac.y2015.agentBuyogV2.flanagan.plot.PlotGraph;
|
---|
41 |
|
---|
42 |
|
---|
43 | public class PropIntDeriv extends BlackBox{
|
---|
44 | private double kp = 1.0D; // proportional gain
|
---|
45 | private double ti = Double.POSITIVE_INFINITY; // integral time constant
|
---|
46 | private double ki = 0.0D; // integral gain
|
---|
47 | private double td = 0.0D; // derivative time constant
|
---|
48 | private double kd = 0.0D; // derivative gain
|
---|
49 |
|
---|
50 | // Constructor - unit proportional gain, zero integral gain, zero derivative gain
|
---|
51 | public PropIntDeriv(){
|
---|
52 | super("PropIntDeriv");
|
---|
53 | super.setSnumer(new ComplexPoly(0.0D, 1.0D, 0.0D));
|
---|
54 | super.setSdenom(new ComplexPoly(0.0D, 1.0D));
|
---|
55 | super.setZtransformMethod(1);
|
---|
56 | super.addDeadTimeExtras();
|
---|
57 | }
|
---|
58 |
|
---|
59 | // Set the proportional gain
|
---|
60 | public void setKp(double kp){
|
---|
61 | this.kp=kp;
|
---|
62 | super.sNumer.resetCoeff(1, new Complex(kp, 0.0));
|
---|
63 | super.calcPolesZerosS();
|
---|
64 | super.addDeadTimeExtras();
|
---|
65 | }
|
---|
66 |
|
---|
67 | // Set the integral gain
|
---|
68 | public void setKi(double ki){
|
---|
69 | this.ki=ki;
|
---|
70 | this.ti=this.kp/ki;
|
---|
71 | super.sNumer.resetCoeff(0, new Complex(ki, 0.0));
|
---|
72 | super.calcPolesZerosS();
|
---|
73 | super.addDeadTimeExtras();
|
---|
74 | }
|
---|
75 |
|
---|
76 | // Set the integral time constant
|
---|
77 | public void setTi(double ti){
|
---|
78 | this.ti=ti;
|
---|
79 | this.ki=this.kp/ti;
|
---|
80 | super.sNumer.resetCoeff(0, new Complex(ki, 0.0));
|
---|
81 | super.calcPolesZerosS();
|
---|
82 | super.addDeadTimeExtras();
|
---|
83 | }
|
---|
84 |
|
---|
85 | // Set the derivative gain
|
---|
86 | public void setKd(double kd){
|
---|
87 | this.kd=kd;
|
---|
88 | this.td=kd/this.kp;
|
---|
89 | super.sNumer.resetCoeff(2, new Complex(kd, 0.0));
|
---|
90 | super.calcPolesZerosS();
|
---|
91 | super.addDeadTimeExtras();
|
---|
92 | }
|
---|
93 |
|
---|
94 | // Set the derivative time constant
|
---|
95 | public void setTd(double td){
|
---|
96 | this.td=td;
|
---|
97 | this.kd=this.kp*td;
|
---|
98 | super.sNumer.resetCoeff(2, new Complex(kd, 0.0));
|
---|
99 | super.calcPolesZerosS();
|
---|
100 | super.addDeadTimeExtras();
|
---|
101 | }
|
---|
102 |
|
---|
103 | // Get the proprtional gain
|
---|
104 | public double getKp(){
|
---|
105 | return this.kp;
|
---|
106 | }
|
---|
107 |
|
---|
108 | // Get the integral gain
|
---|
109 | public double getKi(){
|
---|
110 | return this.ki;
|
---|
111 | }
|
---|
112 |
|
---|
113 | // Get the integral time constant
|
---|
114 | public double getTi(){
|
---|
115 | return this.ti;
|
---|
116 | }
|
---|
117 |
|
---|
118 | // Get the derivative gain
|
---|
119 | public double getKd(){
|
---|
120 | return this.kd;
|
---|
121 | }
|
---|
122 |
|
---|
123 | // Get the derivative time constant
|
---|
124 | public double getTd(){
|
---|
125 | return this.td;
|
---|
126 | }
|
---|
127 |
|
---|
128 | // Perform z transform using an already set delta T
|
---|
129 | public void zTransform(){
|
---|
130 | if(super.deltaT==0.0D)System.out.println("z-transform attempted in PropIntDeriv with a zero sampling period");
|
---|
131 | super.deadTimeWarning("zTransform");
|
---|
132 | if(super.ztransMethod==0){
|
---|
133 | this.mapstozAdHoc();
|
---|
134 | }
|
---|
135 | else{
|
---|
136 | double kit = this.ki*super.deltaT;
|
---|
137 | double kdt = this.kd/super.deltaT;
|
---|
138 | Complex[] coef = Complex.oneDarray(3);
|
---|
139 | coef[0].reset(0.0D,0.0D);
|
---|
140 | coef[1].reset(-1.0D,0.0D);
|
---|
141 | coef[2].reset(1.0D,0.0D);
|
---|
142 | super.zDenom.resetPoly(coef);
|
---|
143 | switch(this.integMethod){
|
---|
144 | // Trapezium rule
|
---|
145 | case 0: coef[0].reset(kdt,0.0D);
|
---|
146 | coef[1].reset(kit/2.0D-2.0D*kdt-this.kp,0.0D);
|
---|
147 | coef[2].reset(this.kp+kit/2.0D+kdt,0.0);
|
---|
148 | super.zNumer.resetPoly(coef);
|
---|
149 | break;
|
---|
150 | // Backward rectangular rule
|
---|
151 | case 1: coef[0].reset(kdt,0.0D);
|
---|
152 | coef[1].reset(-2.0D*kdt-this.kp,0.0D);
|
---|
153 | coef[2].reset(this.kp+kit+kdt,0.0);
|
---|
154 | super.zNumer.resetPoly(coef);
|
---|
155 | break;
|
---|
156 | // Foreward tectangular rule
|
---|
157 | case 2: coef[0].reset(kdt,0.0D);
|
---|
158 | coef[1].reset(kit-2.0D*kdt-this.kp,0.0D);
|
---|
159 | coef[2].reset(this.kp+kdt,0.0);
|
---|
160 | super.zNumer.resetPoly(coef);
|
---|
161 | break;
|
---|
162 | default: System.out.println("Integration method option in PropIntDeriv must be 0,1 or 2");
|
---|
163 | System.out.println("It was set at "+integMethod);
|
---|
164 | System.out.println("z-transform not performed");
|
---|
165 | }
|
---|
166 | }
|
---|
167 | super.zZeros = super.zNumer.roots();
|
---|
168 | super.zPoles = super.zDenom.roots();
|
---|
169 | }
|
---|
170 |
|
---|
171 | // Perform z transform setting delta T
|
---|
172 | public void zTransform(double deltaT){
|
---|
173 | super.setDeltaT(deltaT);
|
---|
174 | this.zTransform();
|
---|
175 | }
|
---|
176 |
|
---|
177 | // Calculate the pole and the zero in the z-domain for an already set sampling period
|
---|
178 | public void calcPolesZerosZ(){
|
---|
179 | if(super.deltaT==0.0D)System.out.println("z-pole and z-zero calculation attempted in PropIntDeriv.calcPolesZerosZ( with a zero sampling period");
|
---|
180 | this.zTransform();
|
---|
181 | super.zPoles[0].reset(0.0D, 0.0D);
|
---|
182 | super.zPoles[1].reset(1.0D, 0.0D);
|
---|
183 | super.zZeros = super.zNumer.roots();
|
---|
184 | }
|
---|
185 |
|
---|
186 | // Calculate the pole and the zero in the z-domain setting the sampling period
|
---|
187 | public void calcPolesZerosZ(double deltaT){
|
---|
188 | this.deltaT = deltaT;
|
---|
189 | this.calcPolesZerosZ();
|
---|
190 | }
|
---|
191 |
|
---|
192 | // Plots the time course for a step input
|
---|
193 | public void stepInput(double stepMag, double finalTime){
|
---|
194 |
|
---|
195 | // Calculate time course outputs
|
---|
196 | int n = 50; // number of points on plot
|
---|
197 | double incrT = finalTime/(double)(n-1); // plotting increment
|
---|
198 | double cdata[][] = new double [2][n]; // plotting array
|
---|
199 | double sum = 0.0D; // integration sum
|
---|
200 |
|
---|
201 | cdata[0][0]=0.0D;
|
---|
202 | for(int i=1; i<n; i++){
|
---|
203 | cdata[0][i]=cdata[0][i-1]+incrT;
|
---|
204 | }
|
---|
205 | double kpterm = this.kp*stepMag;
|
---|
206 | for(int i=0; i<n; i++){
|
---|
207 | sum += ki*incrT*stepMag;
|
---|
208 | cdata[1][i] = kpterm + sum;
|
---|
209 | }
|
---|
210 | if(super.deadTime!=0.0D)for(int i=0; i<n; i++)cdata[0][i] += super.deadTime;
|
---|
211 |
|
---|
212 | // Plot
|
---|
213 | PlotGraph pg = new PlotGraph(cdata);
|
---|
214 |
|
---|
215 | pg.setGraphTitle("Step Input Transient: Step magnitude = "+stepMag);
|
---|
216 | pg.setGraphTitle2(this.getName());
|
---|
217 | pg.setXaxisLegend("Time");
|
---|
218 | pg.setXaxisUnitsName("s");
|
---|
219 | pg.setYaxisLegend("Output");
|
---|
220 | pg.setPoint(0);
|
---|
221 | pg.plot();
|
---|
222 | }
|
---|
223 |
|
---|
224 | // Plots the time course for a unit step input
|
---|
225 | public void stepInput(double finalTime){
|
---|
226 | this.stepInput(1.0D, finalTime);
|
---|
227 | }
|
---|
228 |
|
---|
229 | // Plots the time course for an nth order ramp input (at^n)
|
---|
230 | public void rampInput(double rampGradient, int rampOrder, double finalTime){
|
---|
231 |
|
---|
232 | // Check if really a step input
|
---|
233 | if(rampOrder==0){
|
---|
234 | this.stepInput(rampGradient, finalTime);
|
---|
235 | }
|
---|
236 | else{
|
---|
237 | // Calculate time course outputs
|
---|
238 | int n = 50; // number of points on plot
|
---|
239 | double incrT = finalTime/(double)(n-1); // plotting increment
|
---|
240 | double cdata[][] = new double [2][n]; // plotting array
|
---|
241 | double sum = 0.0D; // integration sum
|
---|
242 |
|
---|
243 | cdata[0][0]=0.0D;
|
---|
244 | cdata[1][0]=0.0D;
|
---|
245 | for(int i=1; i<n; i++){
|
---|
246 | cdata[0][i]=cdata[0][i-1]+incrT;
|
---|
247 | sum += ki*rampGradient*(Math.pow(cdata[0][i],rampOrder+1) - Math.pow(cdata[0][i-1],rampOrder+1))/(double)(rampOrder+1);
|
---|
248 | cdata[1][i] = this.kp*rampGradient*Math.pow(cdata[0][i],rampOrder) + sum;
|
---|
249 | }
|
---|
250 | if(super.deadTime!=0.0D)for(int i=0; i<n; i++)cdata[0][i] += super.deadTime;
|
---|
251 |
|
---|
252 | // Plot
|
---|
253 | PlotGraph pg = new PlotGraph(cdata);
|
---|
254 |
|
---|
255 | pg.setGraphTitle("Ramp (a.t^n) Input Transient: ramp gradient (a) = "+rampGradient + " ramp order (n) = " + rampOrder);
|
---|
256 | pg.setGraphTitle2(this.getName());
|
---|
257 | pg.setXaxisLegend("Time");
|
---|
258 | pg.setXaxisUnitsName("s");
|
---|
259 | pg.setYaxisLegend("Output");
|
---|
260 | pg.setPoint(0);
|
---|
261 | pg.plot();
|
---|
262 | }
|
---|
263 | }
|
---|
264 |
|
---|
265 | // Plots the time course for an nth order ramp input (t^n)
|
---|
266 | public void rampInput(int rampOrder, double finalTime){
|
---|
267 | double rampGradient = 1.0D;
|
---|
268 | this.rampInput(rampGradient, rampOrder, finalTime);
|
---|
269 | }
|
---|
270 |
|
---|
271 | // Plots the time course for a first order ramp input (at)
|
---|
272 | public void rampInput(double rampGradient, double finalTime){
|
---|
273 | int rampOrder = 1;
|
---|
274 | this.rampInput(rampGradient, rampOrder, finalTime);
|
---|
275 | }
|
---|
276 |
|
---|
277 | // Plots the time course for a unit ramp input (t)
|
---|
278 | public void rampInput(double finalTime){
|
---|
279 | double rampGradient = 1.0D;
|
---|
280 | int rampOrder = 1;
|
---|
281 | this.rampInput(rampGradient, rampOrder, finalTime);
|
---|
282 | }
|
---|
283 |
|
---|
284 | // Get the s-domain output for a given s-value and a given input.
|
---|
285 | public Complex getOutputS(Complex sValue, Complex iinput){
|
---|
286 | super.sValue = sValue;
|
---|
287 | super.inputS = iinput;
|
---|
288 | Complex term1 = Complex.plusOne();
|
---|
289 | Complex term2 = Complex.plusOne();
|
---|
290 | Complex term3 = Complex.plusOne();
|
---|
291 | term1 = term1.times(this.kp);
|
---|
292 | term2 = term2.times(this.ki);
|
---|
293 | term2 = term2.over(this.sValue);
|
---|
294 | term3 = term3.times(this.kd);
|
---|
295 | term3 = term3.times(super.sValue);
|
---|
296 | Complex term = term1.plus(term2.plus(term3));
|
---|
297 | super.outputS = term.times(super.inputS);
|
---|
298 | if(super.deadTime!=0.0D)super.outputS = super.outputS.times(Complex.exp(super.sValue.times(-super.deadTime)));
|
---|
299 | return super.outputS; }
|
---|
300 |
|
---|
301 | // Get the s-domain output for the stored input and s-value.
|
---|
302 | public Complex getOutputS(){
|
---|
303 | Complex term1 = Complex.plusOne();
|
---|
304 | Complex term2 = Complex.plusOne();
|
---|
305 | Complex term3 = Complex.plusOne();
|
---|
306 | term1 = term1.times(this.kp);
|
---|
307 | term2 = term2.times(this.ki);
|
---|
308 | term2 = term2.over(this.sValue);
|
---|
309 | term3 = term3.times(this.kd);
|
---|
310 | term3 = term3.times(super.sValue);
|
---|
311 | Complex term = term1.plus(term2.plus(term3));
|
---|
312 | super.outputS = term.times(super.inputS);
|
---|
313 | if(super.deadTime!=0.0D)super.outputS = super.outputS.times(Complex.exp(super.sValue.times(-super.deadTime)));
|
---|
314 | return super.outputS;
|
---|
315 | }
|
---|
316 |
|
---|
317 |
|
---|
318 | // Calculate the current time domain output for a given input and given time
|
---|
319 | // resets deltaT
|
---|
320 | public void calcOutputT(double ttime, double inp){
|
---|
321 | super.setInputT(ttime, inp);
|
---|
322 | this.calcOutputT();
|
---|
323 | }
|
---|
324 |
|
---|
325 | // Calculate the output for the stored sampled input and time
|
---|
326 | public void calcOutputT(){
|
---|
327 | super.deadTimeWarning("zTransform");
|
---|
328 | // proportional term
|
---|
329 | super.outputT[super.sampLen-1]=this.kp*super.inputT[super.sampLen-1];
|
---|
330 | // + integral term
|
---|
331 | if(super.forgetFactor==1.0D){
|
---|
332 | switch(super.integMethod){
|
---|
333 | // trapezium Rule
|
---|
334 | case 0: super.integrationSum += (super.inputT[super.sampLen-1]+super.inputT[super.sampLen-2])*super.deltaT/2.0D;
|
---|
335 | break;
|
---|
336 | // backward rectangular rule
|
---|
337 | case 1: super.integrationSum += super.inputT[super.sampLen-1]*super.deltaT;
|
---|
338 | break;
|
---|
339 | // foreward rectangular rule
|
---|
340 | case 2: super.integrationSum += super.inputT[super.sampLen-2]*super.deltaT;
|
---|
341 | break;
|
---|
342 | default: System.out.println("Integration method option in PropInt must be 0,1 or 2");
|
---|
343 | System.out.println("It was set at "+super.integMethod);
|
---|
344 | System.out.println("getOutput not performed");
|
---|
345 | }
|
---|
346 | }
|
---|
347 | else{
|
---|
348 | switch(super.integMethod){
|
---|
349 | // trapezium Rule
|
---|
350 | case 0: super.integrationSum=0.0D;
|
---|
351 | for(int i=1; i<super.sampLen; i++){
|
---|
352 | super.integrationSum+=Math.pow(super.forgetFactor, super.sampLen-1-i)*(super.inputT[i-1]+super.inputT[i])*super.deltaT/2.0D;
|
---|
353 | };
|
---|
354 | break;
|
---|
355 | // backward rectangular rule
|
---|
356 | case 1: super.integrationSum=0.0D;
|
---|
357 | for(int i=1; i<sampLen; i++){
|
---|
358 | super.integrationSum+=Math.pow(super.forgetFactor, super.sampLen-1-i)*(super.inputT[i])*super.deltaT;
|
---|
359 | };
|
---|
360 | break;
|
---|
361 | // foreward rectangular rule
|
---|
362 | case 2: super.integrationSum=0.0D;
|
---|
363 | for(int i=1; i<super.sampLen; i++){
|
---|
364 | super.integrationSum+=Math.pow(super.forgetFactor, super.sampLen-1-i)*(super.inputT[i-1])*super.deltaT;
|
---|
365 | };
|
---|
366 | break;
|
---|
367 | default: System.out.println("Integration method option in PropInt must be 0,1 or 2");
|
---|
368 | System.out.println("It was set at "+super.integMethod);
|
---|
369 | System.out.println("getOutput not performed");
|
---|
370 | }
|
---|
371 | }
|
---|
372 | super.outputT[super.sampLen-1] += this.ki*super.integrationSum;
|
---|
373 | // + derivative term
|
---|
374 | super.outputT[sampLen-1] += this.kd*(super.inputT[sampLen-1]-super.inputT[sampLen-2])/super.deltaT;
|
---|
375 | }
|
---|
376 |
|
---|
377 |
|
---|
378 | // Deep copy
|
---|
379 | public PropIntDeriv copy(){
|
---|
380 | if(this==null){
|
---|
381 | return null;
|
---|
382 | }
|
---|
383 | else{
|
---|
384 | PropIntDeriv bb = new PropIntDeriv();
|
---|
385 | this.copyBBvariables(bb);
|
---|
386 |
|
---|
387 | bb.kp = this.kp;
|
---|
388 | bb.ti = this.ti;
|
---|
389 | bb.td = this.td;
|
---|
390 | bb.kd = this.kd;
|
---|
391 |
|
---|
392 | return bb;
|
---|
393 | }
|
---|
394 | }
|
---|
395 |
|
---|
396 | // Clone - overrides Java.Object method clone
|
---|
397 | public Object clone(){
|
---|
398 | return (Object)this.copy();
|
---|
399 | }
|
---|
400 | }
|
---|