Commit | Line | Data |
---|---|---|
0788fd3d CZ |
1 | package org.usfirst.frc.team3501.robot.utils; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.MathLib; | |
4 | ||
5 | public class PIDController { | |
6 | ||
7 | private double pConst; | |
8 | private double iConst; | |
9 | private double dConst; | |
10 | private double setPoint; | |
11 | private double previousError; | |
12 | private double errorSum; | |
13 | private double errorIncrement; | |
14 | private double iRange; | |
15 | private double doneRange; | |
16 | private boolean firstCycle; | |
17 | private double maxOutput; | |
18 | private int minDoneCycleCount; | |
19 | private int doneCycleCount; | |
20 | ||
21 | public PIDController() { | |
22 | this(0.0, 0.0, 0.0, 0.0, 0.0); | |
23 | } | |
24 | ||
25 | public PIDController(double p, double i, double d, double eps, | |
26 | double iRange) { | |
27 | this.pConst = p; | |
28 | this.iConst = i; | |
29 | this.dConst = d; | |
30 | this.setPoint = 0.0; | |
31 | ||
32 | this.iRange = iRange; | |
33 | this.errorIncrement = 1.0; | |
34 | ||
35 | this.doneRange = eps; | |
36 | this.doneCycleCount = 0; | |
37 | this.minDoneCycleCount = 5; | |
38 | ||
39 | this.firstCycle = true; | |
40 | ||
41 | this.maxOutput = 1.0; | |
42 | ||
43 | } | |
44 | ||
45 | public PIDController(double p, double i, double d, double eps) { | |
46 | this(p, i, d, eps, eps * 0.8); | |
47 | } | |
48 | ||
49 | public PIDController(double p, double i, double d) { | |
50 | this(p, i, d, 1.0); | |
51 | } | |
52 | ||
53 | public void setConstants(double p, double i, double d) { | |
54 | this.pConst = p; | |
55 | this.iConst = i; | |
56 | this.dConst = d; | |
57 | } | |
58 | ||
59 | public void setConstants(double p, double i, double d, double eps, | |
60 | double iRange) { | |
61 | this.pConst = p; | |
62 | this.iConst = i; | |
63 | this.dConst = d; | |
64 | this.doneRange = eps; // range of error | |
65 | this.iRange = iRange; | |
66 | } | |
67 | ||
68 | public void setConstants(double p, double i, double d, double eps) { | |
69 | setConstants(p, i, d, eps, eps * 0.8); | |
70 | } | |
71 | ||
72 | public void setDoneRange(double range) { | |
73 | this.doneRange = range; | |
74 | } | |
75 | ||
76 | public void setIRange(double eps) { | |
77 | this.iRange = eps; | |
78 | } | |
79 | ||
80 | public void setSetPoint(double val) { | |
81 | this.setPoint = val; | |
82 | } | |
83 | ||
84 | public void setMaxOutput(double max) { | |
85 | if (max < 0.0) { | |
86 | this.maxOutput = 0.0; | |
87 | } else if (max > 1.0) { | |
88 | this.maxOutput = 1.0; | |
89 | } else { | |
90 | this.maxOutput = max; | |
91 | } | |
92 | } | |
93 | ||
94 | public void setMinDoneCycles(int num) { | |
95 | this.minDoneCycleCount = num; | |
96 | } | |
97 | ||
98 | public void resetErrorSum() { | |
99 | this.errorSum = 0.0; | |
100 | } | |
101 | ||
102 | public double getDesiredVal() { | |
103 | return this.setPoint; | |
104 | } | |
105 | ||
106 | public double calcPID(double current) { | |
107 | return calcPIDError(this.setPoint - current); | |
108 | } | |
109 | ||
110 | public double calcPIDError(double error) { | |
111 | double pVal = 0.0; | |
112 | double iVal = 0.0; | |
113 | double dVal = 0.0; | |
114 | ||
115 | if (this.firstCycle) { | |
116 | this.previousError = error; | |
117 | this.firstCycle = false; | |
118 | } | |
119 | ||
120 | pVal = this.pConst * error; | |
121 | ||
122 | // + error outside of acceptable range which might distort the sum calc | |
123 | if (error > this.iRange) { | |
124 | // check if error sum was in the wrong direction | |
125 | if (this.errorSum < 0.0) { | |
126 | this.errorSum = 0.0; | |
127 | } | |
128 | // only allow up to the max contribution per cycle | |
129 | this.errorSum += Math.min(error, this.errorIncrement); | |
130 | } // - error outside of acceptable range | |
131 | else if (error < -1.0 * this.iRange) { | |
132 | // error sum was in the wrong direction | |
133 | if (this.errorSum > 0.0) { | |
134 | this.errorSum = 0.0; | |
135 | } | |
136 | // add either the full error or the max allowable amount to sum | |
137 | this.errorSum += Math.max(error, -1.0 * this.errorIncrement); | |
138 | } | |
139 | ||
140 | // i contribution (final) calculation | |
141 | iVal = this.iConst * this.errorSum; | |
142 | ||
143 | // /////D Calc/////// | |
144 | double deriv = error - this.previousError; | |
145 | dVal = this.dConst * deriv; | |
146 | ||
147 | // overal PID calc | |
148 | double output = pVal + iVal + dVal; | |
149 | ||
150 | // limit the output | |
151 | output = MathLib.limitValue(output, this.maxOutput); | |
152 | ||
153 | // store current value as previous for next cycle | |
154 | this.previousError = error; | |
155 | return output; | |
156 | } | |
157 | ||
158 | public boolean isDone() { | |
159 | double currError = Math.abs(this.previousError); | |
160 | ||
161 | // close enough to target | |
162 | if (currError <= this.doneRange) { | |
163 | this.doneCycleCount++; | |
0788fd3d CZ |
164 | } |
165 | // not close enough to target | |
166 | else { | |
167 | this.doneCycleCount = 0; | |
168 | } | |
169 | ||
170 | return this.doneCycleCount >= this.minDoneCycleCount; | |
171 | } | |
225f2b59 E |
172 | |
173 | public double getP() { | |
174 | return this.pConst; | |
175 | } | |
176 | ||
177 | public double getI() { | |
178 | return this.iConst; | |
179 | } | |
180 | ||
181 | public double getD() { | |
182 | return this.dConst; | |
183 | } | |
0788fd3d | 184 | } |