source: src/main/java/agents/anac/y2015/agentBuyogV2/flanagan/control/PropDeriv.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: 10.2 KB
Line 
1/* Class PropDeriv
2*
3* This class contains the constructor to create an instance of
4* a Proportional plus Derivative(PD) controller and the methods
5* needed to use this controller in control loops in the time
6* 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, 2 May 2005, 2 July 2006, 6 April 2008, 30 October 2009, 7 November 2009
14* 24 May 2010
15*
16*
17* DOCUMENTATION:
18* See Michael T Flanagan's JAVA library on-line web page:
19* http://www.ee.ucl.ac.uk/~mflanaga/java/PropDeriv.html
20* http://www.ee.ucl.ac.uk/~mflanaga/java/
21*
22* Copyright (c) 2006 - 2010 Michael Thomas Flanagan
23*
24* PERMISSION TO COPY:
25* Permission to use, copy and modify this software and its documentation for
26* NON-COMMERCIAL purposes is granted, without fee, provided that an acknowledgement
27* to the author, Michael Thomas Flanagan at www.ee.ac.uk/~mflanaga, appears in all copies.
28*
29* Dr Michael Thomas Flanagan makes no representations about the suitability
30* or fitness of the software for any or for a particular purpose.
31* Michael Thomas Flanagan shall not be liable for any damages suffered
32* as a result of using, modifying or distributing this software or its derivatives.
33*
34***************************************************************************************/
35
36
37package agents.anac.y2015.agentBuyogV2.flanagan.control;
38
39import agents.anac.y2015.agentBuyogV2.flanagan.complex.Complex;
40import agents.anac.y2015.agentBuyogV2.flanagan.complex.ComplexPoly;
41import agents.anac.y2015.agentBuyogV2.flanagan.plot.Plot;
42import agents.anac.y2015.agentBuyogV2.flanagan.plot.PlotGraph;
43
44public class PropDeriv extends BlackBox{
45 private double kp = 1.0D; // proportional gain
46 private double td = 0.0D; // derivative time constant
47 private double kd = 0.0D; // derivative gain
48
49 // Constructor - unit proportional gain, zero derivative gain
50 public PropDeriv(){
51 super("PropDeriv");
52 super.sNumerDeg = 1;
53 super.sDenomDeg = 0;
54 super.setSnumer(new ComplexPoly(1.0D, 0.0D));
55 super.setSdenom(new ComplexPoly(1.0D));
56 super.setZtransformMethod(1);
57 super.addDeadTimeExtras();
58
59 }
60
61 // Set the proportional gain
62 public void setKp(double kp){
63 this.kp = kp;
64 super.sNumer.resetCoeff(0, new Complex(this.kp, 0.0D));
65 if(super.sZeros==null)super.sZeros = Complex.oneDarray(1);
66 super.sZeros[0].reset(-this.kp/this.kd, 0.0D);
67 super.addDeadTimeExtras();
68 }
69
70 // Set the derivative gain
71 public void setKd(double kd){
72 this.kd=kd;
73 this.td=kd/this.kp;
74 super.sNumer.resetCoeff(1, new Complex(this.kd, 0.0D));
75 if(super.sZeros==null)super.sZeros = Complex.oneDarray(1);
76 super.sZeros[0].reset(-this.kp/this.kd, 0.0D);
77 super.addDeadTimeExtras();
78 }
79
80 // Set the derivative time constant
81 public void setTd(double td){
82 this.td=td;
83 this.kd=this.td*this.kp;
84 if(super.sZeros==null)super.sZeros = Complex.oneDarray(1);
85 super.sNumer.resetCoeff(1, new Complex(this.kd, 0.0D));
86 super.sZeros[0].reset(-this.kp/this.kd, 0.0D);
87 super.addDeadTimeExtras();
88 }
89
90 // Get the proprtional gain
91 public double getKp(){
92 return this.kp;
93 }
94
95 // Get the derivative gain
96 public double getKd(){
97 return this.kd;
98 }
99
100 // Get the derivative time constant
101 public double getTd(){
102 return this.td;
103 }
104
105 // Perform z transform using an already set delta T
106 public void zTransform(){
107 if(super.deltaT==0.0D)System.out.println("z-transform attempted in PropDeriv with a zero sampling period");
108 super.deadTimeWarning("zTransform");
109 if(ztransMethod==0){
110 this.mapstozAdHoc();
111 }
112 else{
113 super.zNumerDeg = 1;
114 super.zDenomDeg = 1;
115 super.zNumer = new ComplexPoly(-this.kd, this.kp*super.deltaT + this.kd);
116 super.zDenom = new ComplexPoly(0.0D, super.deltaT);
117 super.zZeros = Complex.oneDarray(1);
118 super.zZeros[0].reset(this.kd/(this.kp*super.deltaT + this.kd),0.0D);
119 super.zPoles = Complex.oneDarray(1);
120 super.zPoles[0].reset(0.0D, 0.0D);
121 }
122 }
123
124 // Perform z transform setting delta T
125 public void zTransform(double deltaT){
126 super.setDeltaT(deltaT);
127 super.deadTimeWarning("zTransform");
128 this.zTransform();
129 }
130
131 // Plots the time course for a step input
132 public void stepInput(double stepMag, double finalTime){
133
134 // Calculate time course outputs
135 int n = 51; // number of points on plot
136 double incrT = finalTime/(double)(n-2); // plotting increment
137 double cdata[][] = new double [2][n]; // plotting array
138
139 cdata[0][0]=0.0D;
140 cdata[1][0]=0.0D;
141 for(int i=2; i<n; i++){
142 cdata[0][i]=cdata[0][i-1]+incrT;
143 }
144 double kpterm = this.kp*stepMag;
145 cdata[1][0]=0.0D;
146 for(int i=1; i<n; i++){
147 cdata[1][i] = kpterm;
148 }
149 if(super.deadTime!=0.0D)for(int i=0; i<n; i++)cdata[0][i] += super.deadTime;
150
151 // Plot
152 PlotGraph pg = new PlotGraph(cdata);
153
154 pg.setGraphTitle("Step Input Transient: Step magnitude = "+stepMag);
155 pg.setGraphTitle2(this.getName());
156 pg.setXaxisLegend("Time");
157 pg.setXaxisUnitsName("s");
158 pg.setYaxisLegend("Output");
159 pg.setPoint(0);
160 pg.setLine(3);
161 pg.plot();
162 }
163
164 // Plots the time course for a unit step input
165 public void stepInput(double finalTime){
166 this.stepInput(1.0D, finalTime);
167 }
168
169 // Plots the time course for an nth order ramp input (at^n)
170 public void rampInput(double rampGradient, int rampOrder, double finalTime){
171
172 if(rampOrder==0){
173 // Check if really a step input (rampOrder, n = 0)
174 this.stepInput(rampGradient, finalTime);
175 }
176 else{
177 // Calculate time course outputs
178 int n = 50; // number of points on plot
179 double incrT = finalTime/(double)(n-1); // plotting increment
180 double cdata[][] = new double [2][n]; // plotting array
181 double sum = 0.0D; // integration sum
182
183 cdata[0][0]=0.0D;
184 cdata[1][0]=0.0D;
185 for(int i=1; i<n; i++){
186 cdata[0][i]=cdata[0][i-1]+incrT;
187 cdata[1][i] = rampGradient*Math.pow(cdata[0][i],rampOrder-1)*(this.kp*cdata[0][i] + this.kd);
188 }
189 if(super.deadTime!=0.0D)for(int i=0; i<n; i++)cdata[0][i] += super.deadTime;
190
191
192 // Plot
193 PlotGraph pg = new PlotGraph(cdata);
194
195 pg.setGraphTitle("Ramp (a.t^n) Input Transient: ramp gradient (a) = "+rampGradient + " ramp order (n) = " + rampOrder);
196 pg.setGraphTitle2(this.getName());
197 pg.setXaxisLegend("Time");
198 pg.setXaxisUnitsName("s");
199 pg.setYaxisLegend("Output");
200 pg.setPoint(0);
201 pg.plot();
202 }
203 }
204
205 // Plots the time course for an nth order ramp input (t^n)
206 public void rampInput(int rampOrder, double finalTime){
207 double rampGradient = 1.0D;
208 this.rampInput(rampGradient, rampOrder, finalTime);
209 }
210
211 // Plots the time course for a first order ramp input (at)
212 public void rampInput(double rampGradient, double finalTime){
213 int rampOrder = 1;
214 this.rampInput(rampGradient, rampOrder, finalTime);
215 }
216
217 // Plots the time course for a unit ramp input (t)
218 public void rampInput(double finalTime){
219 double rampGradient = 1.0D;
220 int rampOrder = 1;
221 this.rampInput(rampGradient, rampOrder, finalTime);
222 }
223
224 // Get the s-domain output for a given s-value and a given input.
225 public Complex getOutputS(Complex sValue, Complex iinput){
226 super.sValue=sValue;
227 super.inputS=iinput;
228 Complex term = this.sValue.times(this.kd);
229 term = term.plus(this.kp);
230 super.outputS=term.times(super.inputS);
231 if(super.deadTime!=0.0D)super.outputS = super.outputS.times(Complex.exp(super.sValue.times(-super.deadTime)));
232 return super.outputS;
233 }
234
235 // Get the s-domain output for the stored input and s-value.
236 public Complex getOutputS(){
237 Complex term = this.sValue.times(this.kd);
238 term = term.plus(this.kp);
239 super.outputS=term.times(super.inputS);
240 if(super.deadTime!=0.0D)super.outputS = super.outputS.times(Complex.exp(super.sValue.times(-super.deadTime)));
241 return super.outputS;
242 }
243
244 // Calculate the current time domain output for a given input and given time
245 // resets deltaT
246 public void calcOutputT(double ttime, double inp){
247 super.setInputT(ttime, inp);
248 this.calcOutputT();
249 }
250
251 // Get the output for the stored sampled input, time and deltaT.
252 public void calcOutputT(){
253 // proportional term
254 super.outputT[super.sampLen-1] = this.kp*super.inputT[sampLen-1];
255 // + derivative term
256 super.outputT[super.sampLen-1] += this.kd*(super.inputT[super.sampLen-1]-super.inputT[super.sampLen-2])/super.deltaT;
257 }
258
259 // Get the s-domain poles
260 public Complex[] getSpoles(){
261 System.out.println("PD controller has no s-domain poles");
262 return null;
263 }
264 // Deep copy
265 public PropDeriv copy(){
266 if(this==null){
267 return null;
268 }
269 else{
270 PropDeriv bb = new PropDeriv();
271 this.copyBBvariables(bb);
272
273 bb.kp = this.kp;
274 bb.td = this.td;
275 bb.kd = this.kd;
276
277 return bb;
278 }
279 }
280
281 // Clone - overrides Java.Object method clone
282 public Object clone(){
283 return (Object)this.copy();
284 }
285}
Note: See TracBrowser for help on using the repository browser.