Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +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