WPILibC++  2020.3.2-60-g3011ebe
TrapezoidProfileSubsystem.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 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 <frc/trajectory/TrapezoidProfile.h>
11 #include <units/units.h>
12 
13 #include "frc2/command/SubsystemBase.h"
14 
15 namespace frc2 {
21 template <class Distance>
23  using Distance_t = units::unit_t<Distance>;
24  using Velocity =
25  units::compound_unit<Distance, units::inverse<units::seconds>>;
26  using Velocity_t = units::unit_t<Velocity>;
27  using State = typename frc::TrapezoidProfile<Distance>::State;
28  using Constraints = typename frc::TrapezoidProfile<Distance>::Constraints;
29 
30  public:
40  explicit TrapezoidProfileSubsystem(Constraints constraints,
41  Distance_t initialPosition = Distance_t{0},
42  units::second_t period = 20_ms)
43  : m_constraints(constraints),
44  m_state{initialPosition, Velocity_t(0)},
45  m_goal{initialPosition, Velocity_t{0}},
46  m_period(period) {}
47 
48  void Periodic() override {
49  auto profile =
50  frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
51  m_state = profile.Calculate(m_period);
52  if (m_enabled) {
53  UseState(m_state);
54  }
55  }
56 
62  void SetGoal(State goal) { m_goal = goal; }
63 
69  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
70 
71  protected:
78  virtual void UseState(State state) = 0;
79 
83  void Enable() { m_enabled = true; }
84 
88  void Disable() { m_enabled = false; }
89 
90  private:
91  Constraints m_constraints;
92  State m_state;
93  State m_goal;
94  units::second_t m_period;
95  bool m_enabled{false};
96 };
97 } // namespace frc2
frc2::TrapezoidProfileSubsystem
A subsystem that generates and runs trapezoidal motion profiles automatically.
Definition: TrapezoidProfileSubsystem.h:22
frc2::TrapezoidProfileSubsystem::UseState
virtual void UseState(State state)=0
Users should override this to consume the current state of the motion profile.
frc2::TrapezoidProfileSubsystem::SetGoal
void SetGoal(State goal)
Sets the goal state for the subsystem.
Definition: TrapezoidProfileSubsystem.h:62
frc::TrapezoidProfile
A trapezoid-shaped velocity profile.
Definition: TrapezoidProfile.h:45
frc2::TrapezoidProfileSubsystem::SetGoal
void SetGoal(Distance_t goal)
Sets the goal state for the subsystem.
Definition: TrapezoidProfileSubsystem.h:69
frc2::TrapezoidProfileSubsystem::Disable
void Disable()
Disable the TrapezoidProfileSubsystem's output.
Definition: TrapezoidProfileSubsystem.h:88
frc2::TrapezoidProfileSubsystem::Enable
void Enable()
Enable the TrapezoidProfileSubsystem's output.
Definition: TrapezoidProfileSubsystem.h:83
frc::TrapezoidProfile::Constraints
Definition: TrapezoidProfile.h:55
frc::TrapezoidProfile::State
Definition: TrapezoidProfile.h:68
frc2::SubsystemBase
A base for subsystems that handles registration in the constructor, and provides a more intuitive met...
Definition: SubsystemBase.h:22
frc2::TrapezoidProfileSubsystem::TrapezoidProfileSubsystem
TrapezoidProfileSubsystem(Constraints constraints, Distance_t initialPosition=Distance_t{0}, units::second_t period=20_ms)
Creates a new TrapezoidProfileSubsystem.
Definition: TrapezoidProfileSubsystem.h:40
frc2::TrapezoidProfileSubsystem::Periodic
void Periodic() override
This method is called periodically by the CommandScheduler.
Definition: TrapezoidProfileSubsystem.h:48