Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[wpimath] PIDController field and method names for error and errorDerivative #7088

Merged
merged 19 commits into from
Sep 23, 2024
Merged
Show file tree
Hide file tree
Changes from 3 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -208,7 +208,7 @@ public void testPositionPIDController() {

assertTrue(
"PID loop did not reach reference within 10 seconds. The current error was"
+ pidController.getPositionError(),
+ pidController.getError(),
pidController.atSetpoint());
}
}
Expand All @@ -233,7 +233,7 @@ public void testVelocityPIDController() {

assertTrue(
"PID loop did not reach reference within 10 seconds. The error was: "
+ pidController.getPositionError(),
+ pidController.getError(),
pidController.atSetpoint());
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,7 @@ public void testInitialSettings() {
assertEquals(
"PID.getPositionError() did not start at " + reference,
reference,
m_controller.getPositionError(),
m_controller.getError(),
0);
m_builder.update();
assertEquals(m_p, m_table.getEntry("Kp").getDouble(9999999), 0);
Expand All @@ -160,18 +160,15 @@ public void testRotateToTarget() {
assertEquals(pidData() + "did not start at 0", 0, me.getMotor().get(), 0);
m_controller.setSetpoint(reference);
assertEquals(
pidData() + "did not have an error of " + reference,
reference,
m_controller.getPositionError(),
0);
pidData() + "did not have an error of " + reference, reference, m_controller.getError(), 0);
Notifier pidRunner =
new Notifier(
() -> me.getMotor().set(m_controller.calculate(me.getEncoder().getDistance())));
pidRunner.startPeriodic(m_controller.getPeriod());
Timer.delay(5);
pidRunner.stop();
assertTrue(
pidData() + "Was not on Target. Controller Error: " + m_controller.getPositionError(),
pidData() + "Was not on Target. Controller Error: " + m_controller.getError(),
m_controller.atSetpoint());

pidRunner.close();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,8 @@ public class PIDController implements Sendable, AutoCloseable {
private boolean m_continuous;

// The error at the time of the most recent call to calculate()
private double m_positionError;
private double m_velocityError;
private double m_error;
private double m_errorDerivative;

// The error at the time of the second-most-recent call to calculate() (used to compute velocity)
private double m_prevError;
Expand All @@ -52,8 +52,8 @@ public class PIDController implements Sendable, AutoCloseable {
private double m_totalError;

// The error that is considered at setpoint.
private double m_positionTolerance = 0.05;
private double m_velocityTolerance = Double.POSITIVE_INFINITY;
private double m_errorTolerance = 0.05;
private double m_errorDerivativeTolerance = Double.POSITIVE_INFINITY;

private double m_setpoint;
private double m_measurement;
Expand Down Expand Up @@ -228,17 +228,37 @@ public double getPeriod() {
*
* @return the position tolerance of the controller.
*/
@Deprecated(forRemoval = true, since = "2025")
public double getPositionTolerance() {
return m_positionTolerance;
return m_error;
narmstro2020 marked this conversation as resolved.
Show resolved Hide resolved
}

/**
* Returns the velocity tolerance of this controller.
*
* @return the velocity tolerance of the controller.
calcmogul marked this conversation as resolved.
Show resolved Hide resolved
*/
@Deprecated(forRemoval = true, since = "2025")
public double getVelocityTolerance() {
return m_velocityTolerance;
return m_errorDerivative;
narmstro2020 marked this conversation as resolved.
Show resolved Hide resolved
}

/**
* Returns the error tolerance of this controller.
*
* @return the error tolerance of the controller.
*/
public double getErrorTolerance() {
return m_errorTolerance;
}

/**
* Returns the error derivative tolerance of this controller.
*
* @return the error derivative tolerance of the controller.
*/
public double getErrorDerivativeTolerance() {
return m_errorDerivativeTolerance;
}

/**
Expand All @@ -261,12 +281,12 @@ public void setSetpoint(double setpoint) {

if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_positionError = m_setpoint - m_measurement;
m_error = m_setpoint - m_measurement;
}

m_velocityError = (m_positionError - m_prevError) / m_period;
m_errorDerivative = (m_error - m_prevError) / m_period;
}

/**
Expand All @@ -288,8 +308,8 @@ public double getSetpoint() {
public boolean atSetpoint() {
return m_haveMeasurement
&& m_haveSetpoint
&& Math.abs(m_positionError) < m_positionTolerance
&& Math.abs(m_velocityError) < m_velocityTolerance;
&& Math.abs(m_error) < m_errorTolerance
&& Math.abs(m_errorDerivative) < m_errorDerivativeTolerance;
}

/**
Expand Down Expand Up @@ -338,39 +358,59 @@ public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
/**
* Sets the error which is considered tolerable for use with atSetpoint().
*
* @param positionTolerance Position error which is tolerable.
* @param errorTolerance Error which is tolerable.
*/
public void setTolerance(double positionTolerance) {
setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
public void setTolerance(double errorTolerance) {
setTolerance(errorTolerance, Double.POSITIVE_INFINITY);
}

/**
* Sets the error which is considered tolerable for use with atSetpoint().
*
* @param positionTolerance Position error which is tolerable.
* @param velocityTolerance Velocity error which is tolerable.
* @param errorTolerance Error which is tolerable.
* @param errorDerivativeTolerance Error derivative which is tolerable.
*/
public void setTolerance(double positionTolerance, double velocityTolerance) {
m_positionTolerance = positionTolerance;
m_velocityTolerance = velocityTolerance;
public void setTolerance(double errorTolerance, double errorDerivativeTolerance) {
m_errorTolerance = errorTolerance;
m_errorDerivativeTolerance = errorDerivativeTolerance;
}

/**
* Returns the difference between the setpoint and the measurement.
*
* @return The error.
*/
@Deprecated(forRemoval = true, since = "2025")
public double getPositionError() {
return m_positionError;
return m_error;
}

/**
* Returns the velocity error.
*
* @return The velocity error.
*/
@Deprecated(forRemoval = true, since = "2025")
public double getVelocityError() {
return m_velocityError;
return m_errorDerivative;
}

/**
* Returns the difference between the setpoint and the measurement.
*
* @return The error.
*/
public double getError() {
return m_error;
}

/**
* Returns the error derivative.
*
* @return The error derivative.
*/
public double getErrorDerivative() {
return m_errorDerivative;
}

/**
Expand All @@ -394,38 +434,38 @@ public double calculate(double measurement, double setpoint) {
*/
public double calculate(double measurement) {
m_measurement = measurement;
m_prevError = m_positionError;
m_prevError = m_error;
m_haveMeasurement = true;

if (m_continuous) {
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
} else {
m_positionError = m_setpoint - m_measurement;
m_error = m_setpoint - m_measurement;
}

m_velocityError = (m_positionError - m_prevError) / m_period;
m_errorDerivative = (m_error - m_prevError) / m_period;

// If the absolute value of the position error is greater than IZone, reset the total error
if (Math.abs(m_positionError) > m_iZone) {
if (Math.abs(m_error) > m_iZone) {
m_totalError = 0;
} else if (m_ki != 0) {
m_totalError =
MathUtil.clamp(
m_totalError + m_positionError * m_period,
m_totalError + m_error * m_period,
m_minimumIntegral / m_ki,
m_maximumIntegral / m_ki);
}

return m_kp * m_positionError + m_ki * m_totalError + m_kd * m_velocityError;
return m_kp * m_error + m_ki * m_totalError + m_kd * m_errorDerivative;
}

/** Resets the previous error and the integral term. */
public void reset() {
m_positionError = 0;
m_error = 0;
m_prevError = 0;
m_totalError = 0;
m_velocityError = 0;
m_errorDerivative = 0;
m_haveMeasurement = false;
}

Expand All @@ -446,5 +486,10 @@ public void initSendable(SendableBuilder builder) {
}
});
builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
builder.addDoubleProperty("measurement", () -> m_measurement, null);
builder.addDoubleProperty("error", this::getError, null);
builder.addDoubleProperty("error derivative", this::getErrorDerivative, null);
builder.addDoubleProperty("previous error", () -> this.m_prevError, null);
builder.addDoubleProperty("total error", this::getAccumulatedError, null);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,7 @@ public double getPeriod() {
* @return the position tolerance of the controller.
*/
public double getPositionTolerance() {
return m_controller.getPositionTolerance();
return m_controller.getErrorTolerance();
}

/**
Expand All @@ -183,7 +183,7 @@ public double getPositionTolerance() {
* @return the velocity tolerance of the controller.
*/
public double getVelocityTolerance() {
return m_controller.getVelocityTolerance();
return m_controller.getErrorDerivative();
narmstro2020 marked this conversation as resolved.
Show resolved Hide resolved
}

/**
Expand Down Expand Up @@ -330,7 +330,7 @@ public void setTolerance(double positionTolerance, double velocityTolerance) {
* @return The error.
*/
public double getPositionError() {
return m_controller.getPositionError();
return m_controller.getError();
}

/**
Expand All @@ -339,7 +339,7 @@ public double getPositionError() {
* @return The change in error per second.
*/
public double getVelocityError() {
return m_controller.getVelocityError();
return m_controller.getErrorDerivative();
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,28 +36,28 @@ void absoluteToleranceTest() {
assertFalse(
controller.atSetpoint(),
"Error was in tolerance when it should not have been. Error was "
+ controller.getPositionError());
+ controller.getError());

controller.calculate(0.0);

assertFalse(
controller.atSetpoint(),
"Error was in tolerance when it should not have been. Error was "
+ controller.getPositionError());
+ controller.getError());

controller.calculate(kSetpoint + kTolerance / 2);

assertTrue(
controller.atSetpoint(),
"Error was not in tolerance when it should have been. Error was "
+ controller.getPositionError());
+ controller.getError());

controller.calculate(kSetpoint + 10 * kTolerance);

assertFalse(
controller.atSetpoint(),
"Error was in tolerance when it should not have been. Error was "
+ controller.getPositionError());
+ controller.getError());
}
}
}
Loading