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.wpilibj.drive;
006
007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.hal.HAL;
010import edu.wpi.first.math.MathUtil;
011import edu.wpi.first.math.geometry.Rotation2d;
012import edu.wpi.first.math.geometry.Translation2d;
013import edu.wpi.first.util.sendable.Sendable;
014import edu.wpi.first.util.sendable.SendableBuilder;
015import edu.wpi.first.util.sendable.SendableRegistry;
016import edu.wpi.first.wpilibj.motorcontrol.MotorController;
017import java.util.function.DoubleConsumer;
018
019/**
020 * A class for driving Mecanum drive platforms.
021 *
022 * <p>Mecanum drives are rectangular with one wheel on each corner. Each wheel has rollers toed in
023 * 45 degrees toward the front or back. When looking at the wheels from the top, the roller axles
024 * should form an X across the robot. Each drive() function provides different inverse kinematic
025 * relations for a Mecanum drive robot.
026 *
027 * <p>Drive base diagram:
028 *
029 * <pre>
030 * \\_______/
031 * \\ |   | /
032 *   |   |
033 * /_|___|_\\
034 * /       \\
035 * </pre>
036 *
037 * <p>Each drive() function provides different inverse kinematic relations for a Mecanum drive
038 * robot.
039 *
040 * <p>This library uses the NWU axes convention (North-West-Up as external reference in the world
041 * frame). The positive X axis points ahead, the positive Y axis points to the left, and the
042 * positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation
043 * around the Z axis is positive.
044 *
045 * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
046 * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
047 * value can be changed with {@link #setDeadband}.
048 *
049 * <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The driveCartesian or
050 * drivePolar methods should be called periodically to avoid Motor Safety timeouts.
051 */
052public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
053  private static int instances;
054
055  private final DoubleConsumer m_frontLeftMotor;
056  private final DoubleConsumer m_rearLeftMotor;
057  private final DoubleConsumer m_frontRightMotor;
058  private final DoubleConsumer m_rearRightMotor;
059
060  // Used for Sendable property getters
061  private double m_frontLeftOutput;
062  private double m_rearLeftOutput;
063  private double m_frontRightOutput;
064  private double m_rearRightOutput;
065
066  private boolean m_reported;
067
068  /**
069   * Wheel speeds for a mecanum drive.
070   *
071   * <p>Uses normalized voltage [-1.0..1.0].
072   */
073  @SuppressWarnings("MemberName")
074  public static class WheelSpeeds {
075    /** Front-left wheel speed. */
076    public double frontLeft;
077
078    /** Front-right wheel speed. */
079    public double frontRight;
080
081    /** Rear-left wheel speed. */
082    public double rearLeft;
083
084    /** Rear-right wheel speed. */
085    public double rearRight;
086
087    /** Constructs a WheelSpeeds with zeroes for all four speeds. */
088    public WheelSpeeds() {}
089
090    /**
091     * Constructs a WheelSpeeds.
092     *
093     * @param frontLeft The front left speed [-1.0..1.0].
094     * @param frontRight The front right speed [-1.0..1.0].
095     * @param rearLeft The rear left speed [-1.0..1.0].
096     * @param rearRight The rear right speed [-1.0..1.0].
097     */
098    public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) {
099      this.frontLeft = frontLeft;
100      this.frontRight = frontRight;
101      this.rearLeft = rearLeft;
102      this.rearRight = rearRight;
103    }
104  }
105
106  /**
107   * Construct a MecanumDrive.
108   *
109   * <p>If a motor needs to be inverted, do so before passing it in.
110   *
111   * @param frontLeftMotor The motor on the front-left corner.
112   * @param rearLeftMotor The motor on the rear-left corner.
113   * @param frontRightMotor The motor on the front-right corner.
114   * @param rearRightMotor The motor on the rear-right corner.
115   */
116  @SuppressWarnings({"removal", "this-escape"})
117  public MecanumDrive(
118      MotorController frontLeftMotor,
119      MotorController rearLeftMotor,
120      MotorController frontRightMotor,
121      MotorController rearRightMotor) {
122    this(
123        (double output) -> frontLeftMotor.set(output),
124        (double output) -> rearLeftMotor.set(output),
125        (double output) -> frontRightMotor.set(output),
126        (double output) -> rearRightMotor.set(output));
127    SendableRegistry.addChild(this, frontLeftMotor);
128    SendableRegistry.addChild(this, rearLeftMotor);
129    SendableRegistry.addChild(this, frontRightMotor);
130    SendableRegistry.addChild(this, rearRightMotor);
131  }
132
133  /**
134   * Construct a MecanumDrive.
135   *
136   * <p>If a motor needs to be inverted, do so before passing it in.
137   *
138   * @param frontLeftMotor The setter for the motor on the front-left corner.
139   * @param rearLeftMotor The setter for the motor on the rear-left corner.
140   * @param frontRightMotor The setter for the motor on the front-right corner.
141   * @param rearRightMotor The setter for the motor on the rear-right corner.
142   */
143  @SuppressWarnings("this-escape")
144  public MecanumDrive(
145      DoubleConsumer frontLeftMotor,
146      DoubleConsumer rearLeftMotor,
147      DoubleConsumer frontRightMotor,
148      DoubleConsumer rearRightMotor) {
149    requireNonNullParam(frontLeftMotor, "frontLeftMotor", "MecanumDrive");
150    requireNonNullParam(rearLeftMotor, "rearLeftMotor", "MecanumDrive");
151    requireNonNullParam(frontRightMotor, "frontRightMotor", "MecanumDrive");
152    requireNonNullParam(rearRightMotor, "rearRightMotor", "MecanumDrive");
153
154    m_frontLeftMotor = frontLeftMotor;
155    m_rearLeftMotor = rearLeftMotor;
156    m_frontRightMotor = frontRightMotor;
157    m_rearRightMotor = rearRightMotor;
158    instances++;
159    SendableRegistry.add(this, "MecanumDrive", instances);
160  }
161
162  @Override
163  public void close() {
164    SendableRegistry.remove(this);
165  }
166
167  /**
168   * Drive method for Mecanum platform.
169   *
170   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
171   * independent of its angle or rotation rate.
172   *
173   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
174   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
175   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
176   *     positive.
177   */
178  public void driveCartesian(double xSpeed, double ySpeed, double zRotation) {
179    driveCartesian(xSpeed, ySpeed, zRotation, Rotation2d.kZero);
180  }
181
182  /**
183   * Drive method for Mecanum platform.
184   *
185   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
186   * independent of its angle or rotation rate.
187   *
188   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
189   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
190   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
191   *     positive.
192   * @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
193   *     controls.
194   */
195  public void driveCartesian(double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
196    if (!m_reported) {
197      HAL.reportUsage("RobotDrive", "MecanumCartesian");
198      m_reported = true;
199    }
200
201    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
202    ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
203
204    var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
205
206    m_frontLeftOutput = speeds.frontLeft * m_maxOutput;
207    m_rearLeftOutput = speeds.rearLeft * m_maxOutput;
208    m_frontRightOutput = speeds.frontRight * m_maxOutput;
209    m_rearRightOutput = speeds.rearRight * m_maxOutput;
210
211    m_frontLeftMotor.accept(m_frontLeftOutput);
212    m_frontRightMotor.accept(m_frontRightOutput);
213    m_rearLeftMotor.accept(m_rearLeftOutput);
214    m_rearRightMotor.accept(m_rearRightOutput);
215
216    feed();
217  }
218
219  /**
220   * Drive method for Mecanum platform.
221   *
222   * <p>Angles are measured counterclockwise from straight ahead. The speed at which the robot
223   * drives (translation) is independent of its angle or rotation rate.
224   *
225   * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
226   * @param angle The gyro heading around the Z axis at which the robot drives.
227   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
228   *     positive.
229   */
230  public void drivePolar(double magnitude, Rotation2d angle, double zRotation) {
231    if (!m_reported) {
232      HAL.reportUsage("RobotDrive", "MecanumPolar");
233      m_reported = true;
234    }
235
236    driveCartesian(
237        magnitude * angle.getCos(), magnitude * angle.getSin(), zRotation, Rotation2d.kZero);
238  }
239
240  /**
241   * Cartesian inverse kinematics for Mecanum platform.
242   *
243   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
244   * independent of its angle or rotation rate.
245   *
246   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
247   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
248   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
249   *     positive.
250   * @return Wheel speeds [-1.0..1.0].
251   */
252  public static WheelSpeeds driveCartesianIK(double xSpeed, double ySpeed, double zRotation) {
253    return driveCartesianIK(xSpeed, ySpeed, zRotation, Rotation2d.kZero);
254  }
255
256  /**
257   * Cartesian inverse kinematics for Mecanum platform.
258   *
259   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent of
260   * its angle or rotation rate.
261   *
262   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
263   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
264   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
265   *     positive.
266   * @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
267   *     controls.
268   * @return Wheel speeds [-1.0..1.0].
269   */
270  public static WheelSpeeds driveCartesianIK(
271      double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
272    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
273    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
274
275    // Compensate for gyro angle.
276    var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus());
277
278    double[] wheelSpeeds = new double[4];
279    wheelSpeeds[MotorType.kFrontLeft.value] = input.getX() + input.getY() + zRotation;
280    wheelSpeeds[MotorType.kFrontRight.value] = input.getX() - input.getY() - zRotation;
281    wheelSpeeds[MotorType.kRearLeft.value] = input.getX() - input.getY() + zRotation;
282    wheelSpeeds[MotorType.kRearRight.value] = input.getX() + input.getY() - zRotation;
283
284    normalize(wheelSpeeds);
285
286    return new WheelSpeeds(
287        wheelSpeeds[MotorType.kFrontLeft.value],
288        wheelSpeeds[MotorType.kFrontRight.value],
289        wheelSpeeds[MotorType.kRearLeft.value],
290        wheelSpeeds[MotorType.kRearRight.value]);
291  }
292
293  @Override
294  public void stopMotor() {
295    m_frontLeftOutput = 0.0;
296    m_frontRightOutput = 0.0;
297    m_rearLeftOutput = 0.0;
298    m_rearRightOutput = 0.0;
299
300    m_frontLeftMotor.accept(0.0);
301    m_frontRightMotor.accept(0.0);
302    m_rearLeftMotor.accept(0.0);
303    m_rearRightMotor.accept(0.0);
304
305    feed();
306  }
307
308  @Override
309  public String getDescription() {
310    return "MecanumDrive";
311  }
312
313  @Override
314  public void initSendable(SendableBuilder builder) {
315    builder.setSmartDashboardType("MecanumDrive");
316    builder.setActuator(true);
317    builder.addDoubleProperty("Front Left Motor Speed", () -> m_frontLeftOutput, m_frontLeftMotor);
318    builder.addDoubleProperty(
319        "Front Right Motor Speed", () -> m_frontRightOutput, m_frontRightMotor);
320    builder.addDoubleProperty("Rear Left Motor Speed", () -> m_rearLeftOutput, m_rearLeftMotor);
321    builder.addDoubleProperty("Rear Right Motor Speed", () -> m_rearRightOutput, m_rearRightMotor);
322  }
323}