diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp index a971009f52..41c4d1f694 100644 --- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp @@ -12,6 +12,7 @@ #include +#include "frc/controller/ControllerUtil.h" #include "frc/smartdashboard/SendableBuilder.h" #include "frc/smartdashboard/SendableRegistry.h" @@ -48,13 +49,7 @@ units::second_t PIDController::GetPeriod() const { return units::second_t(m_period); } -void PIDController::SetSetpoint(double setpoint) { - if (m_maximumInput > m_minimumInput) { - m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput); - } else { - m_setpoint = setpoint; - } -} +void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; } double PIDController::GetSetpoint() const { return m_setpoint; } @@ -66,11 +61,14 @@ bool PIDController::AtSetpoint() const { void PIDController::EnableContinuousInput(double minimumInput, double maximumInput) { m_continuous = true; - SetInputRange(minimumInput, maximumInput); + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; } void PIDController::DisableContinuousInput() { m_continuous = false; } +bool PIDController::IsContinuousInputEnabled() const { return m_continuous; } + void PIDController::SetIntegratorRange(double minimumIntegral, double maximumIntegral) { m_minimumIntegral = minimumIntegral; @@ -83,15 +81,20 @@ void PIDController::SetTolerance(double positionTolerance, m_velocityTolerance = velocityTolerance; } -double PIDController::GetPositionError() const { - return GetContinuousError(m_positionError); -} +double PIDController::GetPositionError() const { return m_positionError; } double PIDController::GetVelocityError() const { return m_velocityError; } double PIDController::Calculate(double measurement) { m_prevError = m_positionError; - m_positionError = GetContinuousError(m_setpoint - measurement); + + if (m_continuous) { + m_positionError = frc::GetModulusError( + m_setpoint, measurement, m_minimumInput, m_maximumInput); + } else { + m_positionError = m_setpoint - measurement; + } + m_velocityError = (m_positionError - m_prevError) / m_period.to(); if (m_Ki != 0) { @@ -125,29 +128,3 @@ void PIDController::InitSendable(frc::SendableBuilder& builder) { builder.AddDoubleProperty("setpoint", [this] { return GetSetpoint(); }, [this](double value) { SetSetpoint(value); }); } - -double PIDController::GetContinuousError(double error) const { - if (m_continuous && m_inputRange > 0) { - error = std::fmod(error, m_inputRange); - if (std::abs(error) > m_inputRange / 2) { - if (error > 0) { - return error - m_inputRange; - } else { - return error + m_inputRange; - } - } - } - - return error; -} - -void PIDController::SetInputRange(double minimumInput, double maximumInput) { - m_minimumInput = minimumInput; - m_maximumInput = maximumInput; - m_inputRange = maximumInput - minimumInput; - - // Clamp setpoint to new input range - if (m_maximumInput > m_minimumInput) { - m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput); - } -} diff --git a/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h new file mode 100644 index 0000000000..d237d6441c --- /dev/null +++ b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +namespace frc { + +/** + * Returns modulus of error where error is the difference between the reference + * and a measurement. + * + * This implements modular subtraction defined as: + * + * e = (r mod m - x mod m) mod m + * + * with an offset in the modulus range for minimum input. + * + * @param reference Reference input of a controller. + * @param measurement The current measurement. + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ +template +T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) { + T modulus = maximumInput - minimumInput; + + if constexpr (std::is_same_v) { + T error = std::fmod(reference, modulus) - std::fmod(measurement, modulus); + + // Moduli on the difference arguments establish a precondition for the + // following modulus. + return std::fmod(error - minimumInput, modulus) + minimumInput; + } else if constexpr (std::is_same_v) { + T error = reference % modulus - measurement % modulus; + + // Moduli on the difference arguments establish a precondition for the + // following modulus. + return (error - minimumInput) % modulus + minimumInput; + } else { + T error = units::math::fmod(reference, modulus) - + units::math::fmod(measurement, modulus); + + // Moduli on the difference arguments establish a precondition for the + // following modulus. + return units::math::fmod(error - minimumInput, modulus) + minimumInput; + } +} + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h index 642cc8878e..6b41af3dae 100644 --- a/wpilibc/src/main/native/include/frc/controller/PIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h @@ -140,6 +140,11 @@ class PIDController : public frc::Sendable, */ void DisableContinuousInput(); + /** + * Returns true if continuous input is enabled. + */ + bool IsContinuousInputEnabled() const; + /** * Sets the minimum and maximum values for the integrator. * @@ -193,16 +198,6 @@ class PIDController : public frc::Sendable, void InitSendable(frc::SendableBuilder& builder) override; - protected: - /** - * Wraps error around for continuous inputs. The original error is returned if - * continuous mode is disabled. - * - * @param error The current error of the PID controller. - * @return Error for continuous inputs. - */ - double GetContinuousError(double error) const; - private: // Factor for "proportional" control double m_Kp; @@ -220,15 +215,10 @@ class PIDController : public frc::Sendable, double m_minimumIntegral = -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; @@ -248,14 +238,6 @@ class PIDController : public frc::Sendable, double m_velocityTolerance = std::numeric_limits::infinity(); double m_setpoint = 0; - - /** - * Sets the minimum and maximum values expected from the input. - * - * @param minimumInput The minimum value expected from the input. - * @param maximumInput The maximum value expected from the input. - */ - void SetInputRange(double minimumInput, double maximumInput); }; } // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h index 3e42ed147a..14f1e9b93f 100644 --- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -14,6 +14,7 @@ #include +#include "frc/controller/ControllerUtil.h" #include "frc/controller/PIDController.h" #include "frc/smartdashboard/Sendable.h" #include "frc/smartdashboard/SendableBuilder.h" @@ -251,6 +252,20 @@ class ProfiledPIDController * @param measurement The current measurement of the process variable. */ double Calculate(Distance_t measurement) { + if (m_controller.IsContinuousInputEnabled()) { + // Get error which is smallest distance between goal and measurement + auto error = frc::GetModulusError( + m_goal.position, measurement, m_minimumInput, m_maximumInput); + + // Recompute the profile goal with the smallest error, thus giving the + // shortest path. The goal may be outside the input range after this + // operation, but that's OK because the controller will still go there and + // report an error of zero. In other words, the setpoint only needs to be + // offset from the measurement by the input range modulus; they don't need + // to be equal. + m_goal.position = Distance_t{error} + measurement; + } + frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint}; m_setpoint = profile.Calculate(GetPeriod()); return m_controller.Calculate(measurement.template to(), @@ -337,6 +352,8 @@ class ProfiledPIDController private: frc2::PIDController m_controller; + Distance_t m_minimumInput{0}; + Distance_t m_maximumInput{0}; typename frc::TrapezoidProfile::State m_goal; typename frc::TrapezoidProfile::State m_setpoint; typename frc::TrapezoidProfile::Constraints m_constraints; diff --git a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp new file mode 100644 index 0000000000..82ca374ac5 --- /dev/null +++ b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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 + +#include "frc/controller/ControllerUtil.h" +#include "gtest/gtest.h" + +TEST(ControllerUtilTest, GetModulusError) { + // Test symmetric range + EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -180.0, 180.0)); + EXPECT_FLOAT_EQ(-20.0, + frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0)); + EXPECT_FLOAT_EQ(-20.0, + frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0)); + + // Test range starting at zero + EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0)); + EXPECT_FLOAT_EQ(-20.0, + frc::GetModulusError(170.0 + 360.0, 190.0, 0.0, 360.0)); + EXPECT_FLOAT_EQ(-20.0, + frc::GetModulusError(170.0, 190.0 + 360.0, 0.0, 360.0)); + + // Test asymmetric range that doesn't start at zero + EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -170.0, 190.0)); + + // Test all supported types + EXPECT_FLOAT_EQ(-20.0, + frc::GetModulusError(170.0, -170.0, -170.0, 190.0)); + EXPECT_EQ(-20, frc::GetModulusError(170, -170, -170, 190)); + EXPECT_EQ(-20_deg, frc::GetModulusError(170_deg, -170_deg, + -170_deg, 190_deg)); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java new file mode 100644 index 0000000000..f1e8716e4b --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.controller; + +public final class ControllerUtil { + /** + * Returns modulus of error where error is the difference between the reference + * and a measurement. + * + *

