WPILibC++  2020.3.2-60-g3011ebe
ProfiledPIDCommand.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <functional>
11 #include <initializer_list>
12 #include <utility>
13 
14 #include <frc/controller/ProfiledPIDController.h>
15 #include <units/units.h>
16 #include <wpi/ArrayRef.h>
17 
18 #include "frc2/command/CommandBase.h"
19 #include "frc2/command/CommandHelper.h"
20 
21 namespace frc2 {
30 template <class Distance>
32  : public CommandHelper<CommandBase, ProfiledPIDCommand<Distance>> {
33  using Distance_t = units::unit_t<Distance>;
34  using Velocity =
35  units::compound_unit<Distance, units::inverse<units::seconds>>;
36  using Velocity_t = units::unit_t<Velocity>;
37  using State = typename frc::TrapezoidProfile<Distance>::State;
38 
39  public:
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)} {
59  this->AddRequirements(requirements);
60  }
61 
73  std::function<Distance_t()> measurementSource,
74  std::function<State()> goalSource,
75  std::function<void(double, State)> useOutput,
76  wpi::ArrayRef<Subsystem*> requirements = {})
77  : m_controller{controller},
78  m_measurement{std::move(measurementSource)},
79  m_goal{std::move(goalSource)},
80  m_useOutput{std::move(useOutput)} {
81  this->AddRequirements(requirements);
82  }
83 
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)
99  : ProfiledPIDCommand(controller, measurementSource,
100  [&goalSource]() {
101  return State{goalSource(), Velocity_t{0}};
102  },
103  useOutput, requirements) {}
104 
116  std::function<Distance_t()> measurementSource,
117  std::function<Distance_t()> goalSource,
118  std::function<void(double, State)> useOutput,
119  wpi::ArrayRef<Subsystem*> requirements = {})
120  : ProfiledPIDCommand(controller, measurementSource,
121  [&goalSource]() {
122  return State{goalSource(), Velocity_t{0}};
123  },
124  useOutput, requirements) {}
125 
137  std::function<Distance_t()> measurementSource, State goal,
138  std::function<void(double, State)> useOutput,
139  std::initializer_list<Subsystem*> requirements)
140  : ProfiledPIDCommand(controller, measurementSource,
141  [goal] { return goal; }, useOutput, requirements) {}
142 
154  std::function<Distance_t()> measurementSource, State goal,
155  std::function<void(double, State)> useOutput,
156  wpi::ArrayRef<Subsystem*> requirements = {})
157  : ProfiledPIDCommand(controller, measurementSource,
158  [goal] { return goal; }, useOutput, requirements) {}
159 
171  std::function<Distance_t()> measurementSource,
172  Distance_t goal,
173  std::function<void(double, State)> useOutput,
174  std::initializer_list<Subsystem*> requirements)
175  : ProfiledPIDCommand(controller, measurementSource,
176  [goal] { return goal; }, useOutput, requirements) {}
177 
189  std::function<Distance_t()> measurementSource,
190  Distance_t goal,
191  std::function<void(double, State)> useOutput,
192  wpi::ArrayRef<Subsystem*> requirements = {})
193  : ProfiledPIDCommand(controller, measurementSource,
194  [goal] { return goal; }, useOutput, requirements) {}
195 
196  ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
197 
198  ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
199 
200  void Initialize() override { m_controller.Reset(m_measurement()); }
201 
202  void Execute() override {
203  m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
204  m_controller.GetSetpoint());
205  }
206 
207  void End(bool interrupted) override {
208  m_useOutput(0, State{Distance_t(0), Velocity_t(0)});
209  }
210 
217 
218  protected:
220  std::function<Distance_t()> m_measurement;
221  std::function<State()> m_goal;
222  std::function<void(double, State)> m_useOutput;
223 };
224 } // namespace frc2
frc2::ProfiledPIDCommand::ProfiledPIDCommand
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
frc2::ProfiledPIDCommand::ProfiledPIDCommand
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
frc2::ProfiledPIDCommand::ProfiledPIDCommand
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
frc2::ProfiledPIDCommand::ProfiledPIDCommand
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
frc2::ProfiledPIDCommand::Initialize
void Initialize() override
The initial subroutine of a command.
Definition: ProfiledPIDCommand.h:200
wpi::ArrayRef
ArrayRef - Represent a constant reference to an array (0 or more elements consecutively in memory),...
Definition: ArrayRef.h:42
frc2::ProfiledPIDCommand::ProfiledPIDCommand
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
frc2::ProfiledPIDCommand::Execute
void Execute() override
The main body of a command.
Definition: ProfiledPIDCommand.h:202
frc2::ProfiledPIDCommand::End
void End(bool interrupted) override
The action to take when the command ends.
Definition: ProfiledPIDCommand.h:207
frc2::ProfiledPIDCommand
A command that controls an output with a ProfiledPIDController.
Definition: ProfiledPIDCommand.h:31
frc2::ProfiledPIDCommand::ProfiledPIDCommand
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
frc::ProfiledPIDController
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:34
frc2::ProfiledPIDCommand::ProfiledPIDCommand
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
frc2::CommandBase::AddRequirements
void AddRequirements(std::initializer_list< Subsystem * > requirements)
Adds the specified requirements to the command.
frc2::CommandHelper
CRTP implementation to allow polymorphic decorator functions in Command.
Definition: CommandHelper.h:26
frc2::ProfiledPIDCommand::ProfiledPIDCommand
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
frc::TrapezoidProfile::State
Definition: TrapezoidProfile.h:68
frc2::ProfiledPIDCommand::GetController
frc::ProfiledPIDController< Distance > & GetController()
Returns the ProfiledPIDController used by the command.
Definition: ProfiledPIDCommand.h:216