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 java.util.Objects.requireNonNull;
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.util.sendable.Sendable;
014import edu.wpi.first.util.sendable.SendableBuilder;
015import edu.wpi.first.util.sendable.SendableRegistry;
016import edu.wpi.first.wpilibj.SpeedController;
017
018/**
019 * A class for driving Mecanum drive platforms.
020 *
021 * <p>Mecanum drives are rectangular with one wheel on each corner. Each wheel has rollers toed in
022 * 45 degrees toward the front or back. When looking at the wheels from the top, the roller axles
023 * should form an X across the robot. Each drive() function provides different inverse kinematic
024 * relations for a Mecanum drive robot.
025 *
026 * <p>Drive base diagram:
027 *
028 * <pre>
029 * \\_______/
030 * \\ |   | /
031 *   |   |
032 * /_|___|_\\
033 * /       \\
034 * </pre>
035 *
036 * <p>Each drive() function provides different inverse kinematic relations for a Mecanum drive
037 * robot. Motor outputs for the right side are negated, so motor direction inversion by the user is
038 * usually unnecessary.
039 *
040 * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
041 * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
042 *
043 * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
044 * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
045 * positive.
046 *
047 * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
048 * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
049 * value can be changed with {@link #setDeadband}.
050 *
051 * <p>RobotDrive porting guide: <br>
052 * In MecanumDrive, the right side motor controllers are automatically inverted, while in
053 * RobotDrive, no motor controllers are automatically inverted. <br>
054 * {@link #driveCartesian(double, double, double, double)} is equivalent to RobotDrive's
055 * mecanumDrive_Cartesian(double, double, double, double) if a deadband of 0 is used, and the ySpeed
056 * and gyroAngle values are inverted compared to RobotDrive (eg driveCartesian(xSpeed, -ySpeed,
057 * zRotation, -gyroAngle). <br>
058 * {@link #drivePolar(double, double, double)} is equivalent to RobotDrive's
059 * mecanumDrive_Polar(double, double, double)} if a deadband of 0 is used.
060 */
061@SuppressWarnings("removal")
062public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
063  private static int instances;
064
065  private final SpeedController m_frontLeftMotor;
066  private final SpeedController m_rearLeftMotor;
067  private final SpeedController m_frontRightMotor;
068  private final SpeedController m_rearRightMotor;
069
070  private boolean m_reported;
071
072  @SuppressWarnings("MemberName")
073  public static class WheelSpeeds {
074    public double frontLeft;
075    public double frontRight;
076    public double rearLeft;
077    public double rearRight;
078
079    /** Constructs a WheelSpeeds with zeroes for all four speeds. */
080    public WheelSpeeds() {}
081
082    /**
083     * Constructs a WheelSpeeds.
084     *
085     * @param frontLeft The front left speed.
086     * @param frontRight The front right speed.
087     * @param rearLeft The rear left speed.
088     * @param rearRight The rear right speed.
089     */
090    public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) {
091      this.frontLeft = frontLeft;
092      this.frontRight = frontRight;
093      this.rearLeft = rearLeft;
094      this.rearRight = rearRight;
095    }
096  }
097
098  /**
099   * Construct a MecanumDrive.
100   *
101   * <p>If a motor needs to be inverted, do so before passing it in.
102   *
103   * @param frontLeftMotor The motor on the front-left corner.
104   * @param rearLeftMotor The motor on the rear-left corner.
105   * @param frontRightMotor The motor on the front-right corner.
106   * @param rearRightMotor The motor on the rear-right corner.
107   */
108  public MecanumDrive(
109      SpeedController frontLeftMotor,
110      SpeedController rearLeftMotor,
111      SpeedController frontRightMotor,
112      SpeedController rearRightMotor) {
113    requireNonNull(frontLeftMotor, "Front-left motor cannot be null");
114    requireNonNull(rearLeftMotor, "Rear-left motor cannot be null");
115    requireNonNull(frontRightMotor, "Front-right motor cannot be null");
116    requireNonNull(rearRightMotor, "Rear-right motor cannot be null");
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 clockwise from the positive X axis. The robot's speed is independent
139   * from its angle or rotation rate.
140   *
141   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
142   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
143   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
144   *     positive.
145   */
146  @SuppressWarnings("ParameterName")
147  public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
148    driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
149  }
150
151  /**
152   * Drive method for Mecanum platform.
153   *
154   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
155   * from its angle or rotation rate.
156   *
157   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
158   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
159   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
160   *     positive.
161   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
162   *     to implement field-oriented controls.
163   */
164  @SuppressWarnings("ParameterName")
165  public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
166    if (!m_reported) {
167      HAL.report(
168          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumCartesian, 4);
169      m_reported = true;
170    }
171
172    ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
173    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
174
175    var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
176
177    m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput);
178    m_frontRightMotor.set(speeds.frontRight * m_maxOutput);
179    m_rearLeftMotor.set(speeds.rearLeft * m_maxOutput);
180    m_rearRightMotor.set(speeds.rearRight * m_maxOutput);
181
182    feed();
183  }
184
185  /**
186   * Drive method for Mecanum platform.
187   *
188   * <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
189   * drives (translation) is independent from its angle or rotation rate.
190   *
191   * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
192   * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180].
193   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
194   *     positive.
195   */
196  @SuppressWarnings("ParameterName")
197  public void drivePolar(double magnitude, double angle, double zRotation) {
198    if (!m_reported) {
199      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumPolar, 4);
200      m_reported = true;
201    }
202
203    driveCartesian(
204        magnitude * Math.cos(angle * (Math.PI / 180.0)),
205        magnitude * Math.sin(angle * (Math.PI / 180.0)),
206        zRotation,
207        0.0);
208  }
209
210  /**
211   * Cartesian inverse kinematics for Mecanum platform.
212   *
213   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
214   * from its angle or rotation rate.
215   *
216   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
217   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
218   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
219   *     positive.
220   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
221   *     to implement field-oriented controls.
222   * @return Wheel speeds.
223   */
224  @SuppressWarnings("ParameterName")
225  public static WheelSpeeds driveCartesianIK(
226      double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
227    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
228    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
229
230    // Compensate for gyro angle.
231    Vector2d input = new Vector2d(ySpeed, xSpeed);
232    input.rotate(-gyroAngle);
233
234    double[] wheelSpeeds = new double[4];
235    wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
236    wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y - zRotation;
237    wheelSpeeds[MotorType.kRearLeft.value] = input.x - input.y + zRotation;
238    wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
239
240    normalize(wheelSpeeds);
241
242    return new WheelSpeeds(
243        wheelSpeeds[MotorType.kFrontLeft.value],
244        wheelSpeeds[MotorType.kFrontRight.value],
245        wheelSpeeds[MotorType.kRearLeft.value],
246        wheelSpeeds[MotorType.kRearRight.value]);
247  }
248
249  @Override
250  public void stopMotor() {
251    m_frontLeftMotor.stopMotor();
252    m_frontRightMotor.stopMotor();
253    m_rearLeftMotor.stopMotor();
254    m_rearRightMotor.stopMotor();
255    feed();
256  }
257
258  @Override
259  public String getDescription() {
260    return "MecanumDrive";
261  }
262
263  @Override
264  public void initSendable(SendableBuilder builder) {
265    builder.setSmartDashboardType("MecanumDrive");
266    builder.setActuator(true);
267    builder.setSafeState(this::stopMotor);
268    builder.addDoubleProperty(
269        "Front Left Motor Speed", m_frontLeftMotor::get, m_frontLeftMotor::set);
270    builder.addDoubleProperty(
271        "Front Right Motor Speed",
272        () -> m_frontRightMotor.get(),
273        value -> m_frontRightMotor.set(value));
274    builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get, m_rearLeftMotor::set);
275    builder.addDoubleProperty(
276        "Rear Right Motor Speed",
277        () -> m_rearRightMotor.get(),
278        value -> m_rearRightMotor.set(value));
279  }
280}