DifferentialDrive

class wpilib.drive.DifferentialDrive(leftMotor, rightMotor)[source]

Bases: wpilib.drive.RobotDriveBase

A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive base, “tank drive”, or West Coast Drive.

These drive bases typically have drop-center / skid-steer with two or more wheels per side (e.g., 6WD or 8WD).

This class takes a SpeedController per side. For four and six motor drivetrains, construct and pass in SpeedControllerGroup instances as follows.

Four motor drivetrain:

def robotInit(self):
    self.frontLeft = wpilib.Spark(1)
    self.rearLeft = wpilib.Spark(2)
    self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)

    self.frontRight = wpilib.Spark(3)
    self.rearRight = wpilib.Spark(4)
    self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight)

    self.drive = DifferentialDrive(self.left, self.right)

Six motor drivetrain:

def robotInit(self):
    self.frontLeft = wpilib.Spark(1)
    self.midLeft = wpilib.Spark(2)
    self.rearLeft = wpilib.Spark(3)
    self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.midLeft, self.rearLeft)

    self.frontRight = wpilib.Spark(4)
    self.midRight = wpilib.Spark(5)
    self.rearRight = wpilib.Spark(6)
    self.right = wpilib.SpeedControllerGroup(self.frontRight, self.midRight, self.rearRight)

    self.drive = DifferentialDrive(self.left, self.right)

A differential drive robot has left and right wheels separated by an arbitrary width.

Drive base diagram:

|_______|
| |   | |
  |   |
|_|___|_|
|       |

Each drive() function provides different inverse kinematic relations for a differential 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 than RobotDriveBase.kDefaultDeadband 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().

Note

RobotDrive porting guide:

tankDrive() is equivalent to RobotDrive.tankDrive() if a deadband of 0 is used.

arcadeDrive() is equivalent to RobotDrive.arcadeDrive() if a deadband of 0 is used and the rotation input is inverted (i.e arcadeDrive(y, -rotation))

curvatureDrive() is similar in concept to RobotDrive.drive() with the addition of a quick turn mode. However, it is not designed to give exactly the same response.

Constructor for DifferentialDrive.

Note

To pass multiple motors per side, use a SpeedControllerGroup. If a motor needs to be inverted, do so before passing it in.

Parameters:
arcadeDrive(xSpeed, zRotation, squaredInputs=True)[source]

Arcade drive method for differential drive platform.

Parameters:
  • xSpeed – The robot’s speed along the X axis [-1.0..1.0]. Forward is positive
  • zRotation – The robot’s zRotation rate around the Z axis [-1.0..1.0]. Clockwise is positive
  • squaredInputs – If set, decreases the sensitivity at low speeds.
curvatureDrive(xSpeed, zRotation, isQuickTurn)[source]

Curvature drive method for differential drive platform.

The zRotation argument controls the curvature of the robot’s path rather than its rate of heading change. This makes the robot more controllable at high speeds. Also handles the robot’s quick turn functionality - “quick turn” overrides constant-curvature turning for turn-in-place maneuvers

Parameters:
  • 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.
  • isQuickTurn – If set, overrides constant-curvature turning for turn-in-place maneuvers.
getDescription()[source]
initSendable(builder)[source]
instances = 0
kDefaultQuickStopAlpha = 0.1
kDefaultQuickStopThreshold = 0.2
setQuickStopAlpha(alpha)[source]

Sets the low-pass filter gain for QuickStop in curvature drive.

The low-pass filter filters incoming rotation rate commands to smooth out high frequency changes.

Parameters:alpha – Low-pass filter gain [0.0..2.0]. Smaller values result in slower output changes. Values between 1.0 and 2.0 result in output oscillation. Values below 0.0 and above 2.0 are unstable.
setQuickStopThreshold(threshold)[source]

Sets the QuickStop speed threshold in curvature drive.

QuickStop compensates for the robot’s moment of inertia when stopping after a QuickTurn.

While QuickTurn is enabled, the QuickStop accumulator takes on the rotation rate value outputted by the low-pass filter when the robot’s speed along the X axis is below the threshold. When QuickTurn is disabled, the accumulator’s value is applied against the computed angular power request to slow the robot’s rotation.

Parameters:threshold – X speed below which quick stop accumulator will receive rotation rate values [0..1.0].
stopMotor()[source]
tankDrive(leftSpeed, rightSpeed, squaredInputs=True)[source]

Provide tank steering using the stored robot configuration.

Parameters:
  • leftSpeed – The robot’s left side speed along the X axis [-1.0..1.0]. Forward is positive.
  • rightSpeed – The robot’s right side speed along the X axis`[-1.0..1.0]`. Forward is positive.
  • squaredInputs – If set, decreases the input sensitivity at low speeds