source: src/main/java/agents/anac/y2015/agentBuyogV2/flanagan/control/PropIntDeriv.java

Last change on this file was 127, checked in by Wouter Pasman, 6 years ago

#41 ROLL BACK of rev.126 . So this version is equal to rev. 125

File size: 15.5 KB
Line 
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
36package agents.anac.y2015.agentBuyogV2.flanagan.control;
37import agents.anac.y2015.agentBuyogV2.flanagan.complex.Complex;
38import agents.anac.y2015.agentBuyogV2.flanagan.complex.ComplexPoly;
39import agents.anac.y2015.agentBuyogV2.flanagan.plot.Plot;
40import agents.anac.y2015.agentBuyogV2.flanagan.plot.PlotGraph;
41
42
43public 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}
Note: See TracBrowser for help on using the repository browser.