diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp new file mode 100644 index 0000000000..ce7f799bd3 --- /dev/null +++ b/wpilibc/src/main/native/cpp/PIDBase.cpp @@ -0,0 +1,566 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "PIDBase.h" + +#include +#include + +#include + +#include "PIDOutput.h" +#include "SmartDashboard/SendableBuilder.h" + +using namespace frc; + +template +constexpr const T& clamp(const T& value, const T& low, const T& high) { + return std::max(low, std::min(value, high)); +} + +/** + * Allocate a PID object with the given constants for P, I, D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param source The PIDSource object that is used to get values + * @param output The PIDOutput object that is set to the output value + */ +PIDBase::PIDBase(double Kp, double Ki, double Kd, PIDSource& source, + PIDOutput& output) + : PIDBase(Kp, Ki, Kd, 0.0, source, output) {} + +/** + * Allocate a PID object with the given constants for P, I, D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param source The PIDSource object that is used to get values + * @param output The PIDOutput object that is set to the output value + */ +PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source, + PIDOutput& output) + : SendableBase(false) { + m_P = Kp; + m_I = Ki; + m_D = Kd; + m_F = Kf; + + // Save original source + m_origSource = std::shared_ptr(&source, NullDeleter()); + + // Create LinearDigitalFilter with original source as its source argument + m_filter = LinearDigitalFilter::MovingAverage(m_origSource, 1); + m_pidInput = &m_filter; + + m_pidOutput = &output; + + m_setpointTimer.Start(); + + static int instances = 0; + instances++; + HAL_Report(HALUsageReporting::kResourceType_PIDController, instances); + SetName("PIDController", instances); +} + +/** + * Read the input, calculate the output accordingly, and write to the output. + * This should only be called by the Notifier. + */ +void PIDBase::Calculate() { + if (m_origSource == nullptr || m_pidOutput == nullptr) return; + + bool enabled; + { + std::lock_guard lock(m_thisMutex); + enabled = m_enabled; + } + + if (enabled) { + double input; + + // Storage for function inputs + PIDSourceType pidSourceType; + double P; + double I; + double D; + double feedForward = CalculateFeedForward(); + double minimumOutput; + double maximumOutput; + + // Storage for function input-outputs + double prevError; + double error; + double totalError; + + { + std::lock_guard lock(m_thisMutex); + + input = m_pidInput->PIDGet(); + + pidSourceType = m_pidInput->GetPIDSourceType(); + P = m_P; + I = m_I; + D = m_D; + minimumOutput = m_minimumOutput; + maximumOutput = m_maximumOutput; + + prevError = m_prevError; + error = GetContinuousError(m_setpoint - input); + totalError = m_totalError; + } + + // Storage for function outputs + double result; + + if (pidSourceType == PIDSourceType::kRate) { + if (P != 0) { + totalError = + clamp(totalError + error, minimumOutput / P, maximumOutput / P); + } + + result = D * error + P * totalError + feedForward; + } else { + if (I != 0) { + totalError = + clamp(totalError + error, minimumOutput / I, maximumOutput / I); + } + + result = + P * error + I * totalError + D * (error - prevError) + feedForward; + } + + result = clamp(result, minimumOutput, maximumOutput); + + { + // Ensures m_enabled check and PIDWrite() call occur atomically + std::lock_guard pidWriteLock(m_pidWriteMutex); + std::unique_lock mainLock(m_thisMutex); + if (m_enabled) { + // Don't block other PIDBase operations on PIDWrite() + mainLock.unlock(); + + m_pidOutput->PIDWrite(result); + } + } + + std::lock_guard lock(m_thisMutex); + m_prevError = m_error; + m_error = error; + m_totalError = totalError; + m_result = result; + } +} + +/** + * Calculate the feed forward term. + * + * Both of the provided feed forward calculations are velocity feed forwards. + * If a different feed forward calculation is desired, the user can override + * this function and provide his or her own. This function does no + * synchronization because the PIDBase class only calls it in synchronized + * code, so be careful if calling it oneself. + * + * If a velocity PID controller is being used, the F term should be set to 1 + * over the maximum setpoint for the output. If a position PID controller is + * being used, the F term should be set to 1 over the maximum speed for the + * output measured in setpoint units per this controller's update period (see + * the default period in this class's constructor). + */ +double PIDBase::CalculateFeedForward() { + if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) { + return m_F * GetSetpoint(); + } else { + double temp = m_F * GetDeltaSetpoint(); + m_prevSetpoint = m_setpoint; + m_setpointTimer.Reset(); + return temp; + } +} + +/** + * Set the PID Controller gain parameters. + * + * Set the proportional, integral, and differential coefficients. + * + * @param p Proportional coefficient + * @param i Integral coefficient + * @param d Differential coefficient + */ +void PIDBase::SetPID(double p, double i, double d) { + { + std::lock_guard lock(m_thisMutex); + m_P = p; + m_I = i; + m_D = d; + } +} + +/** + * Set the PID Controller gain parameters. + * + * Set the proportional, integral, and differential coefficients. + * + * @param p Proportional coefficient + * @param i Integral coefficient + * @param d Differential coefficient + * @param f Feed forward coefficient + */ +void PIDBase::SetPID(double p, double i, double d, double f) { + std::lock_guard lock(m_thisMutex); + m_P = p; + m_I = i; + m_D = d; + m_F = f; +} + +/** + * Set the Proportional coefficient of the PID controller gain. + * + * @param p proportional coefficient + */ +void PIDBase::SetP(double p) { + std::lock_guard lock(m_thisMutex); + m_P = p; +} + +/** + * Set the Integral coefficient of the PID controller gain. + * + * @param i integral coefficient + */ +void PIDBase::SetI(double i) { + std::lock_guard lock(m_thisMutex); + m_I = i; +} + +/** + * Set the Differential coefficient of the PID controller gain. + * + * @param d differential coefficient + */ +void PIDBase::SetD(double d) { + std::lock_guard lock(m_thisMutex); + m_D = d; +} + +/** + * Get the Feed forward coefficient of the PID controller gain. + * + * @param f Feed forward coefficient + */ +void PIDBase::SetF(double f) { + std::lock_guard lock(m_thisMutex); + m_F = f; +} + +/** + * Get the Proportional coefficient. + * + * @return proportional coefficient + */ +double PIDBase::GetP() const { + std::lock_guard lock(m_thisMutex); + return m_P; +} + +/** + * Get the Integral coefficient. + * + * @return integral coefficient + */ +double PIDBase::GetI() const { + std::lock_guard lock(m_thisMutex); + return m_I; +} + +/** + * Get the Differential coefficient. + * + * @return differential coefficient + */ +double PIDBase::GetD() const { + std::lock_guard lock(m_thisMutex); + return m_D; +} + +/** + * Get the Feed forward coefficient. + * + * @return Feed forward coefficient + */ +double PIDBase::GetF() const { + std::lock_guard lock(m_thisMutex); + return m_F; +} + +/** + * Return the current PID result. + * + * This is always centered on zero and constrained the the max and min outs. + * + * @return the latest calculated output + */ +double PIDBase::Get() const { + std::lock_guard lock(m_thisMutex); + return m_result; +} + +/** + * Set 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 PIDBase::SetContinuous(bool continuous) { + std::lock_guard lock(m_thisMutex); + m_continuous = continuous; +} + +/** + * Sets the maximum and minimum values expected from the input. + * + * @param minimumInput the minimum value expected from the input + * @param maximumInput the maximum value expected from the output + */ +void PIDBase::SetInputRange(double minimumInput, double maximumInput) { + { + std::lock_guard lock(m_thisMutex); + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + m_inputRange = maximumInput - minimumInput; + } + + SetSetpoint(m_setpoint); +} + +/** + * Sets the minimum and maximum values to write. + * + * @param minimumOutput the minimum value to write to the output + * @param maximumOutput the maximum value to write to the output + */ +void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) { + std::lock_guard lock(m_thisMutex); + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; +} + +/** + * Set the setpoint for the PIDBase. + * + * @param setpoint the desired setpoint + */ +void PIDBase::SetSetpoint(double setpoint) { + { + std::lock_guard lock(m_thisMutex); + + if (m_maximumInput > m_minimumInput) { + if (setpoint > m_maximumInput) + m_setpoint = m_maximumInput; + else if (setpoint < m_minimumInput) + m_setpoint = m_minimumInput; + else + m_setpoint = setpoint; + } else { + m_setpoint = setpoint; + } + } +} + +/** + * Returns the current setpoint of the PIDBase. + * + * @return the current setpoint + */ +double PIDBase::GetSetpoint() const { + std::lock_guard lock(m_thisMutex); + return m_setpoint; +} + +/** + * Returns the change in setpoint over time of the PIDBase. + * + * @return the change in setpoint over time + */ +double PIDBase::GetDeltaSetpoint() const { + std::lock_guard lock(m_thisMutex); + return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get(); +} + +/** + * Returns the current difference of the input from the setpoint. + * + * @return the current error + */ +double PIDBase::GetError() const { + double setpoint = GetSetpoint(); + { + std::lock_guard lock(m_thisMutex); + return GetContinuousError(setpoint - m_pidInput->PIDGet()); + } +} + +/** + * Returns the current average of the error over the past few iterations. + * + * You can specify the number of iterations to average with SetToleranceBuffer() + * (defaults to 1). This is the same value that is used for OnTarget(). + * + * @return the average error + */ +double PIDBase::GetAvgError() const { return GetError(); } + +/** + * Sets what type of input the PID controller will use. + */ +void PIDBase::SetPIDSourceType(PIDSourceType pidSource) { + m_pidInput->SetPIDSourceType(pidSource); +} +/** + * Returns the type of input the PID controller is using. + * + * @return the PID controller input type + */ +PIDSourceType PIDBase::GetPIDSourceType() const { + return m_pidInput->GetPIDSourceType(); +} + +/* + * Set the percentage error which is considered tolerable for use with + * OnTarget. + * + * @param percentage error which is tolerable + */ +void PIDBase::SetTolerance(double percent) { + std::lock_guard lock(m_thisMutex); + m_toleranceType = kPercentTolerance; + m_tolerance = percent; +} + +/* + * Set the absolute error which is considered tolerable for use with + * OnTarget. + * + * @param percentage error which is tolerable + */ +void PIDBase::SetAbsoluteTolerance(double absTolerance) { + std::lock_guard lock(m_thisMutex); + m_toleranceType = kAbsoluteTolerance; + m_tolerance = absTolerance; +} + +/* + * Set the percentage error which is considered tolerable for use with + * OnTarget. + * + * @param percentage error which is tolerable + */ +void PIDBase::SetPercentTolerance(double percent) { + std::lock_guard lock(m_thisMutex); + m_toleranceType = kPercentTolerance; + m_tolerance = percent; +} + +/* + * Set the number of previous error samples to average for tolerancing. When + * determining whether a mechanism is on target, the user may want to use a + * rolling average of previous measurements instead of a precise position or + * velocity. This is useful for noisy sensors which return a few erroneous + * measurements when the mechanism is on target. However, the mechanism will + * not register as on target for at least the specified bufLength cycles. + * + * @param bufLength Number of previous cycles to average. Defaults to 1. + */ +void PIDBase::SetToleranceBuffer(int bufLength) { + std::lock_guard lock(m_thisMutex); + + // Create LinearDigitalFilter with original source as its source argument + m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength); + m_pidInput = &m_filter; +} + +/* + * Return true if the error is within the percentage of the total input range, + * determined by SetTolerance. This asssumes that the maximum and minimum input + * were set using SetInput. + * + * 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 PIDBase::OnTarget() const { + double error = GetError(); + + std::lock_guard lock(m_thisMutex); + switch (m_toleranceType) { + case kPercentTolerance: + return std::fabs(error) < m_tolerance / 100 * m_inputRange; + break; + case kAbsoluteTolerance: + return std::fabs(error) < m_tolerance; + break; + case kNoTolerance: + // TODO: this case needs an error + return false; + } + return false; +} + +/** + * Reset the previous error, the integral term, and disable the controller. + */ +void PIDBase::Reset() { + std::lock_guard lock(m_thisMutex); + m_prevError = 0; + m_totalError = 0; + m_result = 0; +} + +void PIDBase::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("PIDBase"); + builder.SetSafeState([=]() { Reset(); }); + builder.AddDoubleProperty("p", [=]() { return GetP(); }, + [=](double value) { SetP(value); }); + builder.AddDoubleProperty("i", [=]() { return GetI(); }, + [=](double value) { SetI(value); }); + builder.AddDoubleProperty("d", [=]() { return GetD(); }, + [=](double value) { SetD(value); }); + builder.AddDoubleProperty("f", [=]() { return GetF(); }, + [=](double value) { SetF(value); }); + builder.AddDoubleProperty("setpoint", [=]() { return GetSetpoint(); }, + [=](double value) { SetSetpoint(value); }); +} + +/** + * Wraps error around for continuous inputs. The original error is returned if + * continuous mode is disabled. This is an unsynchronized function. + * + * @param error The current error of the PID controller. + * @return Error for continuous inputs. + */ +double PIDBase::GetContinuousError(double error) const { + if (m_continuous && m_inputRange != 0) { + error = std::fmod(error, m_inputRange); + if (std::fabs(error) > m_inputRange / 2) { + if (error > 0) { + return error - m_inputRange; + } else { + return error + m_inputRange; + } + } + } + + return error; +} diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp index 1e38e221b4..ce395e1113 100644 --- a/wpilibc/src/main/native/cpp/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/PIDController.cpp @@ -7,24 +7,12 @@ #include "PIDController.h" -#include -#include -#include - -#include - #include "Notifier.h" #include "PIDOutput.h" -#include "PIDSource.h" #include "SmartDashboard/SendableBuilder.h" using namespace frc; -template -constexpr const T& clamp(const T& value, const T& low, const T& high) { - return std::max(low, std::min(value, high)); -} - /** * Allocate a PID object with the given constants for P, I, D. * @@ -39,7 +27,7 @@ constexpr const T& clamp(const T& value, const T& low, const T& high) { */ PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source, PIDOutput* output, double period) - : PIDController(Kp, Ki, Kd, 0.0, source, output, period) {} + : PIDController(Kp, Ki, Kd, 0.0, *source, *output, period) {} /** * Allocate a PID object with the given constants for P, I, D. @@ -56,32 +44,7 @@ PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source, PIDController::PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource* source, PIDOutput* output, double period) - : SendableBase(false) { - m_controlLoop = std::make_unique(&PIDController::Calculate, this); - - m_P = Kp; - m_I = Ki; - m_D = Kd; - m_F = Kf; - - // Save original source - m_origSource = std::shared_ptr(source, NullDeleter()); - - // Create LinearDigitalFilter with original source as its source argument - m_filter = LinearDigitalFilter::MovingAverage(m_origSource, 1); - m_pidInput = &m_filter; - - m_pidOutput = output; - m_period = period; - - m_controlLoop->StartPeriodic(m_period); - m_setpointTimer.Start(); - - static int instances = 0; - instances++; - HAL_Report(HALUsageReporting::kResourceType_PIDController, instances); - SetName("PIDController", instances); -} + : PIDController(Kp, Ki, Kd, Kf, *source, *output, period) {} /** * Allocate a PID object with the given constants for P, I, D. @@ -97,7 +60,7 @@ PIDController::PIDController(double Kp, double Ki, double Kd, double Kf, */ PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource& source, PIDOutput& output, double period) - : PIDController(Kp, Ki, Kd, 0.0, &source, &output, period) {} + : PIDController(Kp, Ki, Kd, 0.0, source, output, period) {} /** * Allocate a PID object with the given constants for P, I, D. @@ -114,462 +77,16 @@ PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource& source, PIDController::PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource& source, PIDOutput& output, double period) - : PIDController(Kp, Ki, Kd, Kf, &source, &output, period) {} + : PIDBase(Kp, Ki, Kd, Kf, source, output) { + m_controlLoop = std::make_unique(&PIDController::Calculate, this); + m_controlLoop->StartPeriodic(period); +} PIDController::~PIDController() { - // forcefully stopping the notifier so the callback can successfully run. + // Forcefully stopping the notifier so the callback can successfully run. m_controlLoop->Stop(); } -/** - * Read the input, calculate the output accordingly, and write to the output. - * This should only be called by the Notifier. - */ -void PIDController::Calculate() { - if (m_origSource == nullptr || m_pidOutput == nullptr) return; - - bool enabled; - { - std::lock_guard lock(m_thisMutex); - enabled = m_enabled; - } - - if (enabled) { - double input; - - // Storage for function inputs - PIDSourceType pidSourceType; - double P; - double I; - double D; - double feedForward = CalculateFeedForward(); - double minimumOutput; - double maximumOutput; - - // Storage for function input-outputs - double prevError; - double error; - double totalError; - - { - std::lock_guard lock(m_thisMutex); - - input = m_pidInput->PIDGet(); - - pidSourceType = m_pidInput->GetPIDSourceType(); - P = m_P; - I = m_I; - D = m_D; - minimumOutput = m_minimumOutput; - maximumOutput = m_maximumOutput; - - prevError = m_prevError; - error = GetContinuousError(m_setpoint - input); - totalError = m_totalError; - } - - // Storage for function outputs - double result; - - if (pidSourceType == PIDSourceType::kRate) { - if (P != 0) { - totalError = - clamp(totalError + error, minimumOutput / P, maximumOutput / P); - } - - result = D * error + P * totalError + feedForward; - } else { - if (I != 0) { - totalError = - clamp(totalError + error, minimumOutput / I, maximumOutput / I); - } - - result = - P * error + I * totalError + D * (error - prevError) + feedForward; - } - - result = clamp(result, minimumOutput, maximumOutput); - - { - // Ensures m_enabled check and PIDWrite() call occur atomically - std::lock_guard pidWriteLock(m_pidWriteMutex); - std::unique_lock mainLock(m_thisMutex); - if (m_enabled) { - // Don't block other PIDController operations on PIDWrite() - mainLock.unlock(); - - m_pidOutput->PIDWrite(result); - } - } - - std::lock_guard lock(m_thisMutex); - m_prevError = m_error; - m_error = error; - m_totalError = totalError; - m_result = result; - } -} - -/** - * Calculate the feed forward term. - * - * Both of the provided feed forward calculations are velocity feed forwards. - * If a different feed forward calculation is desired, the user can override - * this function and provide his or her own. This function does no - * synchronization because the PIDController class only calls it in synchronized - * code, so be careful if calling it oneself. - * - * If a velocity PID controller is being used, the F term should be set to 1 - * over the maximum setpoint for the output. If a position PID controller is - * being used, the F term should be set to 1 over the maximum speed for the - * output measured in setpoint units per this controller's update period (see - * the default period in this class's constructor). - */ -double PIDController::CalculateFeedForward() { - if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) { - return m_F * GetSetpoint(); - } else { - double temp = m_F * GetDeltaSetpoint(); - m_prevSetpoint = m_setpoint; - m_setpointTimer.Reset(); - return temp; - } -} - -/** - * Set the PID Controller gain parameters. - * - * Set the proportional, integral, and differential coefficients. - * - * @param p Proportional coefficient - * @param i Integral coefficient - * @param d Differential coefficient - */ -void PIDController::SetPID(double p, double i, double d) { - { - std::lock_guard lock(m_thisMutex); - m_P = p; - m_I = i; - m_D = d; - } -} - -/** - * Set the PID Controller gain parameters. - * - * Set the proportional, integral, and differential coefficients. - * - * @param p Proportional coefficient - * @param i Integral coefficient - * @param d Differential coefficient - * @param f Feed forward coefficient - */ -void PIDController::SetPID(double p, double i, double d, double f) { - std::lock_guard lock(m_thisMutex); - m_P = p; - m_I = i; - m_D = d; - m_F = f; -} - -/** - * Set the Proportional coefficient of the PID controller gain. - * - * @param p proportional coefficient - */ -void PIDController::SetP(double p) { - std::lock_guard lock(m_thisMutex); - m_P = p; -} - -/** - * Set the Integral coefficient of the PID controller gain. - * - * @param i integral coefficient - */ -void PIDController::SetI(double i) { - std::lock_guard lock(m_thisMutex); - m_I = i; -} - -/** - * Set the Differential coefficient of the PID controller gain. - * - * @param d differential coefficient - */ -void PIDController::SetD(double d) { - std::lock_guard lock(m_thisMutex); - m_D = d; -} - -/** - * Get the Feed forward coefficient of the PID controller gain. - * - * @param f Feed forward coefficient - */ -void PIDController::SetF(double f) { - std::lock_guard lock(m_thisMutex); - m_F = f; -} - -/** - * Get the Proportional coefficient. - * - * @return proportional coefficient - */ -double PIDController::GetP() const { - std::lock_guard lock(m_thisMutex); - return m_P; -} - -/** - * Get the Integral coefficient. - * - * @return integral coefficient - */ -double PIDController::GetI() const { - std::lock_guard lock(m_thisMutex); - return m_I; -} - -/** - * Get the Differential coefficient. - * - * @return differential coefficient - */ -double PIDController::GetD() const { - std::lock_guard lock(m_thisMutex); - return m_D; -} - -/** - * Get the Feed forward coefficient. - * - * @return Feed forward coefficient - */ -double PIDController::GetF() const { - std::lock_guard lock(m_thisMutex); - return m_F; -} - -/** - * Return the current PID result. - * - * This is always centered on zero and constrained the the max and min outs. - * - * @return the latest calculated output - */ -double PIDController::Get() const { - std::lock_guard lock(m_thisMutex); - return m_result; -} - -/** - * Set 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 PIDController::SetContinuous(bool continuous) { - std::lock_guard lock(m_thisMutex); - m_continuous = continuous; -} - -/** - * Sets the maximum and minimum values expected from the input. - * - * @param minimumInput the minimum value expected from the input - * @param maximumInput the maximum value expected from the output - */ -void PIDController::SetInputRange(double minimumInput, double maximumInput) { - { - std::lock_guard lock(m_thisMutex); - m_minimumInput = minimumInput; - m_maximumInput = maximumInput; - m_inputRange = maximumInput - minimumInput; - } - - SetSetpoint(m_setpoint); -} - -/** - * Sets the minimum and maximum values to write. - * - * @param minimumOutput the minimum value to write to the output - * @param maximumOutput the maximum value to write to the output - */ -void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) { - std::lock_guard lock(m_thisMutex); - m_minimumOutput = minimumOutput; - m_maximumOutput = maximumOutput; -} - -/** - * Set the setpoint for the PIDController. - * - * @param setpoint the desired setpoint - */ -void PIDController::SetSetpoint(double setpoint) { - { - std::lock_guard lock(m_thisMutex); - - if (m_maximumInput > m_minimumInput) { - if (setpoint > m_maximumInput) - m_setpoint = m_maximumInput; - else if (setpoint < m_minimumInput) - m_setpoint = m_minimumInput; - else - m_setpoint = setpoint; - } else { - m_setpoint = setpoint; - } - } -} - -/** - * Returns the current setpoint of the PIDController. - * - * @return the current setpoint - */ -double PIDController::GetSetpoint() const { - std::lock_guard lock(m_thisMutex); - return m_setpoint; -} - -/** - * Returns the change in setpoint over time of the PIDController. - * - * @return the change in setpoint over time - */ -double PIDController::GetDeltaSetpoint() const { - std::lock_guard lock(m_thisMutex); - return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get(); -} - -/** - * Returns the current difference of the input from the setpoint. - * - * @return the current error - */ -double PIDController::GetError() const { - double setpoint = GetSetpoint(); - { - std::lock_guard lock(m_thisMutex); - return GetContinuousError(setpoint - m_pidInput->PIDGet()); - } -} - -/** - * Returns the current average of the error over the past few iterations. - * - * You can specify the number of iterations to average with SetToleranceBuffer() - * (defaults to 1). This is the same value that is used for OnTarget(). - * - * @return the average error - */ -double PIDController::GetAvgError() const { return GetError(); } - -/** - * Sets what type of input the PID controller will use. - */ -void PIDController::SetPIDSourceType(PIDSourceType pidSource) { - m_pidInput->SetPIDSourceType(pidSource); -} -/** - * Returns the type of input the PID controller is using. - * - * @return the PID controller input type - */ -PIDSourceType PIDController::GetPIDSourceType() const { - return m_pidInput->GetPIDSourceType(); -} - -/* - * Set the percentage error which is considered tolerable for use with - * OnTarget. - * - * @param percentage error which is tolerable - */ -void PIDController::SetTolerance(double percent) { - std::lock_guard lock(m_thisMutex); - m_toleranceType = kPercentTolerance; - m_tolerance = percent; -} - -/* - * Set the absolute error which is considered tolerable for use with - * OnTarget. - * - * @param percentage error which is tolerable - */ -void PIDController::SetAbsoluteTolerance(double absTolerance) { - std::lock_guard lock(m_thisMutex); - m_toleranceType = kAbsoluteTolerance; - m_tolerance = absTolerance; -} - -/* - * Set the percentage error which is considered tolerable for use with - * OnTarget. - * - * @param percentage error which is tolerable - */ -void PIDController::SetPercentTolerance(double percent) { - std::lock_guard lock(m_thisMutex); - m_toleranceType = kPercentTolerance; - m_tolerance = percent; -} - -/* - * Set the number of previous error samples to average for tolerancing. When - * determining whether a mechanism is on target, the user may want to use a - * rolling average of previous measurements instead of a precise position or - * velocity. This is useful for noisy sensors which return a few erroneous - * measurements when the mechanism is on target. However, the mechanism will - * not register as on target for at least the specified bufLength cycles. - * - * @param bufLength Number of previous cycles to average. Defaults to 1. - */ -void PIDController::SetToleranceBuffer(int bufLength) { - std::lock_guard lock(m_thisMutex); - - // Create LinearDigitalFilter with original source as its source argument - m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength); - m_pidInput = &m_filter; -} - -/* - * Return true if the error is within the percentage of the total input range, - * determined by SetTolerance. This asssumes that the maximum and minimum input - * were set using SetInput. - * - * 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 PIDController::OnTarget() const { - double error = GetError(); - - std::lock_guard lock(m_thisMutex); - switch (m_toleranceType) { - case kPercentTolerance: - return std::fabs(error) < m_tolerance / 100 * m_inputRange; - break; - case kAbsoluteTolerance: - return std::fabs(error) < m_tolerance; - break; - case kNoTolerance: - // TODO: this case needs an error - return false; - } - return false; -} - /** * Begin running the PIDController. */ @@ -621,47 +138,11 @@ bool PIDController::IsEnabled() const { void PIDController::Reset() { Disable(); - std::lock_guard lock(m_thisMutex); - m_prevError = 0; - m_totalError = 0; - m_result = 0; + PIDBase::Reset(); } void PIDController::InitSendable(SendableBuilder& builder) { - builder.SetSmartDashboardType("PIDController"); - builder.SetSafeState([=]() { Reset(); }); - builder.AddDoubleProperty("p", [=]() { return GetP(); }, - [=](double value) { SetP(value); }); - builder.AddDoubleProperty("i", [=]() { return GetI(); }, - [=](double value) { SetI(value); }); - builder.AddDoubleProperty("d", [=]() { return GetD(); }, - [=](double value) { SetD(value); }); - builder.AddDoubleProperty("f", [=]() { return GetF(); }, - [=](double value) { SetF(value); }); - builder.AddDoubleProperty("setpoint", [=]() { return GetSetpoint(); }, - [=](double value) { SetSetpoint(value); }); + PIDBase::InitSendable(builder); builder.AddBooleanProperty("enabled", [=]() { return IsEnabled(); }, [=](bool value) { SetEnabled(value); }); } - -/** - * Wraps error around for continuous inputs. The original error is returned if - * continuous mode is disabled. This is an unsynchronized function. - * - * @param error The current error of the PID controller. - * @return Error for continuous inputs. - */ -double PIDController::GetContinuousError(double error) const { - if (m_continuous && m_inputRange != 0) { - error = std::fmod(error, m_inputRange); - if (std::fabs(error) > m_inputRange / 2) { - if (error > 0) { - return error - m_inputRange; - } else { - return error + m_inputRange; - } - } - } - - return error; -} diff --git a/wpilibc/src/main/native/cpp/SynchronousPID.cpp b/wpilibc/src/main/native/cpp/SynchronousPID.cpp new file mode 100644 index 0000000000..9eab610e4b --- /dev/null +++ b/wpilibc/src/main/native/cpp/SynchronousPID.cpp @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "SynchronousPID.h" + +using namespace frc; + +/** + * Allocate a PID object with the given constants for P, I, and D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param source The PIDSource object that is used to get values + * @param output The PIDOutput object that is set to the output percentage + */ +SynchronousPID::SynchronousPID(double Kp, double Ki, double Kd, + PIDSource& source, PIDOutput& output) + : SynchronousPID(Kp, Ki, Kd, 0.0, source, output) {} + +/** + * Allocate a PID object with the given constants for P, I, and D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param Kf the feed forward term + * @param source The PIDSource object that is used to get values + * @param output The PIDOutput object that is set to the output percentage + */ +SynchronousPID::SynchronousPID(double Kp, double Ki, double Kd, double Kf, + PIDSource& source, PIDOutput& output) + : PIDBase(Kp, Ki, Kd, Kf, source, output) { + m_enabled = true; +} + +/** + * Read the input, calculate the output accordingly, and write to the output. + */ +void SynchronousPID::Calculate() { PIDBase::Calculate(); } diff --git a/wpilibc/src/main/native/include/PIDBase.h b/wpilibc/src/main/native/include/PIDBase.h new file mode 100644 index 0000000000..b89eba6960 --- /dev/null +++ b/wpilibc/src/main/native/include/PIDBase.h @@ -0,0 +1,161 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include +#include + +#include "Base.h" +#include "Filters/LinearDigitalFilter.h" +#include "PIDInterface.h" +#include "PIDSource.h" +#include "SmartDashboard/SendableBase.h" +#include "Timer.h" + +namespace frc { + +class PIDOutput; + +/** + * Class implements a PID Control Loop. + * + * Creates a separate thread which reads the given PIDSource and takes care of + * the integral calculations, as well as writing the given PIDOutput. + * + * This feedback controller runs in discrete time, so time deltas are not used + * in the integral and derivative calculations. Therefore, the sample rate + * affects the controller's behavior for a given set of PID constants. + */ +class PIDBase : public SendableBase, public PIDInterface { + public: + PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output); + PIDBase(double p, double i, double d, double f, PIDSource& source, + PIDOutput& output); + ~PIDBase() override = default; + + PIDBase(const PIDBase&) = delete; + PIDBase& operator=(const PIDBase) = delete; + + virtual double Get() const; + virtual void SetContinuous(bool continuous = true); + virtual void SetInputRange(double minimumInput, double maximumInput); + virtual void SetOutputRange(double minimumOutput, double maximumOutput); + void SetPID(double p, double i, double d) override; + virtual void SetPID(double p, double i, double d, double f); + void SetP(double p); + void SetI(double i); + void SetD(double d); + void SetF(double f); + double GetP() const override; + double GetI() const override; + double GetD() const override; + virtual double GetF() const; + + void SetSetpoint(double setpoint) override; + double GetSetpoint() const override; + double GetDeltaSetpoint() const; + + virtual double GetError() const; + + WPI_DEPRECATED("Use a LinearDigitalFilter as the input and GetError().") + virtual double GetAvgError() const; + + virtual void SetPIDSourceType(PIDSourceType pidSource); + virtual PIDSourceType GetPIDSourceType() const; + + WPI_DEPRECATED("Use SetPercentTolerance() instead.") + virtual void SetTolerance(double percent); + virtual void SetAbsoluteTolerance(double absValue); + virtual void SetPercentTolerance(double percentValue); + + WPI_DEPRECATED("Use a LinearDigitalFilter as the input.") + virtual void SetToleranceBuffer(int buf = 1); + + virtual bool OnTarget() const; + + void Reset() override; + + void InitSendable(SendableBuilder& builder) override; + + protected: + // Is the pid controller enabled + bool m_enabled = false; + + mutable wpi::mutex m_thisMutex; + + // Ensures when Disable() is called, PIDWrite() won't run if Calculate() + // is already running at that time. + mutable wpi::mutex m_pidWriteMutex; + + PIDSource* m_pidInput; + PIDOutput* m_pidOutput; + Timer m_setpointTimer; + + virtual void Calculate(); + virtual double CalculateFeedForward(); + double GetContinuousError(double error) const; + + private: + // Factor for "proportional" control + double m_P; + + // Factor for "integral" control + double m_I; + + // Factor for "derivative" control + double m_D; + + // Factor for "feed forward" control + double m_F; + + // |maximum output| + double m_maximumOutput = 1.0; + + // |minimum output| + double m_minimumOutput = -1.0; + + // Maximum input - limit setpoint to this + double m_maximumInput = 0; + + // Minimum input - limit setpoint to this + double m_minimumInput = 0; + + // input range - difference between maximum and minimum + double m_inputRange = 0; + + // Do the endpoints wrap around? eg. Absolute encoder + bool m_continuous = false; + + // The prior error (used to compute velocity) + double m_prevError = 0; + + // The sum of the errors for use in the integral calc + double m_totalError = 0; + + enum { + kAbsoluteTolerance, + kPercentTolerance, + kNoTolerance + } m_toleranceType = kNoTolerance; + + // The percetage or absolute error that is considered on target. + double m_tolerance = 0.05; + + double m_setpoint = 0; + double m_prevSetpoint = 0; + double m_error = 0; + double m_result = 0; + + std::shared_ptr m_origSource; + LinearDigitalFilter m_filter{nullptr, {}, {}}; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/PIDController.h b/wpilibc/src/main/native/include/PIDController.h index 89adb415a3..5b8a73d95d 100644 --- a/wpilibc/src/main/native/include/PIDController.h +++ b/wpilibc/src/main/native/include/PIDController.h @@ -17,9 +17,8 @@ #include "Controller.h" #include "Filters/LinearDigitalFilter.h" #include "Notifier.h" -#include "PIDInterface.h" +#include "PIDBase.h" #include "PIDSource.h" -#include "SmartDashboard/SendableBase.h" #include "Timer.h" namespace frc { @@ -36,7 +35,7 @@ class PIDOutput; * in the integral and derivative calculations. Therefore, the sample rate * affects the controller's behavior for a given set of PID constants. */ -class PIDController : public SendableBase, public PIDInterface { +class PIDController : public PIDBase, public Controller { public: PIDController(double p, double i, double d, PIDSource* source, PIDOutput* output, double period = 0.05); @@ -51,126 +50,17 @@ class PIDController : public SendableBase, public PIDInterface { PIDController(const PIDController&) = delete; PIDController& operator=(const PIDController) = delete; - virtual double Get() const; - virtual void SetContinuous(bool continuous = true); - virtual void SetInputRange(double minimumInput, double maximumInput); - virtual void SetOutputRange(double minimumOutput, double maximumOutput); - void SetPID(double p, double i, double d) override; - virtual void SetPID(double p, double i, double d, double f); - void SetP(double p); - void SetI(double i); - void SetD(double d); - void SetF(double f); - double GetP() const override; - double GetI() const override; - double GetD() const override; - virtual double GetF() const; - - void SetSetpoint(double setpoint) override; - double GetSetpoint() const override; - double GetDeltaSetpoint() const; - - virtual double GetError() const; - - WPI_DEPRECATED("Use a LinearDigitalFilter as the input and GetError().") - virtual double GetAvgError() const; - - virtual void SetPIDSourceType(PIDSourceType pidSource); - virtual PIDSourceType GetPIDSourceType() const; - - WPI_DEPRECATED("Use SetPercentTolerance() instead.") - virtual void SetTolerance(double percent); - virtual void SetAbsoluteTolerance(double absValue); - virtual void SetPercentTolerance(double percentValue); - - WPI_DEPRECATED("Use a LinearDigitalFilter as the input.") - virtual void SetToleranceBuffer(int buf = 1); - - virtual bool OnTarget() const; - void Enable() override; void Disable() override; void SetEnabled(bool enable); - bool IsEnabled() const override; + bool IsEnabled() const; void Reset() override; void InitSendable(SendableBuilder& builder) override; - protected: - PIDSource* m_pidInput; - PIDOutput* m_pidOutput; - - virtual void Calculate(); - virtual double CalculateFeedForward(); - double GetContinuousError(double error) const; - private: - // Factor for "proportional" control - double m_P; - - // Factor for "integral" control - double m_I; - - // Factor for "derivative" control - double m_D; - - // Factor for "feed forward" control - double m_F; - - // |maximum output| - double m_maximumOutput = 1.0; - - // |minimum output| - double m_minimumOutput = -1.0; - - // Maximum input - limit setpoint to this - double m_maximumInput = 0; - - // Minimum input - limit setpoint to this - double m_minimumInput = 0; - - // input range - difference between maximum and minimum - double m_inputRange = 0; - - // Do the endpoints wrap around? eg. Absolute encoder - bool m_continuous = false; - - // Is the pid controller enabled - bool m_enabled = false; - - // The prior error (used to compute velocity) - double m_prevError = 0; - - // The sum of the errors for use in the integral calc - double m_totalError = 0; - - enum { - kAbsoluteTolerance, - kPercentTolerance, - kNoTolerance - } m_toleranceType = kNoTolerance; - - // The percetage or absolute error that is considered on target. - double m_tolerance = 0.05; - - double m_setpoint = 0; - double m_prevSetpoint = 0; - double m_error = 0; - double m_result = 0; - double m_period; - - std::shared_ptr m_origSource; - LinearDigitalFilter m_filter{nullptr, {}, {}}; - - mutable wpi::mutex m_thisMutex; - - // Ensures when Disable() is called, PIDWrite() won't run if Calculate() - // is already running at that time. - mutable wpi::mutex m_pidWriteMutex; - std::unique_ptr m_controlLoop; - Timer m_setpointTimer; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/PIDInterface.h b/wpilibc/src/main/native/include/PIDInterface.h index 53d7a2189e..92a28c87a3 100644 --- a/wpilibc/src/main/native/include/PIDInterface.h +++ b/wpilibc/src/main/native/include/PIDInterface.h @@ -7,11 +7,9 @@ #pragma once -#include "Controller.h" - namespace frc { -class PIDInterface : public Controller { +class PIDInterface { virtual void SetPID(double p, double i, double d) = 0; virtual double GetP() const = 0; virtual double GetI() const = 0; @@ -20,10 +18,6 @@ class PIDInterface : public Controller { virtual void SetSetpoint(double setpoint) = 0; virtual double GetSetpoint() const = 0; - virtual void Enable() = 0; - virtual void Disable() = 0; - virtual bool IsEnabled() const = 0; - virtual void Reset() = 0; }; diff --git a/wpilibc/src/main/native/include/SynchronousPID.h b/wpilibc/src/main/native/include/SynchronousPID.h new file mode 100644 index 0000000000..2e2181e5e3 --- /dev/null +++ b/wpilibc/src/main/native/include/SynchronousPID.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "PIDBase.h" + +namespace frc { + +/** + * Class implements a synchronous PID control loop. + * + * Provides a calculate method for the user to call at their desired update + * rate. + */ +class SynchronousPID : public PIDBase { + public: + SynchronousPID(double Kp, double Ki, double Kd, PIDSource& source, + PIDOutput& output); + SynchronousPID(double Kp, double Ki, double Kd, double Kf, PIDSource& source, + PIDOutput& output); + + SynchronousPID(const SynchronousPID&) = delete; + SynchronousPID& operator=(const SynchronousPID) = delete; + + void Calculate() override; +}; + +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java new file mode 100644 index 0000000000..4d79033b61 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java @@ -0,0 +1,804 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import java.util.concurrent.locks.ReentrantLock; + +import edu.wpi.first.wpilibj.filters.LinearDigitalFilter; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; +import edu.wpi.first.wpilibj.util.BoundaryException; + +import static java.util.Objects.requireNonNull; + +/** + * Class implements a PID Control Loop. + * + *

