Class DifferentialDrive

  • All Implemented Interfaces:
    Sendable, AutoCloseable

    public class DifferentialDrive
    extends 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:

    
     public class Robot {
       Spark m_frontLeft = new Spark(1);
       Spark m_rearLeft = new Spark(2);
       SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
    
       Spark m_frontRight = new Spark(3);
       Spark m_rearRight = new Spark(4);
       SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);
    
       DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
     }
     

    Six motor drivetrain:

    
     public class Robot {
       Spark m_frontLeft = new Spark(1);
       Spark m_midLeft = new Spark(2);
       Spark m_rearLeft = new Spark(3);
       SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
    
       Spark m_frontRight = new Spark(4);
       Spark m_midRight = new Spark(5);
       Spark m_rearRight = new Spark(6);
       SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_midRight, m_rearRight);
    
       DifferentialDrive m_drive = new DifferentialDrive(m_left, m_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 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:
    tankDrive(double, double) is equivalent to RobotDrive.tankDrive(double, double) if a deadband of 0 is used.
    arcadeDrive(double, double) is equivalent to RobotDrive.arcadeDrive(double, double) if a deadband of 0 is used and the the rotation input is inverted eg arcadeDrive(y, -rotation)
    curvatureDrive(double, double, boolean) is similar in concept to RobotDrive.drive(double, double) with the addition of a quick turn mode. However, it is not designed to give exactly the same response.

    • Field Detail

      • kDefaultQuickStopThreshold

        public static final double kDefaultQuickStopThreshold
        See Also:
        Constant Field Values
      • kDefaultQuickStopAlpha

        public static final double kDefaultQuickStopAlpha
        See Also:
        Constant Field Values
    • Constructor Detail

      • DifferentialDrive

        public DifferentialDrive​(SpeedController leftMotor,
                                 SpeedController rightMotor)
        Construct a DifferentialDrive.

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

    • Method Detail

      • arcadeDrive

        public void arcadeDrive​(double xSpeed,
                                double zRotation)
        Arcade drive method for differential drive platform. The calculated values will be squared to decrease sensitivity at low speeds.
        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.
      • arcadeDrive

        public void arcadeDrive​(double xSpeed,
                                double zRotation,
                                boolean squareInputs)
        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 rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.
        squareInputs - If set, decreases the input sensitivity at low speeds.
      • curvatureDrive

        public void curvatureDrive​(double xSpeed,
                                   double zRotation,
                                   boolean isQuickTurn)
        Curvature drive method for differential drive platform.

        The rotation 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.
      • tankDrive

        public void tankDrive​(double leftSpeed,
                              double rightSpeed)
        Tank drive method for differential drive platform. The calculated values will be squared to decrease sensitivity at low speeds.
        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.
      • tankDrive

        public void tankDrive​(double leftSpeed,
                              double rightSpeed,
                              boolean squareInputs)
        Tank drive method for differential drive platform.
        Parameters:
        leftSpeed - The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
        rightSpeed - The robot right side's speed along the X axis [-1.0..1.0]. Forward is positive.
        squareInputs - If set, decreases the input sensitivity at low speeds.
      • setQuickStopThreshold

        public void setQuickStopThreshold​(double threshold)
        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].
      • setQuickStopAlpha

        public void setQuickStopAlpha​(double alpha)
        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.
      • isRightSideInverted

        public boolean isRightSideInverted()
        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

        public void setRightSideInverted​(boolean rightSideInverted)
        Sets if the power sent to the right side of the drivetrain should be multipled by -1.
        Parameters:
        rightSideInverted - true if right side power should be multipled by -1
      • initSendable

        public void initSendable​(SendableBuilder builder)
        Description copied from interface: Sendable
        Initializes this Sendable object.
        Parameters:
        builder - sendable builder