WPILibC++  2020.3.2
TrajectoryConfig.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 <memory>
11 #include <utility>
12 #include <vector>
13 
14 #include <units/units.h>
15 
16 #include "frc/kinematics/DifferentialDriveKinematics.h"
17 #include "frc/kinematics/MecanumDriveKinematics.h"
18 #include "frc/kinematics/SwerveDriveKinematics.h"
19 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
20 #include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
21 #include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
22 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
23 
24 namespace frc {
36  public:
42  TrajectoryConfig(units::meters_per_second_t maxVelocity,
43  units::meters_per_second_squared_t maxAcceleration)
44  : m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
45 
46  TrajectoryConfig(const TrajectoryConfig&) = delete;
47  TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
48 
50  TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
51 
56  void SetStartVelocity(units::meters_per_second_t startVelocity) {
57  m_startVelocity = startVelocity;
58  }
59 
64  void SetEndVelocity(units::meters_per_second_t endVelocity) {
65  m_endVelocity = endVelocity;
66  }
67 
72  void SetReversed(bool reversed) { m_reversed = reversed; }
73 
78  template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
79  TrajectoryConstraint, Constraint>>>
80  void AddConstraint(Constraint constraint) {
81  m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
82  }
83 
90  void SetKinematics(const DifferentialDriveKinematics& kinematics) {
92  DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
93  }
94 
102  AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
103  }
104 
111  template <size_t NumModules>
113  AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
114  }
115 
120  units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
121 
126  units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
127 
132  units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
133 
138  units::meters_per_second_squared_t MaxAcceleration() const {
139  return m_maxAcceleration;
140  }
141 
146  const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
147  const {
148  return m_constraints;
149  }
150 
155  bool IsReversed() const { return m_reversed; }
156 
157  private:
158  units::meters_per_second_t m_startVelocity = 0_mps;
159  units::meters_per_second_t m_endVelocity = 0_mps;
160  units::meters_per_second_t m_maxVelocity;
161  units::meters_per_second_squared_t m_maxAcceleration;
162  std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
163  bool m_reversed = false;
164 };
165 } // namespace frc
frc::TrajectoryConfig::SetStartVelocity
void SetStartVelocity(units::meters_per_second_t startVelocity)
Sets the start velocity of the trajectory.
Definition: TrajectoryConfig.h:56
frc::TrajectoryConfig::IsReversed
bool IsReversed() const
Returns whether the trajectory is reversed or not.
Definition: TrajectoryConfig.h:155
frc::TrajectoryConfig::AddConstraint
void AddConstraint(Constraint constraint)
Adds a user-defined constraint to the trajectory.
Definition: TrajectoryConfig.h:80
frc::TrajectoryConfig::TrajectoryConfig
TrajectoryConfig(units::meters_per_second_t maxVelocity, units::meters_per_second_squared_t maxAcceleration)
Constructs a config object.
Definition: TrajectoryConfig.h:42
frc::TrajectoryConfig::SetKinematics
void SetKinematics(MecanumDriveKinematics kinematics)
Adds a mecanum drive kinematics constraint to ensure that no wheel velocity of a mecanum drive goes a...
Definition: TrajectoryConfig.h:101
frc::TrajectoryConstraint
An interface for defining user-defined velocity and acceleration constraints while generating traject...
Definition: TrajectoryConstraint.h:22
frc::DifferentialDriveKinematicsConstraint
Definition: DifferentialDriveKinematicsConstraint.h:22
frc::TrajectoryConfig::EndVelocity
units::meters_per_second_t EndVelocity() const
Returns the ending velocity of the trajectory.
Definition: TrajectoryConfig.h:126
frc::TrajectoryConfig::SetEndVelocity
void SetEndVelocity(units::meters_per_second_t endVelocity)
Sets the end velocity of the trajectory.
Definition: TrajectoryConfig.h:64
frc::SwerveDriveKinematics
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.h:47
frc::SwerveDriveKinematicsConstraint
Definition: SwerveDriveKinematicsConstraint.h:26
frc::TrajectoryConfig::Constraints
const std::vector< std::unique_ptr< TrajectoryConstraint > > & Constraints() const
Returns the user-defined constraints of the trajectory.
Definition: TrajectoryConfig.h:146
frc::DifferentialDriveKinematics
Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel velo...
Definition: DifferentialDriveKinematics.h:25
frc::TrajectoryConfig::SetKinematics
void SetKinematics(SwerveDriveKinematics< NumModules > &kinematics)
Adds a swerve drive kinematics constraint to ensure that no wheel velocity of a swerve drive goes abo...
Definition: TrajectoryConfig.h:112
frc::TrajectoryConfig::MaxVelocity
units::meters_per_second_t MaxVelocity() const
Returns the maximum velocity of the trajectory.
Definition: TrajectoryConfig.h:132
frc::TrajectoryConfig::SetKinematics
void SetKinematics(const DifferentialDriveKinematics &kinematics)
Adds a differential drive kinematics constraint to ensure that no wheel velocity of a differential dr...
Definition: TrajectoryConfig.h:90
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::TrajectoryConfig::MaxAcceleration
units::meters_per_second_squared_t MaxAcceleration() const
Returns the maximum acceleration of the trajectory.
Definition: TrajectoryConfig.h:138
frc::TrajectoryConfig::StartVelocity
units::meters_per_second_t StartVelocity() const
Returns the starting velocity of the trajectory.
Definition: TrajectoryConfig.h:120
frc::TrajectoryConfig
Represents the configuration for generating a trajectory.
Definition: TrajectoryConfig.h:35
frc::MecanumDriveKinematics
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: MecanumDriveKinematics.h:42
frc::MecanumDriveKinematicsConstraint
Definition: MecanumDriveKinematicsConstraint.h:24
frc::TrajectoryConfig::SetReversed
void SetReversed(bool reversed)
Sets the reversed flag of the trajectory.
Definition: TrajectoryConfig.h:72