edu.wpi.first.wpilibj
Class RobotDrive

java.lang.Object
  extended by edu.wpi.first.wpilibj.RobotDrive
All Implemented Interfaces:
MotorSafety, IDevice, IUtility

public class RobotDrive
extends java.lang.Object
implements MotorSafety, IUtility

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 standard drive trains are supported. In the future other drive types like swerve and meccanum 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.


Nested Class Summary
static class RobotDrive.MotorType
          The location of a motor on the robot for the purpose of driving
 
Field Summary
static double kDefaultExpirationTime
           
static double kDefaultMaxOutput
           
static double kDefaultSensitivity
           
protected static int kMaxNumberOfMotors
           
protected  boolean m_allocatedSpeedControllers
           
protected  SpeedController m_frontLeftMotor
           
protected  SpeedController m_frontRightMotor
           
protected  int[] m_invertedMotors
           
protected  boolean m_isCANInitialized
           
protected  double m_maxOutput
           
protected  SpeedController m_rearLeftMotor
           
protected  SpeedController m_rearRightMotor
           
protected  MotorSafetyHelper m_safetyHelper
           
protected  double m_sensitivity
           
 
Fields inherited from interface edu.wpi.first.wpilibj.MotorSafety
DEFAULT_SAFETY_EXPIRATION
 
Constructor Summary
RobotDrive(int leftMotorChannel, int rightMotorChannel)
          Constructor for RobotDrive with 2 motors specified with channel numbers.
RobotDrive(int frontLeftMotor, int rearLeftMotor, int frontRightMotor, int rearRightMotor)
          Constructor for RobotDrive with 4 motors specified with channel numbers.
RobotDrive(SpeedController leftMotor, SpeedController rightMotor)
          Constructor for RobotDrive with 2 motors specified as SpeedController objects.
RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor, SpeedController frontRightMotor, SpeedController rearRightMotor)
          Constructor for RobotDrive with 4 motors specified as SpeedController objects.
 
Method Summary
 void arcadeDrive(double moveValue, double rotateValue)
          Arcade drive implements single stick driving.
 void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs)
          Arcade drive implements single stick driving.
 void arcadeDrive(GenericHID stick)
          Arcade drive implements single stick driving.
 void arcadeDrive(GenericHID stick, boolean squaredInputs)
          Arcade drive implements single stick driving.
 void arcadeDrive(GenericHID moveStick, int moveAxis, GenericHID rotateStick, int rotateAxis)
          Arcade drive implements single stick driving.
 void arcadeDrive(GenericHID moveStick, int moveAxis, GenericHID rotateStick, int rotateAxis, boolean squaredInputs)
          Arcade drive implements single stick driving.
 void drive(double outputMagnitude, double curve)
          Drive the motors at "speed" and "curve".
 void free()
          Free the speed controllers if they were allocated locally
 java.lang.String getDescription()
           
 double getExpiration()
           
 boolean isAlive()
           
 boolean isSafetyEnabled()
           
protected static double limit(double num)
          Limit motor values to the -1.0 to +1.0 range.
 void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle)
          Drive method for Mecanum wheeled robots.
 void mecanumDrive_Polar(double magnitude, double direction, double rotation)
          Drive method for Mecanum wheeled robots.
protected static void normalize(double[] wheelSpeeds)
          Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
protected static double[] rotateVector(double x, double y, double angle)
          Rotate a vector in Cartesian space.
 void setExpiration(double timeout)
           
 void setInvertedMotor(RobotDrive.MotorType motor, boolean isInverted)
          Invert a motor direction.
 void setLeftRightMotorOutputs(double leftOutput, double rightOutput)
          Set the speed of the right and left motors.
 void setMaxOutput(double maxOutput)
          Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.
 void setSafetyEnabled(boolean enabled)
           
 void setSensitivity(double sensitivity)
          Set the turning sensitivity.
 void stopMotor()
           
 void tankDrive(double leftValue, double rightValue)
          Provide tank steering using the stored robot configuration.
 void tankDrive(GenericHID leftStick, GenericHID rightStick)
          Provide tank steering using the stored robot configuration.
 void tankDrive(GenericHID leftStick, int leftAxis, GenericHID rightStick, int rightAxis)
          Provide tank steering using the stored robot configuration.
 
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Field Detail

