WPILibC++  2020.3.2-60-g3011ebe
TrapezoidProfile.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 <hal/FRCUsageReporting.h>
11 #include <units/units.h>
12 
13 namespace frc {
14 
44 template <class Distance>
46  public:
47  using Distance_t = units::unit_t<Distance>;
48  using Velocity =
49  units::compound_unit<Distance, units::inverse<units::seconds>>;
50  using Velocity_t = units::unit_t<Velocity>;
51  using Acceleration =
52  units::compound_unit<Velocity, units::inverse<units::seconds>>;
53  using Acceleration_t = units::unit_t<Acceleration>;
54 
55  class Constraints {
56  public:
57  Constraints() {
58  HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
59  }
60  Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
61  : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
62  HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
63  }
64  Velocity_t maxVelocity{0};
65  Acceleration_t maxAcceleration{0};
66  };
67 
68  class State {
69  public:
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;
74  }
75  bool operator!=(const State& rhs) const { return !(*this == rhs); }
76  };
77 
85  TrapezoidProfile(Constraints constraints, State goal,
86  State initial = State{Distance_t(0), Velocity_t(0)});
87 
88  TrapezoidProfile(const TrapezoidProfile&) = default;
89  TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
91  TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
92 
99  State Calculate(units::second_t t) const;
100 
106  units::second_t TimeLeftUntil(Distance_t target) const;
107 
111  units::second_t TotalTime() const { return m_endDeccel; }
112 
121  bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
122 
123  private:
132  static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
133  return initial.position > goal.position;
134  }
135 
136  // Flip the sign of the velocity and position if the profile is inverted
137  State Direct(const State& in) const {
138  State result = in;
139  result.position *= m_direction;
140  result.velocity *= m_direction;
141  return result;
142  }
143 
144  // The direction of the profile, either 1 for forwards or -1 for inverted
145  int m_direction;
146 
147  Constraints m_constraints;
148  State m_initial;
149  State m_goal;
150 
151  units::second_t m_endAccel;
152  units::second_t m_endFullSpeed;
153  units::second_t m_endDeccel;
154 };
155 } // namespace frc
156 
157 #include "TrapezoidProfile.inc"
frc::TrapezoidProfile::TrapezoidProfile
TrapezoidProfile(Constraints constraints, State goal, State initial=State{Distance_t(0), Velocity_t(0)})
Construct a TrapezoidProfile.
Definition: TrapezoidProfile.inc:14
frc::TrapezoidProfile
A trapezoid-shaped velocity profile.
Definition: TrapezoidProfile.h:45
frc::TrapezoidProfile::TimeLeftUntil
units::second_t TimeLeftUntil(Distance_t target) const
Returns the time left until a target distance in the profile is reached.
Definition: TrapezoidProfile.inc:91
frc::TrapezoidProfile::Constraints
Definition: TrapezoidProfile.h:55
frc::TrapezoidProfile::State
Definition: TrapezoidProfile.h:68
frc::TrapezoidProfile::TotalTime
units::second_t TotalTime() const
Returns the total time the profile takes to reach the goal.
Definition: TrapezoidProfile.h:111
frc::TrapezoidProfile::Calculate
State Calculate(units::second_t t) const
Calculate the correct position and velocity for the profile at a time t where the beginning of the pr...
Definition: TrapezoidProfile.inc:62
frc
A class that enforces constraints on the differential drive kinematics.
Definition: PDPSim.h:16
frc::TrapezoidProfile::IsFinished
bool IsFinished(units::second_t t) const
Returns true if the profile has reached the goal.
Definition: TrapezoidProfile.h:121