Creates a separate thread which reads the given PIDSource and takes care of the integral + * calculations, as well as writing the given PIDOutput. + * + *

This feedback controller runs in discrete time, so time deltas are not used in the integral + * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a + * given set of PID constants. + */ +public class PIDBase extends SendableBase implements PIDInterface { + public static final double kDefaultPeriod = 0.05; + private static int instances = 0; + + // Factor for "proportional" control + @SuppressWarnings("MemberName") + private double m_P; + + // Factor for "integral" control + @SuppressWarnings("MemberName") + private double m_I; + + // Factor for "derivative" control + @SuppressWarnings("MemberName") + private double m_D; + + // Factor for "feed forward" control + @SuppressWarnings("MemberName") + private double m_F; + + // |maximum output| + private double m_maximumOutput = 1.0; + + // |minimum output| + private double m_minimumOutput = -1.0; + + // Maximum input - limit setpoint to this + private double m_maximumInput = 0.0; + + // Minimum input - limit setpoint to this + private double m_minimumInput = 0.0; + + // Input range - difference between maximum and minimum + private double m_inputRange = 0.0; + + // Do the endpoints wrap around? (e.g., absolute encoder) + private boolean m_continuous = false; + + // Is the PID controller enabled + protected boolean m_enabled = false; + + // The prior error (used to compute velocity) + private double m_prevError = 0.0; + + // The sum of the errors for use in the integral calc + private double m_totalError = 0.0; + + // The tolerance object used to check if on target + private Tolerance m_tolerance; + + private double m_setpoint = 0.0; + private double m_prevSetpoint = 0.0; + @SuppressWarnings("PMD.UnusedPrivateField") + private double m_error = 0.0; + private double m_result = 0.0; + + private PIDSource m_origSource; + private LinearDigitalFilter m_filter; + + protected ReentrantLock m_thisMutex = new ReentrantLock(); + + // Ensures when disable() is called, pidWrite() won't run if calculate() + // is already running at that time. + protected ReentrantLock m_pidWriteMutex = new ReentrantLock(); + + protected PIDSource m_pidInput; + protected PIDOutput m_pidOutput; + protected Timer m_setpointTimer; + + /** + * Tolerance is the type of tolerance used to specify if the PID controller is on target. + * + *

The various implementations of this class such as PercentageTolerance and AbsoluteTolerance + * specify types of tolerance specifications to use. + */ + public interface Tolerance { + boolean onTarget(); + } + + /** + * Used internally for when Tolerance hasn't been set. + */ + public class NullTolerance implements Tolerance { + @Override + public boolean onTarget() { + throw new RuntimeException("No tolerance value set when calling onTarget()."); + } + } + + public class PercentageTolerance implements Tolerance { + private final double m_percentage; + + PercentageTolerance(double value) { + m_percentage = value; + } + + @Override + public boolean onTarget() { + return Math.abs(getError()) < m_percentage / 100 * m_inputRange; + } + } + + public class AbsoluteTolerance implements Tolerance { + private final double m_value; + + AbsoluteTolerance(double value) { + m_value = value; + } + + @Override + public boolean onTarget() { + return Math.abs(getError()) < m_value; + } + } + + /** + * Allocate a PID object with the given constants for P, I, D, and F. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param Kf the feed forward term + * @param source The PIDSource object that is used to get values + * @param output The PIDOutput object that is set to the output percentage + */ + @SuppressWarnings("ParameterName") + public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source, + PIDOutput output) { + super(false); + requireNonNull(source, "Null PIDSource was given"); + requireNonNull(output, "Null PIDOutput was given"); + + m_setpointTimer = new Timer(); + m_setpointTimer.start(); + + m_P = Kp; + m_I = Ki; + m_D = Kd; + m_F = Kf; + + // Save original source + m_origSource = source; + + // Create LinearDigitalFilter with original source as its source argument + m_filter = LinearDigitalFilter.movingAverage(m_origSource, 1); + m_pidInput = m_filter; + + m_pidOutput = output; + + instances++; + HLUsageReporting.reportPIDController(instances); + m_tolerance = new NullTolerance(); + setName("PIDController", instances); + } + + /** + * Allocate a PID object with the given constants for P, I, and D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param source the PIDSource object that is used to get values + * @param output the PIDOutput object that is set to the output percentage + */ + @SuppressWarnings("ParameterName") + public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) { + this(Kp, Ki, Kd, 0.0, source, output); + } + + /** + * Read the input, calculate the output accordingly, and write to the output. This should only be + * called by the PIDTask and is created during initialization. + */ + @SuppressWarnings("LocalVariableName") + protected void calculate() { + if (m_origSource == null || m_pidOutput == null) { + return; + } + + boolean enabled; + + m_thisMutex.lock(); + try { + enabled = m_enabled; + } finally { + m_thisMutex.unlock(); + } + + if (enabled) { + double input; + + // Storage for function inputs + PIDSourceType pidSourceType; + double P; + double I; + double D; + double feedForward = calculateFeedForward(); + double minimumOutput; + double maximumOutput; + + // Storage for function input-outputs + double prevError; + double error; + double totalError; + + m_thisMutex.lock(); + try { + input = m_pidInput.pidGet(); + + pidSourceType = m_pidInput.getPIDSourceType(); + P = m_P; + I = m_I; + D = m_D; + minimumOutput = m_minimumOutput; + maximumOutput = m_maximumOutput; + + prevError = m_prevError; + error = getContinuousError(m_setpoint - input); + totalError = m_totalError; + } finally { + m_thisMutex.unlock(); + } + + // Storage for function outputs + double result; + + if (pidSourceType.equals(PIDSourceType.kRate)) { + if (P != 0) { + totalError = clamp(totalError + error, minimumOutput / P, + maximumOutput / P); + } + + result = P * totalError + D * error + feedForward; + } else { + if (I != 0) { + totalError = clamp(totalError + error, minimumOutput / I, + maximumOutput / I); + } + + result = P * error + I * totalError + D * (error - prevError) + + feedForward; + } + + result = clamp(result, minimumOutput, maximumOutput); + + // Ensures m_enabled check and pidWrite() call occur atomically + m_pidWriteMutex.lock(); + try { + m_thisMutex.lock(); + try { + if (m_enabled) { + // Don't block other PIDController operations on pidWrite() + m_thisMutex.unlock(); + + m_pidOutput.pidWrite(result); + } + } finally { + if (m_thisMutex.isHeldByCurrentThread()) { + m_thisMutex.unlock(); + } + } + } finally { + m_pidWriteMutex.unlock(); + } + + m_thisMutex.lock(); + try { + m_prevError = error; + m_error = error; + m_totalError = totalError; + m_result = result; + } finally { + m_thisMutex.unlock(); + } + } + } + + /** + * Calculate the feed forward term. + * + *

Both of the provided feed forward calculations are velocity feed forwards. If a different + * feed forward calculation is desired, the user can override this function and provide his or + * her own. This function does no synchronization because the PIDController class only calls it + * in synchronized code, so be careful if calling it oneself. + * + *

If a velocity PID controller is being used, the F term should be set to 1 over the maximum + * setpoint for the output. If a position PID controller is being used, the F term should be set + * to 1 over the maximum speed for the output measured in setpoint units per this controller's + * update period (see the default period in this class's constructor). + */ + protected double calculateFeedForward() { + if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) { + return m_F * getSetpoint(); + } else { + double temp = m_F * getDeltaSetpoint(); + m_prevSetpoint = m_setpoint; + m_setpointTimer.reset(); + return temp; + } + } + + /** + * Set the PID Controller gain parameters. Set the proportional, integral, and differential + * coefficients. + * + * @param p Proportional coefficient + * @param i Integral coefficient + * @param d Differential coefficient + */ + @SuppressWarnings("ParameterName") + public void setPID(double p, double i, double d) { + m_thisMutex.lock(); + try { + m_P = p; + m_I = i; + m_D = d; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set the PID Controller gain parameters. Set the proportional, integral, and differential + * coefficients. + * + * @param p Proportional coefficient + * @param i Integral coefficient + * @param d Differential coefficient + * @param f Feed forward coefficient + */ + @SuppressWarnings("ParameterName") + public void setPID(double p, double i, double d, double f) { + m_thisMutex.lock(); + try { + m_P = p; + m_I = i; + m_D = d; + m_F = f; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set the Proportional coefficient of the PID controller gain. + * + * @param p Proportional coefficient + */ + @SuppressWarnings("ParameterName") + public void setP(double p) { + m_thisMutex.lock(); + try { + m_P = p; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set the Integral coefficient of the PID controller gain. + * + * @param i Integral coefficient + */ + @SuppressWarnings("ParameterName") + public void setI(double i) { + m_thisMutex.lock(); + try { + m_I = i; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set the Differential coefficient of the PID controller gain. + * + * @param d differential coefficient + */ + @SuppressWarnings("ParameterName") + public void setD(double d) { + m_thisMutex.lock(); + try { + m_D = d; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set the Feed forward coefficient of the PID controller gain. + * + * @param f feed forward coefficient + */ + @SuppressWarnings("ParameterName") + public void setF(double f) { + m_thisMutex.lock(); + try { + m_F = f; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Get the Proportional coefficient. + * + * @return proportional coefficient + */ + public double getP() { + m_thisMutex.lock(); + try { + return m_P; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Get the Integral coefficient. + * + * @return integral coefficient + */ + public double getI() { + m_thisMutex.lock(); + try { + return m_I; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Get the Differential coefficient. + * + * @return differential coefficient + */ + public double getD() { + m_thisMutex.lock(); + try { + return m_D; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Get the Feed forward coefficient. + * + * @return feed forward coefficient + */ + public double getF() { + m_thisMutex.lock(); + try { + return m_F; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Return the current PID result This is always centered on zero and constrained the the max and + * min outs. + * + * @return the latest calculated output + */ + public double get() { + m_thisMutex.lock(); + try { + return m_result; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set 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 Set to true turns on continuous, false turns off continuous + */ + public void setContinuous(boolean continuous) { + if (continuous && m_inputRange <= 0) { + throw new RuntimeException("No input range set when calling setContinuous()."); + } + m_thisMutex.lock(); + try { + m_continuous = continuous; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set 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. + */ + public void setContinuous() { + setContinuous(true); + } + + /** + * Sets the maximum and minimum values expected from the input and setpoint. + * + * @param minimumInput the minimum value expected from the input + * @param maximumInput the maximum value expected from the input + */ + public void setInputRange(double minimumInput, double maximumInput) { + m_thisMutex.lock(); + try { + if (minimumInput > maximumInput) { + throw new BoundaryException("Lower bound is greater than upper bound"); + } + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + m_inputRange = maximumInput - minimumInput; + } finally { + m_thisMutex.unlock(); + } + + setSetpoint(m_setpoint); + } + + /** + * Sets the minimum and maximum values to write. + * + * @param minimumOutput the minimum percentage to write to the output + * @param maximumOutput the maximum percentage to write to the output + */ + public void setOutputRange(double minimumOutput, double maximumOutput) { + m_thisMutex.lock(); + try { + if (minimumOutput > maximumOutput) { + throw new BoundaryException("Lower bound is greater than upper bound"); + } + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set the setpoint for the PIDController. + * + * @param setpoint the desired setpoint + */ + public void setSetpoint(double setpoint) { + m_thisMutex.lock(); + try { + if (m_maximumInput > m_minimumInput) { + if (setpoint > m_maximumInput) { + m_setpoint = m_maximumInput; + } else if (setpoint < m_minimumInput) { + m_setpoint = m_minimumInput; + } else { + m_setpoint = setpoint; + } + } else { + m_setpoint = setpoint; + } + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the current setpoint of the PIDController. + * + * @return the current setpoint + */ + public double getSetpoint() { + m_thisMutex.lock(); + try { + return m_setpoint; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the change in setpoint over time of the PIDController. + * + * @return the change in setpoint over time + */ + public double getDeltaSetpoint() { + m_thisMutex.lock(); + try { + return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get(); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the current difference of the input from the setpoint. + * + * @return the current error + */ + public double getError() { + m_thisMutex.lock(); + try { + return getContinuousError(getSetpoint() - m_pidInput.pidGet()); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Returns the current difference of the error over the past few iterations. You can specify the + * number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is + * used for the onTarget() function. + * + * @deprecated Use getError(), which is now already filtered. + * @return the current average of the error + */ + @Deprecated + public double getAvgError() { + m_thisMutex.lock(); + try { + return getError(); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Sets what type of input the PID controller will use. + * + * @param pidSource the type of input + */ + void setPIDSourceType(PIDSourceType pidSource) { + m_pidInput.setPIDSourceType(pidSource); + } + + /** + * Returns the type of input the PID controller is using. + * + * @return the PID controller input type + */ + PIDSourceType getPIDSourceType() { + return m_pidInput.getPIDSourceType(); + } + + /** + * Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of + * the range or as an absolute value. The Tolerance object encapsulates those options in an + * object. Use it by creating the type of tolerance that you want to use: setTolerance(new + * PIDController.AbsoluteTolerance(0.1)) + * + * @deprecated Use setPercentTolerance() instead. + * @param tolerance A tolerance object of the right type, e.g. PercentTolerance or + * AbsoluteTolerance + */ + @Deprecated + public void setTolerance(Tolerance tolerance) { + m_tolerance = tolerance; + } + + /** + * Set the absolute error which is considered tolerable for use with OnTarget. + * + * @param absvalue absolute error which is tolerable in the units of the input object + */ + public void setAbsoluteTolerance(double absvalue) { + m_thisMutex.lock(); + try { + m_tolerance = new AbsoluteTolerance(absvalue); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 = + * 15 percent) + * + * @param percentage percent error which is tolerable + */ + public void setPercentTolerance(double percentage) { + m_thisMutex.lock(); + try { + m_tolerance = new PercentageTolerance(percentage); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Set the number of previous error samples to average for tolerancing. When determining whether a + * mechanism is on target, the user may want to use a rolling average of previous measurements + * instead of a precise position or velocity. This is useful for noisy sensors which return a few + * erroneous measurements when the mechanism is on target. However, the mechanism will not + * register as on target for at least the specified bufLength cycles. + * + * @deprecated Use a LinearDigitalFilter as the input. + * @param bufLength Number of previous cycles to average. + */ + @Deprecated + public void setToleranceBuffer(int bufLength) { + m_thisMutex.lock(); + try { + m_filter = LinearDigitalFilter.movingAverage(m_origSource, bufLength); + m_pidInput = m_filter; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Return true if the error is within the percentage of the total input range, determined by + * setTolerance. This assumes that the maximum and minimum input were set using setInput. + * + * @return true if the error is less than the tolerance + */ + public boolean onTarget() { + m_thisMutex.lock(); + try { + return m_tolerance.onTarget(); + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Reset the previous error, the integral term, and disable the controller. + */ + @Override + public void reset() { + m_thisMutex.lock(); + try { + m_prevError = 0; + m_totalError = 0; + m_result = 0; + } finally { + m_thisMutex.unlock(); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PIDController"); + builder.setSafeState(this::reset); + builder.addDoubleProperty("p", this::getP, this::setP); + builder.addDoubleProperty("i", this::getI, this::setI); + builder.addDoubleProperty("d", this::getD, this::setD); + builder.addDoubleProperty("f", this::getF, this::setF); + builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); + } + + /** + * Wraps error around for continuous inputs. The original error is returned if continuous mode is + * disabled. This is an unsynchronized function. + * + * @param error The current error of the PID controller. + * @return Error for continuous inputs. + */ + protected double getContinuousError(double error) { + if (m_continuous && m_inputRange > 0) { + error %= m_inputRange; + if (Math.abs(error) > m_inputRange / 2) { + if (error > 0) { + return error - m_inputRange; + } else { + return error + m_inputRange; + } + } + } + + return error; + } + + private static double clamp(double value, double low, double high) { + return Math.max(low, Math.min(value, high)); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java index 806604d288..20c0622de0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java @@ -7,13 +7,7 @@ package edu.wpi.first.wpilibj; -import java.util.concurrent.locks.ReentrantLock; - -import edu.wpi.first.wpilibj.filters.LinearDigitalFilter; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import edu.wpi.first.wpilibj.util.BoundaryException; - -import static java.util.Objects.requireNonNull; /** * Class implements a PID Control Loop. @@ -25,98 +19,8 @@ import static java.util.Objects.requireNonNull; * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a * given set of PID constants. */ -public class PIDController extends SendableBase implements PIDInterface, Sendable, Controller { - public static final double kDefaultPeriod = .05; - private static int instances = 0; - @SuppressWarnings("MemberName") - private double m_P; // factor for "proportional" control - @SuppressWarnings("MemberName") - private double m_I; // factor for "integral" control - @SuppressWarnings("MemberName") - private double m_D; // factor for "derivative" control - @SuppressWarnings("MemberName") - private double m_F; // factor for feedforward term - private double m_maximumOutput = 1.0; // |maximum output| - private double m_minimumOutput = -1.0; // |minimum output| - private double m_maximumInput = 0.0; // maximum input - limit setpoint to this - private double m_minimumInput = 0.0; // minimum input - limit setpoint to this - private double m_inputRange = 0.0; // input range - difference between maximum and minimum - // do the endpoints wrap around? eg. Absolute encoder - private boolean m_continuous = false; - private boolean m_enabled = false; // is the pid controller enabled - // the prior error (used to compute velocity) - private double m_prevError = 0.0; - // the sum of the errors for use in the integral calc - private double m_totalError = 0.0; - // the tolerance object used to check if on target - private Tolerance m_tolerance; - private double m_setpoint = 0.0; - private double m_prevSetpoint = 0.0; - @SuppressWarnings("PMD.UnusedPrivateField") - private double m_error = 0.0; - private double m_result = 0.0; - private double m_period = kDefaultPeriod; - - PIDSource m_origSource; - LinearDigitalFilter m_filter; - - ReentrantLock m_thisMutex = new ReentrantLock(); - - // Ensures when disable() is called, pidWrite() won't run if calculate() - // is already running at that time. - ReentrantLock m_pidWriteMutex = new ReentrantLock(); - - protected PIDSource m_pidInput; - protected PIDOutput m_pidOutput; - +public class PIDController extends PIDBase implements Controller { Notifier m_controlLoop = new Notifier(this::calculate); - Timer m_setpointTimer = new Timer(); - - /** - * Tolerance is the type of tolerance used to specify if the PID controller is on target. - * - *

The various implementations of this class such as PercentageTolerance and AbsoluteTolerance - * specify types of tolerance specifications to use. - */ - public interface Tolerance { - boolean onTarget(); - } - - /** - * Used internally for when Tolerance hasn't been set. - */ - public class NullTolerance implements Tolerance { - @Override - public boolean onTarget() { - throw new RuntimeException("No tolerance value set when calling onTarget()."); - } - } - - public class PercentageTolerance implements Tolerance { - private final double m_percentage; - - PercentageTolerance(double value) { - m_percentage = value; - } - - @Override - public boolean onTarget() { - return Math.abs(getError()) < m_percentage / 100 * m_inputRange; - } - } - - public class AbsoluteTolerance implements Tolerance { - private final double m_value; - - AbsoluteTolerance(double value) { - m_value = value; - } - - @Override - public boolean onTarget() { - return Math.abs(getError()) < m_value; - } - } /** * Allocate a PID object with the given constants for P, I, D, and F. @@ -133,33 +37,8 @@ public class PIDController extends SendableBase implements PIDInterface, Sendabl @SuppressWarnings("ParameterName") public PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output, double period) { - super(false); - requireNonNull(source, "Null PIDSource was given"); - requireNonNull(output, "Null PIDOutput was given"); - - m_setpointTimer.start(); - - m_P = Kp; - m_I = Ki; - m_D = Kd; - m_F = Kf; - - // Save original source - m_origSource = source; - - // Create LinearDigitalFilter with original source as its source argument - m_filter = LinearDigitalFilter.movingAverage(m_origSource, 1); - m_pidInput = m_filter; - - m_pidOutput = output; - m_period = period; - - m_controlLoop.startPeriodic(m_period); - - instances++; - HLUsageReporting.reportPIDController(instances); - m_tolerance = new NullTolerance(); - setName("PIDController", instances); + super(Kp, Ki, Kd, Kf, source, output); + m_controlLoop.startPeriodic(period); } /** @@ -226,562 +105,6 @@ public class PIDController extends SendableBase implements PIDInterface, Sendabl } } - /** - * Read the input, calculate the output accordingly, and write to the output. This should only be - * called by the Notifier and is created during initialization. - */ - @SuppressWarnings("LocalVariableName") - protected void calculate() { - if (m_origSource == null || m_pidOutput == null) { - return; - } - - boolean enabled; - - m_thisMutex.lock(); - try { - enabled = m_enabled; - } finally { - m_thisMutex.unlock(); - } - - if (enabled) { - double input; - - // Storage for function inputs - PIDSourceType pidSourceType; - double P; - double I; - double D; - double feedForward = calculateFeedForward(); - double minimumOutput; - double maximumOutput; - - // Storage for function input-outputs - double prevError; - double error; - double totalError; - - m_thisMutex.lock(); - try { - input = m_pidInput.pidGet(); - - pidSourceType = m_pidInput.getPIDSourceType(); - P = m_P; - I = m_I; - D = m_D; - minimumOutput = m_minimumOutput; - maximumOutput = m_maximumOutput; - - prevError = m_prevError; - error = getContinuousError(m_setpoint - input); - totalError = m_totalError; - } finally { - m_thisMutex.unlock(); - } - - // Storage for function outputs - double result; - - if (pidSourceType.equals(PIDSourceType.kRate)) { - if (P != 0) { - totalError = clamp(totalError + error, minimumOutput / P, - maximumOutput / P); - } - - result = P * totalError + D * error + feedForward; - } else { - if (I != 0) { - totalError = clamp(totalError + error, minimumOutput / I, - maximumOutput / I); - } - - result = P * error + I * totalError + D * (error - prevError) - + feedForward; - } - - result = clamp(result, minimumOutput, maximumOutput); - - // Ensures m_enabled check and pidWrite() call occur atomically - m_pidWriteMutex.lock(); - try { - m_thisMutex.lock(); - try { - if (m_enabled) { - // Don't block other PIDController operations on pidWrite() - m_thisMutex.unlock(); - - m_pidOutput.pidWrite(result); - } - } finally { - if (m_thisMutex.isHeldByCurrentThread()) { - m_thisMutex.unlock(); - } - } - } finally { - m_pidWriteMutex.unlock(); - } - - m_thisMutex.lock(); - try { - m_prevError = error; - m_error = error; - m_totalError = totalError; - m_result = result; - } finally { - m_thisMutex.unlock(); - } - } - } - - /** - * Calculate the feed forward term. - * - *

Both of the provided feed forward calculations are velocity feed forwards. If a different - * feed forward calculation is desired, the user can override this function and provide his or - * her own. This function does no synchronization because the PIDController class only calls it - * in synchronized code, so be careful if calling it oneself. - * - *

If a velocity PID controller is being used, the F term should be set to 1 over the maximum - * setpoint for the output. If a position PID controller is being used, the F term should be set - * to 1 over the maximum speed for the output measured in setpoint units per this controller's - * update period (see the default period in this class's constructor). - */ - protected double calculateFeedForward() { - if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) { - return m_F * getSetpoint(); - } else { - double temp = m_F * getDeltaSetpoint(); - m_prevSetpoint = m_setpoint; - m_setpointTimer.reset(); - return temp; - } - } - - /** - * Set the PID Controller gain parameters. Set the proportional, integral, and differential - * coefficients. - * - * @param p Proportional coefficient - * @param i Integral coefficient - * @param d Differential coefficient - */ - @SuppressWarnings("ParameterName") - public void setPID(double p, double i, double d) { - m_thisMutex.lock(); - try { - m_P = p; - m_I = i; - m_D = d; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set the PID Controller gain parameters. Set the proportional, integral, and differential - * coefficients. - * - * @param p Proportional coefficient - * @param i Integral coefficient - * @param d Differential coefficient - * @param f Feed forward coefficient - */ - @SuppressWarnings("ParameterName") - public void setPID(double p, double i, double d, double f) { - m_thisMutex.lock(); - try { - m_P = p; - m_I = i; - m_D = d; - m_F = f; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set the Proportional coefficient of the PID controller gain. - * - * @param p Proportional coefficient - */ - @SuppressWarnings("ParameterName") - public void setP(double p) { - m_thisMutex.lock(); - try { - m_P = p; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set the Integral coefficient of the PID controller gain. - * - * @param i Integral coefficient - */ - @SuppressWarnings("ParameterName") - public void setI(double i) { - m_thisMutex.lock(); - try { - m_I = i; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set the Differential coefficient of the PID controller gain. - * - * @param d differential coefficient - */ - @SuppressWarnings("ParameterName") - public void setD(double d) { - m_thisMutex.lock(); - try { - m_D = d; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set the Feed forward coefficient of the PID controller gain. - * - * @param f feed forward coefficient - */ - @SuppressWarnings("ParameterName") - public void setF(double f) { - m_thisMutex.lock(); - try { - m_F = f; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Get the Proportional coefficient. - * - * @return proportional coefficient - */ - public double getP() { - m_thisMutex.lock(); - try { - return m_P; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Get the Integral coefficient. - * - * @return integral coefficient - */ - public double getI() { - m_thisMutex.lock(); - try { - return m_I; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Get the Differential coefficient. - * - * @return differential coefficient - */ - public double getD() { - m_thisMutex.lock(); - try { - return m_D; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Get the Feed forward coefficient. - * - * @return feed forward coefficient - */ - public double getF() { - m_thisMutex.lock(); - try { - return m_F; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Return the current PID result This is always centered on zero and constrained the the max and - * min outs. - * - * @return the latest calculated output - */ - public double get() { - m_thisMutex.lock(); - try { - return m_result; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set 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 Set to true turns on continuous, false turns off continuous - */ - public void setContinuous(boolean continuous) { - if (continuous && m_inputRange <= 0) { - throw new RuntimeException("No input range set when calling setContinuous()."); - } - m_thisMutex.lock(); - try { - m_continuous = continuous; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set 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. - */ - public void setContinuous() { - setContinuous(true); - } - - /** - * Sets the maximum and minimum values expected from the input and setpoint. - * - * @param minimumInput the minimum value expected from the input - * @param maximumInput the maximum value expected from the input - */ - public void setInputRange(double minimumInput, double maximumInput) { - m_thisMutex.lock(); - try { - if (minimumInput > maximumInput) { - throw new BoundaryException("Lower bound is greater than upper bound"); - } - m_minimumInput = minimumInput; - m_maximumInput = maximumInput; - m_inputRange = maximumInput - minimumInput; - } finally { - m_thisMutex.unlock(); - } - - setSetpoint(m_setpoint); - } - - /** - * Sets the minimum and maximum values to write. - * - * @param minimumOutput the minimum percentage to write to the output - * @param maximumOutput the maximum percentage to write to the output - */ - public void setOutputRange(double minimumOutput, double maximumOutput) { - m_thisMutex.lock(); - try { - if (minimumOutput > maximumOutput) { - throw new BoundaryException("Lower bound is greater than upper bound"); - } - m_minimumOutput = minimumOutput; - m_maximumOutput = maximumOutput; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set the setpoint for the PIDController. - * - * @param setpoint the desired setpoint - */ - public void setSetpoint(double setpoint) { - m_thisMutex.lock(); - try { - if (m_maximumInput > m_minimumInput) { - if (setpoint > m_maximumInput) { - m_setpoint = m_maximumInput; - } else if (setpoint < m_minimumInput) { - m_setpoint = m_minimumInput; - } else { - m_setpoint = setpoint; - } - } else { - m_setpoint = setpoint; - } - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Returns the current setpoint of the PIDController. - * - * @return the current setpoint - */ - public double getSetpoint() { - m_thisMutex.lock(); - try { - return m_setpoint; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Returns the change in setpoint over time of the PIDController. - * - * @return the change in setpoint over time - */ - public double getDeltaSetpoint() { - m_thisMutex.lock(); - try { - return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get(); - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Returns the current difference of the input from the setpoint. - * - * @return the current error - */ - public double getError() { - m_thisMutex.lock(); - try { - return getContinuousError(getSetpoint() - m_pidInput.pidGet()); - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Returns the current difference of the error over the past few iterations. You can specify the - * number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is - * used for the onTarget() function. - * - * @deprecated Use getError(), which is now already filtered. - * @return the current average of the error - */ - @Deprecated - public double getAvgError() { - m_thisMutex.lock(); - try { - return getError(); - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Sets what type of input the PID controller will use. - * - * @param pidSource the type of input - */ - void setPIDSourceType(PIDSourceType pidSource) { - m_pidInput.setPIDSourceType(pidSource); - } - - /** - * Returns the type of input the PID controller is using. - * - * @return the PID controller input type - */ - PIDSourceType getPIDSourceType() { - return m_pidInput.getPIDSourceType(); - } - - /** - * Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of - * the range or as an absolute value. The Tolerance object encapsulates those options in an - * object. Use it by creating the type of tolerance that you want to use: setTolerance(new - * PIDController.AbsoluteTolerance(0.1)) - * - * @deprecated Use setPercentTolerance() instead. - * @param tolerance A tolerance object of the right type, e.g. PercentTolerance or - * AbsoluteTolerance - */ - @Deprecated - public void setTolerance(Tolerance tolerance) { - m_tolerance = tolerance; - } - - /** - * Set the absolute error which is considered tolerable for use with OnTarget. - * - * @param absvalue absolute error which is tolerable in the units of the input object - */ - public void setAbsoluteTolerance(double absvalue) { - m_thisMutex.lock(); - try { - m_tolerance = new AbsoluteTolerance(absvalue); - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 = - * 15 percent) - * - * @param percentage percent error which is tolerable - */ - public void setPercentTolerance(double percentage) { - m_thisMutex.lock(); - try { - m_tolerance = new PercentageTolerance(percentage); - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Set the number of previous error samples to average for tolerancing. When determining whether a - * mechanism is on target, the user may want to use a rolling average of previous measurements - * instead of a precise position or velocity. This is useful for noisy sensors which return a few - * erroneous measurements when the mechanism is on target. However, the mechanism will not - * register as on target for at least the specified bufLength cycles. - * - * @deprecated Use a LinearDigitalFilter as the input. - * @param bufLength Number of previous cycles to average. - */ - @Deprecated - public void setToleranceBuffer(int bufLength) { - m_thisMutex.lock(); - try { - m_filter = LinearDigitalFilter.movingAverage(m_origSource, bufLength); - m_pidInput = m_filter; - } finally { - m_thisMutex.unlock(); - } - } - - /** - * Return true if the error is within the percentage of the total input range, determined by - * setTolerance. This assumes that the maximum and minimum input were set using setInput. - * - * @return true if the error is less than the tolerance - */ - public boolean onTarget() { - m_thisMutex.lock(); - try { - return m_tolerance.onTarget(); - } finally { - m_thisMutex.unlock(); - } - } - /** * Begin running the PIDController. */ @@ -830,7 +153,6 @@ public class PIDController extends SendableBase implements PIDInterface, Sendabl /** * Return true if PIDController is enabled. */ - @Override public boolean isEnabled() { m_thisMutex.lock(); try { @@ -841,57 +163,18 @@ public class PIDController extends SendableBase implements PIDInterface, Sendabl } /** - * Reset the previous error,, the integral term, and disable the controller. + * Reset the previous error, the integral term, and disable the controller. */ @Override public void reset() { disable(); - m_thisMutex.lock(); - try { - m_prevError = 0; - m_totalError = 0; - m_result = 0; - } finally { - m_thisMutex.unlock(); - } + super.reset(); } @Override public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("PIDController"); - builder.setSafeState(this::reset); - builder.addDoubleProperty("p", this::getP, this::setP); - builder.addDoubleProperty("i", this::getI, this::setI); - builder.addDoubleProperty("d", this::getD, this::setD); - builder.addDoubleProperty("f", this::getF, this::setF); - builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); + super.initSendable(builder); builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled); } - - /** - * Wraps error around for continuous inputs. The original error is returned if continuous mode is - * disabled. This is an unsynchronized function. - * - * @param error The current error of the PID controller. - * @return Error for continuous inputs. - */ - protected double getContinuousError(double error) { - if (m_continuous && m_inputRange > 0) { - error %= m_inputRange; - if (Math.abs(error) > m_inputRange / 2) { - if (error > 0) { - return error - m_inputRange; - } else { - return error + m_inputRange; - } - } - } - - return error; - } - - private static double clamp(double value, double low, double high) { - return Math.max(low, Math.min(value, high)); - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java index 8b3ee8aa36..fd91ec886d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java @@ -8,7 +8,7 @@ package edu.wpi.first.wpilibj; @SuppressWarnings("SummaryJavadoc") -public interface PIDInterface extends Controller { +public interface PIDInterface { @SuppressWarnings("ParameterName") void setPID(double p, double i, double d); @@ -24,11 +24,5 @@ public interface PIDInterface extends Controller { double getError(); - void enable(); - - void disable(); - - boolean isEnabled(); - void reset(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousPID.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousPID.java new file mode 100644 index 0000000000..4718f357c6 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousPID.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +/** + * Class implements a synchronous PID control loop. + * + *

Provides a calculate method for the user to call at their desired update rate. + */ +public class SynchronousPID extends PIDBase { + /** + * Allocate a PID object with the given constants for P, I, and D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param source The PIDSource object that is used to get values + * @param output The PIDOutput object that is set to the output percentage + */ + @SuppressWarnings("ParameterName") + public SynchronousPID(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) { + this(Kp, Ki, Kd, 0.0, source, output); + } + + /** + * Allocate a PID object with the given constants for P, I, and D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param Kf the feed forward term + * @param source The PIDSource object that is used to get values + * @param output The PIDOutput object that is set to the output percentage + */ + @SuppressWarnings("ParameterName") + public SynchronousPID(double Kp, double Ki, double Kd, double Kf, PIDSource source, + PIDOutput output) { + super(Kp, Ki, Kd, Kf, source, output); + + m_enabled = true; + } + + /** + * Free the PID object. + */ + @Override + public void free() { + m_thisMutex.lock(); + try { + m_pidOutput = null; + m_pidInput = null; + } finally { + m_thisMutex.unlock(); + } + } + + /** + * Read the input, calculate the output accordingly, and write to the output. + */ + @Override + public void calculate() { // NOPMD + super.calculate(); + } +}