WPILibC++ 2024.3.2
TrapezoidProfileSubsystem.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
8#include <units/time.h>
9
11
12namespace frc2 {
13/**
14 * A subsystem that generates and runs trapezoidal motion profiles
15 * automatically. The user specifies how to use the current state of the motion
16 * profile by overriding the `UseState` method.
17 *
18 * This class is provided by the NewCommands VendorDep
19 */
20template <class Distance>
23 using Velocity =
26 using State = typename frc::TrapezoidProfile<Distance>::State;
27 using Constraints = typename frc::TrapezoidProfile<Distance>::Constraints;
28
29 public:
30 /**
31 * Creates a new TrapezoidProfileSubsystem.
32 *
33 * @param constraints The constraints (maximum velocity and acceleration)
34 * for the profiles.
35 * @param initialPosition The initial position of the controller mechanism
36 * when the subsystem is constructed.
37 * @param period The period of the main robot loop, in seconds.
38 */
39 explicit TrapezoidProfileSubsystem(Constraints constraints,
40 Distance_t initialPosition = Distance_t{0},
41 units::second_t period = 20_ms)
42 : m_profile(constraints),
43 m_state{initialPosition, Velocity_t(0)},
44 m_goal{initialPosition, Velocity_t{0}},
45 m_period(period) {}
46
47 void Periodic() override {
48 m_state = m_profile.Calculate(m_period, m_state, m_goal);
49 if (m_enabled) {
50 UseState(m_state);
51 }
52 }
53
54 /**
55 * Sets the goal state for the subsystem.
56 *
57 * @param goal The goal state for the subsystem's motion profile.
58 */
59 void SetGoal(State goal) { m_goal = goal; }
60
61 /**
62 * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
63 *
64 * @param goal The goal position for the subsystem's motion profile.
65 */
66 void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
67
68 protected:
69 /**
70 * Users should override this to consume the current state of the motion
71 * profile.
72 *
73 * @param state The current state of the motion profile.
74 */
75 virtual void UseState(State state) = 0;
76
77 /**
78 * Enable the TrapezoidProfileSubsystem's output.
79 */
80 void Enable() { m_enabled = true; }
81
82 /**
83 * Disable the TrapezoidProfileSubsystem's output.
84 */
85 void Disable() { m_enabled = false; }
86
87 private:
89 State m_state;
90 State m_goal;
91 units::second_t m_period;
92 bool m_enabled{false};
93};
94} // namespace frc2
A base for subsystems that handles registration in the constructor, and provides a more intuitive met...
Definition: SubsystemBase.h:24
A subsystem that generates and runs trapezoidal motion profiles automatically.
Definition: TrapezoidProfileSubsystem.h:21
void SetGoal(State goal)
Sets the goal state for the subsystem.
Definition: TrapezoidProfileSubsystem.h:59
void Disable()
Disable the TrapezoidProfileSubsystem's output.
Definition: TrapezoidProfileSubsystem.h:85
void SetGoal(Distance_t goal)
Sets the goal state for the subsystem.
Definition: TrapezoidProfileSubsystem.h:66
TrapezoidProfileSubsystem(Constraints constraints, Distance_t initialPosition=Distance_t{0}, units::second_t period=20_ms)
Creates a new TrapezoidProfileSubsystem.
Definition: TrapezoidProfileSubsystem.h:39
void Periodic() override
This method is called periodically by the CommandScheduler.
Definition: TrapezoidProfileSubsystem.h:47
void Enable()
Enable the TrapezoidProfileSubsystem's output.
Definition: TrapezoidProfileSubsystem.h:80
virtual void UseState(State state)=0
Users should override this to consume the current state of the motion profile.
Profile constraints.
Definition: TrapezoidProfile.h:58
Profile state.
Definition: TrapezoidProfile.h:90
A trapezoid-shaped velocity profile.
Definition: TrapezoidProfile.h:45
typename units::detail::compound_impl< U, Us... >::type compound_unit
Represents a unit type made up from other units.
Definition: base.h:1434
state
Definition: core.h:2271
Definition: TrapezoidProfileSubsystem.h:12