This implements modular subtraction defined as: + * + *

e = (r mod m - x mod m) mod m + * + *

with an offset in the modulus range for minimum input. + * + * @param reference Reference input of a controller. + * @param measurement The current measurement. + * @param minimumInput The minimum value expected from the input. + * @param maximumInput The maximum value expected from the input. + */ + public static double getModulusError(double reference, double measurement, double minimumInput, + double maximumInput) { + double modulus = maximumInput - minimumInput; + double error = reference % modulus - measurement % modulus; + + // Moduli on the difference arguments establish a precondition for the + // following modulus. + return (error - minimumInput) % modulus + minimumInput; + } + + private ControllerUtil() { + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java index 68c8dfbc61..526e91efad 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java @@ -40,15 +40,10 @@ public class PIDController implements Sendable, AutoCloseable { private double m_minimumIntegral = -1.0; - // Maximum input - limit setpoint to this private double m_maximumInput; - // Minimum input - limit setpoint to this private double m_minimumInput; - // Input range - difference between maximum and minimum - private double m_inputRange; - // Do the endpoints wrap around? eg. Absolute encoder private boolean m_continuous; @@ -196,11 +191,7 @@ public class PIDController implements Sendable, AutoCloseable { * @param setpoint The desired setpoint. */ public void setSetpoint(double setpoint) { - if (m_maximumInput > m_minimumInput) { - m_setpoint = MathUtil.clamp(setpoint, m_minimumInput, m_maximumInput); - } else { - m_setpoint = setpoint; - } + m_setpoint = setpoint; } /** @@ -236,7 +227,8 @@ public class PIDController implements Sendable, AutoCloseable { */ public void enableContinuousInput(double minimumInput, double maximumInput) { m_continuous = true; - setInputRange(minimumInput, maximumInput); + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; } /** @@ -246,6 +238,13 @@ public class PIDController implements Sendable, AutoCloseable { m_continuous = false; } + /** + * Returns true if continuous input is enabled. + */ + public boolean isContinuousInputEnabled() { + return m_continuous; + } + /** * Sets the minimum and maximum values for the integrator. * @@ -286,7 +285,7 @@ public class PIDController implements Sendable, AutoCloseable { * @return The error. */ public double getPositionError() { - return getContinuousError(m_positionError); + return m_positionError; } /** @@ -315,7 +314,14 @@ public class PIDController implements Sendable, AutoCloseable { */ public double calculate(double measurement) { m_prevError = m_positionError; - m_positionError = getContinuousError(m_setpoint - measurement); + + if (m_continuous) { + m_positionError = ControllerUtil.getModulusError(m_setpoint, measurement, m_minimumInput, + m_maximumInput); + } else { + m_positionError = m_setpoint - measurement; + } + m_velocityError = (m_positionError - m_prevError) / m_period; if (m_Ki != 0) { @@ -342,42 +348,4 @@ public class PIDController implements Sendable, AutoCloseable { builder.addDoubleProperty("d", this::getD, this::setD); builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); } - - /** - * Wraps error around for continuous inputs. The original error is returned if continuous mode is - * disabled. - * - * @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; - } - - /** - * Sets the minimum and maximum values expected from the input. - * - * @param minimumInput The minimum value expected from the input. - * @param maximumInput The maximum value expected from the input. - */ - private void setInputRange(double minimumInput, double maximumInput) { - m_minimumInput = minimumInput; - m_maximumInput = maximumInput; - m_inputRange = maximumInput - minimumInput; - - // Clamp setpoint to new input - if (m_maximumInput > m_minimumInput) { - m_setpoint = MathUtil.clamp(m_setpoint, m_minimumInput, m_maximumInput); - } - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java index 7550aa58cc..5cbce5dd06 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java @@ -23,6 +23,8 @@ public class ProfiledPIDController implements Sendable { private static int instances; private PIDController m_controller; + private double m_minimumInput; + private double m_maximumInput; private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); private TrapezoidProfile.Constraints m_constraints; @@ -279,6 +281,18 @@ public class ProfiledPIDController implements Sendable { * @param measurement The current measurement of the process variable. */ public double calculate(double measurement) { + if (m_controller.isContinuousInputEnabled()) { + // Get error which is smallest distance between goal and measurement + double error = ControllerUtil.getModulusError(m_goal.position, measurement, m_minimumInput, + m_maximumInput); + + // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal + // may be outside the input range after this operation, but that's OK because the controller + // will still go there and report an error of zero. In other words, the setpoint only needs to + // be offset from the measurement by the input range modulus; they don't need to be equal. + m_goal.position = error + measurement; + } + var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint); m_setpoint = profile.calculate(getPeriod()); return m_controller.calculate(measurement, m_setpoint.position); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java new file mode 100644 index 0000000000..58e1223d84 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 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.controller; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +class ControllerUtilTest { + @Test + void testGetModulusError() { + // Test symmetric range + assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -180.0, 180.0)); + assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, -170.0, -180.0, 180.0)); + assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0 + 360.0, -180.0, 180.0)); + + // Test range start at zero + assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0, 0.0, 360.0)); + assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, 190.0, 0.0, 360.0)); + assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0 + 360, 0.0, 360.0)); + + // Test asymmetric range that doesn't start at zero + assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -170.0, 190.0)); + } +}