Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Clean up PIDController interface in preparation for ProfiledPIDController

Browse Source
This commit is contained in:
Tyler Veness
2019-08-14 22:17:44 -07:00
committed by Peter Johnson
parent fdc098267e
commit fc98a79dbb
10 changed files with 226 additions and 319 deletions
Download Patch File Download Diff File Expand all files Collapse all files

146
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

@@ -46,17 +46,18 @@ public class PIDController extends SendableBase {
// Minimum input - limit setpoint to this
private double m_minimumInput;
// input range - difference between maximum and minimum
// Input range - difference between maximum and minimum
private double m_inputRange;
// Do the endpoints wrap around? eg. Absolute encoder
private boolean m_continuous;
// The error at the time of the most recent call to calculate()
private double m_currError;
private double m_positionError;
private double m_velocityError;
// The error at the time of the second-most-recent call to calculate() (used to compute velocity)
private double m_prevError = Double.POSITIVE_INFINITY;
private double m_prevError;
// The sum of the errors for use in the integral calc
private double m_totalError;
@@ -68,11 +69,10 @@ public class PIDController extends SendableBase {
private Tolerance m_toleranceType = Tolerance.kAbsolute;
// The percentage or absolute error that is considered at setpoint.
private double m_tolerance = 0.05;
private double m_deltaTolerance = Double.POSITIVE_INFINITY;
private double m_positionTolerance = 0.05;
private double m_velocityTolerance = Double.POSITIVE_INFINITY;
private double m_setpoint;
private double m_output;
/**
* Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of
@@ -191,17 +191,6 @@ public class PIDController extends SendableBase {
return m_period;
}
/**
* Returns the current controller output.
*
* <p>This is always centered around zero and constrained to the min and max outputs.
*
* @return The latest calculated output.
*/
public double getOutput() {
return m_output;
}
/**
* Sets the setpoint for the PIDController.
*
@@ -233,52 +222,28 @@ public class PIDController extends SendableBase {
* @return Whether the error is within the acceptable bounds.
*/
public boolean atSetpoint() {
return atSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType);
return atSetpoint(m_positionTolerance, m_velocityTolerance, m_toleranceType);
}
/**
* Returns true if the error and change in error are below the specified tolerances.
*
* @param tolerance The maximum allowable error.
* @param deltaTolerance The maximum allowable change in error from the previous iteration.
* @param toleranceType Whether the given tolerance values are absolute, or percentages of the
* total input range.
* @param positionTolerance The maximum allowable position error.
* @param velocityTolerance The maximum allowable velocity error.
* @param toleranceType The type of tolerance specified.
* @return Whether the error is within the acceptable bounds.
*/
public boolean atSetpoint(double tolerance, double deltaTolerance, Tolerance toleranceType) {
double error = getError();
double deltaError = (error - m_prevError) / getPeriod();
public boolean atSetpoint(double positionTolerance, double velocityTolerance,
Tolerance toleranceType) {
if (toleranceType == Tolerance.kPercent) {
return Math.abs(error) < tolerance / 100 * m_inputRange
&& Math.abs(deltaError) < deltaTolerance / 100 * m_inputRange;
return Math.abs(m_positionError) < positionTolerance / 100 * m_inputRange
&& Math.abs(m_velocityError) < velocityTolerance / 100 * m_inputRange;
} else {
return Math.abs(error) < tolerance && Math.abs(deltaError) < deltaTolerance;
return Math.abs(m_positionError) < positionTolerance
&& Math.abs(m_velocityError) < velocityTolerance;
}
}
/**
* Sets the PID controller to consider the input to be continuous.
*
* <p>Rather then using the max and min input range as constraints, it considers them to be the
* same point and automatically calculates the shortest route to the setpoint.
*/
public void setContinuous() {
setContinuous(true);
}
/**
* Sets the PID controller to consider the input to be continuous,
*
* <p>Rather then using the max and min input range as constraints, it considers them to be the
* same point and automatically calculates the shortest route to the setpoint.
*
* @param continuous true turns on continuous, false turns off continuous
*/
public void setContinuous(boolean continuous) {
m_continuous = continuous;
}
/**
* Sets the minimum and maximum values expected from the input.
*
@@ -296,6 +261,28 @@ public class PIDController extends SendableBase {
}
}
/**
* Enables continuous input.
*
* <p>Rather then using the max and min input range as constraints, it considers
* them to be the same point and automatically calculates the shortest route
* to the setpoint.
*
* @param minimumInput The minimum value expected from the input.
* @param maximumInput The maximum value expected from the input.
*/
public void enableContinuousInput(double minimumInput, double maximumInput) {
m_continuous = true;
setInputRange(minimumInput, maximumInput);
}
/**
* Disables continuous input.
*/
public void disableContinuousInput() {
m_continuous = false;
}
/**
* Sets the minimum and maximum values to write.
*
@@ -310,43 +297,43 @@ public class PIDController extends SendableBase {
/**
* Sets the absolute error which is considered tolerable for use with atSetpoint().
*
* @param tolerance Absolute error which is tolerable.
* @param positionTolerance Position error which is tolerable.
*/
public void setAbsoluteTolerance(double tolerance) {
setAbsoluteTolerance(tolerance, Double.POSITIVE_INFINITY);
public void setAbsoluteTolerance(double positionTolerance) {
setAbsoluteTolerance(positionTolerance, Double.POSITIVE_INFINITY);
}
/**
* Sets the absolute error which is considered tolerable for use with atSetpoint().
*
* @param tolerance Absolute error which is tolerable.
* @param deltaTolerance Change in absolute error per second which is tolerable.
* @param positionTolerance Position error which is tolerable.
* @param velocityTolerance Velocity error which is tolerable.
*/
public void setAbsoluteTolerance(double tolerance, double deltaTolerance) {
public void setAbsoluteTolerance(double positionTolerance, double velocityTolerance) {
m_toleranceType = Tolerance.kAbsolute;
m_tolerance = tolerance;
m_deltaTolerance = deltaTolerance;
m_positionTolerance = positionTolerance;
m_velocityTolerance = velocityTolerance;
}
/**
* Sets the percent error which is considered tolerable for use with atSetpoint().
*
* @param tolerance Percent error which is tolerable.
* @param positionTolerance Position error which is tolerable.
*/
public void setPercentTolerance(double tolerance) {
setPercentTolerance(tolerance, Double.POSITIVE_INFINITY);
public void setPercentTolerance(double positionTolerance) {
setPercentTolerance(positionTolerance, Double.POSITIVE_INFINITY);
}
/**
* Sets the percent error which is considered tolerable for use with atSetpoint().
*
* @param tolerance Percent error which is tolerable.
* @param deltaTolerance Change in percent error per second which is tolerable.
* @param positionTolerance Position error which is tolerable.
* @param velocityTolerance Velocity error which is tolerable.
*/
public void setPercentTolerance(double tolerance, double deltaTolerance) {
public void setPercentTolerance(double positionTolerance, double velocityTolerance) {
m_toleranceType = Tolerance.kPercent;
m_tolerance = tolerance;
m_deltaTolerance = deltaTolerance;
m_positionTolerance = positionTolerance;
m_velocityTolerance = velocityTolerance;
}
/**
@@ -354,16 +341,15 @@ public class PIDController extends SendableBase {
*
* @return The error.
*/
public double getError() {
return getContinuousError(m_currError);
public double getPositionError() {
return getContinuousError(m_positionError);
}
/**
* Returns the change in error per second.
* Returns the velocity error.
*/
public double getDeltaError() {
double error = getError();
return (error - m_prevError) / getPeriod();
public double getVelocityError() {
return m_velocityError;
}
/**
@@ -384,19 +370,18 @@ public class PIDController extends SendableBase {
* @param measurement The current measurement of the process variable.
*/
public double calculate(double measurement) {
m_prevError = m_currError;
m_currError = getContinuousError(m_setpoint - measurement);
m_prevError = m_positionError;
m_positionError = getContinuousError(m_setpoint - measurement);
m_velocityError = (m_positionError - m_prevError) / m_period;
if (m_Ki != 0) {
m_totalError = clamp(m_totalError + m_currError * getPeriod(), m_minimumOutput / m_Ki,
m_totalError = clamp(m_totalError + m_positionError * m_period, m_minimumOutput / m_Ki,
m_maximumOutput / m_Ki);
}
m_output = clamp(
m_Kp * m_currError + m_Ki * m_totalError + m_Kd * (m_currError - m_prevError) / getPeriod(),
return clamp(
m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError,
m_minimumOutput, m_maximumOutput);
return m_output;
}
/**
@@ -405,7 +390,6 @@ public class PIDController extends SendableBase {
public void reset() {
m_prevError = 0;
m_totalError = 0;
m_output = 0;
}
@Override

3
wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java
View File

@@ -50,8 +50,7 @@ class PIDInputOutputTest {
@Test
void continuousInputTest() {
m_controller.setP(1);
m_controller.setInputRange(-180, 180);
m_controller.setContinuous(true);
m_controller.enableContinuousInput(-180, 180);
assertTrue(m_controller.calculate(-179, 179) < 0.0);
}

89
wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java
View File

@@ -18,29 +18,14 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
class PIDToleranceTest {
private PIDController m_pidController;
private static final double m_setpoint = 50.0;
private static final double m_tolerance = 10.0;
private static final double m_range = 200;
private static class FakeInput {
public double m_val;
FakeInput() {
m_val = 0;
}
public double getMeasurement() {
return m_val;
}
}
private FakeInput m_inp;
private static final double kSetpoint = 50.0;
private static final double kTolerance = 10.0;
private static final double kRange = 200;
@BeforeEach
void setUp() {
m_inp = new FakeInput();
m_pidController = new PIDController(0.05, 0.0, 0.0);
m_pidController.setInputRange(-m_range / 2, m_range / 2);
m_pidController.setInputRange(-kRange / 2, kRange / 2);
}
@AfterEach
@@ -51,60 +36,56 @@ class PIDToleranceTest {
@Test
void initialToleranceTest() {
assertFalse(m_pidController.atSetpoint());
assertTrue(m_pidController.atSetpoint());
}
@Test
void absoluteToleranceTest() {
m_pidController.setAbsoluteTolerance(m_tolerance);
m_pidController.setSetpoint(m_setpoint);
for (int i = 0; i < 50; i++) {
m_pidController.calculate(m_inp.getMeasurement());
}
m_pidController.setAbsoluteTolerance(kTolerance);
m_pidController.setSetpoint(kSetpoint);
m_pidController.calculate(0.0);
assertFalse(m_pidController.atSetpoint(),
"Error was in tolerance when it should not have been. Error was " + m_pidController
.getError());
m_inp.m_val = m_setpoint + m_tolerance / 2;
for (int i = 0; i < 50; i++) {
m_pidController.calculate(m_inp.getMeasurement());
}
.getPositionError());
m_pidController.calculate(kSetpoint + kTolerance / 2);
assertTrue(m_pidController.atSetpoint(),
"Error was not in tolerance when it should have been. Error was " + m_pidController
.getError());
m_inp.m_val = m_setpoint + 10 * m_tolerance;
for (int i = 0; i < 50; i++) {
m_pidController.calculate(m_inp.getMeasurement());
}
.getPositionError());
m_pidController.calculate(kSetpoint + 10 * kTolerance);
assertFalse(m_pidController.atSetpoint(),
"Error was in tolerance when it should not have been. Error was " + m_pidController
.getError());
.getPositionError());
}
@Test
void percentToleranceTest() {
m_pidController.setPercentTolerance(m_tolerance);
m_pidController.setSetpoint(m_setpoint);
for (int i = 0; i < 50; i++) {
m_pidController.calculate(m_inp.getMeasurement());
}
m_pidController.setPercentTolerance(kTolerance);
m_pidController.setSetpoint(kSetpoint);
m_pidController.calculate(0.0);
assertFalse(m_pidController.atSetpoint(),
"Error was in tolerance when it should not have been. Error was " + m_pidController
.getError());
//half of percent tolerance away from setPoint
m_inp.m_val = m_setpoint + m_tolerance / 200 * m_range;
for (int i = 0; i < 50; i++) {
m_pidController.calculate(m_inp.getMeasurement());
}
.getPositionError());
// Half of percent tolerance away from setpoint
m_pidController.calculate(kSetpoint + (kTolerance / 2) / 100 * kRange);
assertTrue(m_pidController.atSetpoint(),
"Error was not in tolerance when it should have been. Error was " + m_pidController
.getError());
//double percent tolerance away from setPoint
m_inp.m_val = m_setpoint + m_tolerance / 50 * m_range;
for (int i = 0; i < 50; i++) {
m_pidController.calculate(m_inp.getMeasurement());
}
.getPositionError());
// Double percent tolerance away from setpoint
m_pidController.calculate(kSetpoint + (kTolerance * 2) / 100 * kRange);
assertFalse(m_pidController.atSetpoint(),
"Error was in tolerance when it should not have been. Error was " + m_pidController
.getError());
.getPositionError());
}
}