Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +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

97
wpilibc/src/main/native/cpp/controller/PIDController.cpp
View File

@@ -18,11 +18,7 @@ using namespace frc2;
PIDController::PIDController(double Kp, double Ki, double Kd,
units::second_t period)
: frc::SendableBase(false),
m_Kp(Kp),
m_Ki(Ki),
m_Kd(Kd),
m_period(period.to<double>()) {
: frc::SendableBase(false), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
static int instances = 0;
instances++;
HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
@@ -45,8 +41,6 @@ units::second_t PIDController::GetPeriod() const {
return units::second_t(m_period);
}
double PIDController::GetOutput() const { return m_output; }
void PIDController::SetSetpoint(double setpoint) {
if (m_maximumInput > m_minimumInput) {
m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
@@ -57,25 +51,20 @@ void PIDController::SetSetpoint(double setpoint) {
double PIDController::GetSetpoint() const { return m_setpoint; }
bool PIDController::AtSetpoint(double tolerance, double deltaTolerance,
bool PIDController::AtSetpoint(double positionTolerance,
double velocityTolerance,
Tolerance toleranceType) const {
double deltaError = GetDeltaError();
if (toleranceType == Tolerance::kPercent) {
return std::abs(m_currError) < tolerance / 100 * m_inputRange &&
std::abs(deltaError) < deltaTolerance / 100 * m_inputRange;
if (m_toleranceType == Tolerance::kPercent) {
return std::abs(m_positionError) < positionTolerance / 100 * m_inputRange &&
std::abs(m_velocityError) < velocityTolerance / 100 * m_inputRange;
} else {
return std::abs(m_currError) < tolerance &&
std::abs(deltaError) < deltaTolerance;
return std::abs(m_positionError) < positionTolerance &&
std::abs(m_velocityError) < velocityTolerance;
}
}
bool PIDController::AtSetpoint() const {
return AtSetpoint(m_tolerance, m_deltaTolerance, m_toleranceType);
}
void PIDController::SetContinuous(bool continuous) {
m_continuous = continuous;
return AtSetpoint(m_positionTolerance, m_velocityTolerance);
}
void PIDController::SetInputRange(double minimumInput, double maximumInput) {
@@ -89,36 +78,53 @@ void PIDController::SetInputRange(double minimumInput, double maximumInput) {
}
}
void PIDController::EnableContinuousInput(double minimumInput,
double maximumInput) {
m_continuous = true;
SetInputRange(minimumInput, maximumInput);
}
void PIDController::DisableContinuousInput() { m_continuous = false; }
void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) {
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
void PIDController::SetAbsoluteTolerance(double tolerance,
double deltaTolerance) {
void PIDController::SetAbsoluteTolerance(double positionTolerance,
double velocityTolerance) {
m_toleranceType = Tolerance::kAbsolute;
m_tolerance = tolerance;
m_deltaTolerance = deltaTolerance;
m_positionTolerance = positionTolerance;
m_velocityTolerance = velocityTolerance;
}
void PIDController::SetPercentTolerance(double tolerance,
double deltaTolerance) {
void PIDController::SetPercentTolerance(double positionTolerance,
double velocityTolerance) {
m_toleranceType = Tolerance::kPercent;
m_tolerance = tolerance;
m_deltaTolerance = deltaTolerance;
m_positionTolerance = positionTolerance;
m_velocityTolerance = velocityTolerance;
}
double PIDController::GetError() const {
return GetContinuousError(m_currError);
double PIDController::GetPositionError() const {
return GetContinuousError(m_positionError);
}
/**
* Returns the change in error per second of the PIDController.
*
* @return The change in error per second.
*/
double PIDController::GetDeltaError() const {
return (m_currError - m_prevError) / m_period;
double PIDController::GetVelocityError() const { return m_velocityError; }
double PIDController::Calculate(double measurement) {
m_prevError = m_positionError;
m_positionError = GetContinuousError(m_setpoint - measurement);
m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
if (m_Ki != 0) {
m_totalError =
std::clamp(m_totalError + m_positionError * m_period.to<double>(),
m_minimumOutput / m_Ki, m_maximumOutput / m_Ki);
}
return std::clamp(
m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError,
m_minimumOutput, m_maximumOutput);
}
double PIDController::Calculate(double measurement, double setpoint) {
@@ -130,7 +136,6 @@ double PIDController::Calculate(double measurement, double setpoint) {
void PIDController::Reset() {
m_prevError = 0;
m_totalError = 0;
m_output = 0;
}
void PIDController::InitSendable(frc::SendableBuilder& builder) {
@@ -159,19 +164,3 @@ double PIDController::GetContinuousError(double error) const {
return error;
}
double PIDController::Calculate(double measurement) {
m_prevError = m_currError;
m_currError = GetContinuousError(m_setpoint - measurement);
if (m_Ki != 0) {
m_totalError = std::clamp(m_totalError + m_currError * m_period,
m_minimumOutput / m_Ki, m_maximumOutput / m_Ki);
}
m_output = std::clamp(m_Kp * m_currError + m_Ki * m_totalError +
m_Kd * (m_currError - m_prevError) / m_period,
m_minimumOutput, m_maximumOutput);
return m_output;
}

101
wpilibc/src/main/native/include/frc/controller/PIDController.h
View File

@@ -30,7 +30,7 @@ class PIDController : public frc::SendableBase {
* @param Ki The integral coefficient.
* @param Kd The derivative coefficient.
* @param period The period between controller updates in seconds. The
* default is 0.02 seconds.
* default is 20 milliseconds.
*/
PIDController(double Kp, double Ki, double Kd,
units::second_t period = 20_ms);
@@ -102,16 +102,6 @@ class PIDController : public frc::SendableBase {
*/
units::second_t GetPeriod() const;
/**
* Returns the current controller output.
*
* This is always centered around zero and constrained to the min and max
* outputs.
*
* @return The latest calculated output.
*/
double GetOutput() const;
/**
* Sets the setpoint for the PIDController.
*
@@ -131,37 +121,22 @@ class PIDController : public frc::SendableBase {
*
* This will return false until at least one input value has been computed.
*
* @param tolerance The maximum allowable error.
* @param deltaTolerance The maximum allowable change in error.
* @param toleranceType The type of tolerances specified.
* @param positionTolerance The maximum allowable position error.
* @param velocityTolerance The maximum allowable velocity error.
* @param toleranceType The type of tolerance specified.
*/
bool AtSetpoint(
double tolerance,
double deltaTolerance = std::numeric_limits<double>::infinity(),
double positionTolerance,
double velocityTolerance = std::numeric_limits<double>::infinity(),
Tolerance toleranceType = Tolerance::kAbsolute) const;
/**
* Returns true if the error is within the tolerance of the error.
*
* Currently this just reports on target as the actual value passes through
* the setpoint. Ideally it should be based on being within the tolerance for
* some period of time.
*
* This will return false until at least one input value has been computed.
*/
bool AtSetpoint() const;
/**
* Sets the PID controller to consider the input to be continuous.
*
* 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
*/
void SetContinuous(bool continuous = true);
/**
* Sets the minimum and maximum values expected from the input.
*
@@ -170,6 +145,23 @@ class PIDController : public frc::SendableBase {
*/
void SetInputRange(double minimumInput, double maximumInput);
/**
* Enables continuous input.
*
* 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.
*/
void EnableContinuousInput(double minimumInput, double maximumInput);
/**
* Disables continuous input.
*/
void DisableContinuousInput();
/**
* Sets the minimum and maximum values to write.
*
@@ -182,36 +174,33 @@ class PIDController : public frc::SendableBase {
* Sets the absolute error which is considered tolerable for use with
* AtSetpoint().
*
* @param tolerance Error which is tolerable.
* @param deltaTolerance Change in error per second which is tolerable.
* @param positionTolerance Position error which is tolerable.
* @param velociytTolerance Velocity error which is tolerable.
*/
void SetAbsoluteTolerance(
double tolerance,
double deltaTolerance = std::numeric_limits<double>::infinity());
double positionTolerance,
double velocityTolerance = std::numeric_limits<double>::infinity());
/**
* Sets the percentage error which is considered tolerable for use with
* 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 velociytTolerance Velocity error which is tolerable.
*/
void SetPercentTolerance(
double tolerance,
double deltaTolerance = std::numeric_limits<double>::infinity());
double positionTolerance,
double velocityTolerance = std::numeric_limits<double>::infinity());
/**
* Returns the difference between the setpoint and the measurement.
*
* @return The error.
*/
double GetError() const;
double GetPositionError() const;
/**
* Returns the change in error per second.
* Returns the velocity error.
*/
double GetDeltaError() const;
double GetVelocityError() const;
/**
* Returns the next output of the PID controller.
@@ -256,7 +245,7 @@ class PIDController : public frc::SendableBase {
double m_Kd;
// The period (in seconds) of the control loop running this controller
double m_period;
units::second_t m_period;
// |maximum output|
double m_maximumOutput = 1.0;
@@ -270,30 +259,30 @@ class PIDController : public frc::SendableBase {
// Minimum input - limit setpoint to this
double m_minimumInput = 0;
// input range - difference between maximum and minimum
// Input range - difference between maximum and minimum
double m_inputRange = 0;
// Do the endpoints wrap around? eg. Absolute encoder
bool m_continuous = false;
// The error at the time of the most recent call to calculate()
double m_currError = 0;
// The error at the time of the most recent call to Calculate()
double m_positionError = 0;
double m_velocityError = 0;
// The error at the time of the second-most-recent call to calculate() (used
// The error at the time of the second-most-recent call to Calculate() (used
// to compute velocity)
double m_prevError = std::numeric_limits<double>::infinity();
double m_prevError = 0;
// The sum of the errors for use in the integral calc
double m_totalError = 0;
Tolerance m_toleranceType = Tolerance::kAbsolute;
// The percentage or absolute error that is considered at setpoint.
double m_tolerance = 0.05;
double m_deltaTolerance = std::numeric_limits<double>::infinity();
// The error that is considered at setpoint.
double m_positionTolerance = 0.05;
double m_velocityTolerance = std::numeric_limits<double>::infinity();
double m_setpoint = 0;
double m_output = 0;
};
} // namespace frc2

5
wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp
View File

@@ -36,8 +36,7 @@ TEST_F(PIDInputOutputTest, InputRangeTest) {
TEST_F(PIDInputOutputTest, ContinuousInputTest) {
controller->SetP(1);
controller->SetInputRange(-180, 180);
controller->SetContinuous(true);
controller->EnableContinuousInput(-180, 180);
EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
}
@@ -65,6 +64,6 @@ TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
controller->Calculate(0, 0);
EXPECT_DOUBLE_EQ(-.01 / controller->GetPeriod().to<double>(),
EXPECT_DOUBLE_EQ(-.01_s / controller->GetPeriod(),
controller->Calculate(.0025, 0));
}

80
wpilibc/src/test/native/cpp/PIDToleranceTest.cpp
View File

@@ -5,105 +5,71 @@
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/Timer.h"
#include "frc/controller/PIDController.h"
#include "gtest/gtest.h"
using namespace frc;
class PIDToleranceTest : public testing::Test {
protected:
const double setpoint = 50.0;
const double range = 200;
const double tolerance = 10.0;
static constexpr double kSetpoint = 50.0;
static constexpr double kRange = 200;
static constexpr double kTolerance = 10.0;
class FakeInput {
public:
double val = 0;
double Get() { return val; }
};
class FakeOutput {
public:
void Set(double output) {}
};
FakeInput inp;
FakeOutput out;
frc2::PIDController* pidController;
void SetUp() override {
pidController = new frc2::PIDController(0.5, 0.0, 0.0);
pidController->SetInputRange(-range / 2, range / 2);
pidController->SetInputRange(-kRange / 2, kRange / 2);
}
void TearDown() override { delete pidController; }
void Reset() { inp.val = 0; }
};
TEST_F(PIDToleranceTest, Initial) { EXPECT_FALSE(pidController->AtSetpoint()); }
TEST_F(PIDToleranceTest, Initial) { EXPECT_TRUE(pidController->AtSetpoint()); }
TEST_F(PIDToleranceTest, Absolute) {
Reset();
pidController->SetAbsoluteTolerance(kTolerance);
pidController->SetSetpoint(kSetpoint);
pidController->SetAbsoluteTolerance(tolerance);
pidController->SetSetpoint(setpoint);
pidController->Calculate(0.0);
EXPECT_FALSE(pidController->AtSetpoint())
<< "Error was in tolerance when it should not have been. Error was "
<< pidController->GetError();
<< pidController->GetPositionError();
inp.val = setpoint + tolerance / 2;
for (int i = 0; i <= 50; i++) {
pidController->Calculate(inp.Get());
}
pidController->Calculate(kSetpoint + kTolerance / 2);
EXPECT_TRUE(pidController->AtSetpoint())
<< "Error was not in tolerance when it should have been. Error was "
<< pidController->GetError();
<< pidController->GetPositionError();
inp.val = setpoint + 10 * tolerance;
for (int i = 0; i <= 50; i++) {
pidController->Calculate(inp.Get());
}
pidController->Calculate(kSetpoint + 10 * kTolerance);
EXPECT_FALSE(pidController->AtSetpoint())
<< "Error was in tolerance when it should not have been. Error was "
<< pidController->GetError();
<< pidController->GetPositionError();
}
TEST_F(PIDToleranceTest, Percent) {
Reset();
pidController->SetPercentTolerance(kTolerance);
pidController->SetSetpoint(kSetpoint);
pidController->SetPercentTolerance(tolerance);
pidController->SetSetpoint(setpoint);
pidController->Calculate(0.0);
EXPECT_FALSE(pidController->AtSetpoint())
<< "Error was in tolerance when it should not have been. Error was "
<< pidController->GetError();
<< pidController->GetPositionError();
inp.val =
setpoint + (tolerance) / 200 *
range; // half of percent tolerance away from setpoint
for (int i = 0; i <= 50; i++) {
pidController->Calculate(inp.Get());
}
// Half of percent tolerance away from setpoint
pidController->Calculate(kSetpoint + (kTolerance / 2) / 100 * kRange);
EXPECT_TRUE(pidController->AtSetpoint())
<< "Error was not in tolerance when it should have been. Error was "
<< pidController->GetError();
<< pidController->GetPositionError();
inp.val =
setpoint +
(tolerance) / 50 * range; // double percent tolerance away from setPoint
for (int i = 0; i <= 50; i++) {
pidController->Calculate(inp.Get());
}
// Double percent tolerance away from setpoint
pidController->Calculate(kSetpoint + (kTolerance * 2) / 100 * kRange);
EXPECT_FALSE(pidController->AtSetpoint())
<< "Error was in tolerance when it should not have been. Error was "
<< pidController->GetError();
<< pidController->GetPositionError();
}

8
wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
View File

@@ -152,11 +152,11 @@ TEST_P(MotorEncoderTest, PositionPIDController) {
Wait(10.0);
pidRunner.Stop();
RecordProperty("PIDError", pidController.GetError());
RecordProperty("PIDError", pidController.GetPositionError());
EXPECT_TRUE(pidController.AtSetpoint())
<< "PID loop did not converge within 10 seconds. Goal was: " << goal
<< " Error was: " << pidController.GetError();
<< " Error was: " << pidController.GetPositionError();
}
/**
@@ -179,11 +179,11 @@ TEST_P(MotorEncoderTest, VelocityPIDController) {
pidRunner.StartPeriodic(pidController.GetPeriod());
Wait(10.0);
pidRunner.Stop();
RecordProperty("PIDError", pidController.GetError());
RecordProperty("PIDError", pidController.GetPositionError());
EXPECT_TRUE(pidController.AtSetpoint())
<< "PID loop did not converge within 10 seconds. Goal was: " << 600
<< " Error was: " << pidController.GetError();
<< " Error was: " << pidController.GetPositionError();
}
/**

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());
}
}

4
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
View File

@@ -190,7 +190,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
assertTrue(
"PID loop did not reach reference within 10 seconds. The current error was" + pidController
.getError(), pidController.atSetpoint());
.getPositionError(), pidController.atSetpoint());
pidController.close();
}
@@ -211,7 +211,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
pidRunner.stop();
assertTrue("PID loop did not reach reference within 10 seconds. The error was: " + pidController
.getError(), pidController.atSetpoint());
.getPositionError(), pidController.atSetpoint());
pidController.close();
}

12
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
View File

@@ -131,8 +131,8 @@ public class PIDTest extends AbstractComsSetup {
setupOutputRange();
double reference = 2500.0;
m_controller.setSetpoint(reference);
assertEquals("PID.getError() did not start at " + reference, reference, m_controller.getError(),
0);
assertEquals("PID.getPositionError() did not start at " + reference,
reference, m_controller.getPositionError(), 0);
m_builder.updateTable();
assertEquals(k_p, m_table.getEntry("Kp").getDouble(9999999), 0);
assertEquals(k_i, m_table.getEntry("Ki").getDouble(9999999), 0);
@@ -155,17 +155,17 @@ public class PIDTest extends AbstractComsSetup {
setupAbsoluteTolerance();
setupOutputRange();
double reference = 1000.0;
assertEquals(pidData() + "did not start at 0", 0, m_controller.getOutput(), 0);
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.getError(), 0);
m_controller.getPositionError(), 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.getError(),
m_controller.atSetpoint());
assertTrue(pidData() + "Was not on Target. Controller Error: "
+ m_controller.getPositionError(), m_controller.atSetpoint());
}
private String pidData() {