1 | /* Class PropInt
|
---|
2 | *
|
---|
3 | * This class contains the constructor to create an instance of
|
---|
4 | * a proportional plus integral(PI) 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/PropInt.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 | public class PropInt extends BlackBox{
|
---|
43 | private double kp = 1.0D; // proportional gain
|
---|
44 | private double ti = Double.POSITIVE_INFINITY; // integral time constant
|
---|
45 | private double ki = 0.0D; // integral gain
|
---|
46 |
|
---|
47 | // Constructor - unit proportional gain, zero integral gain
|
---|
48 | public PropInt(){
|
---|
49 | super("PropInt");
|
---|
50 | super.setSnumer(new ComplexPoly(0.0D, 1.0D));
|
---|
51 | super.setSdenom(new ComplexPoly(0.0D, 1.0D));
|
---|
52 | super.setZtransformMethod(1);
|
---|
53 | super.addDeadTimeExtras();
|
---|
54 | }
|
---|
55 |
|
---|
56 | // Set the proportional gain
|
---|
57 | public void setKp(double kp){
|
---|
58 | super.sNumer.resetCoeff(1, new Complex(kp, 0.0));
|
---|
59 | super.calcPolesZerosS();
|
---|
60 | super.addDeadTimeExtras();
|
---|
61 | }
|
---|
62 |
|
---|
63 | // Set the integral gain
|
---|
64 | public void setKi(double ki){
|
---|
65 | this.ki=ki;
|
---|
66 | this.ti=this.kp/ki;
|
---|
67 | super.sNumer.resetCoeff(0, new Complex(ki, 0.0));
|
---|
68 | super.calcPolesZerosS();
|
---|
69 | super.addDeadTimeExtras();
|
---|
70 | }
|
---|
71 |
|
---|
72 |
|
---|
73 | // Set the integral time constant
|
---|
74 | public void setTi(double ti){
|
---|
75 | this.ti=ti;
|
---|
76 | this.ki=this.kp/ti;
|
---|
77 | super.sNumer.resetCoeff(0, new Complex(ki, 0.0));
|
---|
78 | super.calcPolesZerosS();
|
---|
79 | super.addDeadTimeExtras();
|
---|
80 | }
|
---|
81 |
|
---|
82 | // Get the proprtional gain
|
---|
83 | public double getKp(){
|
---|
84 | return this.kp;
|
---|
85 | }
|
---|
86 |
|
---|
87 | // Get the integral gain
|
---|
88 | public double getKi(){
|
---|
89 | return this.ki;
|
---|
90 | }
|
---|
91 |
|
---|
92 | // Get the integral time constant
|
---|
93 | public double getTi(){
|
---|
94 | return this.ti;
|
---|
95 | }
|
---|
96 |
|
---|
97 |
|
---|
98 | // Perform z transform using an already set delta T
|
---|
99 | public void zTransform(){
|
---|
100 | super.deadTimeWarning("zTransform");
|
---|
101 | if(super.deltaT==0.0D)System.out.println("z-transform attempted in PropInt with a zero sampling period");
|
---|
102 | if(super.ztransMethod==0){
|
---|
103 | this.mapstozAdHoc();
|
---|
104 | }
|
---|
105 | else{
|
---|
106 | super.zDenom = new ComplexPoly(1);
|
---|
107 | Complex[] coef = Complex.oneDarray(2);
|
---|
108 | coef[0].reset(-1.0D,0.0D);
|
---|
109 | coef[1].reset(1.0D,0.0D);
|
---|
110 | super.zDenom.resetPoly(coef);
|
---|
111 | Complex[] zPoles = Complex.oneDarray(1);
|
---|
112 | zPoles[0].reset(1.0D, 0.0D);
|
---|
113 | super.zNumer = new ComplexPoly(1);
|
---|
114 | Complex[] zZeros = Complex.oneDarray(1);
|
---|
115 | double kit = this.ki*super.deltaT;
|
---|
116 | switch(this.integMethod){
|
---|
117 | // trapezium rule
|
---|
118 | case 0: coef[0].reset(kit/2.0D - this.kp, 0.0D);
|
---|
119 | coef[1].reset(kit/2.0D + this.kp, 0.0D);
|
---|
120 | super.zNumer.resetPoly(coef);
|
---|
121 | zZeros[0].reset((this.kp - kit/2.0D)/(this.kp + kit/2.0D), 0.0D);
|
---|
122 | break;
|
---|
123 | // backward rectangular rule
|
---|
124 | case 1: coef[0].reset(-this.kp, 0.0D);
|
---|
125 | coef[1].reset(kit + this.kp, 0.0D);
|
---|
126 | super.zNumer.resetPoly(coef);
|
---|
127 | zZeros[0].reset(this.kp/(this.kp + kit), 0.0D);
|
---|
128 | break;
|
---|
129 | // foreward rectangular rule
|
---|
130 | case 2: coef[0].reset(this.kp - kit, 0.0D);
|
---|
131 | coef[1].reset(this.kp, 0.0D);
|
---|
132 | super.zNumer.resetPoly(coef);
|
---|
133 | zZeros[0].reset((this.kp - kit)/this.kp, 0.0D);
|
---|
134 | break;
|
---|
135 | default: System.out.println("Integration method option in PropInt must be 0,1 or 2");
|
---|
136 | System.out.println("It was set at "+integMethod);
|
---|
137 | System.out.println("z-transform not performed");
|
---|
138 | }
|
---|
139 | }
|
---|
140 | }
|
---|
141 |
|
---|
142 | // Perform z transform setting delta T
|
---|
143 | public void zTransform(double deltaT){
|
---|
144 | super.setDeltaT(deltaT);
|
---|
145 | this.zTransform();
|
---|
146 | }
|
---|
147 |
|
---|
148 | // Plots the time course for a step input
|
---|
149 | public void stepInput(double stepMag, double finalTime){
|
---|
150 |
|
---|
151 | // Calculate time course outputs
|
---|
152 | int n = 50; // number of points on plot
|
---|
153 | double incrT = finalTime/(double)(n-1); // plotting increment
|
---|
154 | double cdata[][] = new double [2][n]; // plotting array
|
---|
155 | double sum = 0.0D; // integration sum
|
---|
156 |
|
---|
157 | cdata[0][0]=0.0D;
|
---|
158 | for(int i=1; i<n; i++){
|
---|
159 | cdata[0][i]=cdata[0][i-1]+incrT;
|
---|
160 | }
|
---|
161 | double kpterm = this.kp*stepMag;
|
---|
162 | for(int i=0; i<n; i++){
|
---|
163 | sum += ki*incrT*stepMag;
|
---|
164 | cdata[1][i] = kpterm + sum;
|
---|
165 | cdata[0][i] += super.deadTime;
|
---|
166 | }
|
---|
167 |
|
---|
168 | // Plot
|
---|
169 | PlotGraph pg = new PlotGraph(cdata);
|
---|
170 |
|
---|
171 | pg.setGraphTitle("Step Input Transient: Step magnitude = "+stepMag);
|
---|
172 | pg.setGraphTitle2(this.getName());
|
---|
173 | pg.setXaxisLegend("Time");
|
---|
174 | pg.setXaxisUnitsName("s");
|
---|
175 | pg.setYaxisLegend("Output");
|
---|
176 | pg.setPoint(0);
|
---|
177 | pg.plot();
|
---|
178 | }
|
---|
179 |
|
---|
180 | // Plots the time course for a unit step input
|
---|
181 | public void stepInput(double finalTime){
|
---|
182 | this.stepInput(1.0D, finalTime);
|
---|
183 | }
|
---|
184 |
|
---|
185 | // Plots the time course for an nth order ramp input (at^n)
|
---|
186 | public void rampInput(double rampGradient, int rampOrder, double finalTime){
|
---|
187 |
|
---|
188 | // Calculate time course outputs
|
---|
189 | int n = 50; // number of points on plot
|
---|
190 | double incrT = finalTime/(double)(n-1); // plotting increment
|
---|
191 | double cdata[][] = new double [2][n]; // plotting array
|
---|
192 | double sum = 0.0D; // integration sum
|
---|
193 |
|
---|
194 | cdata[0][0]=0.0D;
|
---|
195 | cdata[1][0]=0.0D;
|
---|
196 | for(int i=1; i<n; i++){
|
---|
197 | cdata[0][i]=cdata[0][i-1]+incrT;
|
---|
198 | sum += rampGradient*(Math.pow(cdata[0][i],rampOrder+1) - Math.pow(cdata[0][i-1],rampOrder+1))/(double)(rampOrder+1);
|
---|
199 | cdata[1][i] = this.kp*rampGradient*Math.pow(cdata[0][i],rampOrder) + sum;
|
---|
200 | }
|
---|
201 | for(int i=0; i<n; i++){
|
---|
202 | cdata[0][i] += super.deadTime;
|
---|
203 | }
|
---|
204 |
|
---|
205 | // Plot
|
---|
206 | PlotGraph pg = new PlotGraph(cdata);
|
---|
207 |
|
---|
208 | pg.setGraphTitle("Ramp (a.t^n) Input Transient: ramp gradient (a) = "+rampGradient + " ramp order (n) = " + rampOrder);
|
---|
209 | pg.setGraphTitle2(this.getName());
|
---|
210 | pg.setXaxisLegend("Time");
|
---|
211 | pg.setXaxisUnitsName("s");
|
---|
212 | pg.setYaxisLegend("Output");
|
---|
213 | pg.setPoint(0);
|
---|
214 | pg.plot();
|
---|
215 | }
|
---|
216 |
|
---|
217 | // Plots the time course for an nth order ramp input (t^n)
|
---|
218 | public void rampInput(int rampOrder, double finalTime){
|
---|
219 | double rampGradient = 1.0D;
|
---|
220 | this.rampInput(rampGradient, rampOrder, finalTime);
|
---|
221 | }
|
---|
222 |
|
---|
223 | // Plots the time course for a first order ramp input (at)
|
---|
224 | public void rampInput(double rampGradient, double finalTime){
|
---|
225 | int rampOrder = 1;
|
---|
226 | this.rampInput(rampGradient, rampOrder, finalTime);
|
---|
227 | }
|
---|
228 |
|
---|
229 | // Plots the time course for a unit ramp input (t)
|
---|
230 | public void rampInput(double finalTime){
|
---|
231 | double rampGradient = 1.0D;
|
---|
232 | int rampOrder = 1;
|
---|
233 | this.rampInput(rampGradient, rampOrder, finalTime);
|
---|
234 | }
|
---|
235 |
|
---|
236 | // Get the s-domain output for a given s-value and a given input.
|
---|
237 | public Complex getOutputS(Complex sValue, Complex iinput){
|
---|
238 | super.sValue=sValue;
|
---|
239 | super.inputS=iinput;
|
---|
240 | Complex term = super.sValue.times(this.kp);
|
---|
241 | term = term.plus(this.ki);
|
---|
242 | term = term.over(super.sValue);
|
---|
243 | super.outputS=term.times(super.inputS);
|
---|
244 | if(super.deadTime!=0.0D)super.outputS = super.outputS.times(Complex.exp(super.sValue.times(-super.deadTime)));
|
---|
245 | return super.outputS;
|
---|
246 | }
|
---|
247 |
|
---|
248 | // Get the s-domain output for the stored input and s-value.
|
---|
249 | public Complex getOutputS(){
|
---|
250 | Complex term = super.sValue.times(this.kp);
|
---|
251 | term = term.plus(this.ki);
|
---|
252 | term = term.over(super.sValue);
|
---|
253 | super.outputS=term.times(super.inputS);
|
---|
254 | if(super.deadTime!=0.0D)super.outputS = super.outputS.times(Complex.exp(super.sValue.times(-super.deadTime)));
|
---|
255 | return super.outputS;
|
---|
256 | }
|
---|
257 |
|
---|
258 | // Calculate the current time domain output for a given input and given time
|
---|
259 | // resets deltaT
|
---|
260 | public void calcOutputT(double ttime, double inp){
|
---|
261 | super.setInputT(ttime, inp);
|
---|
262 | this.calcOutputT();
|
---|
263 | }
|
---|
264 |
|
---|
265 | // calculate the output for the stored sampled input and time.
|
---|
266 | public void calcOutputT(){
|
---|
267 | super.deadTimeWarning("calcOutputT()");
|
---|
268 | // proportional term
|
---|
269 | super.outputT[super.sampLen-1]=this.kp*super.inputT[super.sampLen-1];
|
---|
270 | // + integral term
|
---|
271 | if(super.forgetFactor==1.0D){
|
---|
272 | switch(super.integMethod){
|
---|
273 | // trapezium Rule
|
---|
274 | case 0: super.integrationSum += (super.inputT[super.sampLen-1]+super.inputT[super.sampLen-2])*super.deltaT/2.0D;
|
---|
275 | break;
|
---|
276 | // backward rectangular rule
|
---|
277 | case 1: super.integrationSum += super.inputT[super.sampLen-1]*super.deltaT;
|
---|
278 | break;
|
---|
279 | // foreward rectangular rule
|
---|
280 | case 2: super.integrationSum += super.inputT[super.sampLen-2]*super.deltaT;
|
---|
281 | break;
|
---|
282 | default: System.out.println("Integration method option in PropInt must be 0,1 or 2");
|
---|
283 | System.out.println("It was set at "+super.integMethod);
|
---|
284 | System.out.println("getOutput not performed");
|
---|
285 | }
|
---|
286 | }
|
---|
287 | else{
|
---|
288 | switch(super.integMethod){
|
---|
289 | // trapezium Rule
|
---|
290 | case 0: super.integrationSum=0.0D;
|
---|
291 | for(int i=1; i<super.sampLen; i++){
|
---|
292 | super.integrationSum+=Math.pow(super.forgetFactor, super.sampLen-1-i)*(super.inputT[i-1]+super.inputT[i])*super.deltaT/2.0D;
|
---|
293 | };
|
---|
294 | break;
|
---|
295 | // backward rectangular rule
|
---|
296 | case 1: super.integrationSum=0.0D;
|
---|
297 | for(int i=1; i<sampLen; i++){
|
---|
298 | super.integrationSum+=Math.pow(super.forgetFactor, super.sampLen-1-i)*(super.inputT[i])*super.deltaT;
|
---|
299 | };
|
---|
300 | break;
|
---|
301 | // foreward rectangular rule
|
---|
302 | case 2: super.integrationSum=0.0D;
|
---|
303 | for(int i=1; i<super.sampLen; i++){
|
---|
304 | super.integrationSum+=Math.pow(super.forgetFactor, super.sampLen-1-i)*(super.inputT[i-1])*super.deltaT;
|
---|
305 | };
|
---|
306 | break;
|
---|
307 | default: System.out.println("Integration method option in PropInt must be 0,1 or 2");
|
---|
308 | System.out.println("It was set at "+super.integMethod);
|
---|
309 | System.out.println("getOutput not performed");
|
---|
310 | }
|
---|
311 | }
|
---|
312 | super.outputT[super.sampLen-1] += this.ki*super.integrationSum;
|
---|
313 | }
|
---|
314 |
|
---|
315 |
|
---|
316 | // Deep copy
|
---|
317 | public PropInt copy(){
|
---|
318 | if(this==null){
|
---|
319 | return null;
|
---|
320 | }
|
---|
321 | else{
|
---|
322 | PropInt bb = new PropInt();
|
---|
323 | this.copyBBvariables(bb);
|
---|
324 |
|
---|
325 | bb.kp = this.kp;
|
---|
326 | bb.ti = this.ti;
|
---|
327 | bb.ki = this.ki;
|
---|
328 |
|
---|
329 | return bb;
|
---|
330 | }
|
---|
331 | }
|
---|
332 |
|
---|
333 | // Clone - overrides Java.Object method clone
|
---|
334 | public Object clone(){
|
---|
335 | return (Object)this.copy();
|
---|
336 | }
|
---|
337 | }
|
---|