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}