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