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.wpilibj2.command;
006
007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.math.controller.HolonomicDriveController;
010import edu.wpi.first.math.controller.PIDController;
011import edu.wpi.first.math.controller.ProfiledPIDController;
012import edu.wpi.first.math.controller.SimpleMotorFeedforward;
013import edu.wpi.first.math.geometry.Pose2d;
014import edu.wpi.first.math.geometry.Rotation2d;
015import edu.wpi.first.math.kinematics.ChassisSpeeds;
016import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
017import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
018import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
019import edu.wpi.first.math.trajectory.Trajectory;
020import edu.wpi.first.wpilibj.Timer;
021import java.util.function.Consumer;
022import java.util.function.Supplier;
023
024/**
025 * A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController
026 * ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a mecanum drive.
027 *
028 * <p>The command handles trajectory-following, Velocity PID calculations, and feedforwards
029 * internally. This is intended to be a more-or-less "complete solution" that can be used by teams
030 * without a great deal of controls expertise.
031 *
032 * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID
033 * functionality of a "smart" motor controller) may use the secondary constructor that omits the PID
034 * and feedforward functionality, returning only the raw wheel speeds from the PID controllers.
035 *
036 * <p>The robot angle controller does not follow the angle given by the trajectory but rather goes
037 * to the angle given in the final state of the trajectory.
038 *
039 * <p>This class is provided by the NewCommands VendorDep
040 */
041public class MecanumControllerCommand extends Command {
042  private final Timer m_timer = new Timer();
043  private final boolean m_usePID;
044  private final Trajectory m_trajectory;
045  private final Supplier<Pose2d> m_pose;
046  private final SimpleMotorFeedforward m_feedforward;
047  private final MecanumDriveKinematics m_kinematics;
048  private final HolonomicDriveController m_controller;
049  private final Supplier<Rotation2d> m_desiredRotation;
050  private final double m_maxWheelVelocityMetersPerSecond;
051  private final PIDController m_frontLeftController;
052  private final PIDController m_rearLeftController;
053  private final PIDController m_frontRightController;
054  private final PIDController m_rearRightController;
055  private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
056  private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
057  private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
058  private MecanumDriveWheelSpeeds m_prevSpeeds;
059  private double m_prevTime;
060
061  /**
062   * Constructs a new MecanumControllerCommand that when executed will follow the provided
063   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
064   * 12 as a voltage output to the motor.
065   *
066   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
067   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
068   *
069   * @param trajectory The trajectory to follow.
070   * @param pose A function that supplies the robot pose - use one of the odometry classes to
071   *     provide this.
072   * @param feedforward The feedforward to use for the drivetrain.
073   * @param kinematics The kinematics for the robot drivetrain.
074   * @param xController The Trajectory Tracker PID controller for the robot's x position.
075   * @param yController The Trajectory Tracker PID controller for the robot's y position.
076   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
077   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
078   *     step.
079   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
080   * @param frontLeftController The front left wheel velocity PID.
081   * @param rearLeftController The rear left wheel velocity PID.
082   * @param frontRightController The front right wheel velocity PID.
083   * @param rearRightController The rear right wheel velocity PID.
084   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
085   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
086   *     voltages.
087   * @param requirements The subsystems to require.
088   */
089  @SuppressWarnings("this-escape")
090  public MecanumControllerCommand(
091      Trajectory trajectory,
092      Supplier<Pose2d> pose,
093      SimpleMotorFeedforward feedforward,
094      MecanumDriveKinematics kinematics,
095      PIDController xController,
096      PIDController yController,
097      ProfiledPIDController thetaController,
098      Supplier<Rotation2d> desiredRotation,
099      double maxWheelVelocityMetersPerSecond,
100      PIDController frontLeftController,
101      PIDController rearLeftController,
102      PIDController frontRightController,
103      PIDController rearRightController,
104      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
105      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
106      Subsystem... requirements) {
107    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
108    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
109    m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
110    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
111
112    m_controller =
113        new HolonomicDriveController(
114            requireNonNullParam(xController, "xController", "MecanumControllerCommand"),
115            requireNonNullParam(yController, "yController", "MecanumControllerCommand"),
116            requireNonNullParam(thetaController, "thetaController", "MecanumControllerCommand"));
117
118    m_desiredRotation =
119        requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
120
121    m_maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond;
122
123    m_frontLeftController =
124        requireNonNullParam(frontLeftController, "frontLeftController", "MecanumControllerCommand");
125    m_rearLeftController =
126        requireNonNullParam(rearLeftController, "rearLeftController", "MecanumControllerCommand");
127    m_frontRightController =
128        requireNonNullParam(
129            frontRightController, "frontRightController", "MecanumControllerCommand");
130    m_rearRightController =
131        requireNonNullParam(rearRightController, "rearRightController", "MecanumControllerCommand");
132
133    m_currentWheelSpeeds =
134        requireNonNullParam(currentWheelSpeeds, "currentWheelSpeeds", "MecanumControllerCommand");
135
136    m_outputDriveVoltages =
137        requireNonNullParam(outputDriveVoltages, "outputDriveVoltages", "MecanumControllerCommand");
138
139    m_outputWheelSpeeds = null;
140
141    m_usePID = true;
142
143    addRequirements(requirements);
144  }
145
146  /**
147   * Constructs a new MecanumControllerCommand that when executed will follow the provided
148   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
149   * 12 as a voltage output to the motor.
150   *
151   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
152   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
153   *
154   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
155   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
156   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
157   * should be used.
158   *
159   * @param trajectory The trajectory to follow.
160   * @param pose A function that supplies the robot pose - use one of the odometry classes to
161   *     provide this.
162   * @param feedforward The feedforward to use for the drivetrain.
163   * @param kinematics The kinematics for the robot drivetrain.
164   * @param xController The Trajectory Tracker PID controller for the robot's x position.
165   * @param yController The Trajectory Tracker PID controller for the robot's y position.
166   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
167   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
168   * @param frontLeftController The front left wheel velocity PID.
169   * @param rearLeftController The rear left wheel velocity PID.
170   * @param frontRightController The front right wheel velocity PID.
171   * @param rearRightController The rear right wheel velocity PID.
172   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
173   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
174   *     voltages.
175   * @param requirements The subsystems to require.
176   */
177  public MecanumControllerCommand(
178      Trajectory trajectory,
179      Supplier<Pose2d> pose,
180      SimpleMotorFeedforward feedforward,
181      MecanumDriveKinematics kinematics,
182      PIDController xController,
183      PIDController yController,
184      ProfiledPIDController thetaController,
185      double maxWheelVelocityMetersPerSecond,
186      PIDController frontLeftController,
187      PIDController rearLeftController,
188      PIDController frontRightController,
189      PIDController rearRightController,
190      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
191      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
192      Subsystem... requirements) {
193    this(
194        trajectory,
195        pose,
196        feedforward,
197        kinematics,
198        xController,
199        yController,
200        thetaController,
201        () ->
202            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
203        maxWheelVelocityMetersPerSecond,
204        frontLeftController,
205        rearLeftController,
206        frontRightController,
207        rearRightController,
208        currentWheelSpeeds,
209        outputDriveVoltages,
210        requirements);
211  }
212
213  /**
214   * Constructs a new MecanumControllerCommand that when executed will follow the provided
215   * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
216   *
217   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
218   * this is left to the user, since it is not appropriate for paths with nonstationary end-states.
219   *
220   * @param trajectory The trajectory to follow.
221   * @param pose A function that supplies the robot pose - use one of the odometry classes to
222   *     provide this.
223   * @param kinematics The kinematics for the robot drivetrain.
224   * @param xController The Trajectory Tracker PID controller for the robot's x position.
225   * @param yController The Trajectory Tracker PID controller for the robot's y position.
226   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
227   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
228   *     step.
229   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
230   * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
231   * @param requirements The subsystems to require.
232   */
233  @SuppressWarnings("this-escape")
234  public MecanumControllerCommand(
235      Trajectory trajectory,
236      Supplier<Pose2d> pose,
237      MecanumDriveKinematics kinematics,
238      PIDController xController,
239      PIDController yController,
240      ProfiledPIDController thetaController,
241      Supplier<Rotation2d> desiredRotation,
242      double maxWheelVelocityMetersPerSecond,
243      Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
244      Subsystem... requirements) {
245    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
246    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
247    m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
248    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
249
250    m_controller =
251        new HolonomicDriveController(
252            requireNonNullParam(xController, "xController", "MecanumControllerCommand"),
253            requireNonNullParam(yController, "yController", "MecanumControllerCommand"),
254            requireNonNullParam(thetaController, "thetaController", "MecanumControllerCommand"));
255
256    m_desiredRotation =
257        requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
258
259    m_maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond;
260
261    m_frontLeftController = null;
262    m_rearLeftController = null;
263    m_frontRightController = null;
264    m_rearRightController = null;
265
266    m_currentWheelSpeeds = null;
267
268    m_outputWheelSpeeds =
269        requireNonNullParam(outputWheelSpeeds, "outputWheelSpeeds", "MecanumControllerCommand");
270
271    m_outputDriveVoltages = null;
272
273    m_usePID = false;
274
275    addRequirements(requirements);
276  }
277
278  /**
279   * Constructs a new MecanumControllerCommand that when executed will follow the provided
280   * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
281   *
282   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
283   * this is left to the user, since it is not appropriate for paths with nonstationary end-states.
284   *
285   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
286   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
287   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
288   * should be used.
289   *
290   * @param trajectory The trajectory to follow.
291   * @param pose A function that supplies the robot pose - use one of the odometry classes to
292   *     provide this.
293   * @param kinematics The kinematics for the robot drivetrain.
294   * @param xController The Trajectory Tracker PID controller for the robot's x position.
295   * @param yController The Trajectory Tracker PID controller for the robot's y position.
296   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
297   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
298   * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
299   * @param requirements The subsystems to require.
300   */
301  public MecanumControllerCommand(
302      Trajectory trajectory,
303      Supplier<Pose2d> pose,
304      MecanumDriveKinematics kinematics,
305      PIDController xController,
306      PIDController yController,
307      ProfiledPIDController thetaController,
308      double maxWheelVelocityMetersPerSecond,
309      Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
310      Subsystem... requirements) {
311    this(
312        trajectory,
313        pose,
314        kinematics,
315        xController,
316        yController,
317        thetaController,
318        () ->
319            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
320        maxWheelVelocityMetersPerSecond,
321        outputWheelSpeeds,
322        requirements);
323  }
324
325  @Override
326  public void initialize() {
327    var initialState = m_trajectory.sample(0);
328
329    var initialXVelocity =
330        initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos();
331    var initialYVelocity =
332        initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
333
334    m_prevSpeeds =
335        m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
336
337    m_timer.restart();
338  }
339
340  @Override
341  public void execute() {
342    double curTime = m_timer.get();
343    double dt = curTime - m_prevTime;
344
345    var desiredState = m_trajectory.sample(curTime);
346
347    var targetChassisSpeeds =
348        m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
349    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
350
351    targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
352
353    var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
354    var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
355    var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
356    var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
357
358    double frontLeftOutput;
359    double rearLeftOutput;
360    double frontRightOutput;
361    double rearRightOutput;
362
363    if (m_usePID) {
364      final double frontLeftFeedforward =
365          m_feedforward.calculate(
366              frontLeftSpeedSetpoint,
367              (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
368
369      final double rearLeftFeedforward =
370          m_feedforward.calculate(
371              rearLeftSpeedSetpoint,
372              (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
373
374      final double frontRightFeedforward =
375          m_feedforward.calculate(
376              frontRightSpeedSetpoint,
377              (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
378
379      final double rearRightFeedforward =
380          m_feedforward.calculate(
381              rearRightSpeedSetpoint,
382              (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
383
384      frontLeftOutput =
385          frontLeftFeedforward
386              + m_frontLeftController.calculate(
387                  m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
388
389      rearLeftOutput =
390          rearLeftFeedforward
391              + m_rearLeftController.calculate(
392                  m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
393
394      frontRightOutput =
395          frontRightFeedforward
396              + m_frontRightController.calculate(
397                  m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
398
399      rearRightOutput =
400          rearRightFeedforward
401              + m_rearRightController.calculate(
402                  m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
403
404      m_outputDriveVoltages.accept(
405          new MecanumDriveMotorVoltages(
406              frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
407
408    } else {
409      m_outputWheelSpeeds.accept(
410          new MecanumDriveWheelSpeeds(
411              frontLeftSpeedSetpoint,
412              frontRightSpeedSetpoint,
413              rearLeftSpeedSetpoint,
414              rearRightSpeedSetpoint));
415    }
416
417    m_prevTime = curTime;
418    m_prevSpeeds = targetWheelSpeeds;
419  }
420
421  @Override
422  public void end(boolean interrupted) {
423    m_timer.stop();
424  }
425
426  @Override
427  public boolean isFinished() {
428    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
429  }
430}