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