PhoenixPIDController.hpp

Go to the documentation of this file.

/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
#pragma once
 
#include "ctre/phoenix6/networking/Wrappers.hpp"
#include <algorithm>
#include <gcem.hpp>
#include <limits>
#include <string>
#include <wpi/units/time.hpp>
 
#if __has_include(<wpi/telemetry/TelemetryLoggable.hpp>) && __has_include(<wpi/tunables/ComplexTunable.hpp>)
#include <wpi/math/util/MathShared.hpp>
#include <wpi/telemetry/TelemetryLoggable.hpp>
#include <wpi/tunables/ComplexTunable.hpp>
#include <wpi/tunables/Tunable.hpp>
#include <wpi/util/UsageReporting.hpp>
#define CTRE_PHOENIX6_USE_WPILIB
#endif
 
namespace ctre {
namespace phoenix6 {
namespace swerve {
 
/**
 * \brief Phoenix-centric PID controller taken from WPI's wpi#math#PIDController class.
 *
 * This class differs from the WPI implementation by using explicit timestamps for
 * integral/derivative calculations. Ideally, these timestamps come from the StatusSignal.
 */
class PhoenixPIDController
#if defined(CTRE_PHOENIX6_USE_WPILIB) || defined(_CTRE_DOCS_)
    : public wpi::telemetry::TelemetryLoggable, public wpi::tunables::ComplexTunable
#endif
{
public:
    /**
     * Allocates a PhoenixPIDController with the given constants for Kp, Ki, and Kd.
     *
     * \param Kp The proportional coefficient.
     * \param Ki The integral coefficient.
     * \param Kd The derivative coefficient.
     */
    constexpr PhoenixPIDController(double Kp, double Ki, double Kd) :
        m_Kp{Kp}, m_Ki{Ki}, m_Kd{Kd}
    {
#ifdef CTRE_PHOENIX6_USE_WPILIB
        if !consteval {
            ++instances;
            wpi::util::ReportUsage("PhoenixPIDController", std::to_string(instances));
        }
#endif
    }
 
#if defined(CTRE_PHOENIX6_USE_WPILIB) || defined(_CTRE_DOCS_)
    constexpr ~PhoenixPIDController() override = default;
 
    constexpr PhoenixPIDController(PhoenixPIDController const &) = default;
    constexpr PhoenixPIDController &operator=(PhoenixPIDController const &) = default;
    constexpr PhoenixPIDController(PhoenixPIDController &&) = default;
    constexpr PhoenixPIDController &operator=(PhoenixPIDController &&) = default;
#endif
 
    /**
     * Sets the PID Controller gain parameters.
     *
     * Sets the proportional, integral, and differential coefficients.
     *
     * \param Kp The proportional coefficient.
     * \param Ki The integral coefficient.
     * \param Kd The differential coefficient.
     */
    constexpr void SetPID(double Kp, double Ki, double Kd)
    {
        m_Kp = Kp;
        m_Ki = Ki;
        m_Kd = Kd;
    }
 
    /**
     * Sets the proportional coefficient of the PID controller gain.
     *
     * This setter is intended for online tuning.
     *
     * \param Kp The proportional coefficient.
     */
    constexpr void SetP(double Kp) { m_Kp = Kp; }
 
    /**
     * Sets the integral coefficient of the PID controller gain.
     *
     * This setter is intended for online tuning.
     *
     * \param Ki The integral coefficient.
     */
    constexpr void SetI(double Ki) { m_Ki = Ki; }
 
    /**
     * Sets the differential coefficient of the PID controller gain.
     *
     * This setter is intended for online tuning.
     *
     * \param Kd The differential coefficient.
     */
    constexpr void SetD(double Kd) { m_Kd = Kd; }
 
    /**
     * Sets the IZone range. When the absolute value of the position error is
     * greater than IZone, the total accumulated error will reset to zero,
     * disabling integral gain until the absolute value of the position error is
     * less than IZone. This is used to prevent integral windup. Must be
     * non-negative. Passing a value of zero will effectively disable integral
     * gain. Passing a value of infinity disables IZone functionality.
     *
     * \param iZone Maximum magnitude of error to allow integral control. Must be
     *   >= 0.
     */
    constexpr void SetIZone(double iZone)
    {
        if (!std::is_constant_evaluated() && iZone < 0) {
            std::string const error = "IZone must be a non-negative number, got " + std::to_string(iZone) + "!";
            networking::Wrappers::ReportError(true, 0, error.c_str(), "PhoenixPIDController");
        }
        m_iZone = iZone;
    }
 
    /**
     * Gets the proportional coefficient.
     *
     * \returns proportional coefficient
     */
    constexpr double GetP() const { return m_Kp; }
 
    /**
     * Gets the integral coefficient.
     *
     * \returns integral coefficient
     */
    constexpr double GetI() const { return m_Ki; }
 
    /**
     * Gets the differential coefficient.
     *
     * \returns differential coefficient
     */
    constexpr double GetD() const { return m_Kd; }
 
    /**
     * Get the IZone range.
     *
     * \returns Maximum magnitude of error to allow integral control.
     */
    constexpr double GetIZone() const { return m_iZone; }
 
    /**
     * Gets the error tolerance of this controller. Defaults to 0.05.
     *
     * \returns The error tolerance of the controller.
     */
    constexpr double GetErrorTolerance() const { return m_errorTolerance; }
 
    /**
     * Gets the error derivative tolerance of this controller. Defaults to ∞.
     *
     * \returns The error derivative tolerance of the controller.
     */
    constexpr double GetErrorDerivativeTolerance() const { return m_errorDerivativeTolerance; }
 
    /**
     * Gets the accumulated error used in the integral calculation of this
     * controller.
     *
     * \returns The accumulated error of this controller.
     */
    constexpr double GetAccumulatedError() const { return m_totalError; }
 
    /**
     * Sets the setpoint for the PIDController.
     *
     * \param setpoint The desired setpoint.
     */
    constexpr void SetSetpoint(double setpoint)
    {
        m_setpoint = setpoint;
        m_haveSetpoint = true;
 
        if (m_continuous) {
            double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
            m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
        } else {
            m_error = m_setpoint - m_measurement;
        }
 
        if (m_period > 0_s) {
            m_errorDerivative = (m_error - m_prevError) / m_period.value();
        } else {
            m_errorDerivative = 0.0;
        }
    }
 
    /**
     * Returns the current setpoint of the PIDController.
     *
     * \returns The current setpoint.
     */
    constexpr double GetSetpoint() const { return m_setpoint; }
 
    /**
     * Returns true if the error is within the tolerance of the setpoint.
     * The error tolerance defaults to 0.05, and the error derivative tolerance
     * defaults to ∞.
     *
     * This will return false until at least one input value has been computed.
     */
    constexpr bool AtSetpoint() const
    {
        return m_haveMeasurement && m_haveSetpoint &&
            gcem::abs(m_error) < m_errorTolerance &&
            gcem::abs(m_errorDerivative) < m_errorDerivativeTolerance;
    }
 
    /**
     * Enables continuous input.
     *
     * Rather than 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.
     */
    constexpr void EnableContinuousInput(double minimumInput, double maximumInput)
    {
        m_continuous = true;
        m_minimumInput = minimumInput;
        m_maximumInput = maximumInput;
    }
 
    /**
     * Disables continuous input.
     */
    constexpr void DisableContinuousInput() { m_continuous = false; }
 
    /**
     * Returns true if continuous input is enabled.
     */
    constexpr bool IsContinuousInputEnabled() const { return m_continuous; }
 
    /**
     * Sets the minimum and maximum contributions of the integral term.
     *
     * The internal integrator is clamped so that the integral term's contribution
     * to the output stays between minimumIntegral and maximumIntegral. This
     * prevents integral windup.
     *
     * \param minimumIntegral The minimum contribution of the integral term.
     * \param maximumIntegral The maximum contribution of the integral term.
     */
    constexpr void SetIntegratorRange(double minimumIntegral, double maximumIntegral)
    {
        m_minimumIntegral = minimumIntegral;
        m_maximumIntegral = maximumIntegral;
    }
 
    /**
     * Sets the error which is considered tolerable for use with AtSetpoint().
     *
     * \param errorTolerance error which is tolerable.
     * \param errorDerivativeTolerance error derivative which is tolerable.
     */
    constexpr void SetTolerance(
        double errorTolerance,
        double errorDerivativeTolerance = std::numeric_limits<double>::infinity()
    ) {
        m_errorTolerance = errorTolerance;
        m_errorDerivativeTolerance = errorDerivativeTolerance;
    }
 
    /**
     * Returns the difference between the setpoint and the measurement.
     */
    constexpr double GetError() const { return m_error; }
 
    /**
     * Returns the error derivative.
     */
    constexpr double GetErrorDerivative() const { return m_errorDerivative; }
 
    /**
     * Returns the next output of the PID controller.
     *
     * \param measurement The current measurement of the process variable.
     * \param timestamp The current timestamp to use for calculating integral/derivative error.
     */
    constexpr double Calculate(double measurement, wpi::units::second_t timestamp)
    {
        m_measurement = measurement;
        m_prevError = m_error;
        m_haveMeasurement = true;
 
        m_period = timestamp - m_lastTimestamp;
        m_lastTimestamp = timestamp;
 
        if (m_continuous) {
            double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
            m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
        } else {
            m_error = m_setpoint - m_measurement;
        }
 
        m_errorDerivative = (m_error - m_prevError) / m_period.value();
 
        // If the absolute value of the position error is outside of IZone, reset
        // the total error
        if (gcem::abs(m_error) > m_iZone) {
            m_totalError = 0;
        } else if (m_Ki != 0) {
            m_totalError = std::clamp(
                m_totalError + m_error * m_period.value(),
                m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki
            );
        }
 
        m_lastAppliedOutput = m_Kp * m_error + m_Ki * m_totalError + m_Kd * m_errorDerivative;
        return m_lastAppliedOutput;
    }
 
    /**
     * Returns the next output of the PID controller.
     *
     * \param measurement The current measurement of the process variable.
     * \param setpoint The new setpoint of the controller.
     * \param timestamp The current timestamp to use for calculating integral/derivative error
     */
    constexpr double Calculate(double measurement, double setpoint, wpi::units::second_t timestamp)
    {
        m_setpoint = setpoint;
        m_haveSetpoint = true;
        return Calculate(measurement, timestamp);
    }
 
    /**
     * Returns the last applied output from this PID controller.
     */
    constexpr double GetLastAppliedOutput() const { return m_lastAppliedOutput; }
 
    /**
     * Reset the previous error and the integral term.
     */
    constexpr void Reset()
    {
        m_error = 0;
        m_prevError = 0;
        m_totalError = 0;
        m_errorDerivative = 0;
        m_haveMeasurement = false;
    }
 
#if defined(CTRE_PHOENIX6_USE_WPILIB) || defined(_CTRE_DOCS_)
    /**
     * Logs the object to a wpi::telemetry::TelemetryTable.
     *
     * \param table telemetry table
     */
    void LogTo(wpi::telemetry::TelemetryTable &table) const override;
    /**
     * Gets the telemetry table type.
     *
     * \returns Table type
     */
    constexpr std::string_view GetTelemetryType() const override { return "PIDController"; }
 
    /**
     * Publishes the tunable to the given table.
     *
     * This function may be called when the tunable is initially published or
     * republished during backend migration, and it must not throw. If an
     * exception escapes, registry and backend state is not guaranteed to be
     * restored.
     *
     * \param table table
     */
    void PublishTunable(wpi::tunables::TunableTable &table) override;
    /**
     * Gets the tunable table type.
     *
     * \returns Table type
     */
    constexpr std::string_view GetTunableType() const override { return "PIDController"; }
#endif
 
private:
    /**
     * Returns modulus of input.
     *
     * \param input        Input value to wrap.
     * \param minimumInput The minimum value expected from the input.
     * \param maximumInput The maximum value expected from the input.
     */
    template <typename T>
    static constexpr T InputModulus(T input, T minimumInput, T maximumInput)
    {
        T const modulus = maximumInput - minimumInput;
 
        // Wrap input if it's above the maximum input
        int const numMax = (input - minimumInput) / modulus;
        input -= numMax * modulus;
 
        // Wrap input if it's below the minimum input
        int const numMin = (input - maximumInput) / modulus;
        input -= numMin * modulus;
 
        return input;
    }
 
#if defined(CTRE_PHOENIX6_USE_WPILIB) || defined(_CTRE_DOCS_)
    template <typename T, typename... I>
    using Tunable = wpi::tunables::Tunable<T, I...>;
#else
    template <typename T, typename... I>
    using Tunable = T;
#endif
 
    // Factor for "proportional" control
    Tunable<double> m_Kp;
 
    // Factor for "integral" control
    Tunable<double> m_Ki;
 
    // Factor for "derivative" control
    Tunable<double> m_Kd;
 
    // The error range where "integral" control applies
    Tunable<double> m_iZone{std::numeric_limits<double>::infinity()};
 
    double m_maximumIntegral = 1.0;
 
    double m_minimumIntegral = -1.0;
 
    double m_maximumInput = 0;
 
    double m_minimumInput = 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_error = 0;
    double m_errorDerivative = 0;
 
    // The error at the time of the second-most-recent call to Calculate() (used
    // to compute velocity)
    double m_prevError = 0;
 
    // The sum of the errors for use in the integral calc
    double m_totalError = 0;
 
    // The error that is considered at setpoint.
    double m_errorTolerance = 0.05;
    double m_errorDerivativeTolerance = std::numeric_limits<double>::infinity();
 
    double m_setpoint = 0;
    double m_measurement = 0;
 
    bool m_haveSetpoint = false;
    bool m_haveMeasurement = false;
 
    double m_lastAppliedOutput;
 
    // The last timestamp acquired when performing a calculation
    wpi::units::second_t m_lastTimestamp;
    // The period (in seconds) of the current loop calling the controller
    wpi::units::second_t m_period;
 
    // Usage reporting instances
    inline static int instances = 0;
};
 
}
}
}
 
#ifdef CTRE_PHOENIX6_USE_WPILIB
#undef CTRE_PHOENIX6_USE_WPILIB
#endif