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
;
23 public PIDController() {
24 this(0.0, 0.0, 0.0, 0.0, 0.0);
27 public PIDController(double p
, double i
, double d
, double eps
,
35 this.errorIncrement
= 1.0;
38 this.doneCycleCount
= 0;
39 this.minDoneCycleCount
= 5;
41 this.firstCycle
= true;
47 public void setName(String s
) {
51 public PIDController(double p
, double i
, double d
, double eps
) {
52 this(p
, i
, d
, eps
, eps
* 0.8);
55 public PIDController(double p
, double i
, double d
) {
59 public void setConstants(double p
, double i
, double d
) {
64 System
.out
.println(this.name
+ " " + " P: " + this.pConst
+ " I: "
65 + this.iConst
+ " D: " + this.dConst
);
68 public void setConstants(double p
, double i
, double d
, double eps
,
73 this.doneRange
= eps
; // range of error
77 public void setConstants(double p
, double i
, double d
, double eps
) {
78 setConstants(p
, i
, d
, eps
, eps
* 0.8);
81 public void setDoneRange(double range
) {
82 this.doneRange
= range
;
85 public void setIRange(double eps
) {
89 public void setSetPoint(double val
) {
93 public void setMaxOutput(double max
) {
96 } else if (max
> 1.0) {
103 public void setMinDoneCycles(int num
) {
104 this.minDoneCycleCount
= num
;
107 public void resetErrorSum() {
111 public double getDesiredVal() {
112 return this.setPoint
;
115 public double calcPID(double current
) {
116 return calcPIDError(this.setPoint
- current
);
119 public double calcPIDError(double error
) {
124 if (this.firstCycle
) {
125 this.previousError
= error
;
126 this.firstCycle
= false;
129 pVal
= this.pConst
* error
;
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) {
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) {
145 // add either the full error or the max allowable amount to sum
146 this.errorSum
+= Math
.max(error
, -1.0 * this.errorIncrement
);
149 // i contribution (final) calculation
150 iVal
= this.iConst
* this.errorSum
;
152 // /////D Calc///////
153 double deriv
= error
- this.previousError
;
154 dVal
= this.dConst
* deriv
;
157 double output
= pVal
+ iVal
+ dVal
;
160 output
= MathLib
.limitValue(output
, this.maxOutput
);
162 // store current value as previous for next cycle
163 this.previousError
= error
;
167 public boolean isDone() {
168 double currError
= Math
.abs(this.previousError
);
170 // close enough to target
171 if (currError
<= this.doneRange
) {
172 this.doneCycleCount
++;
174 // not close enough to target
176 this.doneCycleCount
= 0;
179 return this.doneCycleCount
>= this.minDoneCycleCount
;
182 public double getP() {
186 public double getI() {
190 public double getD() {