m_safetyHelper

protected MotorSafetyHelper m_safetyHelper

kDefaultExpirationTime

public static final double kDefaultExpirationTime
See Also:
Constant Field Values

kDefaultSensitivity

public static final double kDefaultSensitivity
See Also:
Constant Field Values

kDefaultMaxOutput

public static final double kDefaultMaxOutput
See Also:
Constant Field Values

kMaxNumberOfMotors

protected static final int kMaxNumberOfMotors
See Also:
Constant Field Values

m_invertedMotors

protected final int[] m_invertedMotors

m_sensitivity

protected double m_sensitivity

m_maxOutput

protected double m_maxOutput

m_frontLeftMotor

protected SpeedController m_frontLeftMotor

m_frontRightMotor

protected SpeedController m_frontRightMotor

m_rearLeftMotor

protected SpeedController m_rearLeftMotor

m_rearRightMotor

protected SpeedController m_rearRightMotor

m_allocatedSpeedControllers

protected boolean m_allocatedSpeedControllers

m_isCANInitialized

protected boolean m_isCANInitialized
Constructor Detail

RobotDrive

public 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 Jaguars for controlling the motors.

Parameters:
leftMotorChannel - The PWM channel number on the default digital module that drives the left motor.
rightMotorChannel - The PWM channel number on the default digital module that drives the right motor.

RobotDrive

public RobotDrive(int frontLeftMotor,
                  int rearLeftMotor,
                  int frontRightMotor,
                  int rearRightMotor)
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 Jaguars for controlling the motors.

Parameters:
frontLeftMotor - Front left motor channel number on the default digital module
rearLeftMotor - Rear Left motor channel number on the default digital module
frontRightMotor - Front right motor channel number on the default digital module
rearRightMotor - Rear Right motor channel number on the default digital module

RobotDrive

public 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 dead-band elimination.

Parameters:
leftMotor - The left SpeedController object used to drive the robot.
rightMotor - the right SpeedController object used to drive the robot.

RobotDrive

public 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:
rearLeftMotor - The back left SpeedController object used to drive the robot.
frontLeftMotor - The front left SpeedController object used to drive the robot
rearRightMotor - The back right SpeedController object used to drive the robot.
frontRightMotor - The front right SpeedController object used to drive the robot.
Method Detail

drive

public void drive(double outputMagnitude,
                  double curve)
Drive the motors at "speed" and "curve". The speed and curve are -1.0 to +1.0 values where 0.0 represents stopped and not turning. The algorithm for adding in the direction attempts to provide a constant turn radius for differing speeds. This function will most likely be used in an autonomous routine.

Parameters:
outputMagnitude - The forward component of the output magnitude to send to the motors.
curve - The rate of turn, constant for different forward speeds.

tankDrive

public void tankDrive(GenericHID leftStick,
                      GenericHID rightStick)
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:
leftStick - The joystick to control the left side of the robot.
rightStick - The joystick to control the right side of the robot.

tankDrive

public void tankDrive(GenericHID leftStick,
                      int leftAxis,
                      GenericHID rightStick,
                      int rightAxis)
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:
leftStick - The Joystick object to use for the left side of the robot.
leftAxis - The axis to select on the left side Joystick object.
rightStick - The Joystick object to use for the right side of the robot.
rightAxis - The axis to select on the right side Joystick object.

tankDrive

public void tankDrive(double leftValue,
                      double rightValue)
Provide tank steering using the stored robot configuration. This function lets you directly provide joystick values from any source.

Parameters:
leftValue - The value of the left stick.
rightValue - The value of the right stick.

arcadeDrive

public void arcadeDrive(GenericHID stick,
                        boolean squaredInputs)
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:
stick - The 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.
squaredInputs - If true, the sensitivity will be increased for small values

arcadeDrive

public void arcadeDrive(GenericHID stick)
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:
stick - The 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.

arcadeDrive

public void arcadeDrive(GenericHID moveStick,
                        int moveAxis,
                        GenericHID rotateStick,
                        int rotateAxis,
                        boolean squaredInputs)
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:
moveStick - The Joystick object that represents the forward/backward direction
moveAxis - The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
rotateStick - The Joystick object that represents the rotation value
rotateAxis - The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
squaredInputs - Setting this parameter to true increases the sensitivity at lower speeds

