WPILibC++  2020.3.2-60-g3011ebe
frc::MecanumDrive Class Reference

A class for driving Mecanum drive platforms. More...

#include <MecanumDrive.h>

Inheritance diagram for frc::MecanumDrive:
frc::RobotDriveBase frc::Sendable frc::SendableHelper< MecanumDrive > frc::MotorSafety frc::ErrorBase

Public Member Functions

 MecanumDrive (SpeedController &frontLeftMotor, SpeedController &rearLeftMotor, SpeedController &frontRightMotor, SpeedController &rearRightMotor)
 Construct a MecanumDrive. More...
 
 MecanumDrive (MecanumDrive &&)=default
 
MecanumDriveoperator= (MecanumDrive &&)=default
 
void DriveCartesian (double ySpeed, double xSpeed, double zRotation, double gyroAngle=0.0)
 Drive method for Mecanum platform. More...
 
void DrivePolar (double magnitude, double angle, double zRotation)
 Drive method for Mecanum platform. More...
 
bool IsRightSideInverted () const
 Gets if the power sent to the right side of the drivetrain is multipled by -1. More...
 
void SetRightSideInverted (bool rightSideInverted)
 Sets if the power sent to the right side of the drivetrain should be multipled by -1. More...
 
void StopMotor () override
 
void GetDescription (wpi::raw_ostream &desc) const override
 
void InitSendable (SendableBuilder &builder) override
 Initializes this Sendable object. More...
 
- Public Member Functions inherited from frc::RobotDriveBase
 RobotDriveBase (RobotDriveBase &&)=default
 
RobotDriveBaseoperator= (RobotDriveBase &&)=default
 
void SetDeadband (double deadband)
 Sets the deadband applied to the drive inputs (e.g., joystick values). More...
 
void SetMaxOutput (double maxOutput)
 Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus or to limit the maximum output. More...
 
void FeedWatchdog ()
 Feed the motor safety object. More...
 
- Public Member Functions inherited from frc::MotorSafety
 MotorSafety (MotorSafety &&rhs)
 
MotorSafetyoperator= (MotorSafety &&rhs)
 
void Feed ()
 Feed the motor safety object. More...
 
void SetExpiration (double expirationTime)
 Set the expiration time for the corresponding motor safety object. More...
 
double GetExpiration () const
 Retrieve the timeout value for the corresponding motor safety object. More...
 
bool IsAlive () const
 Determine if the motor is still operating or has timed out. More...
 
void SetSafetyEnabled (bool enabled)
 Enable/disable motor safety for this device. More...
 
bool IsSafetyEnabled () const
 Return the state of the motor safety enabled flag. More...
 
void Check ()
 Check if this motor has exceeded its timeout. More...
 
- Public Member Functions inherited from frc::ErrorBase
 ErrorBase (const ErrorBase &)=default
 
ErrorBaseoperator= (const ErrorBase &)=default
 
 ErrorBase (ErrorBase &&)=default
 
ErrorBaseoperator= (ErrorBase &&)=default
 
virtual ErrorGetError ()
 Retrieve the current error. More...
 
virtual const ErrorGetError () const
 Retrieve the current error. More...
 
virtual void ClearError () const
 Clear the current error information associated with this sensor.
 
virtual void SetErrnoError (const wpi::Twine &contextMessage, wpi::StringRef filename, wpi::StringRef function, int lineNumber) const
 Set error information associated with a C library call that set an error to the "errno" global variable. More...
 
virtual void SetImaqError (int success, const wpi::Twine &contextMessage, wpi::StringRef filename, wpi::StringRef function, int lineNumber) const
 Set the current error information associated from the nivision Imaq API. More...
 
virtual void SetError (Error::Code code, const wpi::Twine &contextMessage, wpi::StringRef filename, wpi::StringRef function, int lineNumber) const
 Set the current error information associated with this sensor. More...
 
virtual void SetErrorRange (Error::Code code, int32_t minRange, int32_t maxRange, int32_t requestedValue, const wpi::Twine &contextMessage, wpi::StringRef filename, wpi::StringRef function, int lineNumber) const
 Set the current error information associated with this sensor. More...
 
virtual void SetWPIError (const wpi::Twine &errorMessage, Error::Code code, const wpi::Twine &contextMessage, wpi::StringRef filename, wpi::StringRef function, int lineNumber) const
 Set the current error information associated with this sensor. More...
 
virtual void CloneError (const ErrorBase &rhs) const
 
virtual bool StatusIsFatal () const
 Check if the current error code represents a fatal error. More...
 
void ClearGlobalErrors ()
 Clear global errors.
 
- Public Member Functions inherited from frc::SendableHelper< MecanumDrive >
 SendableHelper (const SendableHelper &rhs)=default
 
 SendableHelper (SendableHelper &&rhs)
 
