Source code

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