arcadeDrive

public void arcadeDrive(GenericHID moveStick,
                        int moveAxis,
                        GenericHID rotateStick,
                        int rotateAxis)
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:
moveStick - The Joystick object that represents the forward/backward direction
moveAxis - The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
rotateStick - The Joystick object that represents the rotation value
rotateAxis - The axis on the rotation object to use for the rotate right/left (typically X_AXIS)

arcadeDrive

public void arcadeDrive(double moveValue,
                        double rotateValue,
                        boolean squaredInputs)
Arcade drive implements single stick driving. This function lets you directly provide joystick values from any source.

Parameters:
moveValue - The value to use for forwards/backwards
rotateValue - The value to use for the rotate right/left
squaredInputs - If set, increases the sensitivity at low speeds

arcadeDrive

public void arcadeDrive(double moveValue,
                        double rotateValue)
Arcade drive implements single stick driving. This function lets you directly provide joystick values from any source.

Parameters:
moveValue - The value to use for fowards/backwards
rotateValue - The value to use for the rotate right/left

mecanumDrive_Cartesian

public void mecanumDrive_Cartesian(double x,
                                   double y,
                                   double rotation,
                                   double gyroAngle)
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:
x - The speed that the robot should drive in the X direction. [-1.0..1.0]
y - The 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]
rotation - The rate of rotation for the robot that is completely independent of the translation. [-1.0..1.0]
gyroAngle - The current angle reading from the gyro. Use this to implement field-oriented controls.

mecanumDrive_Polar

public void 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:
magnitude - The speed that the robot should drive in a given direction.
direction - The direction the robot should drive in degrees. The direction and maginitute are independent of the rotation rate.
rotation - The rate of rotation for the robot that is completely independent of the magnitute or direction. [-1.0..1.0]

setLeftRightMotorOutputs

public void setLeftRightMotorOutputs(double leftOutput,
                                     double rightOutput)
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 "leftSpeed" and "rightSpeed" and includes flipping the direction of one side for opposing motors.

Parameters:
leftOutput - The speed to send to the left side of the robot.
rightOutput - The speed to send to the right side of the robot.

limit

protected static double limit(double num)
Limit motor values to the -1.0 to +1.0 range.


normalize

protected static void normalize(double[] wheelSpeeds)
Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.


rotateVector

protected static double[] rotateVector(double x,
                                       double y,
                                       double angle)
Rotate a vector in Cartesian space.


setInvertedMotor

public void setInvertedMotor(RobotDrive.MotorType motor,
                             boolean isInverted)
Invert a motor direction. This is used when a motor should run in the opposite direction as the drive code would normally run it. Motors that are direct drive would be inverted, the drive code assumes that the motors are geared with one reversal.

Parameters:
motor - The motor index to invert.
isInverted - True if the motor should be inverted when operated.

setSensitivity

public void setSensitivity(double sensitivity)
Set the turning sensitivity. This only impacts the drive() entry-point.

Parameters:
sensitivity - Effectively sets the turning sensitivity (or turn radius for a given value)

setMaxOutput

public void setMaxOutput(double maxOutput)
Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.

Parameters:
maxOutput - Multiplied with the output percentage computed by the drive functions.

free

public void free()
Free the speed controllers if they were allocated locally


setExpiration

public void setExpiration(double timeout)
Specified by:
setExpiration in interface MotorSafety

getExpiration

public double getExpiration()
Specified by:
getExpiration in interface MotorSafety

isAlive

public boolean isAlive()
Specified by:
isAlive in interface MotorSafety

isSafetyEnabled

public boolean isSafetyEnabled()
Specified by:
isSafetyEnabled in interface MotorSafety

setSafetyEnabled

public void setSafetyEnabled(boolean enabled)
Specified by:
setSafetyEnabled in interface MotorSafety

getDescription

public java.lang.String getDescription()
Specified by:
getDescription in interface MotorSafety

stopMotor

public void stopMotor()
Specified by:
stopMotor in interface MotorSafety