WPILibC++  2019.3.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
frc::RobotDrive Class Reference

Utility class for handling Robot drive based on a definition of the motor configuration. More...

#include <RobotDrive.h>

Inheritance diagram for frc::RobotDrive:
frc::MotorSafety frc::ErrorBase

Public Types

enum  MotorType { kFrontLeftMotor = 0, kFrontRightMotor = 1, kRearLeftMotor = 2, kRearRightMotor = 3 }
 

Public Member Functions

 RobotDrive (int leftMotorChannel, int rightMotorChannel)
 Constructor for RobotDrive with 2 motors specified with channel numbers. More...
 
 RobotDrive (int frontLeftMotorChannel, int rearLeftMotorChannel, int frontRightMotorChannel, int rearRightMotorChannel)
 Constructor for RobotDrive with 4 motors specified with channel numbers. More...
 
 RobotDrive (SpeedController *leftMotor, SpeedController *rightMotor)
 Constructor for RobotDrive with 2 motors specified as SpeedController objects. More...
 
 RobotDrive (SpeedController &leftMotor, SpeedController &rightMotor)
 
 RobotDrive (std::shared_ptr< SpeedController > leftMotor, std::shared_ptr< SpeedController > rightMotor)
 
 RobotDrive (SpeedController *frontLeftMotor, SpeedController *rearLeftMotor, SpeedController *frontRightMotor, SpeedController *rearRightMotor)
 Constructor for RobotDrive with 4 motors specified as SpeedController objects. More...
 
 RobotDrive (SpeedController &frontLeftMotor, SpeedController &rearLeftMotor, SpeedController &frontRightMotor, SpeedController &rearRightMotor)
 
 RobotDrive (std::shared_ptr< SpeedController > frontLeftMotor, std::shared_ptr< SpeedController > rearLeftMotor, std::shared_ptr< SpeedController > frontRightMotor, std::shared_ptr< SpeedController > rearRightMotor)
 
 RobotDrive (RobotDrive &&)=default
 
RobotDriveoperator= (RobotDrive &&)=default
 
void Drive (double outputMagnitude, double curve)
 Drive the motors at "outputMagnitude" and "curve". More...
 
void TankDrive (GenericHID *leftStick, GenericHID *rightStick, bool squaredInputs=true)
 Provide tank steering using the stored robot configuration. More...
 
void TankDrive (GenericHID &leftStick, GenericHID &rightStick, bool squaredInputs=true)
 Provide tank steering using the stored robot configuration. More...
 
void TankDrive (GenericHID *leftStick, int leftAxis, GenericHID *rightStick, int rightAxis, bool squaredInputs=true)
 Provide tank steering using the stored robot configuration. More...
 
void TankDrive (GenericHID &leftStick, int leftAxis, GenericHID &rightStick, int rightAxis, bool squaredInputs=true)
 
void TankDrive (double leftValue, double rightValue, bool squaredInputs=true)
 Provide tank steering using the stored robot configuration. More...
 
void ArcadeDrive (GenericHID *stick, bool squaredInputs=true)
 Arcade drive implements single stick driving. More...
 
void ArcadeDrive (GenericHID &stick, bool squaredInputs=true)
 Arcade drive implements single stick driving. More...
 
void ArcadeDrive (GenericHID *moveStick, int moveChannel, GenericHID *rotateStick, int rotateChannel, bool squaredInputs=true)
 Arcade drive implements single stick driving. More...
 
void ArcadeDrive (GenericHID &moveStick, int moveChannel, GenericHID &rotateStick, int rotateChannel, bool squaredInputs=true)
 Arcade drive implements single stick driving. More...
 
void ArcadeDrive (double moveValue, double rotateValue, bool squaredInputs=true)
 Arcade drive implements single stick driving. More...
 
void MecanumDrive_Cartesian (double x, double y, double rotation, double gyroAngle=0.0)
 Drive method for Mecanum wheeled robots. More...
 
void MecanumDrive_Polar (double magnitude, double direction, double rotation)
 Drive method for Mecanum wheeled robots. More...
 
void HolonomicDrive (double magnitude, double direction, double rotation)
 Holonomic Drive method for Mecanum wheeled robots. More...
 
