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  public MecanumDrive(
108      MotorController frontLeftMotor,
109      MotorController rearLeftMotor,
110      MotorController frontRightMotor,
111      MotorController rearRightMotor) {
112    requireNonNullParam(frontLeftMotor, "frontLeftMotor", "MecanumDrive");
113    requireNonNullParam(rearLeftMotor, "rearLeftMotor", "MecanumDrive");
114    requireNonNullParam(frontRightMotor, "frontRightMotor", "MecanumDrive");
115    requireNonNullParam(rearRightMotor, "rearRightMotor", "MecanumDrive");
116
117    m_frontLeftMotor = frontLeftMotor;
118    m_rearLeftMotor = rearLeftMotor;
119    m_frontRightMotor = frontRightMotor;
120    m_rearRightMotor = rearRightMotor;
121    SendableRegistry.addChild(this, m_frontLeftMotor);
122    SendableRegistry.addChild(this, m_rearLeftMotor);
123    SendableRegistry.addChild(this, m_frontRightMotor);
124    SendableRegistry.addChild(this, m_rearRightMotor);
125    instances++;
126    SendableRegistry.addLW(this, "MecanumDrive", instances);
127  }
128
129  @Override
130  public void close() {
131    SendableRegistry.remove(this);
132  }
133
134  /**
135   * Drive method for Mecanum platform.
136   *
137   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
138   * independent of its angle or rotation rate.
139   *
140   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
141   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
142   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
143   *     positive.
144   */
145  public void driveCartesian(double xSpeed, double ySpeed, double zRotation) {
146    driveCartesian(xSpeed, ySpeed, zRotation, new Rotation2d());
147  }
148
149  /**
150   * Drive method for Mecanum platform.
151   *
152   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
153   * independent of its angle or rotation rate.
154   *
155   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
156   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
157   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
158   *     positive.
159   * @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
160   *     controls.
161   */
162  public void driveCartesian(double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
163    if (!m_reported) {
164      HAL.report(
165          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumCartesian, 4);
166      m_reported = true;
167    }
168
169    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
170    ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
171
172    var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
173
174    m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput);
175    m_frontRightMotor.set(speeds.frontRight * m_maxOutput);
176    m_rearLeftMotor.set(speeds.rearLeft * m_maxOutput);
177    m_rearRightMotor.set(speeds.rearRight * m_maxOutput);
178
179    feed();
180  }
181
182  /**
183   * Drive method for Mecanum platform.
184   *
185   * <p>Angles are measured counterclockwise from straight ahead. The speed at which the robot
186   * drives (translation) is independent of its angle or rotation rate.
187   *
188   * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
189   * @param angle The gyro heading around the Z axis at which the robot drives.
190   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
191   *     positive.
192   */
193  public void drivePolar(double magnitude, Rotation2d angle, double zRotation) {
194    if (!m_reported) {
195      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumPolar, 4);
196      m_reported = true;
197    }
198
199    driveCartesian(
200        magnitude * angle.getCos(), magnitude * angle.getSin(), zRotation, new Rotation2d());
201  }
202
203  /**
204   * Cartesian inverse kinematics for Mecanum platform.
205   *
206   * <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
207   * independent of its angle or rotation rate.
208   *
209   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
210   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
211   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
212   *     positive.
213   * @return Wheel speeds [-1.0..1.0].
214   */
215  public static WheelSpeeds driveCartesianIK(double xSpeed, double ySpeed, double zRotation) {
216    return driveCartesianIK(xSpeed, ySpeed, zRotation, new Rotation2d());
217  }
218
219  /**
220   * Cartesian inverse kinematics for Mecanum platform.
221   *
222   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent of
223   * its angle or rotation rate.
224   *
225   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
226   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
227   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
228   *     positive.
229   * @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
230   *     controls.
231   * @return Wheel speeds [-1.0..1.0].
232   */
233  public static WheelSpeeds driveCartesianIK(
234      double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
235    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
236    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
237
238    // Compensate for gyro angle.
239    var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus());
240
241    double[] wheelSpeeds = new double[4];
242    wheelSpeeds[MotorType.kFrontLeft.value] = input.getX() + input.getY() + zRotation;
243    wheelSpeeds[MotorType.kFrontRight.value] = input.getX() - input.getY() - zRotation;
244    wheelSpeeds[MotorType.kRearLeft.value] = input.getX() - input.getY() + zRotation;
245    wheelSpeeds[MotorType.kRearRight.value] = input.getX() + input.getY() - zRotation;
246
247    normalize(wheelSpeeds);
248
249    return new WheelSpeeds(
250        wheelSpeeds[MotorType.kFrontLeft.value],
251        wheelSpeeds[MotorType.kFrontRight.value],
252        wheelSpeeds[MotorType.kRearLeft.value],
253        wheelSpeeds[MotorType.kRearRight.value]);
254  }
255
256  @Override
257  public void stopMotor() {
258    m_frontLeftMotor.stopMotor();
259    m_frontRightMotor.stopMotor();
260    m_rearLeftMotor.stopMotor();
261    m_rearRightMotor.stopMotor();
262    feed();
263  }
264
265  @Override
266  public String getDescription() {
267    return "MecanumDrive";
268  }
269
270  @Override
271  public void initSendable(SendableBuilder builder) {
272    builder.setSmartDashboardType("MecanumDrive");
273    builder.setActuator(true);
274    builder.setSafeState(this::stopMotor);
275    builder.addDoubleProperty(
276        "Front Left Motor Speed", m_frontLeftMotor::get, m_frontLeftMotor::set);
277    builder.addDoubleProperty(
278        "Front Right Motor Speed", m_frontRightMotor::get, m_frontRightMotor::set);
279    builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get, m_rearLeftMotor::set);
280    builder.addDoubleProperty(
281        "Rear Right Motor Speed", m_rearRightMotor::get, m_rearRightMotor::set);
282  }
283}