10 #include <hal/FRCUsageReporting.h>
11 #include <units/units.h>
44 template <
class Distance>
47 using Distance_t = units::unit_t<Distance>;
49 units::compound_unit<Distance, units::inverse<units::seconds>>;
50 using Velocity_t = units::unit_t<Velocity>;
52 units::compound_unit<Velocity, units::inverse<units::seconds>>;
53 using Acceleration_t = units::unit_t<Acceleration>;
58 HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
60 Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
61 : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
62 HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
64 Velocity_t maxVelocity{0};
65 Acceleration_t maxAcceleration{0};
70 Distance_t position{0};
71 Velocity_t velocity{0};
72 bool operator==(
const State& rhs)
const {
73 return position == rhs.position && velocity == rhs.velocity;
75 bool operator!=(
const State& rhs)
const {
return !(*
this == rhs); }
86 State initial =
State{Distance_t(0), Velocity_t(0)});
111 units::second_t
TotalTime()
const {
return m_endDeccel; }
132 static bool ShouldFlipAcceleration(
const State& initial,
const State& goal) {
133 return initial.position > goal.position;
137 State Direct(
const State& in)
const {
139 result.position *= m_direction;
140 result.velocity *= m_direction;
147 Constraints m_constraints;
151 units::second_t m_endAccel;
152 units::second_t m_endFullSpeed;
153 units::second_t m_endDeccel;
157 #include "TrapezoidProfile.inc"