virtual void SetLeftRightMotorOutputs (double leftOutput, double rightOutput)
 Set the speed of the right and left motors. More...
 
void SetInvertedMotor (MotorType motor, bool isInverted)
 
void SetSensitivity (double sensitivity)
 Set the turning sensitivity. More...
 
void SetMaxOutput (double maxOutput)
 Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus. More...
 
void StopMotor () override
 
void GetDescription (wpi::raw_ostream &desc) const override
 
- 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 (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...
 

Protected Member Functions

void InitRobotDrive ()
 Common function to initialize all the robot drive constructors. More...
 
double Limit (double number)
 Limit motor values to the -1.0 to +1.0 range.
 
void Normalize (double *wheelSpeeds)
 Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
 
void RotateVector (double &x, double &y, double angle)
 Rotate a vector in Cartesian space.
 

Protected Attributes

double m_sensitivity = 0.5
 
double m_maxOutput = 1.0
 
std::shared_ptr< SpeedControllerm_frontLeftMotor
 
std::shared_ptr< SpeedControllerm_frontRightMotor
 
std::shared_ptr< SpeedControllerm_rearLeftMotor
 
std::shared_ptr< SpeedControllerm_rearRightMotor
 
- Protected Attributes inherited from frc::ErrorBase
Error m_error
 

Static Protected Attributes

static constexpr int kMaxNumberOfMotors = 4
 

Additional Inherited Members

- 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 const ErrorGetGlobalError ()
 Retrieve the current global error.
 

Detailed Description

Utility class for handling Robot drive based on a definition of the motor configuration.

The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and mecanum drive trains are supported. In the future other drive types like swerve might be implemented. Motor channel numbers are passed supplied on creation of the class. Those are used for either the Drive function (intended for hand created drive code, such as autonomous) or with the Tank/Arcade functions intended to be used for Operator Control driving.

Constructor & Destructor Documentation

frc::RobotDrive::RobotDrive ( int  leftMotorChannel,
int  rightMotorChannel 
)

Constructor for RobotDrive with 2 motors specified with channel numbers.

Set up parameters for a two wheel drive system where the left and right motor pwm channels are specified in the call. This call assumes Talons for controlling the motors.

Parameters
leftMotorChannelThe PWM channel number that drives the left motor. 0-9 are on-board, 10-19 are on the MXP port
rightMotorChannelThe PWM channel number that drives the right motor. 0-9 are on-board, 10-19 are on the MXP port
frc::RobotDrive::RobotDrive ( int  frontLeftMotorChannel,
int  rearLeftMotorChannel,
int  frontRightMotorChannel,
int  rearRightMotorChannel 
)

Constructor for RobotDrive with 4 motors specified with channel numbers.

Set up parameters for a four wheel drive system where all four motor pwm channels are specified in the call. This call assumes Talons for controlling the motors.

Parameters
frontLeftMotorFront left motor channel number. 0-9 are on-board, 10-19 are on the MXP port
rearLeftMotorRear Left motor channel number. 0-9 are on-board, 10-19 are on the MXP port
frontRightMotorFront right motor channel number. 0-9 are on-board, 10-19 are on the MXP port
rearRightMotorRear Right motor channel number. 0-9 are on-board, 10-19 are on the MXP port
frc::RobotDrive::RobotDrive ( SpeedController leftMotor,
SpeedController rightMotor 
)

Constructor for RobotDrive with 2 motors specified as SpeedController objects.

The SpeedController version of the constructor enables programs to use the RobotDrive classes with subclasses of the SpeedController objects, for example, versions with ramping or reshaping of the curve to suit motor bias or deadband elimination.

Parameters
leftMotorThe left SpeedController object used to drive the robot.
rightMotorThe right SpeedController object used to drive the robot.
frc::RobotDrive::RobotDrive ( SpeedController frontLeftMotor,
SpeedController rearLeftMotor,
SpeedController frontRightMotor,
SpeedController rearRightMotor 
)

Constructor for RobotDrive with 4 motors specified as SpeedController objects.

Speed controller input version of RobotDrive (see previous comments).

Parameters
frontLeftMotorThe front left SpeedController object used to drive the robot.
rearLeftMotorThe back left SpeedController object used to drive the robot.
frontRightMotorThe front right SpeedController object used to drive the robot.
rearRightMotorThe back right SpeedController object used to drive the robot.

Member Function Documentation

void frc::RobotDrive::ArcadeDrive ( GenericHID stick,
bool  squaredInputs = true 
)

Arcade drive implements single stick driving.

Given a single Joystick, the class assumes the Y axis for the move value and the X axis for the rotate value. (Should add more information here regarding the way that arcade drive works.)

Parameters
stickThe joystick to use for Arcade single-stick driving. The Y-axis will be selected for forwards/backwards and the X-axis will be selected for rotation rate.
squaredInputsIf true, the sensitivity will be decreased for small values
void frc::RobotDrive::ArcadeDrive ( GenericHID stick,
bool  squaredInputs = true 
)

Arcade drive implements single stick driving.

Given a single Joystick, the class assumes the Y axis for the move value and the X axis for the rotate value. (Should add more information here regarding the way that arcade drive works.)

Parameters
stickThe joystick to use for Arcade single-stick driving. The Y-axis will be selected for forwards/backwards and the X-axis will be selected for rotation rate.
squaredInputsIf true, the sensitivity will be decreased for small values
void frc::RobotDrive::ArcadeDrive ( GenericHID moveStick,
int  moveChannel,
GenericHID rotateStick,
int  rotateChannel,
bool  squaredInputs = true 
)

Arcade drive implements single stick driving.

Given two joystick instances and two axis, compute the values to send to either two or four motors.

Parameters
moveStickThe Joystick object that represents the forward/backward direction
moveAxisThe axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
rotateStickThe Joystick object that represents the rotation value
rotateAxisThe axis on the rotation object to use for the rotate right/left (typically X_AXIS)
squaredInputsSetting this parameter to true increases the sensitivity at lower speeds
void frc::RobotDrive::ArcadeDrive ( GenericHID moveStick,
int  moveChannel,
GenericHID rotateStick,
int  rotateChannel,
bool  squaredInputs = true 
)

Arcade drive implements single stick driving.

Given two joystick instances and two axis, compute the values to send to either two or four motors.

Parameters
moveStickThe Joystick object that represents the forward/backward direction
moveAxisThe axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
rotateStickThe Joystick object that represents the rotation value
rotateAxisThe axis on the rotation object to use for the rotate right/left (typically X_AXIS)
squaredInputsSetting this parameter to true increases the sensitivity at lower speeds
void frc::RobotDrive::ArcadeDrive ( double  moveValue,
double  rotateValue,
bool  squaredInputs = true 
)

Arcade drive implements single stick driving.

This function lets you directly provide joystick values from any source.

Parameters
moveValueThe value to use for fowards/backwards
rotateValueThe value to use for the rotate right/left
squaredInputsIf set, increases the sensitivity at low speeds
void frc::RobotDrive::Drive ( double  outputMagnitude,
double  curve 
)

Drive the motors at "outputMagnitude" and "curve".

Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0 represents stopped and not turning. curve < 0 will turn left and curve > 0 will turn right.

The algorithm for steering provides a constant turn radius for any normal speed range, both forward and backward. Increasing m_sensitivity causes sharper turns for fixed values of curve.

This function will most likely be used in an autonomous routine.

Parameters
outputMagnitudeThe speed setting for the outside wheel in a turn, forward or backwards, +1 to -1.
curveThe rate of turn, constant for different forward speeds. Set curve < 0 for left turn or curve > 0 for right turn.

Set curve = e^(-r/w) to get a turn radius r for wheelbase w of your robot. Conversely, turn radius r = -ln(curve)*w for a given value of curve and wheelbase w.

void frc::RobotDrive::HolonomicDrive ( double  magnitude,
double  direction,
double  rotation 
)

Holonomic Drive method for Mecanum wheeled robots.

This is an alias to MecanumDrive_Polar() for backward compatability

Parameters
magnitudeThe speed that the robot should drive in a given direction. [-1.0..1.0]
directionThe direction the robot should drive. The direction and magnitude are independent of the rotation rate.
rotationThe rate of rotation for the robot that is completely independent of the magnitude or direction. [-1.0..1.0]
void frc::RobotDrive::InitRobotDrive ( )
protected

Common function to initialize all the robot drive constructors.

Create a motor safety object (the real reason for the common code) and initialize all the motor assignments. The default timeout is set for the robot drive.

void frc::RobotDrive::MecanumDrive_Cartesian ( double  x,
double  y,
double  rotation,
double  gyroAngle = 0.0 
)

Drive method for Mecanum wheeled robots.

A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the top, the roller axles should form an X across the robot.

This is designed to be directly driven by joystick axes.

Parameters
xThe speed that the robot should drive in the X direction. [-1.0..1.0]
yThe speed that the robot should drive in the Y direction. This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
rotationThe rate of rotation for the robot that is completely independent of the translation. [-1.0..1.0]
gyroAngleThe current angle reading from the gyro. Use this to implement field-oriented controls.
void frc::RobotDrive::MecanumDrive_Polar ( double  magnitude,
double  direction,
double  rotation 
)

Drive method for Mecanum wheeled robots.

A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the top, the roller axles should form an X across the robot.

Parameters
magnitudeThe speed that the robot should drive in a given direction. [-1.0..1.0]
directionThe direction the robot should drive in degrees. The direction and maginitute are independent of the rotation rate.
rotationThe rate of rotation for the robot that is completely independent of the magnitute or direction. [-1.0..1.0]
virtual void frc::RobotDrive::SetLeftRightMotorOutputs ( double  leftOutput,
double  rightOutput 
)
virtual

Set the speed of the right and left motors.

This is used once an appropriate drive setup function is called such as TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput" and includes flipping the direction of one side for opposing motors.

Parameters
leftOutputThe speed to send to the left side of the robot.
rightOutputThe speed to send to the right side of the robot.
void frc::RobotDrive::SetMaxOutput ( double  maxOutput)

Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.

Parameters
maxOutputMultiplied with the output percentage computed by the drive functions.
void frc::RobotDrive::SetSensitivity ( double  sensitivity)

Set the turning sensitivity.

This only impacts the Drive() entry-point.

Parameters
sensitivityEffectively sets the turning sensitivity (or turn radius for a given value)
void frc::RobotDrive::TankDrive ( GenericHID leftStick,
GenericHID rightStick,
bool  squaredInputs = true 
)

Provide tank steering using the stored robot configuration.

Drive the robot using two joystick inputs. The Y-axis will be selected from each Joystick object.

Parameters
leftStickThe joystick to control the left side of the robot.
rightStickThe joystick to control the right side of the robot.
squaredInputsIf true, the sensitivity will be decreased for small values
void frc::RobotDrive::TankDrive ( GenericHID leftStick,
GenericHID rightStick,
bool  squaredInputs = true 
)

Provide tank steering using the stored robot configuration.

Drive the robot using two joystick inputs. The Y-axis will be selected from each Joystick object.

Parameters
leftStickThe joystick to control the left side of the robot.
rightStickThe joystick to control the right side of the robot.
squaredInputsIf true, the sensitivity will be decreased for small values
void frc::RobotDrive::TankDrive ( GenericHID leftStick,
int  leftAxis,
GenericHID rightStick,
int  rightAxis,
bool  squaredInputs = true 
)

Provide tank steering using the stored robot configuration.

This function lets you pick the axis to be used on each Joystick object for the left and right sides of the robot.

Parameters
leftStickThe Joystick object to use for the left side of the robot.
leftAxisThe axis to select on the left side Joystick object.
rightStickThe Joystick object to use for the right side of the robot.
rightAxisThe axis to select on the right side Joystick object.
squaredInputsIf true, the sensitivity will be decreased for small values
void frc::RobotDrive::TankDrive ( double  leftValue,
double  rightValue,
bool  squaredInputs = true 
)

Provide tank steering using the stored robot configuration.

This function lets you directly provide joystick values from any source.

Parameters
leftValueThe value of the left stick.
rightValueThe value of the right stick.
squaredInputsIf true, the sensitivity will be decreased for small values

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