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: Add IZone (#5315)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Gold856
2023-06-20 02:01:01 -04:00
committed by GitHub
parent f5b0d1484b
commit 991f4b0f62
7 changed files with 158 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,6 +24,9 @@ public class PIDController implements Sendable, AutoCloseable {
// Factor for "derivative" control
private double m_kd;
// The error range where "integral" control applies
private double m_iZone = Double.POSITIVE_INFINITY;
// The period (in seconds) of the loop that calls the controller
private final double m_period;
@@ -141,6 +144,22 @@ public class PIDController implements Sendable, AutoCloseable {
m_kd = kd;
}
/**
* Sets the IZone range. When the absolute value of the position error is greater than IZone, the
* total accumulated error will reset to zero, disabling integral gain until the absolute value of
* the position error is less than IZone. This is used to prevent integral windup. Must be
* non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
* of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
*
* @param iZone Maximum magnitude of error to allow integral control.
*/
public void setIZone(double iZone) {
if (iZone < 0) {
throw new IllegalArgumentException("IZone must be a non-negative number!");
}
m_iZone = iZone;
}
/**
* Get the Proportional coefficient.
*
@@ -168,6 +187,15 @@ public class PIDController implements Sendable, AutoCloseable {
return m_kd;
}
/**
* Get the IZone range.
*
* @return Maximum magnitude of error to allow integral control.
*/
public double getIZone() {
return m_iZone;
}
/**
* Returns the period of this controller.
*
@@ -351,7 +379,10 @@ public class PIDController implements Sendable, AutoCloseable {
m_velocityError = (m_positionError - m_prevError) / m_period;
if (m_ki != 0) {
// If the absolute value of the position error is greater than IZone, reset the total error
if (Math.abs(m_positionError) > m_iZone) {
m_totalError = 0;
} else if (m_ki != 0) {
m_totalError =
MathUtil.clamp(
m_totalError + m_positionError * m_period,
@@ -377,6 +408,7 @@ public class PIDController implements Sendable, AutoCloseable {
builder.addDoubleProperty("p", this::getP, this::setP);
builder.addDoubleProperty("i", this::getI, this::setI);
builder.addDoubleProperty("d", this::getD, this::setD);
builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
}
}

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

@@ -98,6 +98,19 @@ public class ProfiledPIDController implements Sendable {
m_controller.setD(Kd);
}
/**
* Sets the IZone range. When the absolute value of the position error is greater than IZone, the
* total accumulated error will reset to zero, disabling integral gain until the absolute value of
* the position error is less than IZone. This is used to prevent integral windup. Must be
* non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
* of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
*
* @param iZone Maximum magnitude of error to allow integral control.
*/
public void setIZone(double iZone) {
m_controller.setIZone(iZone);
}
/**
* Gets the proportional coefficient.
*
@@ -125,6 +138,15 @@ public class ProfiledPIDController implements Sendable {
return m_controller.getD();
}
/**
* Get the IZone range.
*
* @return Maximum magnitude of error to allow integral control.
*/
public double getIZone() {
return m_controller.getIZone();
}
/**
* Gets the period of this controller.
*
@@ -400,6 +422,7 @@ public class ProfiledPIDController implements Sendable {
builder.addDoubleProperty("p", this::getP, this::setP);
builder.addDoubleProperty("i", this::getI, this::setI);
builder.addDoubleProperty("d", this::getD, this::setD);
builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
}
}