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 CommandBase {
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  public MecanumControllerCommand(
090      Trajectory trajectory,
091      Supplier<Pose2d> pose,
092      SimpleMotorFeedforward feedforward,
093      MecanumDriveKinematics kinematics,
094      PIDController xController,
095      PIDController yController,
096      ProfiledPIDController thetaController,
097      Supplier<Rotation2d> desiredRotation,
098      double maxWheelVelocityMetersPerSecond,
099      PIDController frontLeftController,
100      PIDController rearLeftController,
101      PIDController frontRightController,
102      PIDController rearRightController,
103      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
104      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
105      Subsystem... requirements) {
106    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
107    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
108    m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
109    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
110
111    m_controller =
112        new HolonomicDriveController(
113            requireNonNullParam(xController, "xController", "MecanumControllerCommand"),
114            requireNonNullParam(yController, "yController", "MecanumControllerCommand"),
115            requireNonNullParam(thetaController, "thetaController", "MecanumControllerCommand"));
116
117    m_desiredRotation =
118        requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
119
120    m_maxWheelVelocityMetersPerSecond =
121        requireNonNullParam(
122            maxWheelVelocityMetersPerSecond,
123            "maxWheelVelocityMetersPerSecond",
124            "MecanumControllerCommand");
125
126    m_frontLeftController =
127        requireNonNullParam(frontLeftController, "frontLeftController", "MecanumControllerCommand");
128    m_rearLeftController =
129        requireNonNullParam(rearLeftController, "rearLeftController", "MecanumControllerCommand");
130    m_frontRightController =
131        requireNonNullParam(
132            frontRightController, "frontRightController", "MecanumControllerCommand");
133    m_rearRightController =
134        requireNonNullParam(rearRightController, "rearRightController", "MecanumControllerCommand");
135
136    m_currentWheelSpeeds =
137        requireNonNullParam(currentWheelSpeeds, "currentWheelSpeeds", "MecanumControllerCommand");
138
139    m_outputDriveVoltages =
140        requireNonNullParam(outputDriveVoltages, "outputDriveVoltages", "MecanumControllerCommand");
141
142    m_outputWheelSpeeds = null;
143
144    m_usePID = true;
145
146    addRequirements(requirements);
147  }
148
149  /**
150   * Constructs a new MecanumControllerCommand that when executed will follow the provided
151   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
152   * 12 as a voltage output to the motor.
153   *
154   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
155   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
156   *
157   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
158   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
159   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
160   * should be used.
161   *
162   * @param trajectory The trajectory to follow.
163   * @param pose A function that supplies the robot pose - use one of the odometry classes to
164   *     provide this.
165   * @param feedforward The feedforward to use for the drivetrain.
166   * @param kinematics The kinematics for the robot drivetrain.
167   * @param xController The Trajectory Tracker PID controller for the robot's x position.
168   * @param yController The Trajectory Tracker PID controller for the robot's y position.
169   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
170   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
171   * @param frontLeftController The front left wheel velocity PID.
172   * @param rearLeftController The rear left wheel velocity PID.
173   * @param frontRightController The front right wheel velocity PID.
174   * @param rearRightController The rear right wheel velocity PID.
175   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
176   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
177   *     voltages.
178   * @param requirements The subsystems to require.
179   */
180  public MecanumControllerCommand(
181      Trajectory trajectory,
182      Supplier<Pose2d> pose,
183      SimpleMotorFeedforward feedforward,
184      MecanumDriveKinematics kinematics,
185      PIDController xController,
186      PIDController yController,
187      ProfiledPIDController thetaController,
188      double maxWheelVelocityMetersPerSecond,
189      PIDController frontLeftController,
190      PIDController rearLeftController,
191      PIDController frontRightController,
192      PIDController rearRightController,
193      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
194      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
195      Subsystem... requirements) {
196    this(
197        trajectory,
198        pose,
199        feedforward,
200        kinematics,
201        xController,
202        yController,
203        thetaController,
204        () ->
205            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
206        maxWheelVelocityMetersPerSecond,
207        frontLeftController,
208        rearLeftController,
209        frontRightController,
210        rearRightController,
211        currentWheelSpeeds,
212        outputDriveVoltages,
213        requirements);
214  }
215
216  /**
217   * Constructs a new MecanumControllerCommand that when executed will follow the provided
218   * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
219   *
220   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
221   * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
222   *
223   * @param trajectory The trajectory to follow.
224   * @param pose A function that supplies the robot pose - use one of the odometry classes to
225   *     provide this.
226   * @param kinematics The kinematics for the robot drivetrain.
227   * @param xController The Trajectory Tracker PID controller for the robot's x position.
228   * @param yController The Trajectory Tracker PID controller for the robot's y position.
229   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
230   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
231   *     step.
232   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
233   * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
234   * @param requirements The subsystems to require.
235   */
236  public MecanumControllerCommand(
237      Trajectory trajectory,
238      Supplier<Pose2d> pose,
239      MecanumDriveKinematics kinematics,
240      PIDController xController,
241      PIDController yController,
242      ProfiledPIDController thetaController,
243      Supplier<Rotation2d> desiredRotation,
244      double maxWheelVelocityMetersPerSecond,
245      Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
246      Subsystem... requirements) {
247    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
248    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
249    m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
250    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
251
252    m_controller =
253        new HolonomicDriveController(
254            requireNonNullParam(xController, "xController", "MecanumControllerCommand"),
255            requireNonNullParam(yController, "yController", "MecanumControllerCommand"),
256            requireNonNullParam(thetaController, "thetaController", "MecanumControllerCommand"));
257
258    m_desiredRotation =
259        requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
260
261    m_maxWheelVelocityMetersPerSecond =
262        requireNonNullParam(
263            maxWheelVelocityMetersPerSecond,
264            "maxWheelVelocityMetersPerSecond",
265            "MecanumControllerCommand");
266
267    m_frontLeftController = null;
268    m_rearLeftController = null;
269    m_frontRightController = null;
270    m_rearRightController = null;
271
272    m_currentWheelSpeeds = null;
273
274    m_outputWheelSpeeds =
275        requireNonNullParam(outputWheelSpeeds, "outputWheelSpeeds", "MecanumControllerCommand");
276
277    m_outputDriveVoltages = null;
278
279    m_usePID = false;
280
281    addRequirements(requirements);
282  }
283
284  /**
285   * Constructs a new MecanumControllerCommand that when executed will follow the provided
286   * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
287   *
288   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
289   * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
290   *
291   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
292   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
293   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
294   * should be used.
295   *
296   * @param trajectory The trajectory to follow.
297   * @param pose A function that supplies the robot pose - use one of the odometry classes to
298   *     provide this.
299   * @param kinematics The kinematics for the robot drivetrain.
300   * @param xController The Trajectory Tracker PID controller for the robot's x position.
301   * @param yController The Trajectory Tracker PID controller for the robot's y position.
302   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
303   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
304   * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
305   * @param requirements The subsystems to require.
306   */
307  public MecanumControllerCommand(
308      Trajectory trajectory,
309      Supplier<Pose2d> pose,
310      MecanumDriveKinematics kinematics,
311      PIDController xController,
312      PIDController yController,
313      ProfiledPIDController thetaController,
314      double maxWheelVelocityMetersPerSecond,
315      Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
316      Subsystem... requirements) {
317    this(
318        trajectory,
319        pose,
320        kinematics,
321        xController,
322        yController,
323        thetaController,
324        () ->
325            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
326        maxWheelVelocityMetersPerSecond,
327        outputWheelSpeeds,
328        requirements);
329  }
330
331  @Override
332  public void initialize() {
333    var initialState = m_trajectory.sample(0);
334
335    var initialXVelocity =
336        initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos();
337    var initialYVelocity =
338        initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
339
340    m_prevSpeeds =
341        m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
342
343    m_timer.reset();
344    m_timer.start();
345  }
346
347  @Override
348  public void execute() {
349    double curTime = m_timer.get();
350    double dt = curTime - m_prevTime;
351
352    var desiredState = m_trajectory.sample(curTime);
353
354    var targetChassisSpeeds =
355        m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
356    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
357
358    targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
359
360    var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
361    var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
362    var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
363    var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
364
365    double frontLeftOutput;
366    double rearLeftOutput;
367    double frontRightOutput;
368    double rearRightOutput;
369
370    if (m_usePID) {
371      final double frontLeftFeedforward =
372          m_feedforward.calculate(
373              frontLeftSpeedSetpoint,
374              (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
375
376      final double rearLeftFeedforward =
377          m_feedforward.calculate(
378              rearLeftSpeedSetpoint,
379              (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
380
381      final double frontRightFeedforward =
382          m_feedforward.calculate(
383              frontRightSpeedSetpoint,
384              (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
385
386      final double rearRightFeedforward =
387          m_feedforward.calculate(
388              rearRightSpeedSetpoint,
389              (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
390
391      frontLeftOutput =
392          frontLeftFeedforward
393              + m_frontLeftController.calculate(
394                  m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
395
396      rearLeftOutput =
397          rearLeftFeedforward
398              + m_rearLeftController.calculate(
399                  m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
400
401      frontRightOutput =
402          frontRightFeedforward
403              + m_frontRightController.calculate(
404                  m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
405
406      rearRightOutput =
407          rearRightFeedforward
408              + m_rearRightController.calculate(
409                  m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
410
411      m_outputDriveVoltages.accept(
412          new MecanumDriveMotorVoltages(
413              frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
414
415    } else {
416      m_outputWheelSpeeds.accept(
417          new MecanumDriveWheelSpeeds(
418              frontLeftSpeedSetpoint,
419              frontRightSpeedSetpoint,
420              rearLeftSpeedSetpoint,
421              rearRightSpeedSetpoint));
422    }
423
424    m_prevTime = curTime;
425    m_prevSpeeds = targetWheelSpeeds;
426  }
427
428  @Override
429  public void end(boolean interrupted) {
430    m_timer.stop();
431  }
432
433  @Override
434  public boolean isFinished() {
435    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
436  }
437}