Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)

Browse Source
This commit is contained in:
Oblarg
2019-11-20 23:11:46 -05:00
committed by Peter Johnson
parent f62e23f1af
commit fa85fbfc1c
16 changed files with 320 additions and 448 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <algorithm>
#include <cmath>
#include <functional>
#include <limits>
@@ -14,6 +16,7 @@
#include "frc/controller/PIDController.h"
#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableHelper.h"
#include "frc/trajectory/TrapezoidProfile.h"
@@ -23,8 +26,20 @@ namespace frc {
* Implements a PID control loop whose setpoint is constrained by a trapezoid
* profile.
*/
class ProfiledPIDController : public Sendable,
public SendableHelper<ProfiledPIDController> {
template <class Distance>
class ProfiledPIDController
: public Sendable,
public SendableHelper<ProfiledPIDController<Distance>> {
using Distance_t = units::unit_t<Distance>;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Velocity_t = units::unit_t<Velocity>;
using Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using Acceleration_t = units::unit_t<Acceleration>;
using State = typename TrapezoidProfile<Distance>::State;
using Constraints = typename TrapezoidProfile<Distance>::Constraints;
public:
/**
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
@@ -38,8 +53,8 @@ class ProfiledPIDController : public Sendable,
* default is 20 milliseconds.
*/
ProfiledPIDController(double Kp, double Ki, double Kd,
frc::TrapezoidProfile::Constraints constraints,
units::second_t period = 20_ms);
Constraints constraints, units::second_t period = 20_ms)
: m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
~ProfiledPIDController() override = default;
@@ -57,96 +72,98 @@ class ProfiledPIDController : public Sendable,
* @param Ki Integral coefficient
* @param Kd Differential coefficient
*/
void SetPID(double Kp, double Ki, double Kd);
void SetPID(double Kp, double Ki, double Kd) {
m_controller.SetPID(Kp, Ki, Kd);
}
/**
* Sets the proportional coefficient of the PID controller gain.
*
* @param Kp proportional coefficient
*/
void SetP(double Kp);
void SetP(double Kp) { m_controller.SetP(Kp); }
/**
* Sets the integral coefficient of the PID controller gain.
*
* @param Ki integral coefficient
*/
void SetI(double Ki);
void SetI(double Ki) { m_controller.SetI(Ki); }
/**
* Sets the differential coefficient of the PID controller gain.
*
* @param Kd differential coefficient
*/
void SetD(double Kd);
void SetD(double Kd) { m_controller.SetD(Kd); }
/**
* Gets the proportional coefficient.
*
* @return proportional coefficient
*/
double GetP() const;
double GetP() const { return m_controller.GetP(); }
/**
* Gets the integral coefficient.
*
* @return integral coefficient
*/
double GetI() const;
double GetI() const { return m_controller.GetI(); }
/**
* Gets the differential coefficient.
*
* @return differential coefficient
*/
double GetD() const;
double GetD() const { return m_controller.GetD(); }
/**
* Gets the period of this controller.
*
* @return The period of the controller.
*/
units::second_t GetPeriod() const;
units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
/**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
*/
void SetGoal(TrapezoidProfile::State goal);
void SetGoal(State goal) { m_goal = goal; }
/**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
*/
void SetGoal(units::meter_t goal);
void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
/**
* Gets the goal for the ProfiledPIDController.
*/
TrapezoidProfile::State GetGoal() const;
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;
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(frc::TrapezoidProfile::Constraints constraints);
void SetConstraints(Constraints constraints) { m_constraints = constraints; }
/**
* Returns the current setpoint of the ProfiledPIDController.
*
* @return The current setpoint.
*/
TrapezoidProfile::State GetSetpoint() const;
State GetSetpoint() const { return m_setpoint; }
/**
* Returns true if the error is within the tolerance of the error.
@@ -157,7 +174,7 @@ class ProfiledPIDController : public Sendable,
*
* This will return false until at least one input value has been computed.
*/
bool AtSetpoint() const;
bool AtSetpoint() const { return m_controller.AtSetpoint(); }
/**
* Enables continuous input.
@@ -169,12 +186,15 @@ class ProfiledPIDController : public Sendable,
* @param minimumInput The minimum value expected from the input.
* @param maximumInput The maximum value expected from the input.
*/
void EnableContinuousInput(double minimumInput, double maximumInput);
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
m_controller.EnableContinuousInput(minimumInput.template to<double>(),
maximumInput.template to<double>());
}
/**
* Disables continuous input.
*/
void DisableContinuousInput();
void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
/**
* Sets the minimum and maximum values for the integrator.
@@ -185,7 +205,9 @@ class ProfiledPIDController : public Sendable,
* @param minimumIntegral The minimum value of the integrator.
* @param maximumIntegral The maximum value of the integrator.
*/
void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
}
/**
* Sets the error which is considered tolerable for use with
@@ -196,26 +218,37 @@ class ProfiledPIDController : public Sendable,
*/
void SetTolerance(
double positionTolerance,
double velocityTolerance = std::numeric_limits<double>::infinity());
double velocityTolerance = std::numeric_limits<double>::infinity()) {
m_controller.SetTolerance(positionTolerance, velocityTolerance);
}
/**
* Returns the difference between the setpoint and the measurement.
*
* @return The error.
*/
double GetPositionError() const;
Distance_t GetPositionError() const {
return Distance_t(m_controller.GetPositionError());
}
/**
* Returns the change in error per second.
*/
double GetVelocityError() const;
Velocity_t GetVelocityError() const {
return Velocity_t(m_controller.GetVelocityError());
}
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
*/
double Calculate(units::meter_t measurement);
double Calculate(Distance_t measurement) {
frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
m_setpoint = profile.Calculate(GetPeriod());
return m_controller.Calculate(measurement.template to<double>(),
m_setpoint.position.template to<double>());
}
/**
* Returns the next output of the PID controller.
@@ -223,15 +256,20 @@ class ProfiledPIDController : public Sendable,
* @param measurement The current measurement of the process variable.
* @param goal The new goal of the controller.
*/
double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
double Calculate(Distance_t measurement, State goal) {
SetGoal(goal);
return Calculate(measurement);
}
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
* @param goal The new goal of the controller.
*/
double Calculate(units::meter_t measurement, units::meter_t goal);
double Calculate(Distance_t measurement, Distance_t goal) {
SetGoal(goal);
return Calculate(measurement);
}
/**
* Returns the next output of the PID controller.
@@ -240,21 +278,36 @@ class ProfiledPIDController : public Sendable,
* @param goal The new goal of the controller.
* @param constraints Velocity and acceleration constraints for goal.
*/
double Calculate(units::meter_t measurement, units::meter_t goal,
frc::TrapezoidProfile::Constraints constraints);
double Calculate(
Distance_t measurement, Distance_t goal,
typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
SetConstraints(constraints);
return Calculate(measurement, goal);
}
/**
* Reset the previous error, the integral term, and disable the controller.
*/
void Reset();
void Reset() { m_controller.Reset(); }
void InitSendable(frc::SendableBuilder& builder) override;
void InitSendable(frc::SendableBuilder& builder) override {
builder.SetSmartDashboardType("ProfiledPIDController");
builder.AddDoubleProperty("p", [this] { return GetP(); },
[this](double value) { SetP(value); });
builder.AddDoubleProperty("i", [this] { return GetI(); },
[this](double value) { SetI(value); });
builder.AddDoubleProperty("d", [this] { return GetD(); },
[this](double value) { SetD(value); });
builder.AddDoubleProperty(
"goal", [this] { return GetGoal().position.template to<double>(); },
[this](double value) { SetGoal(Distance_t{value}); });
}
private:
frc2::PIDController m_controller;
frc::TrapezoidProfile::State m_goal;
frc::TrapezoidProfile::State m_setpoint;
frc::TrapezoidProfile::Constraints m_constraints;
typename frc::TrapezoidProfile<Distance>::State m_goal;
typename frc::TrapezoidProfile<Distance>::State m_setpoint;
typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
};
} // namespace frc