1 package org
.usfirst
.frc
.team3501
.robot
.utils
;
3 import org
.usfirst
.frc
.team3501
.robot
.MathLib
;
5 public class PIDController
{
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
;
21 public PIDController() {
22 this(0.0, 0.0, 0.0, 0.0, 0.0);
25 public PIDController(double p
, double i
, double d
, double eps
,
33 this.errorIncrement
= 1.0;
36 this.doneCycleCount
= 0;
37 this.minDoneCycleCount
= 5;
39 this.firstCycle
= true;
45 public PIDController(double p
, double i
, double d
, double eps
) {
46 this(p
, i
, d
, eps
, eps
* 0.8);
49 public PIDController(double p
, double i
, double d
) {
53 public void setConstants(double p
, double i
, double d
) {
59 public void setConstants(double p
, double i
, double d
, double eps
,
64 this.doneRange
= eps
; // range of error
68 public void setConstants(double p
, double i
, double d
, double eps
) {
69 setConstants(p
, i
, d
, eps
, eps
* 0.8);
72 public void setDoneRange(double range
) {
73 this.doneRange
= range
;
76 public void setIRange(double eps
) {
80 public void setSetPoint(double val
) {
84 public void setMaxOutput(double max
) {
87 } else if (max
> 1.0) {
94 public void setMinDoneCycles(int num
) {
95 this.minDoneCycleCount
= num
;
98 public void resetErrorSum() {
102 public double getDesiredVal() {
103 return this.setPoint
;
106 public double calcPID(double current
) {
107 return calcPIDError(this.setPoint
- current
);
110 public double calcPIDError(double error
) {
115 if (this.firstCycle
) {
116 this.previousError
= error
;
117 this.firstCycle
= false;
120 pVal
= this.pConst
* error
;
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) {
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) {
136 // add either the full error or the max allowable amount to sum
137 this.errorSum
+= Math
.max(error
, -1.0 * this.errorIncrement
);
140 // i contribution (final) calculation
141 iVal
= this.iConst
* this.errorSum
;
143 // /////D Calc///////
144 double deriv
= error
- this.previousError
;
145 dVal
= this.dConst
* deriv
;
148 double output
= pVal
+ iVal
+ dVal
;
151 output
= MathLib
.limitValue(output
, this.maxOutput
);
153 // store current value as previous for next cycle
154 this.previousError
= error
;
158 public boolean isDone() {
159 double currError
= Math
.abs(this.previousError
);
161 // close enough to target
162 if (currError
<= this.doneRange
) {
163 this.doneCycleCount
++;
165 // not close enough to target
167 this.doneCycleCount
= 0;
170 return this.doneCycleCount
>= this.minDoneCycleCount
;
173 public double getP() {
177 public double getI() {
181 public double getD() {