Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] PIDController: Update field and method names for error and errorDerivative (#7088)

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-09-23 13:57:20 -04:00
committed by GitHub
parent 64e5e6db59
commit 6281ec0810
9 changed files with 192 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
View File

@@ -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;
@@ -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;
@@ -227,18 +227,40 @@ public class PIDController implements Sendable, AutoCloseable {
* Returns the position tolerance of this controller.
*
* @return the position tolerance of the controller.
* @deprecated Use getErrorTolerance() instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public double getPositionTolerance() {
return m_positionTolerance;
return m_errorTolerance;
}
/**
* Returns the velocity tolerance of this controller.
*
* @return the velocity tolerance of the controller.
* @deprecated Use getErrorDerivativeTolerance() instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public double getVelocityTolerance() {
return m_velocityTolerance;
return m_errorDerivativeTolerance;
}
/**
* 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;
}
/**
@@ -261,12 +283,12 @@ public class PIDController implements Sendable, AutoCloseable {
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;
}
/**
@@ -288,8 +310,8 @@ public class PIDController implements Sendable, AutoCloseable {
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;
}
/**
@@ -338,21 +360,43 @@ public class PIDController implements Sendable, AutoCloseable {
/**
* 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 Use getError() instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public double getPositionError() {
return m_error;
}
/**
* Returns the velocity error.
*
* @return The velocity error.
* @deprecated Use getErrorDerivative() instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public double getVelocityError() {
return m_errorDerivative;
}
/**
@@ -360,17 +404,17 @@ public class PIDController implements Sendable, AutoCloseable {
*
* @return The error.
*/
public double getPositionError() {
return m_positionError;
public double getError() {
return m_error;
}
/**
* Returns the velocity error.
* Returns the error derivative.
*
* @return The velocity error.
* @return The error derivative.
*/
public double getVelocityError() {
return m_velocityError;
public double getErrorDerivative() {
return m_errorDerivative;
}
/**
@@ -394,38 +438,38 @@ public class PIDController implements Sendable, AutoCloseable {
*/
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;
}
@@ -446,5 +490,10 @@ public class PIDController implements Sendable, AutoCloseable {
}
});
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);
}
}

8
wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
View File

@@ -174,7 +174,7 @@ public class ProfiledPIDController implements Sendable {
* @return the position tolerance of the controller.
*/
public double getPositionTolerance() {
return m_controller.getPositionTolerance();
return m_controller.getErrorTolerance();
}
/**
@@ -183,7 +183,7 @@ public class ProfiledPIDController implements Sendable {
* @return the velocity tolerance of the controller.
*/
public double getVelocityTolerance() {
return m_controller.getVelocityTolerance();
return m_controller.getErrorDerivativeTolerance();
}
/**
@@ -330,7 +330,7 @@ public class ProfiledPIDController implements Sendable {
* @return The error.
*/
public double getPositionError() {
return m_controller.getPositionError();
return m_controller.getError();
}
/**
@@ -339,7 +339,7 @@ public class ProfiledPIDController implements Sendable {
* @return The change in error per second.
*/
public double getVelocityError() {
return m_controller.getVelocityError();
return m_controller.getErrorDerivative();
}
/**