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