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}