mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-03 03:01:44 +00:00
[wpimath] Make controllers and some trajectory classes constexpr (#7343)
This commit is contained in:
Tyler Veness
@@ -81,9 +81,9 @@ class WPILIB_DLLEXPORT ArmFeedforward {
|
||||
* @return The computed feedforward, in volts.
|
||||
*/
|
||||
[[deprecated("Use the current/next velocity overload instead.")]]
|
||||
units::volt_t Calculate(units::unit_t<Angle> angle,
|
||||
units::unit_t<Velocity> velocity,
|
||||
units::unit_t<Acceleration> acceleration) const {
|
||||
constexpr units::volt_t Calculate(
|
||||
units::unit_t<Angle> angle, units::unit_t<Velocity> velocity,
|
||||
units::unit_t<Acceleration> acceleration) const {
|
||||
return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) +
|
||||
kV * velocity + kA * acceleration;
|
||||
}
|
||||
@@ -105,7 +105,9 @@ class WPILIB_DLLEXPORT ArmFeedforward {
|
||||
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
|
||||
units::unit_t<Velocity> currentVelocity,
|
||||
units::unit_t<Velocity> nextVelocity,
|
||||
units::second_t dt) const;
|
||||
units::second_t dt) const {
|
||||
return Calculate(currentAngle, currentVelocity, nextVelocity);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoint assuming discrete
|
||||
@@ -118,8 +120,12 @@ class WPILIB_DLLEXPORT ArmFeedforward {
|
||||
* @param currentVelocity The current velocity.
|
||||
* @return The computed feedforward in volts.
|
||||
*/
|
||||
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
|
||||
units::unit_t<Velocity> currentVelocity) const;
|
||||
constexpr units::volt_t Calculate(
|
||||
units::unit_t<Angle> currentAngle,
|
||||
units::unit_t<Velocity> currentVelocity) const {
|
||||
return kS * wpi::sgn(currentVelocity) +
|
||||
kG * units::math::cos(currentAngle) + kV * currentVelocity;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints assuming discrete
|
||||
@@ -156,7 +162,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
|
||||
* @param acceleration The acceleration of the arm.
|
||||
* @return The maximum possible velocity at the given acceleration and angle.
|
||||
*/
|
||||
units::unit_t<Velocity> MaxAchievableVelocity(
|
||||
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
|
||||
units::volt_t maxVoltage, units::unit_t<Angle> angle,
|
||||
units::unit_t<Acceleration> acceleration) {
|
||||
// Assume max velocity is positive
|
||||
@@ -181,7 +187,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
|
||||
* @param acceleration The acceleration of the arm.
|
||||
* @return The minimum possible velocity at the given acceleration and angle.
|
||||
*/
|
||||
units::unit_t<Velocity> MinAchievableVelocity(
|
||||
constexpr units::unit_t<Velocity> MinAchievableVelocity(
|
||||
units::volt_t maxVoltage, units::unit_t<Angle> angle,
|
||||
units::unit_t<Acceleration> acceleration) {
|
||||
// Assume min velocity is negative, ks flips sign
|
||||
@@ -206,7 +212,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
|
||||
* @param velocity The velocity of the arm.
|
||||
* @return The maximum possible acceleration at the given velocity and angle.
|
||||
*/
|
||||
units::unit_t<Acceleration> MaxAchievableAcceleration(
|
||||
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
|
||||
units::volt_t maxVoltage, units::unit_t<Angle> angle,
|
||||
units::unit_t<Velocity> velocity) {
|
||||
return (maxVoltage - kS * wpi::sgn(velocity) -
|
||||
@@ -230,7 +236,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
|
||||
* @param velocity The velocity of the arm.
|
||||
* @return The minimum possible acceleration at the given velocity and angle.
|
||||
*/
|
||||
units::unit_t<Acceleration> MinAchievableAcceleration(
|
||||
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
|
||||
units::volt_t maxVoltage, units::unit_t<Angle> angle,
|
||||
units::unit_t<Velocity> velocity) {
|
||||
return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
|
||||
@@ -241,28 +247,28 @@ class WPILIB_DLLEXPORT ArmFeedforward {
|
||||
*
|
||||
* @return The static gain.
|
||||
*/
|
||||
units::volt_t GetKs() const { return kS; }
|
||||
constexpr units::volt_t GetKs() const { return kS; }
|
||||
|
||||
/**
|
||||
* Returns the gravity gain.
|
||||
*
|
||||
* @return The gravity gain.
|
||||
*/
|
||||
units::volt_t GetKg() const { return kG; }
|
||||
constexpr units::volt_t GetKg() const { return kG; }
|
||||
|
||||
/**
|
||||
* Returns the velocity gain.
|
||||
*
|
||||
* @return The velocity gain.
|
||||
*/
|
||||
units::unit_t<kv_unit> GetKv() const { return kV; }
|
||||
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
|
||||
|
||||
/**
|
||||
* Returns the acceleration gain.
|
||||
*
|
||||
* @return The acceleration gain.
|
||||
*/
|
||||
units::unit_t<ka_unit> GetKa() const { return kA; }
|
||||
constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
|
||||
|
||||
private:
|
||||
/// The static gain, in volts.
|
||||
|
||||
@@ -6,6 +6,7 @@
|
||||
|
||||
#include <limits>
|
||||
|
||||
#include <gcem.hpp>
|
||||
#include <wpi/SymbolExports.h>
|
||||
#include <wpi/sendable/Sendable.h>
|
||||
#include <wpi/sendable/SendableHelper.h>
|
||||
@@ -37,57 +38,60 @@ class WPILIB_DLLEXPORT BangBangController
|
||||
*
|
||||
* @param tolerance Tolerance for atSetpoint.
|
||||
*/
|
||||
explicit BangBangController(
|
||||
double tolerance = std::numeric_limits<double>::infinity());
|
||||
constexpr explicit BangBangController(
|
||||
double tolerance = std::numeric_limits<double>::infinity())
|
||||
: m_tolerance(tolerance) {}
|
||||
|
||||
/**
|
||||
* Sets the setpoint for the bang-bang controller.
|
||||
*
|
||||
* @param setpoint The desired setpoint.
|
||||
*/
|
||||
void SetSetpoint(double setpoint);
|
||||
constexpr void SetSetpoint(double setpoint) { m_setpoint = setpoint; }
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the bang-bang controller.
|
||||
*
|
||||
* @return The current setpoint.
|
||||
*/
|
||||
double GetSetpoint() const;
|
||||
constexpr double GetSetpoint() const { return m_setpoint; }
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the setpoint.
|
||||
*
|
||||
* @return Whether the error is within the acceptable bounds.
|
||||
*/
|
||||
bool AtSetpoint() const;
|
||||
constexpr bool AtSetpoint() const {
|
||||
return gcem::abs(m_setpoint - m_measurement) < m_tolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the error within which AtSetpoint will return true.
|
||||
*
|
||||
* @param tolerance Position error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(double tolerance);
|
||||
constexpr void SetTolerance(double tolerance) { m_tolerance = tolerance; }
|
||||
|
||||
/**
|
||||
* Returns the current tolerance of the controller.
|
||||
*
|
||||
* @return The current tolerance.
|
||||
*/
|
||||
double GetTolerance() const;
|
||||
constexpr double GetTolerance() const { return m_tolerance; }
|
||||
|
||||
/**
|
||||
* Returns the current measurement of the process variable.
|
||||
*
|
||||
* @return The current measurement of the process variable.
|
||||
*/
|
||||
double GetMeasurement() const;
|
||||
constexpr double GetMeasurement() const { return m_measurement; }
|
||||
|
||||
/**
|
||||
* Returns the current error.
|
||||
*
|
||||
* @return The current error.
|
||||
*/
|
||||
double GetError() const;
|
||||
constexpr double GetError() const { return m_setpoint - m_measurement; }
|
||||
|
||||
/**
|
||||
* Returns the calculated control output.
|
||||
@@ -99,7 +103,12 @@ class WPILIB_DLLEXPORT BangBangController
|
||||
* @param setpoint The setpoint for the process variable.
|
||||
* @return The calculated motor output (0 or 1).
|
||||
*/
|
||||
double Calculate(double measurement, double setpoint);
|
||||
constexpr double Calculate(double measurement, double setpoint) {
|
||||
m_measurement = measurement;
|
||||
m_setpoint = setpoint;
|
||||
|
||||
return measurement < setpoint ? 1 : 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the calculated control output.
|
||||
@@ -107,7 +116,9 @@ class WPILIB_DLLEXPORT BangBangController
|
||||
* @param measurement The most recent measurement of the process variable.
|
||||
* @return The calculated motor output (0 or 1).
|
||||
*/
|
||||
double Calculate(double measurement);
|
||||
constexpr double Calculate(double measurement) {
|
||||
return Calculate(measurement, m_setpoint);
|
||||
}
|
||||
|
||||
void InitSendable(wpi::SendableBuilder& builder) override;
|
||||
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <wpi/SymbolExports.h>
|
||||
|
||||
@@ -38,7 +40,10 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
|
||||
DifferentialDriveAccelerationLimiter(
|
||||
LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
|
||||
units::meters_per_second_squared_t maxLinearAccel,
|
||||
units::radians_per_second_squared_t maxAngularAccel);
|
||||
units::radians_per_second_squared_t maxAngularAccel)
|
||||
: DifferentialDriveAccelerationLimiter(system, trackwidth,
|
||||
-maxLinearAccel, maxLinearAccel,
|
||||
maxAngularAccel) {}
|
||||
|
||||
/**
|
||||
* Constructs a DifferentialDriveAccelerationLimiter.
|
||||
@@ -56,7 +61,17 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
|
||||
LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
|
||||
units::meters_per_second_squared_t minLinearAccel,
|
||||
units::meters_per_second_squared_t maxLinearAccel,
|
||||
units::radians_per_second_squared_t maxAngularAccel);
|
||||
units::radians_per_second_squared_t maxAngularAccel)
|
||||
: m_system{std::move(system)},
|
||||
m_trackwidth{trackwidth},
|
||||
m_minLinearAccel{minLinearAccel},
|
||||
m_maxLinearAccel{maxLinearAccel},
|
||||
m_maxAngularAccel{maxAngularAccel} {
|
||||
if (minLinearAccel > maxLinearAccel) {
|
||||
throw std::invalid_argument(
|
||||
"maxLinearAccel must be greater than minLinearAccel");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next voltage pair subject to acceleration constraints.
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
|
||||
#include "frc/controller/DifferentialDriveWheelVoltages.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "frc/system/plant/LinearSystemId.h"
|
||||
#include "units/acceleration.h"
|
||||
#include "units/angular_acceleration.h"
|
||||
#include "units/angular_velocity.h"
|
||||
@@ -38,11 +39,16 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
|
||||
* @param trackwidth The distance between the differential drive's left and
|
||||
* right wheels, in meters.
|
||||
*/
|
||||
DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
|
||||
decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_rad_per_s) kVAngular,
|
||||
decltype(1_V / 1_rad_per_s_sq) kAAngular,
|
||||
units::meter_t trackwidth);
|
||||
constexpr DifferentialDriveFeedforward(
|
||||
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_rad_per_s) kVAngular,
|
||||
decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
|
||||
// See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
|
||||
// decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
|
||||
// 1_rad_per_s_sq))
|
||||
: DifferentialDriveFeedforward{kVLinear, kALinear,
|
||||
kVAngular * 2.0 / trackwidth * 1_rad,
|
||||
kAAngular * 2.0 / trackwidth * 1_rad} {}
|
||||
|
||||
/**
|
||||
* Creates a new DifferentialDriveFeedforward with the specified parameters.
|
||||
@@ -55,10 +61,16 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
|
||||
* @param kAAngular The angular acceleration gain in volts per (meters per
|
||||
* second squared).
|
||||
*/
|
||||
DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
|
||||
decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_mps) kVAngular,
|
||||
decltype(1_V / 1_mps_sq) kAAngular);
|
||||
constexpr DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
|
||||
decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_mps) kVAngular,
|
||||
decltype(1_V / 1_mps_sq) kAAngular)
|
||||
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
kVLinear, kALinear, kVAngular, kAAngular)},
|
||||
m_kVLinear{kVLinear},
|
||||
m_kALinear{kALinear},
|
||||
m_kVAngular{kVAngular},
|
||||
m_kAAngular{kAAngular} {}
|
||||
|
||||
/**
|
||||
* Calculates the differential drive feedforward inputs given velocity
|
||||
|
||||
@@ -77,8 +77,9 @@ class ElevatorFeedforward {
|
||||
* @deprecated Use the current/next velocity overload instead.
|
||||
*/
|
||||
[[deprecated("Use the current/next velocity overload instead.")]]
|
||||
units::volt_t Calculate(units::unit_t<Velocity> velocity,
|
||||
units::unit_t<Acceleration> acceleration) const {
|
||||
constexpr units::volt_t Calculate(
|
||||
units::unit_t<Velocity> velocity,
|
||||
units::unit_t<Acceleration> acceleration) const {
|
||||
return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
|
||||
}
|
||||
|
||||
@@ -99,8 +100,8 @@ class ElevatorFeedforward {
|
||||
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
|
||||
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
|
||||
|
||||
Vectord<1> r{currentVelocity.value()};
|
||||
Vectord<1> nextR{nextVelocity.value()};
|
||||
Vectord<1> r{{currentVelocity.value()}};
|
||||
Vectord<1> nextR{{nextVelocity.value()}};
|
||||
|
||||
return kG + kS * wpi::sgn(currentVelocity.value()) +
|
||||
units::volt_t{feedforward.Calculate(r, nextR)(0)};
|
||||
@@ -220,28 +221,28 @@ class ElevatorFeedforward {
|
||||
*
|
||||
* @return The static gain.
|
||||
*/
|
||||
units::volt_t GetKs() const { return kS; }
|
||||
constexpr units::volt_t GetKs() const { return kS; }
|
||||
|
||||
/**
|
||||
* Returns the gravity gain.
|
||||
*
|
||||
* @return The gravity gain.
|
||||
*/
|
||||
units::volt_t GetKg() const { return kG; }
|
||||
constexpr units::volt_t GetKg() const { return kG; }
|
||||
|
||||
/**
|
||||
* Returns the velocity gain.
|
||||
*
|
||||
* @return The velocity gain.
|
||||
*/
|
||||
units::unit_t<kv_unit> GetKv() const { return kV; }
|
||||
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
|
||||
|
||||
/**
|
||||
* Returns the acceleration gain.
|
||||
*
|
||||
* @return The acceleration gain.
|
||||
*/
|
||||
units::unit_t<ka_unit> GetKa() const { return kA; }
|
||||
constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
|
||||
|
||||
private:
|
||||
/// The static gain.
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
|
||||
#include "frc/controller/PIDController.h"
|
||||
@@ -13,6 +15,7 @@
|
||||
#include "frc/kinematics/ChassisSpeeds.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "units/angle.h"
|
||||
#include "units/angular_velocity.h"
|
||||
#include "units/velocity.h"
|
||||
|
||||
namespace frc {
|
||||
@@ -41,20 +44,34 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
* @param thetaController A profiled PID controller to respond to error in
|
||||
* angle.
|
||||
*/
|
||||
HolonomicDriveController(
|
||||
constexpr HolonomicDriveController(
|
||||
PIDController xController, PIDController yController,
|
||||
ProfiledPIDController<units::radian> thetaController);
|
||||
ProfiledPIDController<units::radian> thetaController)
|
||||
: m_xController(std::move(xController)),
|
||||
m_yController(std::move(yController)),
|
||||
m_thetaController(std::move(thetaController)) {
|
||||
m_thetaController.EnableContinuousInput(0_deg, 360.0_deg);
|
||||
}
|
||||
|
||||
HolonomicDriveController(const HolonomicDriveController&) = default;
|
||||
HolonomicDriveController& operator=(const HolonomicDriveController&) =
|
||||
constexpr HolonomicDriveController(const HolonomicDriveController&) = default;
|
||||
constexpr HolonomicDriveController& operator=(
|
||||
const HolonomicDriveController&) = default;
|
||||
constexpr HolonomicDriveController(HolonomicDriveController&&) = default;
|
||||
constexpr HolonomicDriveController& operator=(HolonomicDriveController&&) =
|
||||
default;
|
||||
HolonomicDriveController(HolonomicDriveController&&) = default;
|
||||
HolonomicDriveController& operator=(HolonomicDriveController&&) = default;
|
||||
|
||||
/**
|
||||
* Returns true if the pose error is within tolerance of the reference.
|
||||
*/
|
||||
bool AtReference() const;
|
||||
constexpr bool AtReference() const {
|
||||
const auto& eTranslate = m_poseError.Translation();
|
||||
const auto& eRotate = m_rotationError;
|
||||
const auto& tolTranslate = m_poseTolerance.Translation();
|
||||
const auto& tolRotate = m_poseTolerance.Rotation();
|
||||
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
|
||||
units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
|
||||
units::math::abs(eRotate.Radians()) < tolRotate.Radians();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerable for use with
|
||||
@@ -62,7 +79,9 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
*
|
||||
* @param tolerance Pose error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(const Pose2d& tolerance);
|
||||
constexpr void SetTolerance(const Pose2d& tolerance) {
|
||||
m_poseTolerance = tolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
@@ -75,10 +94,41 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
* @param desiredHeading The desired heading.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Pose2d& trajectoryPose,
|
||||
units::meters_per_second_t desiredLinearVelocity,
|
||||
const Rotation2d& desiredHeading);
|
||||
constexpr ChassisSpeeds Calculate(
|
||||
const Pose2d& currentPose, const Pose2d& trajectoryPose,
|
||||
units::meters_per_second_t desiredLinearVelocity,
|
||||
const Rotation2d& desiredHeading) {
|
||||
// If this is the first run, then we need to reset the theta controller to
|
||||
// the current pose's heading.
|
||||
if (m_firstRun) {
|
||||
m_thetaController.Reset(currentPose.Rotation().Radians());
|
||||
m_firstRun = false;
|
||||
}
|
||||
|
||||
// Calculate feedforward velocities (field-relative)
|
||||
auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
|
||||
auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
|
||||
auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
|
||||
currentPose.Rotation().Radians(), desiredHeading.Radians())};
|
||||
|
||||
m_poseError = trajectoryPose.RelativeTo(currentPose);
|
||||
m_rotationError = desiredHeading - currentPose.Rotation();
|
||||
|
||||
if (!m_enabled) {
|
||||
return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
|
||||
currentPose.Rotation());
|
||||
}
|
||||
|
||||
// Calculate feedback velocities (based on position error).
|
||||
auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
|
||||
currentPose.X().value(), trajectoryPose.X().value())};
|
||||
auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
|
||||
currentPose.Y().value(), trajectoryPose.Y().value())};
|
||||
|
||||
// Return next output.
|
||||
return ChassisSpeeds::FromFieldRelativeSpeeds(
|
||||
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
@@ -90,9 +140,12 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
* @param desiredHeading The desired heading.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Trajectory::State& desiredState,
|
||||
const Rotation2d& desiredHeading);
|
||||
constexpr ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Trajectory::State& desiredState,
|
||||
const Rotation2d& desiredHeading) {
|
||||
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
|
||||
desiredHeading);
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting purposes. When
|
||||
@@ -101,7 +154,7 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
void SetEnabled(bool enabled);
|
||||
constexpr void SetEnabled(bool enabled) { m_enabled = enabled; }
|
||||
|
||||
/**
|
||||
* Returns the X PIDController
|
||||
@@ -109,7 +162,9 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
* @deprecated Use GetXController() instead.
|
||||
*/
|
||||
[[deprecated("Use GetXController() instead")]]
|
||||
PIDController& getXController();
|
||||
constexpr PIDController& getXController() {
|
||||
return m_xController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the Y PIDController
|
||||
@@ -117,7 +172,9 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
* @deprecated Use GetYController() instead.
|
||||
*/
|
||||
[[deprecated("Use GetYController() instead")]]
|
||||
PIDController& getYController();
|
||||
constexpr PIDController& getYController() {
|
||||
return m_yController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotation ProfiledPIDController
|
||||
@@ -125,22 +182,26 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
* @deprecated Use GetThetaController() instead.
|
||||
*/
|
||||
[[deprecated("Use GetThetaController() instead")]]
|
||||
ProfiledPIDController<units::radian>& getThetaController();
|
||||
constexpr ProfiledPIDController<units::radian>& getThetaController() {
|
||||
return m_thetaController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the X PIDController
|
||||
*/
|
||||
PIDController& GetXController();
|
||||
constexpr PIDController& GetXController() { return m_xController; }
|
||||
|
||||
/**
|
||||
* Returns the Y PIDController
|
||||
*/
|
||||
PIDController& GetYController();
|
||||
constexpr PIDController& GetYController() { return m_yController; }
|
||||
|
||||
/**
|
||||
* Returns the rotation ProfiledPIDController
|
||||
*/
|
||||
ProfiledPIDController<units::radian>& GetThetaController();
|
||||
constexpr ProfiledPIDController<units::radian>& GetThetaController() {
|
||||
return m_thetaController;
|
||||
}
|
||||
|
||||
private:
|
||||
Pose2d m_poseError;
|
||||
|
||||
@@ -4,14 +4,20 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <type_traits>
|
||||
|
||||
#include <gcem.hpp>
|
||||
#include <wpi/SymbolExports.h>
|
||||
#include <wpi/sendable/Sendable.h>
|
||||
#include <wpi/sendable/SendableHelper.h>
|
||||
#include <wpi/sendable/SendableRegistry.h>
|
||||
|
||||
#include "frc/MathUtil.h"
|
||||
#include "units/time.h"
|
||||
#include "wpimath/MathShared.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
@@ -31,15 +37,55 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @param period The period between controller updates in seconds. The
|
||||
* default is 20 milliseconds. Must be positive.
|
||||
*/
|
||||
PIDController(double Kp, double Ki, double Kd,
|
||||
units::second_t period = 20_ms);
|
||||
constexpr PIDController(double Kp, double Ki, double Kd,
|
||||
units::second_t period = 20_ms)
|
||||
: m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
|
||||
bool invalidGains = false;
|
||||
if (Kp < 0.0) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"Kp must be a non-negative number, got {}!", Kp);
|
||||
invalidGains = true;
|
||||
}
|
||||
if (Ki < 0.0) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"Ki must be a non-negative number, got {}!", Ki);
|
||||
invalidGains = true;
|
||||
}
|
||||
if (Kd < 0.0) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"Kd must be a non-negative number, got {}!", Kd);
|
||||
invalidGains = true;
|
||||
}
|
||||
if (invalidGains) {
|
||||
m_Kp = 0.0;
|
||||
m_Ki = 0.0;
|
||||
m_Kd = 0.0;
|
||||
wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0.");
|
||||
}
|
||||
|
||||
~PIDController() override = default;
|
||||
if (period <= 0_s) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"Controller period must be a positive number, got {}!",
|
||||
period.value());
|
||||
m_period = 20_ms;
|
||||
wpi::math::MathSharedStore::ReportWarning(
|
||||
"Controller period defaulted to 20ms.");
|
||||
}
|
||||
if (!std::is_constant_evaluated()) {
|
||||
++instances;
|
||||
|
||||
PIDController(const PIDController&) = default;
|
||||
PIDController& operator=(const PIDController&) = default;
|
||||
PIDController(PIDController&&) = default;
|
||||
PIDController& operator=(PIDController&&) = default;
|
||||
wpi::math::MathSharedStore::ReportUsage(
|
||||
wpi::math::MathUsageId::kController_PIDController2, instances);
|
||||
wpi::SendableRegistry::Add(this, "PIDController", instances);
|
||||
}
|
||||
}
|
||||
|
||||
constexpr ~PIDController() override = default;
|
||||
|
||||
constexpr PIDController(const PIDController&) = default;
|
||||
constexpr PIDController& operator=(const PIDController&) = default;
|
||||
constexpr PIDController(PIDController&&) = default;
|
||||
constexpr PIDController& operator=(PIDController&&) = default;
|
||||
|
||||
/**
|
||||
* Sets the PID Controller gain parameters.
|
||||
@@ -50,28 +96,32 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @param Ki The integral coefficient. Must be >= 0.
|
||||
* @param Kd The differential coefficient. Must be >= 0.
|
||||
*/
|
||||
void SetPID(double Kp, double Ki, double Kd);
|
||||
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.
|
||||
*
|
||||
* @param Kp The proportional coefficient. Must be >= 0.
|
||||
*/
|
||||
void SetP(double Kp);
|
||||
constexpr void SetP(double Kp) { m_Kp = Kp; }
|
||||
|
||||
/**
|
||||
* Sets the integral coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Ki The integral coefficient. Must be >= 0.
|
||||
*/
|
||||
void SetI(double Ki);
|
||||
constexpr void SetI(double Ki) { m_Ki = Ki; }
|
||||
|
||||
/**
|
||||
* Sets the differential coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kd The differential coefficient. Must be >= 0.
|
||||
*/
|
||||
void SetD(double Kd);
|
||||
constexpr void SetD(double Kd) { m_Kd = Kd; }
|
||||
|
||||
/**
|
||||
* Sets the IZone range. When the absolute value of the position error is
|
||||
@@ -84,56 +134,64 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @param iZone Maximum magnitude of error to allow integral control. Must be
|
||||
* >= 0.
|
||||
*/
|
||||
void SetIZone(double iZone);
|
||||
constexpr void SetIZone(double iZone) {
|
||||
if (std::is_constant_evaluated() && iZone < 0) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"IZone must be a non-negative number, got {}!", iZone);
|
||||
}
|
||||
m_iZone = iZone;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the proportional coefficient.
|
||||
*
|
||||
* @return proportional coefficient
|
||||
*/
|
||||
double GetP() const;
|
||||
constexpr double GetP() const { return m_Kp; }
|
||||
|
||||
/**
|
||||
* Gets the integral coefficient.
|
||||
*
|
||||
* @return integral coefficient
|
||||
*/
|
||||
double GetI() const;
|
||||
constexpr double GetI() const { return m_Ki; }
|
||||
|
||||
/**
|
||||
* Gets the differential coefficient.
|
||||
*
|
||||
* @return differential coefficient
|
||||
*/
|
||||
double GetD() const;
|
||||
constexpr double GetD() const { return m_Kd; }
|
||||
|
||||
/**
|
||||
* Get the IZone range.
|
||||
*
|
||||
* @return Maximum magnitude of error to allow integral control.
|
||||
*/
|
||||
double GetIZone() const;
|
||||
constexpr double GetIZone() const { return m_iZone; }
|
||||
|
||||
/**
|
||||
* Gets the period of this controller.
|
||||
*
|
||||
* @return The period of the controller.
|
||||
*/
|
||||
units::second_t GetPeriod() const;
|
||||
constexpr units::second_t GetPeriod() const { return m_period; }
|
||||
|
||||
/**
|
||||
* Gets the error tolerance of this controller.
|
||||
*
|
||||
* @return The error tolerance of the controller.
|
||||
*/
|
||||
double GetErrorTolerance() const;
|
||||
constexpr double GetErrorTolerance() const { return m_errorTolerance; }
|
||||
|
||||
/**
|
||||
* Gets the error derivative tolerance of this controller.
|
||||
*
|
||||
* @return The error derivative tolerance of the controller.
|
||||
*/
|
||||
double GetErrorDerivativeTolerance() const;
|
||||
constexpr double GetErrorDerivativeTolerance() const {
|
||||
return m_errorDerivativeTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the position tolerance of this controller.
|
||||
@@ -142,7 +200,9 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @deprecated Use GetErrorTolerance() instead.
|
||||
*/
|
||||
[[deprecated("Use the GetErrorTolerance method instead.")]]
|
||||
double GetPositionTolerance() const;
|
||||
constexpr double GetPositionTolerance() const {
|
||||
return m_errorTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the velocity tolerance of this controller.
|
||||
@@ -151,7 +211,9 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @deprecated Use GetErrorDerivativeTolerance() instead.
|
||||
*/
|
||||
[[deprecated("Use the GetErrorDerivativeTolerance method instead.")]]
|
||||
double GetVelocityTolerance() const;
|
||||
constexpr double GetVelocityTolerance() const {
|
||||
return m_errorDerivativeTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the accumulated error used in the integral calculation of this
|
||||
@@ -159,28 +221,45 @@ class WPILIB_DLLEXPORT PIDController
|
||||
*
|
||||
* @return The accumulated error of this controller.
|
||||
*/
|
||||
double GetAccumulatedError() const;
|
||||
constexpr double GetAccumulatedError() const { return m_totalError; }
|
||||
|
||||
/**
|
||||
* Sets the setpoint for the PIDController.
|
||||
*
|
||||
* @param setpoint The desired setpoint.
|
||||
*/
|
||||
void SetSetpoint(double 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;
|
||||
}
|
||||
|
||||
m_errorDerivative = (m_error - m_prevError) / m_period.value();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the PIDController.
|
||||
*
|
||||
* @return The current setpoint.
|
||||
*/
|
||||
double GetSetpoint() const;
|
||||
constexpr double GetSetpoint() const { return m_setpoint; }
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the setpoint.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*/
|
||||
bool AtSetpoint() const;
|
||||
constexpr bool AtSetpoint() const {
|
||||
return m_haveMeasurement && m_haveSetpoint &&
|
||||
gcem::abs(m_error) < m_errorTolerance &&
|
||||
gcem::abs(m_errorDerivative) < m_errorDerivativeTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables continuous input.
|
||||
@@ -192,17 +271,22 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @param minimumInput The minimum value expected from the input.
|
||||
* @param maximumInput The maximum value expected from the input.
|
||||
*/
|
||||
void EnableContinuousInput(double minimumInput, double maximumInput);
|
||||
constexpr void EnableContinuousInput(double minimumInput,
|
||||
double maximumInput) {
|
||||
m_continuous = true;
|
||||
m_minimumInput = minimumInput;
|
||||
m_maximumInput = maximumInput;
|
||||
}
|
||||
|
||||
/**
|
||||
* Disables continuous input.
|
||||
*/
|
||||
void DisableContinuousInput();
|
||||
constexpr void DisableContinuousInput() { m_continuous = false; }
|
||||
|
||||
/**
|
||||
* Returns true if continuous input is enabled.
|
||||
*/
|
||||
bool IsContinuousInputEnabled() const;
|
||||
constexpr bool IsContinuousInputEnabled() const { return m_continuous; }
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum contributions of the integral term.
|
||||
@@ -214,7 +298,11 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @param minimumIntegral The minimum contribution of the integral term.
|
||||
* @param maximumIntegral The maximum contribution of the integral term.
|
||||
*/
|
||||
void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
|
||||
constexpr void SetIntegratorRange(double minimumIntegral,
|
||||
double maximumIntegral) {
|
||||
m_minimumIntegral = minimumIntegral;
|
||||
m_maximumIntegral = maximumIntegral;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the error which is considered tolerable for use with AtSetpoint().
|
||||
@@ -222,40 +310,73 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @param errorTolerance error which is tolerable.
|
||||
* @param errorDerivativeTolerance error derivative which is tolerable.
|
||||
*/
|
||||
void SetTolerance(double errorTolerance,
|
||||
double errorDerivativeTolerance =
|
||||
std::numeric_limits<double>::infinity());
|
||||
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.
|
||||
*/
|
||||
double GetError() const;
|
||||
constexpr double GetError() const { return m_error; }
|
||||
|
||||
/**
|
||||
* Returns the error derivative.
|
||||
*/
|
||||
double GetErrorDerivative() const;
|
||||
constexpr double GetErrorDerivative() const { return m_errorDerivative; }
|
||||
|
||||
/**
|
||||
* Returns the difference between the setpoint and the measurement.
|
||||
* @deprecated Use GetError() instead.
|
||||
*/
|
||||
[[deprecated("Use GetError method instead.")]]
|
||||
double GetPositionError() const;
|
||||
constexpr double GetPositionError() const {
|
||||
return m_error;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the velocity error.
|
||||
* @deprecated Use GetErrorDerivative() instead.
|
||||
*/
|
||||
[[deprecated("Use GetErrorDerivative method instead.")]]
|
||||
double GetVelocityError() const;
|
||||
constexpr double GetVelocityError() const {
|
||||
return m_errorDerivative;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
*/
|
||||
double Calculate(double measurement);
|
||||
constexpr double Calculate(double measurement) {
|
||||
m_measurement = measurement;
|
||||
m_prevError = m_error;
|
||||
m_haveMeasurement = 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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
return m_Kp * m_error + m_Ki * m_totalError + m_Kd * m_errorDerivative;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
@@ -263,12 +384,22 @@ class WPILIB_DLLEXPORT PIDController
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @param setpoint The new setpoint of the controller.
|
||||
*/
|
||||
double Calculate(double measurement, double setpoint);
|
||||
constexpr double Calculate(double measurement, double setpoint) {
|
||||
m_setpoint = setpoint;
|
||||
m_haveSetpoint = true;
|
||||
return Calculate(measurement);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the previous error, the integral term, and disable the controller.
|
||||
*/
|
||||
void Reset();
|
||||
constexpr void Reset() {
|
||||
m_error = 0;
|
||||
m_prevError = 0;
|
||||
m_totalError = 0;
|
||||
m_errorDerivative = 0;
|
||||
m_haveMeasurement = false;
|
||||
}
|
||||
|
||||
void InitSendable(wpi::SendableBuilder& builder) override;
|
||||
|
||||
@@ -319,6 +450,9 @@ class WPILIB_DLLEXPORT PIDController
|
||||
|
||||
bool m_haveSetpoint = false;
|
||||
bool m_haveMeasurement = false;
|
||||
|
||||
// Usage reporting instances
|
||||
inline static int instances = 0;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -4,10 +4,9 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
#include <type_traits>
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
#include <wpi/sendable/Sendable.h>
|
||||
@@ -56,23 +55,27 @@ class ProfiledPIDController
|
||||
* @param period The period between controller updates in seconds. The
|
||||
* default is 20 milliseconds. Must be positive.
|
||||
*/
|
||||
ProfiledPIDController(double Kp, double Ki, double Kd,
|
||||
Constraints constraints, units::second_t period = 20_ms)
|
||||
constexpr ProfiledPIDController(double Kp, double Ki, double Kd,
|
||||
Constraints constraints,
|
||||
units::second_t period = 20_ms)
|
||||
: m_controller{Kp, Ki, Kd, period},
|
||||
m_constraints{constraints},
|
||||
m_profile{m_constraints} {
|
||||
int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
|
||||
wpi::math::MathSharedStore::ReportUsage(
|
||||
wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
|
||||
wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances);
|
||||
if (!std::is_constant_evaluated()) {
|
||||
int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
|
||||
wpi::math::MathSharedStore::ReportUsage(
|
||||
wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
|
||||
wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances);
|
||||
}
|
||||
}
|
||||
|
||||
~ProfiledPIDController() override = default;
|
||||
constexpr ~ProfiledPIDController() override = default;
|
||||
|
||||
ProfiledPIDController(const ProfiledPIDController&) = default;
|
||||
ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
|
||||
ProfiledPIDController(ProfiledPIDController&&) = default;
|
||||
ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
|
||||
constexpr ProfiledPIDController(const ProfiledPIDController&) = default;
|
||||
constexpr ProfiledPIDController& operator=(const ProfiledPIDController&) =
|
||||
default;
|
||||
constexpr ProfiledPIDController(ProfiledPIDController&&) = default;
|
||||
constexpr ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
|
||||
|
||||
/**
|
||||
* Sets the PID Controller gain parameters.
|
||||
@@ -83,7 +86,7 @@ class ProfiledPIDController
|
||||
* @param Ki The integral coefficient. Must be >= 0.
|
||||
* @param Kd The differential coefficient. Must be >= 0.
|
||||
*/
|
||||
void SetPID(double Kp, double Ki, double Kd) {
|
||||
constexpr void SetPID(double Kp, double Ki, double Kd) {
|
||||
m_controller.SetPID(Kp, Ki, Kd);
|
||||
}
|
||||
|
||||
@@ -92,21 +95,21 @@ class ProfiledPIDController
|
||||
*
|
||||
* @param Kp The proportional coefficient. Must be >= 0.
|
||||
*/
|
||||
void SetP(double Kp) { m_controller.SetP(Kp); }
|
||||
constexpr void SetP(double Kp) { m_controller.SetP(Kp); }
|
||||
|
||||
/**
|
||||
* Sets the integral coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Ki The integral coefficient. Must be >= 0.
|
||||
*/
|
||||
void SetI(double Ki) { m_controller.SetI(Ki); }
|
||||
constexpr void SetI(double Ki) { m_controller.SetI(Ki); }
|
||||
|
||||
/**
|
||||
* Sets the differential coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kd The differential coefficient. Must be >= 0.
|
||||
*/
|
||||
void SetD(double Kd) { m_controller.SetD(Kd); }
|
||||
constexpr void SetD(double Kd) { m_controller.SetD(Kd); }
|
||||
|
||||
/**
|
||||
* Sets the IZone range. When the absolute value of the position error is
|
||||
@@ -119,49 +122,51 @@ class ProfiledPIDController
|
||||
* @param iZone Maximum magnitude of error to allow integral control. Must be
|
||||
* >= 0.
|
||||
*/
|
||||
void SetIZone(double iZone) { m_controller.SetIZone(iZone); }
|
||||
constexpr void SetIZone(double iZone) { m_controller.SetIZone(iZone); }
|
||||
|
||||
/**
|
||||
* Gets the proportional coefficient.
|
||||
*
|
||||
* @return proportional coefficient
|
||||
*/
|
||||
double GetP() const { return m_controller.GetP(); }
|
||||
constexpr double GetP() const { return m_controller.GetP(); }
|
||||
|
||||
/**
|
||||
* Gets the integral coefficient.
|
||||
*
|
||||
* @return integral coefficient
|
||||
*/
|
||||
double GetI() const { return m_controller.GetI(); }
|
||||
constexpr double GetI() const { return m_controller.GetI(); }
|
||||
|
||||
/**
|
||||
* Gets the differential coefficient.
|
||||
*
|
||||
* @return differential coefficient
|
||||
*/
|
||||
double GetD() const { return m_controller.GetD(); }
|
||||
constexpr double GetD() const { return m_controller.GetD(); }
|
||||
|
||||
/**
|
||||
* Get the IZone range.
|
||||
*
|
||||
* @return Maximum magnitude of error to allow integral control.
|
||||
*/
|
||||
double GetIZone() const { return m_controller.GetIZone(); }
|
||||
constexpr double GetIZone() const { return m_controller.GetIZone(); }
|
||||
|
||||
/**
|
||||
* Gets the period of this controller.
|
||||
*
|
||||
* @return The period of the controller.
|
||||
*/
|
||||
units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
|
||||
constexpr units::second_t GetPeriod() const {
|
||||
return m_controller.GetPeriod();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the position tolerance of this controller.
|
||||
*
|
||||
* @return The position tolerance of the controller.
|
||||
*/
|
||||
double GetPositionTolerance() const {
|
||||
constexpr double GetPositionTolerance() const {
|
||||
return m_controller.GetErrorTolerance();
|
||||
}
|
||||
|
||||
@@ -170,7 +175,7 @@ class ProfiledPIDController
|
||||
*
|
||||
* @return The velocity tolerance of the controller.
|
||||
*/
|
||||
double GetVelocityTolerance() const {
|
||||
constexpr double GetVelocityTolerance() const {
|
||||
return m_controller.GetErrorDerivativeTolerance();
|
||||
}
|
||||
|
||||
@@ -180,7 +185,7 @@ class ProfiledPIDController
|
||||
*
|
||||
* @return The accumulated error of this controller.
|
||||
*/
|
||||
double GetAccumulatedError() const {
|
||||
constexpr double GetAccumulatedError() const {
|
||||
return m_controller.GetAccumulatedError();
|
||||
}
|
||||
|
||||
@@ -189,33 +194,33 @@ class ProfiledPIDController
|
||||
*
|
||||
* @param goal The desired unprofiled setpoint.
|
||||
*/
|
||||
void SetGoal(State goal) { m_goal = goal; }
|
||||
constexpr void SetGoal(State goal) { m_goal = goal; }
|
||||
|
||||
/**
|
||||
* Sets the goal for the ProfiledPIDController.
|
||||
*
|
||||
* @param goal The desired unprofiled setpoint.
|
||||
*/
|
||||
void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t{0}}; }
|
||||
constexpr void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t{0}}; }
|
||||
|
||||
/**
|
||||
* Gets the goal for the ProfiledPIDController.
|
||||
*/
|
||||
State GetGoal() const { return m_goal; }
|
||||
constexpr State GetGoal() const { return m_goal; }
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the error.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*/
|
||||
bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
|
||||
constexpr bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
|
||||
|
||||
/**
|
||||
* Set velocity and acceleration constraints for goal.
|
||||
*
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
*/
|
||||
void SetConstraints(Constraints constraints) {
|
||||
constexpr void SetConstraints(Constraints constraints) {
|
||||
m_constraints = constraints;
|
||||
m_profile = TrapezoidProfile<Distance>{m_constraints};
|
||||
}
|
||||
@@ -224,14 +229,14 @@ class ProfiledPIDController
|
||||
* Get the velocity and acceleration constraints for this controller.
|
||||
* @return Velocity and acceleration constraints.
|
||||
*/
|
||||
Constraints GetConstraints() { return m_constraints; }
|
||||
constexpr Constraints GetConstraints() { return m_constraints; }
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the ProfiledPIDController.
|
||||
*
|
||||
* @return The current setpoint.
|
||||
*/
|
||||
State GetSetpoint() const { return m_setpoint; }
|
||||
constexpr State GetSetpoint() const { return m_setpoint; }
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the error.
|
||||
@@ -242,7 +247,7 @@ class ProfiledPIDController
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*/
|
||||
bool AtSetpoint() const { return m_controller.AtSetpoint(); }
|
||||
constexpr bool AtSetpoint() const { return m_controller.AtSetpoint(); }
|
||||
|
||||
/**
|
||||
* Enables continuous input.
|
||||
@@ -254,7 +259,8 @@ class ProfiledPIDController
|
||||
* @param minimumInput The minimum value expected from the input.
|
||||
* @param maximumInput The maximum value expected from the input.
|
||||
*/
|
||||
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
|
||||
constexpr void EnableContinuousInput(Distance_t minimumInput,
|
||||
Distance_t maximumInput) {
|
||||
m_controller.EnableContinuousInput(minimumInput.value(),
|
||||
maximumInput.value());
|
||||
m_minimumInput = minimumInput;
|
||||
@@ -264,7 +270,9 @@ class ProfiledPIDController
|
||||
/**
|
||||
* Disables continuous input.
|
||||
*/
|
||||
void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
|
||||
constexpr void DisableContinuousInput() {
|
||||
m_controller.DisableContinuousInput();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum contributions of the integral term.
|
||||
@@ -276,7 +284,8 @@ class ProfiledPIDController
|
||||
* @param minimumIntegral The minimum contribution of the integral term.
|
||||
* @param maximumIntegral The maximum contribution of the integral term.
|
||||
*/
|
||||
void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
|
||||
constexpr void SetIntegratorRange(double minimumIntegral,
|
||||
double maximumIntegral) {
|
||||
m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
|
||||
}
|
||||
|
||||
@@ -287,9 +296,9 @@ class ProfiledPIDController
|
||||
* @param positionTolerance Position error which is tolerable.
|
||||
* @param velocityTolerance Velocity error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(Distance_t positionTolerance,
|
||||
Velocity_t velocityTolerance = Velocity_t{
|
||||
std::numeric_limits<double>::infinity()}) {
|
||||
constexpr void SetTolerance(Distance_t positionTolerance,
|
||||
Velocity_t velocityTolerance = Velocity_t{
|
||||
std::numeric_limits<double>::infinity()}) {
|
||||
m_controller.SetTolerance(positionTolerance.value(),
|
||||
velocityTolerance.value());
|
||||
}
|
||||
@@ -299,14 +308,14 @@ class ProfiledPIDController
|
||||
*
|
||||
* @return The error.
|
||||
*/
|
||||
Distance_t GetPositionError() const {
|
||||
constexpr Distance_t GetPositionError() const {
|
||||
return Distance_t{m_controller.GetError()};
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the change in error per second.
|
||||
*/
|
||||
Velocity_t GetVelocityError() const {
|
||||
constexpr Velocity_t GetVelocityError() const {
|
||||
return Velocity_t{m_controller.GetErrorDerivative()};
|
||||
}
|
||||
|
||||
@@ -315,7 +324,7 @@ class ProfiledPIDController
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
*/
|
||||
double Calculate(Distance_t measurement) {
|
||||
constexpr double Calculate(Distance_t measurement) {
|
||||
if (m_controller.IsContinuousInputEnabled()) {
|
||||
// Get error which is smallest distance between goal and measurement
|
||||
auto errorBound = (m_maximumInput - m_minimumInput) / 2.0;
|
||||
@@ -345,7 +354,7 @@ class ProfiledPIDController
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @param goal The new goal of the controller.
|
||||
*/
|
||||
double Calculate(Distance_t measurement, State goal) {
|
||||
constexpr double Calculate(Distance_t measurement, State goal) {
|
||||
SetGoal(goal);
|
||||
return Calculate(measurement);
|
||||
}
|
||||
@@ -355,7 +364,7 @@ class ProfiledPIDController
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @param goal The new goal of the controller.
|
||||
*/
|
||||
double Calculate(Distance_t measurement, Distance_t goal) {
|
||||
constexpr double Calculate(Distance_t measurement, Distance_t goal) {
|
||||
SetGoal(goal);
|
||||
return Calculate(measurement);
|
||||
}
|
||||
@@ -367,7 +376,7 @@ class ProfiledPIDController
|
||||
* @param goal The new goal of the controller.
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
*/
|
||||
double Calculate(
|
||||
constexpr double Calculate(
|
||||
Distance_t measurement, Distance_t goal,
|
||||
typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
|
||||
SetConstraints(constraints);
|
||||
@@ -379,7 +388,7 @@ class ProfiledPIDController
|
||||
*
|
||||
* @param measurement The current measured State of the system.
|
||||
*/
|
||||
void Reset(const State& measurement) {
|
||||
constexpr void Reset(const State& measurement) {
|
||||
m_controller.Reset();
|
||||
m_setpoint = measurement;
|
||||
}
|
||||
@@ -390,7 +399,8 @@ class ProfiledPIDController
|
||||
* @param measuredPosition The current measured position of the system.
|
||||
* @param measuredVelocity The current measured velocity of the system.
|
||||
*/
|
||||
void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
|
||||
constexpr void Reset(Distance_t measuredPosition,
|
||||
Velocity_t measuredVelocity) {
|
||||
Reset(State{measuredPosition, measuredVelocity});
|
||||
}
|
||||
|
||||
@@ -400,7 +410,7 @@ class ProfiledPIDController
|
||||
* @param measuredPosition The current measured position of the system. The
|
||||
* velocity is assumed to be zero.
|
||||
*/
|
||||
void Reset(Distance_t measuredPosition) {
|
||||
constexpr void Reset(Distance_t measuredPosition) {
|
||||
Reset(measuredPosition, Velocity_t{0});
|
||||
}
|
||||
|
||||
|
||||
@@ -5,12 +5,15 @@
|
||||
#pragma once
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
#include <wpi/deprecated.h>
|
||||
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/kinematics/ChassisSpeeds.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "units/angle.h"
|
||||
#include "units/angular_velocity.h"
|
||||
#include "units/length.h"
|
||||
#include "units/math.h"
|
||||
#include "units/velocity.h"
|
||||
|
||||
namespace frc {
|
||||
@@ -59,7 +62,11 @@ class WPILIB_DLLEXPORT RamseteController {
|
||||
* @deprecated Use LTVUnicycleController instead.
|
||||
*/
|
||||
[[deprecated("Use LTVUnicycleController instead.")]]
|
||||
RamseteController(units::unit_t<b_unit> b, units::unit_t<zeta_unit> zeta);
|
||||
constexpr RamseteController(units::unit_t<b_unit> b,
|
||||
units::unit_t<zeta_unit> zeta)
|
||||
: m_b{b}, m_zeta{zeta} {}
|
||||
|
||||
WPI_IGNORE_DEPRECATED
|
||||
|
||||
/**
|
||||
* Construct a Ramsete unicycle controller. The default arguments for
|
||||
@@ -69,12 +76,24 @@ class WPILIB_DLLEXPORT RamseteController {
|
||||
* @deprecated Use LTVUnicycleController instead.
|
||||
*/
|
||||
[[deprecated("Use LTVUnicycleController instead.")]]
|
||||
RamseteController();
|
||||
constexpr RamseteController()
|
||||
: RamseteController{units::unit_t<b_unit>{2.0},
|
||||
units::unit_t<zeta_unit>{0.7}} {}
|
||||
|
||||
WPI_UNIGNORE_DEPRECATED
|
||||
|
||||
/**
|
||||
* Returns true if the pose error is within tolerance of the reference.
|
||||
*/
|
||||
bool AtReference() const;
|
||||
constexpr bool AtReference() const {
|
||||
const auto& eTranslate = m_poseError.Translation();
|
||||
const auto& eRotate = m_poseError.Rotation();
|
||||
const auto& tolTranslate = m_poseTolerance.Translation();
|
||||
const auto& tolRotate = m_poseTolerance.Rotation();
|
||||
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
|
||||
units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
|
||||
units::math::abs(eRotate.Radians()) < tolRotate.Radians();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerable for use with
|
||||
@@ -82,7 +101,9 @@ class WPILIB_DLLEXPORT RamseteController {
|
||||
*
|
||||
* @param poseTolerance Pose error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(const Pose2d& poseTolerance);
|
||||
constexpr void SetTolerance(const Pose2d& poseTolerance) {
|
||||
m_poseTolerance = poseTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the Ramsete controller.
|
||||
@@ -95,9 +116,34 @@ class WPILIB_DLLEXPORT RamseteController {
|
||||
* @param linearVelocityRef The desired linear velocity.
|
||||
* @param angularVelocityRef The desired angular velocity.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
units::radians_per_second_t angularVelocityRef);
|
||||
constexpr ChassisSpeeds Calculate(
|
||||
const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
units::radians_per_second_t angularVelocityRef) {
|
||||
if (!m_enabled) {
|
||||
return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
|
||||
}
|
||||
|
||||
m_poseError = poseRef.RelativeTo(currentPose);
|
||||
|
||||
// Aliases for equation readability
|
||||
const auto& eX = m_poseError.X();
|
||||
const auto& eY = m_poseError.Y();
|
||||
const auto& eTheta = m_poseError.Rotation().Radians();
|
||||
const auto& vRef = linearVelocityRef;
|
||||
const auto& omegaRef = angularVelocityRef;
|
||||
|
||||
// k = 2ζ√(ω_ref² + b v_ref²)
|
||||
auto k = 2.0 * m_zeta *
|
||||
units::math::sqrt(units::math::pow<2>(omegaRef) +
|
||||
m_b * units::math::pow<2>(vRef));
|
||||
|
||||
// v_cmd = v_ref cos(e_θ) + k e_x
|
||||
// ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y
|
||||
return ChassisSpeeds{
|
||||
vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps,
|
||||
omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY};
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the Ramsete controller.
|
||||
@@ -109,15 +155,18 @@ class WPILIB_DLLEXPORT RamseteController {
|
||||
* @param desiredState The desired pose, linear velocity, and angular velocity
|
||||
* from a trajectory.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Trajectory::State& desiredState);
|
||||
constexpr ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Trajectory::State& desiredState) {
|
||||
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
|
||||
desiredState.velocity * desiredState.curvature);
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting purposes.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
void SetEnabled(bool enabled);
|
||||
constexpr void SetEnabled(bool enabled) { m_enabled = enabled; }
|
||||
|
||||
private:
|
||||
units::unit_t<b_unit> m_b;
|
||||
@@ -126,6 +175,19 @@ class WPILIB_DLLEXPORT RamseteController {
|
||||
Pose2d m_poseError;
|
||||
Pose2d m_poseTolerance;
|
||||
bool m_enabled = true;
|
||||
|
||||
/**
|
||||
* Returns sin(x) / x.
|
||||
*
|
||||
* @param x Value of which to take sinc(x).
|
||||
*/
|
||||
static constexpr decltype(1 / 1_rad) Sinc(units::radian_t x) {
|
||||
if (units::math::abs(x) < 1e-9_rad) {
|
||||
return decltype(1 / 1_rad){1.0 - 1.0 / 6.0 * x.value() * x.value()};
|
||||
} else {
|
||||
return units::math::sin(x) / x;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
@@ -199,28 +199,28 @@ class SimpleMotorFeedforward {
|
||||
*
|
||||
* @return The static gain.
|
||||
*/
|
||||
units::volt_t GetKs() const { return kS; }
|
||||
constexpr units::volt_t GetKs() const { return kS; }
|
||||
|
||||
/**
|
||||
* Returns the velocity gain.
|
||||
*
|
||||
* @return The velocity gain.
|
||||
*/
|
||||
units::unit_t<kv_unit> GetKv() const { return kV; }
|
||||
constexpr units::unit_t<kv_unit> GetKv() const { return kV; }
|
||||
|
||||
/**
|
||||
* Returns the acceleration gain.
|
||||
*
|
||||
* @return The acceleration gain.
|
||||
*/
|
||||
units::unit_t<ka_unit> GetKa() const { return kA; }
|
||||
constexpr units::unit_t<ka_unit> GetKa() const { return kA; }
|
||||
|
||||
/**
|
||||
* Returns the period.
|
||||
*
|
||||
* @return The period.
|
||||
*/
|
||||
units::second_t GetDt() const { return m_dt; }
|
||||
constexpr units::second_t GetDt() const { return m_dt; }
|
||||
|
||||
private:
|
||||
/** The static gain. */
|
||||
|
||||
Reference in New Issue
Block a user