WPILibC++ 2024.3.1
TrapezoidProfile.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
7#include <wpi/deprecated.h>
8
9#include "units/time.h"
10#include "wpimath/MathShared.h"
11
12namespace frc {
13
14/**
15 * A trapezoid-shaped velocity profile.
16 *
17 * While this class can be used for a profiled movement from start to finish,
18 * the intended usage is to filter a reference's dynamics based on trapezoidal
19 * velocity constraints. To compute the reference obeying this constraint, do
20 * the following.
21 *
22 * Initialization:
23 * @code{.cpp}
24 * TrapezoidProfile::Constraints constraints{kMaxV, kMaxA};
25 * double previousProfiledReference = initialReference;
26 * TrapezoidProfile profile{constraints};
27 * @endcode
28 *
29 * Run on update:
30 * @code{.cpp}
31 * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
32 * previousProfiledReference,
33 * unprofiledReference);
34 * @endcode
35 *
36 * where `unprofiledReference` is free to change between calls. Note that when
37 * the unprofiled reference is within the constraints, `Calculate()` returns the
38 * unprofiled reference unchanged.
39 *
40 * Otherwise, a timer can be started to provide monotonic values for
41 * `Calculate()` and to determine when the profile has completed via
42 * `IsFinished()`.
43 */
44template <class Distance>
46 public:
48 using Velocity =
54
55 /**
56 * Profile constraints.
57 */
59 public:
60 /// Maximum velocity.
62
63 /// Maximum acceleration.
65
66 /**
67 * Default constructor.
68 */
72 }
73
74 /**
75 * Constructs constraints for a Trapezoid Profile.
76 *
77 * @param maxVelocity Maximum velocity.
78 * @param maxAcceleration Maximum acceleration.
79 */
84 }
85 };
86
87 /**
88 * Profile state.
89 */
90 class State {
91 public:
92 /// The position at this state.
94
95 /// The velocity at this state.
97
98 bool operator==(const State&) const = default;
99 };
100
101 /**
102 * Constructs a TrapezoidProfile.
103 *
104 * @param constraints The constraints on the profile, like maximum velocity.
105 */
106 TrapezoidProfile(Constraints constraints); // NOLINT
107
108 /**
109 * Constructs a TrapezoidProfile.
110 *
111 * @param constraints The constraints on the profile, like maximum velocity.
112 * @param goal The desired state when the profile is complete.
113 * @param initial The initial state (usually the current state).
114 * @deprecated Pass the desired and current state into calculate instead of
115 * constructing a new TrapezoidProfile with the desired and current state
116 */
117 WPI_DEPRECATED(
118 "Pass the desired and current state into calculate instead of "
119 "constructing a new TrapezoidProfile with the desired and current "
120 "state")
121 TrapezoidProfile(Constraints constraints, State goal,
122 State initial = State{Distance_t{0}, Velocity_t{0}});
123
128
129 /**
130 * Calculates the position and velocity for the profile at a time t where the
131 * current state is at time t = 0.
132 *
133 * @param t How long to advance from the current state toward the desired
134 * state.
135 * @return The position and velocity of the profile at time t.
136 * @deprecated Pass the desired and current state into calculate instead of
137 * constructing a new TrapezoidProfile with the desired and current state
138 */
139 [[deprecated(
140 "Pass the desired and current state into calculate instead of "
141 "constructing a new TrapezoidProfile with the desired and current "
142 "state")]]
143 State Calculate(units::second_t t) const;
144
145 /**
146 * Calculates the position and velocity for the profile at a time t where the
147 * current state is at time t = 0.
148 *
149 * @param t How long to advance from the current state toward the desired
150 * state.
151 * @param current The current state.
152 * @param goal The desired state when the profile is complete.
153 * @return The position and velocity of the profile at time t.
154 */
155 State Calculate(units::second_t t, State current, State goal);
156
157 /**
158 * Returns the time left until a target distance in the profile is reached.
159 *
160 * @param target The target distance.
161 * @return The time left until a target distance in the profile is reached.
162 */
163 units::second_t TimeLeftUntil(Distance_t target) const;
164
165 /**
166 * Returns the total time the profile takes to reach the goal.
167 *
168 * @return The total time the profile takes to reach the goal.
169 */
170 units::second_t TotalTime() const { return m_endDeccel; }
171
172 /**
173 * Returns true if the profile has reached the goal.
174 *
175 * The profile has reached the goal if the time since the profile started has
176 * exceeded the profile's total time.
177 *
178 * @param t The time since the beginning of the profile.
179 * @return True if the profile has reached the goal.
180 */
181 bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
182
183 private:
184 /**
185 * Returns true if the profile inverted.
186 *
187 * The profile is inverted if goal position is less than the initial position.
188 *
189 * @param initial The initial state (usually the current state).
190 * @param goal The desired state when the profile is complete.
191 */
192 static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
193 return initial.position > goal.position;
194 }
195
196 // Flip the sign of the velocity and position if the profile is inverted
197 State Direct(const State& in) const {
198 State result = in;
199 result.position *= m_direction;
200 result.velocity *= m_direction;
201 return result;
202 }
203
204 // The direction of the profile, either 1 for forwards or -1 for inverted
205 int m_direction;
206
207 Constraints m_constraints;
208 State m_current;
209 State m_goal; // TODO: remove
210 bool m_newAPI; // TODO: remove
211
212 units::second_t m_endAccel;
213 units::second_t m_endFullSpeed;
214 units::second_t m_endDeccel;
215};
216} // namespace frc
217
218#include "TrapezoidProfile.inc"
Profile constraints.
Definition: TrapezoidProfile.h:58
Constraints(Velocity_t maxVelocity, Acceleration_t maxAcceleration)
Constructs constraints for a Trapezoid Profile.
Definition: TrapezoidProfile.h:80
Constraints()
Default constructor.
Definition: TrapezoidProfile.h:69
Velocity_t maxVelocity
Maximum velocity.
Definition: TrapezoidProfile.h:61
Acceleration_t maxAcceleration
Maximum acceleration.
Definition: TrapezoidProfile.h:64
Profile state.
Definition: TrapezoidProfile.h:90
Velocity_t velocity
The velocity at this state.
Definition: TrapezoidProfile.h:96
bool operator==(const State &) const =default
Distance_t position
The position at this state.
Definition: TrapezoidProfile.h:93
A trapezoid-shaped velocity profile.
Definition: TrapezoidProfile.h:45
TrapezoidProfile & operator=(const TrapezoidProfile &)=default
TrapezoidProfile(Constraints constraints)
Constructs a TrapezoidProfile.
Definition: TrapezoidProfile.inc:14
units::compound_unit< Velocity, units::inverse< units::seconds > > Acceleration
Definition: TrapezoidProfile.h:52
units::compound_unit< Distance, units::inverse< units::seconds > > Velocity
Definition: TrapezoidProfile.h:49
TrapezoidProfile(const TrapezoidProfile &)=default
State Calculate(units::second_t t) const
Calculates the position and velocity for the profile at a time t where the current state is at time t...
Definition: TrapezoidProfile.inc:67
units::second_t TimeLeftUntil(Distance_t target) const
Returns the time left until a target distance in the profile is reached.
Definition: TrapezoidProfile.inc:171
units::second_t TotalTime() const
Returns the total time the profile takes to reach the goal.
Definition: TrapezoidProfile.h:170
bool IsFinished(units::second_t t) const
Returns true if the profile has reached the goal.
Definition: TrapezoidProfile.h:181
TrapezoidProfile & operator=(TrapezoidProfile &&)=default
units::unit_t< Velocity > Velocity_t
Definition: TrapezoidProfile.h:50
TrapezoidProfile(TrapezoidProfile &&)=default
static void ReportUsage(MathUsageId id, int count)
Definition: MathShared.h:73
typename units::detail::compound_impl< U, Us... >::type compound_unit
Represents a unit type made up from other units.
Definition: base.h:1434
constexpr auto in(type t, int set) -> bool
Definition: core.h:611
State
Possible state of a SysId routine.
Definition: SysIdRoutineLog.h:25
Definition: AprilTagDetector_cv.h:11