001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.trajectory;
006
007import static edu.wpi.first.units.Units.MetersPerSecond;
008import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
009
010import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
011import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
012import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
013import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
014import edu.wpi.first.math.trajectory.constraint.MecanumDriveKinematicsConstraint;
015import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
016import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
017import edu.wpi.first.units.measure.LinearAcceleration;
018import edu.wpi.first.units.measure.LinearVelocity;
019import java.util.ArrayList;
020import java.util.List;
021
022/**
023 * Represents the configuration for generating a trajectory. This class stores the start velocity,
024 * end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
025 *
026 * <p>The class must be constructed with a max velocity and max acceleration. The other parameters
027 * (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable values
028 * (0, 0, {}, false). These values can be changed via the setXXX methods.
029 */
030public class TrajectoryConfig {
031  private final double m_maxVelocity;
032  private final double m_maxAcceleration;
033  private final List<TrajectoryConstraint> m_constraints;
034  private double m_startVelocity;
035  private double m_endVelocity;
036  private boolean m_reversed;
037
038  /**
039   * Constructs the trajectory configuration class.
040   *
041   * @param maxVelocity The max velocity for the trajectory in m/s.
042   * @param maxAcceleration The max acceleration for the trajectory in m/s².
043   */
044  public TrajectoryConfig(double maxVelocity, double maxAcceleration) {
045    m_maxVelocity = maxVelocity;
046    m_maxAcceleration = maxAcceleration;
047    m_constraints = new ArrayList<>();
048  }
049
050  /**
051   * Constructs the trajectory configuration class.
052   *
053   * @param maxVelocity The max velocity for the trajectory in m/s.
054   * @param maxAcceleration The max acceleration for the trajectory in m/s².
055   */
056  public TrajectoryConfig(LinearVelocity maxVelocity, LinearAcceleration maxAcceleration) {
057    this(maxVelocity.in(MetersPerSecond), maxAcceleration.in(MetersPerSecondPerSecond));
058  }
059
060  /**
061   * Adds a user-defined constraint to the trajectory.
062   *
063   * @param constraint The user-defined constraint.
064   * @return Instance of the current config object.
065   */
066  public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
067    m_constraints.add(constraint);
068    return this;
069  }
070
071  /**
072   * Adds all user-defined constraints from a list to the trajectory.
073   *
074   * @param constraints List of user-defined constraints.
075   * @return Instance of the current config object.
076   */
077  public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
078    m_constraints.addAll(constraints);
079    return this;
080  }
081
082  /**
083   * Adds a differential drive kinematics constraint to ensure that no wheel velocity of a
084   * differential drive goes above the max velocity.
085   *
086   * @param kinematics The differential drive kinematics.
087   * @return Instance of the current config object.
088   */
089  public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
090    addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
091    return this;
092  }
093
094  /**
095   * Adds a mecanum drive kinematics constraint to ensure that no wheel velocity of a mecanum drive
096   * goes above the max velocity.
097   *
098   * @param kinematics The mecanum drive kinematics.
099   * @return Instance of the current config object.
100   */
101  public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
102    addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
103    return this;
104  }
105
106  /**
107   * Adds a swerve drive kinematics constraint to ensure that no wheel velocity of a swerve drive
108   * goes above the max velocity.
109   *
110   * @param kinematics The swerve drive kinematics.
111   * @return Instance of the current config object.
112   */
113  public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
114    addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
115    return this;
116  }
117
118  /**
119   * Returns the starting velocity of the trajectory.
120   *
121   * @return The starting velocity of the trajectory in m/s.
122   */
123  public double getStartVelocity() {
124    return m_startVelocity;
125  }
126
127  /**
128   * Sets the start velocity of the trajectory.
129   *
130   * @param startVelocity The start velocity of the trajectory in m/s.
131   * @return Instance of the current config object.
132   */
133  public TrajectoryConfig setStartVelocity(double startVelocity) {
134    m_startVelocity = startVelocity;
135    return this;
136  }
137
138  /**
139   * Sets the start velocity of the trajectory.
140   *
141   * @param startVelocity The start velocity of the trajectory.
142   * @return Instance of the current config object.
143   */
144  public TrajectoryConfig setStartVelocity(LinearVelocity startVelocity) {
145    return setStartVelocity(startVelocity.in(MetersPerSecond));
146  }
147
148  /**
149   * Returns the starting velocity of the trajectory.
150   *
151   * @return The starting velocity of the trajectory in m/s.
152   */
153  public double getEndVelocity() {
154    return m_endVelocity;
155  }
156
157  /**
158   * Sets the end velocity of the trajectory.
159   *
160   * @param endVelocity The end velocity of the trajectory in m/s.
161   * @return Instance of the current config object.
162   */
163  public TrajectoryConfig setEndVelocity(double endVelocity) {
164    m_endVelocity = endVelocity;
165    return this;
166  }
167
168  /**
169   * Sets the end velocity of the trajectory.
170   *
171   * @param endVelocity The end velocity of the trajectory.
172   * @return Instance of the current config object.
173   */
174  public TrajectoryConfig setEndVelocity(LinearVelocity endVelocity) {
175    return setEndVelocity(endVelocity.in(MetersPerSecond));
176  }
177
178  /**
179   * Returns the maximum velocity of the trajectory.
180   *
181   * @return The maximum velocity of the trajectory in m/s.
182   */
183  public double getMaxVelocity() {
184    return m_maxVelocity;
185  }
186
187  /**
188   * Returns the maximum acceleration of the trajectory.
189   *
190   * @return The maximum acceleration of the trajectory in m/s².
191   */
192  public double getMaxAcceleration() {
193    return m_maxAcceleration;
194  }
195
196  /**
197   * Returns the user-defined constraints of the trajectory.
198   *
199   * @return The user-defined constraints of the trajectory.
200   */
201  public List<TrajectoryConstraint> getConstraints() {
202    return m_constraints;
203  }
204
205  /**
206   * Returns whether the trajectory is reversed or not.
207   *
208   * @return whether the trajectory is reversed or not.
209   */
210  public boolean isReversed() {
211    return m_reversed;
212  }
213
214  /**
215   * Sets the reversed flag of the trajectory.
216   *
217   * @param reversed Whether the trajectory should be reversed or not.
218   * @return Instance of the current config object.
219   */
220  public TrajectoryConfig setReversed(boolean reversed) {
221    m_reversed = reversed;
222    return this;
223  }
224}