Class MecanumDrive
- All Implemented Interfaces:
Sendable
,AutoCloseable
public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable
Mecanum drives are rectangular with one wheel on each corner. Each wheel has rollers toed in 45 degrees toward the front or back. When looking at the wheels from the top, the roller axles should form an X across the robot. Each drive() function provides different inverse kinematic relations for a Mecanum drive robot.
Drive base diagram:
\\_______/ \\ | | / | | /_|___|_\\ / \\
Each drive() function provides different inverse kinematic relations for a Mecanum drive robot. Motor outputs for the right side are negated, so motor direction inversion by the user is usually unnecessary.
This library uses the NED axes convention (North-East-Down as external reference in the world frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
The positive X axis points ahead, the positive Y axis points right, and the positive Z axis points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is positive.
Inputs smaller then 0.02 will
be set to 0, and larger values will be scaled so that the full range is still used. This deadband
value can be changed with RobotDriveBase.setDeadband(double)
.
RobotDrive porting guide:
In MecanumDrive, the right side motor controllers are automatically inverted, while in
RobotDrive, no motor controllers are automatically inverted.
driveCartesian(double, double, double, double)
is equivalent to RobotDrive's
mecanumDrive_Cartesian(double, double, double, double) if a deadband of 0 is used, and the ySpeed
and gyroAngle values are inverted compared to RobotDrive (eg driveCartesian(xSpeed, -ySpeed,
zRotation, -gyroAngle).
drivePolar(double, double, double)
is equivalent to RobotDrive's
mecanumDrive_Polar(double, double, double)} if a deadband of 0 is used.
-
Nested Class Summary
Nested Classes Modifier and Type Class Description static class
MecanumDrive.WheelSpeeds
Nested classes/interfaces inherited from class edu.wpi.first.wpilibj.drive.RobotDriveBase
RobotDriveBase.MotorType
-
Field Summary
Fields inherited from class edu.wpi.first.wpilibj.drive.RobotDriveBase
kDefaultDeadband, kDefaultMaxOutput, m_deadband, m_maxOutput
-
Constructor Summary
Constructors Constructor Description MecanumDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor, SpeedController frontRightMotor, SpeedController rearRightMotor)
Construct a MecanumDrive. -
Method Summary
Modifier and Type Method Description void
close()
void
driveCartesian(double ySpeed, double xSpeed, double zRotation)
Drive method for Mecanum platform.void
driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle)
Drive method for Mecanum platform.static MecanumDrive.WheelSpeeds
driveCartesianIK(double ySpeed, double xSpeed, double zRotation, double gyroAngle)
Cartesian inverse kinematics for Mecanum platform.void
drivePolar(double magnitude, double angle, double zRotation)
Drive method for Mecanum platform.String
getDescription()
void
initSendable(SendableBuilder builder)
Initializes thisSendable
object.void
stopMotor()
Methods inherited from class edu.wpi.first.wpilibj.drive.RobotDriveBase
applyDeadband, feedWatchdog, normalize, setDeadband, setMaxOutput
Methods inherited from class edu.wpi.first.wpilibj.MotorSafety
check, checkMotors, feed, getExpiration, isAlive, isSafetyEnabled, setExpiration, setSafetyEnabled
-
Constructor Details
-
MecanumDrive
public MecanumDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor, SpeedController frontRightMotor, SpeedController rearRightMotor)Construct a MecanumDrive.If a motor needs to be inverted, do so before passing it in.
- Parameters:
frontLeftMotor
- The motor on the front-left corner.rearLeftMotor
- The motor on the rear-left corner.frontRightMotor
- The motor on the front-right corner.rearRightMotor
- The motor on the rear-right corner.
-
-
Method Details
-
close
- Specified by:
close
in interfaceAutoCloseable
-
driveCartesian
Drive method for Mecanum platform.Angles are measured clockwise from the positive X axis. The robot's speed is independent from its angle or rotation rate.
- Parameters:
ySpeed
- The robot's speed along the Y axis [-1.0..1.0]. Right is positive.xSpeed
- The robot's speed along the X axis [-1.0..1.0]. Forward is positive.zRotation
- The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.
-
driveCartesian
Drive method for Mecanum platform.Angles are measured clockwise from the positive X axis. The robot's speed is independent from its angle or rotation rate.
- Parameters:
ySpeed
- The robot's speed along the Y axis [-1.0..1.0]. Right is positive.xSpeed
- The robot's speed along the X axis [-1.0..1.0]. Forward is positive.zRotation
- The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.gyroAngle
- The current angle reading from the gyro in degrees around the Z axis. Use this to implement field-oriented controls.
-
drivePolar
Drive method for Mecanum platform.Angles are measured counter-clockwise from straight ahead. The speed at which the robot drives (translation) is independent from its angle or rotation rate.
- Parameters:
magnitude
- The robot's speed at a given angle [-1.0..1.0]. Forward is positive.angle
- The angle around the Z axis at which the robot drives in degrees [-180..180].zRotation
- The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.
-
driveCartesianIK
public static MecanumDrive.WheelSpeeds driveCartesianIK(double ySpeed, double xSpeed, double zRotation, double gyroAngle)Cartesian inverse kinematics for Mecanum platform.Angles are measured clockwise from the positive X axis. The robot's speed is independent from its angle or rotation rate.
- Parameters:
ySpeed
- The robot's speed along the Y axis [-1.0..1.0]. Right is positive.xSpeed
- The robot's speed along the X axis [-1.0..1.0]. Forward is positive.zRotation
- The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.gyroAngle
- The current angle reading from the gyro in degrees around the Z axis. Use this to implement field-oriented controls.- Returns:
- Wheel speeds.
-
stopMotor
- Specified by:
stopMotor
in classRobotDriveBase
-
getDescription
- Specified by:
getDescription
in classRobotDriveBase
-
initSendable
Description copied from interface:Sendable
Initializes thisSendable
object.- Specified by:
initSendable
in interfaceSendable
- Parameters:
builder
- sendable builder
-