42 lines
1 KiB
Java
42 lines
1 KiB
Java
package org.firstinspires.ftc.teamcode.kinematics;
|
|
|
|
import com.qualcomm.robotcore.hardware.PIDCoefficients;
|
|
|
|
public class PID {
|
|
public final PIDCoefficients coefficients;
|
|
private double lastX;
|
|
private double lastUpdateTime;
|
|
private boolean firstIteration;
|
|
private double i;
|
|
|
|
private static final double NANOS_PER_SEC = 1000 * 1000 * 1000;
|
|
|
|
public PID(PIDCoefficients coefficients) {
|
|
this.coefficients = coefficients;
|
|
this.firstIteration = true;
|
|
this.i = 0;
|
|
}
|
|
|
|
public double update(double x) {
|
|
double time = System.nanoTime() / NANOS_PER_SEC;
|
|
|
|
double res = this.coefficients.p * x;
|
|
if (!this.firstIteration) {
|
|
double dt = time - this.lastUpdateTime;
|
|
|
|
this.i += x * dt;
|
|
double d = (x - lastX) / dt;
|
|
|
|
res += this.coefficients.i * this.i;
|
|
res += this.coefficients.d * d;
|
|
} else {
|
|
this.firstIteration = false;
|
|
}
|
|
|
|
this.lastX = x;
|
|
this.lastUpdateTime = time;
|
|
|
|
return res;
|
|
}
|
|
}
|