SendableHelperoperator= (const SendableHelper &rhs)=default
 
SendableHelperoperator= (SendableHelper &&rhs)
 
std::string GetName () const
 Gets the name of this Sendable object. More...
 
void SetName (const wpi::Twine &name)
 Sets the name of this Sendable object. More...
 
void SetName (const wpi::Twine &subsystem, const wpi::Twine &name)
 Sets both the subsystem name and device name of this Sendable object. More...
 
std::string GetSubsystem () const
 Gets the subsystem name of this Sendable object. More...
 
void SetSubsystem (const wpi::Twine &subsystem)
 Sets the subsystem name of this Sendable object. More...
 

Additional Inherited Members

- Public Types inherited from frc::RobotDriveBase
enum  MotorType {
  kFrontLeft = 0, kFrontRight = 1, kRearLeft = 2, kRearRight = 3,
  kLeft = 0, kRight = 1, kBack = 2
}
 The location of a motor on the robot for the purpose of driving.
 
- Static Public Member Functions inherited from frc::MotorSafety
static void CheckMotors ()
 Check the motors to see if any have timed out. More...
 
- Static Public Member Functions inherited from frc::ErrorBase
static void SetGlobalError (Error::Code code, const wpi::Twine &contextMessage, wpi::StringRef filename, wpi::StringRef function, int lineNumber)
 
static void SetGlobalWPIError (const wpi::Twine &errorMessage, const wpi::Twine &contextMessage, wpi::StringRef filename, wpi::StringRef function, int lineNumber)
 
static Error GetGlobalError ()
 Retrieve the last global error.
 
static std::vector< ErrorGetGlobalErrors ()
 Retrieve all global errors.
 
- Protected Member Functions inherited from frc::RobotDriveBase
double ApplyDeadband (double number, double deadband)
 Returns 0.0 if the given value is within the specified range around zero. More...
 
void Normalize (wpi::MutableArrayRef< double > wheelSpeeds)
 Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
 
- Protected Member Functions inherited from frc::SendableHelper< MecanumDrive >
void SetName (const wpi::Twine &moduleType, int channel)
 Sets the name of the sensor with a channel number. More...
 
void SetName (const wpi::Twine &moduleType, int moduleNumber, int channel)
 Sets the name of the sensor with a module and channel number. More...
 
void AddChild (std::shared_ptr< Sendable > child)
 Add a child component. More...
 
void AddChild (void *child)
 Add a child component. More...
 
- Protected Attributes inherited from frc::RobotDriveBase
double m_deadband = 0.02
 
double m_maxOutput = 1.0
 
- Protected Attributes inherited from frc::ErrorBase
Error m_error
 

Detailed Description

A class for driving Mecanum drive platforms.

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.

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 to the 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 SetDeadband().

RobotDrive porting guide:
In MecanumDrive, the right side speed controllers are automatically inverted, while in RobotDrive, no speed controllers are automatically inverted.
DriveCartesian(double, double, double, double) is equivalent to RobotDrive::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::MecanumDrive_Polar(double, double, double) if a deadband of 0 is used.

Constructor & Destructor Documentation

◆ MecanumDrive()

frc::MecanumDrive::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.

Member Function Documentation

◆ DriveCartesian()

void frc::MecanumDrive::DriveCartesian ( double  ySpeed,
double  xSpeed,
double  zRotation,
double  gyroAngle = 0.0 
)

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
ySpeedThe robot's speed along the Y axis [-1.0..1.0]. Right is positive.
xSpeedThe robot's speed along the X axis [-1.0..1.0]. Forward is positive.
zRotationThe robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.
gyroAngleThe current angle reading from the gyro in degrees around the Z axis. Use this to implement field-oriented controls.

◆ DrivePolar()

void frc::MecanumDrive::DrivePolar ( double  magnitude,
double  angle,
double  zRotation 
)

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
magnitudeThe robot's speed at a given angle [-1.0..1.0]. Forward is positive.
angleThe angle around the Z axis at which the robot drives in degrees [-180..180].
zRotationThe robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.

◆ InitSendable()

void frc::MecanumDrive::InitSendable ( SendableBuilder builder)
overridevirtual

Initializes this Sendable object.

Parameters
buildersendable builder

Implements frc::Sendable.

◆ IsRightSideInverted()

bool frc::MecanumDrive::IsRightSideInverted ( ) const

Gets if the power sent to the right side of the drivetrain is multipled by -1.

Returns
true if the right side is inverted

◆ SetRightSideInverted()

void frc::MecanumDrive::SetRightSideInverted ( bool  rightSideInverted)

Sets if the power sent to the right side of the drivetrain should be multipled by -1.

Parameters
rightSideInvertedtrue if right side power should be multipled by -1

The documentation for this class was generated from the following file: