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.wpilibj.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@SuppressWarnings("MemberName")
040public class MecanumControllerCommand extends CommandBase {
041  private final Timer m_timer = new Timer();
042  private final boolean m_usePID;
043  private final Trajectory m_trajectory;
044  private final Supplier<Pose2d> m_pose;
045  private final SimpleMotorFeedforward m_feedforward;
046  private final MecanumDriveKinematics m_kinematics;
047  private final HolonomicDriveController m_controller;
048  private final Supplier<Rotation2d> m_desiredRotation;
049  private final double m_maxWheelVelocityMetersPerSecond;
050  private final PIDController m_frontLeftController;
051  private final PIDController m_rearLeftController;
052  private final PIDController m_frontRightController;
053  private final PIDController m_rearRightController;
054  private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
055  private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
056  private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
057  private MecanumDriveWheelSpeeds m_prevSpeeds;
058  private double m_prevTime;
059
060  /**
061   * Constructs a new MecanumControllerCommand that when executed will follow the provided
062   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
063   * 12 as a voltage output to the motor.
064   *
065   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
066   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
067   *
068   * @param trajectory The trajectory to follow.
069   * @param pose A function that supplies the robot pose - use one of the odometry classes to
070   *     provide this.
071   * @param feedforward The feedforward to use for the drivetrain.
072   * @param kinematics The kinematics for the robot drivetrain.
073   * @param xController The Trajectory Tracker PID controller for the robot's x position.
074   * @param yController The Trajectory Tracker PID controller for the robot's y position.
075   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
076   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
077   *     step.
078   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
079   * @param frontLeftController The front left wheel velocity PID.
080   * @param rearLeftController The rear left wheel velocity PID.
081   * @param frontRightController The front right wheel velocity PID.
082   * @param rearRightController The rear right wheel velocity PID.
083   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
084   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
085   *     voltages.
086   * @param requirements The subsystems to require.
087   */
088  @SuppressWarnings("ParameterName")
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", "SwerveControllerCommand"),
114            requireNonNullParam(yController, "xController", "SwerveControllerCommand"),
115            requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
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  @SuppressWarnings("ParameterName")
181  public MecanumControllerCommand(
182      Trajectory trajectory,
183      Supplier<Pose2d> pose,
184      SimpleMotorFeedforward feedforward,
185      MecanumDriveKinematics kinematics,
186      PIDController xController,
187      PIDController yController,
188      ProfiledPIDController thetaController,
189      double maxWheelVelocityMetersPerSecond,
190      PIDController frontLeftController,
191      PIDController rearLeftController,
192      PIDController frontRightController,
193      PIDController rearRightController,
194      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
195      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
196      Subsystem... requirements) {
197    this(
198        trajectory,
199        pose,
200        feedforward,
201        kinematics,
202        xController,
203        yController,
204        thetaController,
205        () ->
206            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
207        maxWheelVelocityMetersPerSecond,
208        frontLeftController,
209        rearLeftController,
210        frontRightController,
211        rearRightController,
212        currentWheelSpeeds,
213        outputDriveVoltages,
214        requirements);
215  }
216
217  /**
218   * Constructs a new MecanumControllerCommand that when executed will follow the provided
219   * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
220   *
221   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
222   * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
223   *
224   * @param trajectory The trajectory to follow.
225   * @param pose A function that supplies the robot pose - use one of the odometry classes to
226   *     provide this.
227   * @param kinematics The kinematics for the robot drivetrain.
228   * @param xController The Trajectory Tracker PID controller for the robot's x position.
229   * @param yController The Trajectory Tracker PID controller for the robot's y position.
230   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
231   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
232   *     step.
233   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
234   * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
235   * @param requirements The subsystems to require.
236   */
237  @SuppressWarnings("ParameterName")
238  public MecanumControllerCommand(
239      Trajectory trajectory,
240      Supplier<Pose2d> pose,
241      MecanumDriveKinematics kinematics,
242      PIDController xController,
243      PIDController yController,
244      ProfiledPIDController thetaController,
245      Supplier<Rotation2d> desiredRotation,
246      double maxWheelVelocityMetersPerSecond,
247      Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
248      Subsystem... requirements) {
249    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
250    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
251    m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
252    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
253
254    m_controller =
255        new HolonomicDriveController(
256            requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
257            requireNonNullParam(yController, "xController", "SwerveControllerCommand"),
258            requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
259
260    m_desiredRotation =
261        requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
262
263    m_maxWheelVelocityMetersPerSecond =
264        requireNonNullParam(
265            maxWheelVelocityMetersPerSecond,
266            "maxWheelVelocityMetersPerSecond",
267            "MecanumControllerCommand");
268
269    m_frontLeftController = null;
270    m_rearLeftController = null;
271    m_frontRightController = null;
272    m_rearRightController = null;
273
274    m_currentWheelSpeeds = null;
275
276    m_outputWheelSpeeds =
277        requireNonNullParam(outputWheelSpeeds, "outputWheelSpeeds", "MecanumControllerCommand");
278
279    m_outputDriveVoltages = null;
280
281    m_usePID = false;
282
283    addRequirements(requirements);
284  }
285
286  /**
287   * Constructs a new MecanumControllerCommand that when executed will follow the provided
288   * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
289   *
290   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
291   * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
292   *
293   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
294   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
295   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
296   * should be used.
297   *
298   * @param trajectory The trajectory to follow.
299   * @param pose A function that supplies the robot pose - use one of the odometry classes to
300   *     provide this.
301   * @param kinematics The kinematics for the robot drivetrain.
302   * @param xController The Trajectory Tracker PID controller for the robot's x position.
303   * @param yController The Trajectory Tracker PID controller for the robot's y position.
304   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
305   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
306   * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
307   * @param requirements The subsystems to require.
308   */
309  @SuppressWarnings("ParameterName")
310  public MecanumControllerCommand(
311      Trajectory trajectory,
312      Supplier<Pose2d> pose,
313      MecanumDriveKinematics kinematics,
314      PIDController xController,
315      PIDController yController,
316      ProfiledPIDController thetaController,
317      double maxWheelVelocityMetersPerSecond,
318      Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
319      Subsystem... requirements) {
320    this(
321        trajectory,
322        pose,
323        kinematics,
324        xController,
325        yController,
326        thetaController,
327        () ->
328            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
329        maxWheelVelocityMetersPerSecond,
330        outputWheelSpeeds,
331        requirements);
332  }
333
334  @Override
335  public void initialize() {
336    var initialState = m_trajectory.sample(0);
337
338    var initialXVelocity =
339        initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos();
340    var initialYVelocity =
341        initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
342
343    m_prevSpeeds =
344        m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
345
346    m_timer.reset();
347    m_timer.start();
348  }
349
350  @Override
351  @SuppressWarnings("LocalVariableName")
352  public void execute() {
353    double curTime = m_timer.get();
354    double dt = curTime - m_prevTime;
355
356    var desiredState = m_trajectory.sample(curTime);
357
358    var targetChassisSpeeds =
359        m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
360    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
361
362    targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
363
364    var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
365    var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
366    var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
367    var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
368
369    double frontLeftOutput;
370    double rearLeftOutput;
371    double frontRightOutput;
372    double rearRightOutput;
373
374    if (m_usePID) {
375      final double frontLeftFeedforward =
376          m_feedforward.calculate(
377              frontLeftSpeedSetpoint,
378              (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
379
380      final double rearLeftFeedforward =
381          m_feedforward.calculate(
382              rearLeftSpeedSetpoint,
383              (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
384
385      final double frontRightFeedforward =
386          m_feedforward.calculate(
387              frontRightSpeedSetpoint,
388              (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
389
390      final double rearRightFeedforward =
391          m_feedforward.calculate(
392              rearRightSpeedSetpoint,
393              (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
394
395      frontLeftOutput =
396          frontLeftFeedforward
397              + m_frontLeftController.calculate(
398                  m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
399
400      rearLeftOutput =
401          rearLeftFeedforward
402              + m_rearLeftController.calculate(
403                  m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
404
405      frontRightOutput =
406          frontRightFeedforward
407              + m_frontRightController.calculate(
408                  m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
409
410      rearRightOutput =
411          rearRightFeedforward
412              + m_rearRightController.calculate(
413                  m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
414
415      m_outputDriveVoltages.accept(
416          new MecanumDriveMotorVoltages(
417              frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
418
419    } else {
420      m_outputWheelSpeeds.accept(
421          new MecanumDriveWheelSpeeds(
422              frontLeftSpeedSetpoint,
423              frontRightSpeedSetpoint,
424              rearLeftSpeedSetpoint,
425              rearRightSpeedSetpoint));
426    }
427
428    m_prevTime = curTime;
429    m_prevSpeeds = targetWheelSpeeds;
430  }
431
432  @Override
433  public void end(boolean interrupted) {
434    m_timer.stop();
435  }
436
437  @Override
438  public boolean isFinished() {
439    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
440  }
441}