11 #include <initializer_list>
14 #include <frc/controller/ProfiledPIDController.h>
15 #include <units/units.h>
16 #include <wpi/ArrayRef.h>
18 #include "frc2/command/CommandBase.h"
19 #include "frc2/command/CommandHelper.h"
30 template <
class Distance>
32 :
public CommandHelper<CommandBase, ProfiledPIDCommand<Distance>> {
33 using Distance_t = units::unit_t<Distance>;
35 units::compound_unit<Distance, units::inverse<units::seconds>>;
36 using Velocity_t = units::unit_t<Velocity>;
51 std::function<Distance_t()> measurementSource,
52 std::function<State()> goalSource,
53 std::function<
void(
double, State)> useOutput,
54 std::initializer_list<Subsystem*> requirements)
55 : m_controller{controller},
56 m_measurement{std::move(measurementSource)},
57 m_goal{std::move(goalSource)},
58 m_useOutput{std::move(useOutput)} {
73 std::function<Distance_t()> measurementSource,
74 std::function<State()> goalSource,
75 std::function<
void(
double, State)> useOutput,
77 : m_controller{controller},
78 m_measurement{std::move(measurementSource)},
79 m_goal{std::move(goalSource)},
80 m_useOutput{std::move(useOutput)} {
95 std::function<Distance_t()> measurementSource,
96 std::function<Distance_t()> goalSource,
97 std::function<
void(
double, State)> useOutput,
98 std::initializer_list<Subsystem*> requirements)
101 return State{goalSource(), Velocity_t{0}};
103 useOutput, requirements) {}
116 std::function<Distance_t()> measurementSource,
117 std::function<Distance_t()> goalSource,
118 std::function<
void(
double, State)> useOutput,
122 return State{goalSource(), Velocity_t{0}};
124 useOutput, requirements) {}
137 std::function<Distance_t()> measurementSource, State goal,
138 std::function<
void(
double, State)> useOutput,
139 std::initializer_list<Subsystem*> requirements)
141 [goal] {
return goal; }, useOutput, requirements) {}
154 std::function<Distance_t()> measurementSource, State goal,
155 std::function<
void(
double, State)> useOutput,
158 [goal] {
return goal; }, useOutput, requirements) {}
171 std::function<Distance_t()> measurementSource,
173 std::function<
void(
double, State)> useOutput,
174 std::initializer_list<Subsystem*> requirements)
176 [goal] {
return goal; }, useOutput, requirements) {}
189 std::function<Distance_t()> measurementSource,
191 std::function<
void(
double, State)> useOutput,
194 [goal] {
return goal; }, useOutput, requirements) {}
200 void Initialize()
override { m_controller.Reset(m_measurement()); }
203 m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
204 m_controller.GetSetpoint());
207 void End(
bool interrupted)
override {
208 m_useOutput(0, State{Distance_t(0), Velocity_t(0)});
220 std::function<Distance_t()> m_measurement;
221 std::function<State()> m_goal;
222 std::function<void(
double, State)> m_useOutput;
ProfiledPIDCommand(frc::ProfiledPIDController< Distance > controller, std::function< Distance_t()> measurementSource, Distance_t goal, std::function< void(double, State)> useOutput, wpi::ArrayRef< Subsystem * > requirements={})
Creates a new PIDCommand, which controls the given output with a ProfiledPIDController with a constan...
Definition: ProfiledPIDCommand.h:188
ProfiledPIDCommand(frc::ProfiledPIDController< Distance > controller, std::function< Distance_t()> measurementSource, std::function< Distance_t()> goalSource, std::function< void(double, State)> useOutput, std::initializer_list< Subsystem * > requirements)
Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
Definition: ProfiledPIDCommand.h:94
ProfiledPIDCommand(frc::ProfiledPIDController< Distance > controller, std::function< Distance_t()> measurementSource, std::function< Distance_t()> goalSource, std::function< void(double, State)> useOutput, wpi::ArrayRef< Subsystem * > requirements={})
Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
Definition: ProfiledPIDCommand.h:115
ProfiledPIDCommand(frc::ProfiledPIDController< Distance > controller, std::function< Distance_t()> measurementSource, std::function< State()> goalSource, std::function< void(double, State)> useOutput, wpi::ArrayRef< Subsystem * > requirements={})
Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
Definition: ProfiledPIDCommand.h:72
void Initialize() override
The initial subroutine of a command.
Definition: ProfiledPIDCommand.h:200
ArrayRef - Represent a constant reference to an array (0 or more elements consecutively in memory),...
Definition: ArrayRef.h:42
ProfiledPIDCommand(frc::ProfiledPIDController< Distance > controller, std::function< Distance_t()> measurementSource, State goal, std::function< void(double, State)> useOutput, wpi::ArrayRef< Subsystem * > requirements={})
Creates a new PIDCommand, which controls the given output with a ProfiledPIDController with a constan...
Definition: ProfiledPIDCommand.h:153
void Execute() override
The main body of a command.
Definition: ProfiledPIDCommand.h:202
void End(bool interrupted) override
The action to take when the command ends.
Definition: ProfiledPIDCommand.h:207
A command that controls an output with a ProfiledPIDController.
Definition: ProfiledPIDCommand.h:31
ProfiledPIDCommand(frc::ProfiledPIDController< Distance > controller, std::function< Distance_t()> measurementSource, State goal, std::function< void(double, State)> useOutput, std::initializer_list< Subsystem * > requirements)
Creates a new PIDCommand, which controls the given output with a ProfiledPIDController with a constan...
Definition: ProfiledPIDCommand.h:136
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:33
ProfiledPIDCommand(frc::ProfiledPIDController< Distance > controller, std::function< Distance_t()> measurementSource, Distance_t goal, std::function< void(double, State)> useOutput, std::initializer_list< Subsystem * > requirements)
Creates a new PIDCommand, which controls the given output with a ProfiledPIDController with a constan...
Definition: ProfiledPIDCommand.h:170
void AddRequirements(std::initializer_list< Subsystem * > requirements)
Adds the specified requirements to the command.
CRTP implementation to allow polymorphic decorator functions in Command.
Definition: CommandHelper.h:26
ProfiledPIDCommand(frc::ProfiledPIDController< Distance > controller, std::function< Distance_t()> measurementSource, std::function< State()> goalSource, std::function< void(double, State)> useOutput, std::initializer_list< Subsystem * > requirements)
Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
Definition: ProfiledPIDCommand.h:50
Definition: TrapezoidProfile.h:68
frc::ProfiledPIDController< Distance > & GetController()
Returns the ProfiledPIDController used by the command.
Definition: ProfiledPIDCommand.h:216