Package trclib.drivebase

Class TrcDriveBase

java.lang.Object
trclib.drivebase.TrcDriveBase
All Implemented Interfaces:
TrcExclusiveSubsystem
Direct Known Subclasses:
TrcSimpleDriveBase
public abstract class TrcDriveBase extends Object implements TrcExclusiveSubsystem
This class implements a platform independent drive base. It is intended to be extended by subclasses that implements different drive base configurations (e.g. SimpleDriveBase, MecanumDriveBase and SwerveDriveBase). The subclasses must provide the tankDrive method and optionally overriding holonomicDrive method if it support it. If the subclass does support holonomic drive, it should override the supportsHolonomicDrive method. It must also provide the getOdometryDelta method where it will calculate the drive base position and velocity info according to sensors such as encoders and gyro.

  • Nested Class Summary

    Nested Classes
    Modifier and Type
    Class
    Description
    static enum 
    TrcDriveBase.DriveOrientation
    This enum specifies all the drive orientation modes: - ROBOT: Robot centric driving mode.
    static interface 
    TrcDriveBase.MotorPowerMapper
    This interface is provided by the caller to translate the motor power to actual motor power according to the motor curve.
    protected static class 
    TrcDriveBase.MotorsState
    This class stores the states of all motors of the drivebase.
    static class 
    TrcDriveBase.Odometry
    This class implements the drive base odometry.
    static enum 
    TrcDriveBase.OdometryType
    This specifies the type of Drive Base Odometry is in use.

    Nested classes/interfaces inherited from interface trclib.robotcore.TrcExclusiveSubsystem

    TrcExclusiveSubsystem.OwnershipParams

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    private TrcWrapValueConverter
    absOdoHeadingWrapConverter
     
    private TrcDriveBaseOdometry
    absoluteOdometry
     
    protected double
    angleScale
     
    private boolean
    antiTippingEnabled
     
    private static final double
    DEF_SENSITIVITY
     
    private TrcDriveBase.DriveOrientation
    driveOrientation
     
    private String
    driveOwner
     
    private final TrcTimer
    driveTimer
     
    private TrcEvent
    driveTimerEvent
     
    private final org.apache.commons.math3.linear.RealMatrix
    enuToNwuChangeOfBasis
     
    private double
    fieldForwardHeading
     
    private final TrcGyro
    gyro
     
    private Double
    gyroAssistHeading
     
    private TrcPidController
    gyroAssistPidCtrl
     
    private final String
    moduleName
     
    protected TrcDriveBase.MotorPowerMapper
    motorPowerMapper
     
    private final TrcMotor[]
    motors
     
    private final TrcDriveBase.MotorsState
    motorsState
     
    private final org.apache.commons.math3.linear.RealMatrix
    nwuToEnuChangeOfBasis
     
    protected final TrcDriveBase.Odometry
    odometry
     
    private final TrcTaskMgr.TaskObject
    odometryTaskObj
     
    private TrcDriveBase.OdometryType
    odometryType
     
    private TrcOdometryWheels
    odometryWheels
     
    private TrcDriveBase.Odometry
    referenceOdometry
     
    private final Stack<TrcDriveBase.Odometry>
    referenceOdometryStack
     
    private boolean
    robotTurning
     
    private double
    sensitivity
     
    protected double
    stallStartTime
     
    protected double
    stallVelThreshold
     
    private static final boolean
    SYNC_GYRO_DATA
     
    private boolean
    synchronizeOdometries
     
    final TrcDbgTrace
    tracer
     
    private static final boolean
    USE_CURVED_PATH
     
    protected double
    xScale
     
    private TrcPidController
    xTippingPidCtrl
     
    private double
    xTippingTolerance
     
    protected double
    yScale
     
    private TrcPidController
    yTippingPidCtrl
     
    private double
    yTippingTolerance
     

  • Constructor Summary

    Constructors
    Constructor
    Description
    TrcDriveBase(TrcMotor... motors)
    Constructor: Create an instance of the object.
    TrcDriveBase(TrcMotor[] motors, TrcGyro gyro)
    Constructor: Create an instance of the object.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    arcadeDrive(double drivePower, double turnPower)
    This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
    void
    arcadeDrive(double drivePower, double turnPower, boolean inverted, double driveTime, TrcEvent event)
    This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
    void
    arcadeDrive(double drivePower, double turnPower, double driveTime, TrcEvent event)
    This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
    void
    arcadeDrive(String owner, double drivePower, double turnPower, boolean inverted)
    This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
    void
    arcadeDrive(String owner, double drivePower, double turnPower, boolean inverted, double driveTime, TrcEvent event)
    This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
    void
    clearReferenceOdometry()
    This method clears the reference odometry.
    void
    curveDrive(double magnitude, double curve)
    This method drives the motors with the given magnitude and curve values.
    void
    curveDrive(double magnitude, double curve, boolean inverted, double driveTime, TrcEvent event)
    This method drives the motors at "magnitude" and "curve".
    void
    curveDrive(double magnitude, double curve, double driveTime, TrcEvent event)
    This method drives the motors with the given magnitude and curve values.
    void
    curveDrive(String owner, double magnitude, double curve, boolean inverted)
    This method drives the motors at "magnitude" and "curve".
    void
    curveDrive(String owner, double magnitude, double curve, boolean inverted, double driveTime, TrcEvent event)
    This method drives the motors at "magnitude" and "curve".
    void
    disableAntiTipping()
    This method disables anti-tipping drive.
    private void
    driveTimerHandler(Object context)
    This method is called when the drivebase drive timer has expired.
    void
    enableAntiTipping(TrcPidController.PidCoefficients yTippingPidCoeffs, double yTolerance, TrcPidController.PidInput yPidInput)
    This method enables anti-tipping drive.
    void
    enableAntiTipping(TrcPidController.PidCoefficients xTippingPidCoeffs, double xTolerance, TrcPidController.PidInput xPidInput, TrcPidController.PidCoefficients yTippingPidCoeffs, double yTolerance, TrcPidController.PidInput yPidInput)
    This method enables anti-tipping drive.
    double
    getAntiTippingPower(boolean xTippingControl)
    This method calculates and returns the anti-tipping power.
    double
    getDriveGyroAngle()
    This method returns robot heading to be maintained in teleop drive according to drive orientation mode.
    TrcDriveBase.DriveOrientation
    getDriveOrientation()
    This method returns the current drive orientation mode.
    TrcPose2D
    getFieldPosition()
    This method returns the robot position in reference to the field origin.
    TrcPose2D
    getFieldVelocity()
    This method returns the robot velocity in reference to the field origin.
    protected double
    getGyroAssistPower(double turnPower)
    This method calculates and returns the gyro assist power to be added to turnPower to compensate heading drift.
    double
    getHeading()
    This method returns the heading of the drive base in degrees.
    TrcMotor[]
    getMotors()
    This method returns an array of motors on the drive base.
    int
    getNumMotors()
    This method returns the number of motors in the drive train.
    protected abstract TrcDriveBase.Odometry
    getOdometryDelta(TrcOdometrySensor.Odometry[] prevOdometries, TrcOdometrySensor.Odometry[] currOdometries)
    This method is called periodically to calculate the delta between the previous and current motor odometries.
    TrcPose2D
    getPositionRelativeTo(TrcPose2D posPose)
    This method returns the robot position relative to pose.
    TrcPose2D
    getPositionRelativeTo(TrcPose2D posPose, boolean transformAngle)
    This method returns the robot position relative to pose.
    TrcDriveBase.Odometry
    getReferenceOdometry()
    This method returns the reference odometry if there is any.
    TrcPose2D
    getRelativePosition()
    This method returns the robot position relative to the reference position, or the robot field position if there is no reference odometry set.
    TrcPose2D
    getRelativeVelocity()
    This method returns the robot velocity relative to the reference velocity, or the robot field velocity if there is no reference odometry set.
    double
    getTurnRate()
    This method returns the drive base turn rate.
    TrcPose2D
    getVelocityRelativeTo(TrcPose2D velPose, double refAngle)
    This method returns the robot velocity relative to pose.
    double
    getXPosition()
    This method returns the X position in scaled unit.
    double
    getXVelocity()
    This method returns the drive base velocity in the X direction.
    double
    getYPosition()
    This method returns the Y position in scaled unit.
    double
    getYVelocity()
    This method returns the drive base velocity in the Y direction.
    void
    holonomicDrive(double x, double y, double rotation)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(double x, double y, double rotation, boolean inverted)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(double x, double y, double rotation, boolean inverted, double driveTime, TrcEvent event)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(double x, double y, double rotation, double gyroAngle, double driveTime, TrcEvent event)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(double x, double y, double rotation, double driveTime, TrcEvent event)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(String owner, double x, double y, double rotation)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(String owner, double x, double y, double rotation, boolean inverted)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(String owner, double x, double y, double rotation, boolean inverted, double gyroAngle)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(String owner, double x, double y, double rotation, boolean inverted, double gyroAngle, double driveTime, TrcEvent event)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(String owner, double x, double y, double rotation, boolean inverted, double driveTime, TrcEvent event)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(String owner, double x, double y, double rotation, double gyroAngle)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(String owner, double x, double y, double rotation, double gyroAngle, double driveTime, TrcEvent event)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive(String owner, double x, double y, double rotation, double driveTime, TrcEvent event)
    This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction.
    void
    holonomicDrive_Polar(double magnitude, double direction, double rotation)
    This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
    void
    holonomicDrive_Polar(double magnitude, double direction, double rotation, boolean inverted, double driveTime, TrcEvent event)
    This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
    void
    holonomicDrive_Polar(double magnitude, double direction, double rotation, double gyroAngle)
    This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
    void
    holonomicDrive_Polar(double magnitude, double direction, double rotation, double gyroAngle, double driveTime, TrcEvent event)
    This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
    void
    holonomicDrive_Polar(double magnitude, double direction, double rotation, double driveTime, TrcEvent event)
    This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
    void
    holonomicDrive_Polar(String owner, double magnitude, double direction, double rotation, boolean inverted, double driveTime, TrcEvent event)
    This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
    void
    holonomicDrive_Polar(String owner, double magnitude, double direction, double rotation, double gyroAngle, double driveTime, TrcEvent event)
    This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
    void
    holonomicDrive_Polar(String owner, double magnitude, double direction, double rotation, double driveTime, TrcEvent event)
    This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
    boolean
    isAntiTippingEnabled()
    This method checks if anti-tipping drive is enabled.
    boolean
    isGyroAssistEnabled()
    This method checks if Gyro Assist is enabled.
    boolean
    isStalled(double stallTime)
    This method determines if the drive base is stalled.
    boolean
    isSynchronizeOdometriesEnabled()
    This method checks if synchronize odometries is enabled.
    private void
    odometryTask(TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop)
    This method is called periodically to update the drive base odometry (position and velocity of both x, y and angle).
    TrcDriveBase.Odometry
    popReferenceOdometry()
    This method returns the current reference odometry.
    void
    printMotorsState()
    This method is called to print the state info of all motors on the drive base for debugging purpose.
    void
    pushReferenceOdometry()
    This method pushes the existing reference odometry onto the stack if there is one and set the given robot's absolute odometry as the new reference odometry.
    void
    resetOdometry()
    This method resets the drive base odometry.
    void
    resetOdometry(boolean resetHardware)
    This method resets the drive base odometry.
    void
    resetOdometry(boolean resetPositionOdometry, boolean resetHeadingOdometry, boolean resetHardware)
    This method resets the drive base odometry.
    void
    setBrakeMode(boolean enabled)
    This method enables/disables brake mode of the drive base.
    void
    setDriveBaseOdometry(boolean resetHardware)
    This method sets the built-in motor odometry as the drive base's odometry device.
    void
    setDriveBaseOdometry(TrcDriveBaseOdometry absoluteOdometry, Double headingWrapRangeLow, Double headingWrapRangeHigh)
    This method sets the given odometry device as the drive base's odometry device overriding the built-in odometry.
    void
    setDriveBaseOdometry(TrcOdometryWheels odometryWheels)
    This method sets the given odometry device as the drive base's odometry device overriding the built-in odometry.
    void
    setDriveOrientation(TrcDriveBase.DriveOrientation orientation, boolean resetHeading)
    This method sets the drive orientation mode.
    protected void
    setDriveTime(String owner, double driveTime, TrcEvent event)
    This method is called by the subclass to set a drive time timer.
    void
    setFieldPosition(TrcPose2D pose)
    This method sets the robot's absolute field position to the given pose.
    void
    setFieldPosition(TrcPose2D pose, boolean positionOnly)
    This method sets the robot's absolute field position to the given pose.
    void
    setGyroAssistEnabled(TrcPidController turnPidCtrl)
    This method enables/disables gyro assist drive.
    protected void
    setInvertedMotor(int index, boolean inverted)
    This method inverts direction of a given motor in the drive train.
    private void
    setMotorOdometryEnabled(boolean enabled, boolean resetHardware)
    This method enables/disables drive motor odometry.
    void
    setMotorPowerMapper(TrcDriveBase.MotorPowerMapper motorPowerMapper)
    This method sets a motor power mapper.
    void
    setOdometryEnabled(boolean enabled)
    This method is called to enable/disable the odometry task that keeps track of the robot position and orientation.
    void
    setOdometryEnabled(boolean enabled, boolean resetHardware)
    This method is called to enable/disable the odometry task that keeps track of the robot position and orientation.
    void
    setOdometryScales(double yScale)
    This method sets the position scales.
    void
    setOdometryScales(double xScale, double yScale)
    This method sets the position scales.
    void
    setOdometryScales(double xScale, double yScale, double angleScale)
    This method sets the position scales.
    void
    setReferenceOdometry()
    This method sets the current robot position and velocity as the reference odometry.
    void
    setSensitivity(double sensitivity)
    This method sets the sensitivity for the drive() method.
    protected void
    setStallVelocityThreshold(double stallVelThreshold)
    This method is called by the subclass to set the stall detection velocity threshold value.
    void
    setSynchronizeOdometriesEnabled(boolean synchronizeOdometries)
    This method enables/disables synchronize odometries.
    void
    stop()
    This methods stops the drive base.
    void
    stop(String owner)
    This methods stops the drive base.
    private void
    stopTask(TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop)
    This method is called when the competition mode is about to end to stop the drive base.
    boolean
    supportsHolonomicDrive()
    This method checks if it supports holonomic drive.
    private void
    synchronizeOdometries(TrcOdometrySensor.Odometry[] odometries)
    This method synchronizes the odometries of all the drive base motors.
    void
    tankDrive(double leftPower, double rightPower)
    This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
    void
    tankDrive(double leftPower, double rightPower, boolean inverted)
    This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
    void
    tankDrive(double leftPower, double rightPower, double driveTime, TrcEvent event)
    This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
    void
    tankDrive(String owner, double leftPower, double rightPower)
    This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
    abstract void
    tankDrive(String owner, double leftPower, double rightPower, boolean inverted, double driveTime, TrcEvent event)
    This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
    void
    tankDrive(String owner, double leftPower, double rightPower, double driveTime, TrcEvent event)
    This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
    private void
    updateOdometry(TrcDriveBase.Odometry delta, double angle)
    This method updates the current robot odometry with the delta either using 0 or 1st order dynamics depending on the value of USE_CURVED_PATH.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

    Methods inherited from interface trclib.robotcore.TrcExclusiveSubsystem

    acquireExclusiveAccess, acquireOwnership, cancelExclusiveAccess, getCurrentOwner, hasOwnership, releaseExclusiveAccess, releaseOwnership, validateOwnership

  • Field Details

    • moduleName

      private final String moduleName

    • USE_CURVED_PATH

      private static final boolean USE_CURVED_PATH
      See Also:

    • SYNC_GYRO_DATA

      private static final boolean SYNC_GYRO_DATA
      See Also:

    • DEF_SENSITIVITY

      private static final double DEF_SENSITIVITY
      See Also:

    • tracer

      public final TrcDbgTrace tracer

    • motors

      private final TrcMotor[] motors

    • gyro

      private final TrcGyro gyro

    • odometry

      protected final TrcDriveBase.Odometry odometry

    • motorsState

      private final TrcDriveBase.MotorsState motorsState

    • driveTimer

      private final TrcTimer driveTimer

    • driveTimerEvent

      private TrcEvent driveTimerEvent

    • odometryTaskObj

      private final TrcTaskMgr.TaskObject odometryTaskObj

    • xScale

      protected double xScale

    • yScale

      protected double yScale

    • angleScale

      protected double angleScale

    • referenceOdometryStack

      private final Stack<TrcDriveBase.Odometry> referenceOdometryStack

    • driveOrientation

      private TrcDriveBase.DriveOrientation driveOrientation

    • fieldForwardHeading

      private double fieldForwardHeading

    • driveOwner

      private String driveOwner

    • stallStartTime

      protected double stallStartTime

    • stallVelThreshold

      protected double stallVelThreshold

    • odometryType

      private TrcDriveBase.OdometryType odometryType

    • odometryWheels

      private TrcOdometryWheels odometryWheels

    • absoluteOdometry

      private TrcDriveBaseOdometry absoluteOdometry

    • absOdoHeadingWrapConverter

      private TrcWrapValueConverter absOdoHeadingWrapConverter

    • motorPowerMapper

      protected TrcDriveBase.MotorPowerMapper motorPowerMapper

    • sensitivity

      private double sensitivity

    • gyroAssistPidCtrl

      private TrcPidController gyroAssistPidCtrl

    • gyroAssistHeading

      private Double gyroAssistHeading

    • robotTurning

      private boolean robotTurning

    • xTippingPidCtrl

      private TrcPidController xTippingPidCtrl

    • yTippingPidCtrl

      private TrcPidController yTippingPidCtrl

    • xTippingTolerance

      private double xTippingTolerance

    • yTippingTolerance

      private double yTippingTolerance

    • antiTippingEnabled

      private boolean antiTippingEnabled

    • referenceOdometry

      private TrcDriveBase.Odometry referenceOdometry

    • synchronizeOdometries

      private boolean synchronizeOdometries

    • enuToNwuChangeOfBasis

      private final org.apache.commons.math3.linear.RealMatrix enuToNwuChangeOfBasis

    • nwuToEnuChangeOfBasis

      private final org.apache.commons.math3.linear.RealMatrix nwuToEnuChangeOfBasis

  • Constructor Details

    • TrcDriveBase

      public TrcDriveBase(TrcMotor[] motors, TrcGyro gyro)
      Constructor: Create an instance of the object.
      Parameters:
      motors - specifies the array of motors in the drive base.
      gyro - specifies the gyro. If none, it can be set to null.

    • TrcDriveBase

      public TrcDriveBase(TrcMotor... motors)
      Constructor: Create an instance of the object.
      Parameters:
      motors - specifies the array of motors in the drive base.

  • Method Details

    • tankDrive

      public abstract void tankDrive(String owner, double leftPower, double rightPower, boolean inverted, double driveTime, TrcEvent event)
      This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      leftPower - specifies left power value.
      rightPower - specifies right power value.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • getOdometryDelta

      protected abstract TrcDriveBase.Odometry getOdometryDelta(TrcOdometrySensor.Odometry[] prevOdometries, TrcOdometrySensor.Odometry[] currOdometries)
      This method is called periodically to calculate the delta between the previous and current motor odometries.
      Parameters:
      prevOdometries - specifies the previous motor odometries.
      currOdometries - specifies the current motor odometries.
      Returns:
      an Odometry object describing the odometry changes since the last update.

    • getMotors

      public TrcMotor[] getMotors()
      This method returns an array of motors on the drive base.
      Returns:
      motor array.

    • setDriveTime

      protected void setDriveTime(String owner, double driveTime, TrcEvent event)
      This method is called by the subclass to set a drive time timer.
      Parameters:
      owner - specifies the owner's ID string to be used to stop the drivebase, can be null if drivebase is not owned.
      driveTime - specifies the drive time in seconds after which the drive base is stopped.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • driveTimerHandler

      private void driveTimerHandler(Object context)
      This method is called when the drivebase drive timer has expired. It will stop the drivebase.
      Parameters:
      context - specifies the timer object (not used).

    • setDriveOrientation

      public void setDriveOrientation(TrcDriveBase.DriveOrientation orientation, boolean resetHeading)
      This method sets the drive orientation mode.
      Parameters:
      orientation - specifies the drive orientation (FIELD, ROBOT, INVERTED).
      resetHeading - specifies true to reset robot heading, false otherwise (only applicable for FIELD mode).

    • getDriveOrientation

      public TrcDriveBase.DriveOrientation getDriveOrientation()
      This method returns the current drive orientation mode.

    • getDriveGyroAngle

      public double getDriveGyroAngle()
      This method returns robot heading to be maintained in teleop drive according to drive orientation mode.
      Returns:
      robot heading to be maintained.

    • isSynchronizeOdometriesEnabled

      public boolean isSynchronizeOdometriesEnabled()
      This method checks if synchronize odometries is enabled.
      Returns:
      true if it is enabled, false if it is disabled.

    • setSynchronizeOdometriesEnabled

      public void setSynchronizeOdometriesEnabled(boolean synchronizeOdometries)
      This method enables/disables synchronize odometries.
      Parameters:
      synchronizeOdometries - specifies true to enable, false to disable.

    • setMotorOdometryEnabled

      private void setMotorOdometryEnabled(boolean enabled, boolean resetHardware)
      This method enables/disables drive motor odometry.
      Parameters:
      enabled - specifies true to enable drive motor odometry, false to disable.
      resetHardware - specifies true to also reset motor encoder when enabling, false otherwise.

    • setOdometryEnabled

      public void setOdometryEnabled(boolean enabled, boolean resetHardware)
      This method is called to enable/disable the odometry task that keeps track of the robot position and orientation.
      Parameters:
      enabled - specifies true to enable, false to disable.
      resetHardware - specifies true to reset odometry hardware, false otherwise. This is only applicable when enabling odometry, not used when disabling.

    • setOdometryEnabled

      public void setOdometryEnabled(boolean enabled)
      This method is called to enable/disable the odometry task that keeps track of the robot position and orientation.
      Parameters:
      enabled - specifies true to enable, false to disable.

    • getFieldPosition

      public TrcPose2D getFieldPosition()
      This method returns the robot position in reference to the field origin. By default, the field origin is the robot's starting position.
      Returns:
      a copy of the robot position relative to the field origin.

    • getFieldVelocity

      public TrcPose2D getFieldVelocity()
      This method returns the robot velocity in reference to the field origin. By default, the field origin is the robot's starting position.
      Returns:
      a copy of the robot velocity relative to the field origin.

    • setFieldPosition

      public void setFieldPosition(TrcPose2D pose, boolean positionOnly)
      This method sets the robot's absolute field position to the given pose. This can be used to set the robot's starting position relative to the field origin.
      Parameters:
      pose - specifies the absolute position of the robot relative to the field origin.
      positionOnly - specifies true for setting position only but not heading, false to also set heading.

    • setFieldPosition

      public void setFieldPosition(TrcPose2D pose)
      This method sets the robot's absolute field position to the given pose. This can be used to set the robot's starting position relative to the field origin.
      Parameters:
      pose - specifies the absolute position of the robot relative to the field origin.

    • getPositionRelativeTo

      public TrcPose2D getPositionRelativeTo(TrcPose2D posPose, boolean transformAngle)
      This method returns the robot position relative to pose.
      Parameters:
      posPose - specifies the position to be referenced to.
      transformAngle - specifies true to also transform angle, false to leave it alone.
      Returns:
      position transformed into the new reference pose.

    • getPositionRelativeTo

      public TrcPose2D getPositionRelativeTo(TrcPose2D posPose)
      This method returns the robot position relative to pose.
      Parameters:
      posPose - specifies the position to be referenced to.
      Returns:
      position transformed into the new reference pose.

    • getVelocityRelativeTo

      public TrcPose2D getVelocityRelativeTo(TrcPose2D velPose, double refAngle)
      This method returns the robot velocity relative to pose.
      Parameters:
      velPose - specifies the velocity to be referenced to.
      refAngle - specifies the reference angle to be relative to.
      Returns:
      velocity transformed into the new reference pose.

    • getRelativePosition

      public TrcPose2D getRelativePosition()
      This method returns the robot position relative to the reference position, or the robot field position if there is no reference odometry set.
      Returns:
      robot position relative to the reference position, or robot field position if no reference odometry set.

    • getRelativeVelocity

      public TrcPose2D getRelativeVelocity()
      This method returns the robot velocity relative to the reference velocity, or the robot field velocity if there is no reference odometry set.
      Returns:
      robot velocity relative to the reference velocity, or robot field velocity if no reference odometry set.

    • getReferenceOdometry

      public TrcDriveBase.Odometry getReferenceOdometry()
      This method returns the reference odometry if there is any.
      Returns:
      the reference odometry.

    • setReferenceOdometry

      public void setReferenceOdometry()
      This method sets the current robot position and velocity as the reference odometry. All relative positions and velocities will be relative to this reference odometry.

    • clearReferenceOdometry

      public void clearReferenceOdometry()
      This method clears the reference odometry. All relative positions and velocities will instead be absolute positions and velocities.

    • pushReferenceOdometry

      public void pushReferenceOdometry()
      This method pushes the existing reference odometry onto the stack if there is one and set the given robot's absolute odometry as the new reference odometry.

    • popReferenceOdometry

      public TrcDriveBase.Odometry popReferenceOdometry()
      This method returns the current reference odometry. If the reference stack is not empty, it pops an odometry from the stack as the new reference odometry, otherwise the reference odometry is cleared.
      Returns:
      current reference odometry.

    • setOdometryScales

      public void setOdometryScales(double xScale, double yScale, double angleScale)
      This method sets the position scales. The raw position from the encoder is in encoder counts. By setting the scale factor, one could make getPosition to return unit in inches, for example.
      Parameters:
      xScale - specifies the X position scale.
      yScale - specifies the Y position scale.
      angleScale - specifies the angle scale.

    • setOdometryScales

      public void setOdometryScales(double xScale, double yScale)
      This method sets the position scales. The raw position from the encoder is in encoder counts. By setting the scale factor, one could make getPosition to return unit in inches, for example.
      Parameters:
      xScale - specifies the X position scale.
      yScale - specifies the Y position scale.

    • setOdometryScales

      public void setOdometryScales(double yScale)
      This method sets the position scales. The raw position from the encoder is in encoder counts. By setting the scale factor, one could make getPosition to return unit in inches, for example.
      Parameters:
      yScale - specifies the Y position scale.

    • getXPosition

      public double getXPosition()
      This method returns the X position in scaled unit.
      Returns:
      X position.

    • getYPosition

      public double getYPosition()
      This method returns the Y position in scaled unit.
      Returns:
      Y position.

    • getHeading

      public double getHeading()
      This method returns the heading of the drive base in degrees. If there is a gyro, the gyro heading is returned, otherwise it returns the rotation position by using the encoders.
      Returns:
      drive base heading.

    • getXVelocity

      public double getXVelocity()
      This method returns the drive base velocity in the X direction.
      Returns:
      X velocity.

    • getYVelocity

      public double getYVelocity()
      This method returns the drive base velocity in the Y direction.
      Returns:
      Y velocity.

    • getTurnRate

      public double getTurnRate()
      This method returns the drive base turn rate.
      Returns:
      turn rate.

    • resetOdometry

      public void resetOdometry(boolean resetPositionOdometry, boolean resetHeadingOdometry, boolean resetHardware)
      This method resets the drive base odometry. This includes the motor encoders, drive base position, velocity and gyro heading.
      Parameters:
      resetPositionOdometry - specifies true for resetting position odometry, false otherwise.
      resetHeadingOdometry - specifies true to also reset the heading odometry, false otherwise.
      resetHardware - specifies true to do a hardware reset, false to do a soft reset.

    • resetOdometry

      public void resetOdometry(boolean resetHardware)
      This method resets the drive base odometry. This includes the motor encoders, drive base position, velocity and gyro heading.
      Parameters:
      resetHardware - specifies true for resetting hardware position, false for resetting software position.

    • resetOdometry

      public void resetOdometry()
      This method resets the drive base odometry. This includes the motor encoders, drive base position, velocity and gyro heading.

    • setDriveBaseOdometry

      public void setDriveBaseOdometry(boolean resetHardware)
      This method sets the built-in motor odometry as the drive base's odometry device.
      Parameters:
      resetHardware - specifies true for resetting hardware position, false for resetting software position.

    • setDriveBaseOdometry

      public void setDriveBaseOdometry(TrcOdometryWheels odometryWheels)
      This method sets the given odometry device as the drive base's odometry device overriding the built-in odometry.
      Parameters:
      odometryWheels - specifies the odometry wheels as the drive base odometry device.

    • setDriveBaseOdometry

      public void setDriveBaseOdometry(TrcDriveBaseOdometry absoluteOdometry, Double headingWrapRangeLow, Double headingWrapRangeHigh)
      This method sets the given odometry device as the drive base's odometry device overriding the built-in odometry.
      Parameters:
      absoluteOdometry - specifies the drive base absolute odometry device.
      headingWrapRangeLow - specifies the low value of the heading wrap range, can be null if no wrapping.
      headingWrapRangeHigh - specifies the high value of the heading wrap range, can be null if no wrapping.

    • printMotorsState

      public void printMotorsState()
      This method is called to print the state info of all motors on the drive base for debugging purpose.

    • setMotorPowerMapper

      public void setMotorPowerMapper(TrcDriveBase.MotorPowerMapper motorPowerMapper)
      This method sets a motor power mapper. If null, it unsets the previously set mapper.
      Parameters:
      motorPowerMapper - specifies the motor power mapper. If null, clears the mapper.

    • setSensitivity

      public void setSensitivity(double sensitivity)
      This method sets the sensitivity for the drive() method.
      Parameters:
      sensitivity - specifies the sensitivity value.

    • setGyroAssistEnabled

      public void setGyroAssistEnabled(TrcPidController turnPidCtrl)
      This method enables/disables gyro assist drive.
      Parameters:
      turnPidCtrl - specifies the turn PID controller to enable GyroAssist, null to disable.

    • isGyroAssistEnabled

      public boolean isGyroAssistEnabled()
      This method checks if Gyro Assist is enabled.
      Returns:
      true if Gyro Assist is enabled, false otherwise.

    • getGyroAssistPower

      protected double getGyroAssistPower(double turnPower)
      This method calculates and returns the gyro assist power to be added to turnPower to compensate heading drift.
      Parameters:
      turnPower - specifies the turn power.
      Returns:
      gyro assist power.

    • enableAntiTipping

      public void enableAntiTipping(TrcPidController.PidCoefficients xTippingPidCoeffs, double xTolerance, TrcPidController.PidInput xPidInput, TrcPidController.PidCoefficients yTippingPidCoeffs, double yTolerance, TrcPidController.PidInput yPidInput)
      This method enables anti-tipping drive.
      Parameters:
      xTippingPidCoeffs - specifies the anti-tipping PID controller coefficients for the X direction, null for non-holonomic drive base.
      xTolerance - specifies the X tipping tolerance.
      xPidInput - specifies the X PidInput provider.
      yTippingPidCoeffs - specifies the anti-tipping PID controller coefficients for the Y direction.
      yTolerance - specifies the Y tipping tolerance.
      yPidInput - specifies the Y PidInput provider.

    • enableAntiTipping

      public void enableAntiTipping(TrcPidController.PidCoefficients yTippingPidCoeffs, double yTolerance, TrcPidController.PidInput yPidInput)
      This method enables anti-tipping drive.
      Parameters:
      yTippingPidCoeffs - specifies the anti-tipping PID controller coefficients for the Y direction.
      yTolerance - specifies the Y tipping tolerance.
      yPidInput - specifies the Y PidInput provider.

    • disableAntiTipping

      public void disableAntiTipping()
      This method disables anti-tipping drive.

    • isAntiTippingEnabled

      public boolean isAntiTippingEnabled()
      This method checks if anti-tipping drive is enabled.
      Returns:
      true if anti-tipping drive is enabled, false otherwise.

    • getAntiTippingPower

      public double getAntiTippingPower(boolean xTippingControl)
      This method calculates and returns the anti-tipping power.
      Parameters:
      xTippingControl - specifies true for the X anti-tipping power, false for the Y anti-tipping power.
      Returns:
      calculated anti-tipping power.

    • supportsHolonomicDrive

      public boolean supportsHolonomicDrive()
      This method checks if it supports holonomic drive. Subclasses that support holonomic drive should override this method.
      Returns:
      true if this drive base supports holonomic drive, false otherwise.

    • getNumMotors

      public int getNumMotors()
      This method returns the number of motors in the drive train.
      Returns:
      number of motors.

    • setInvertedMotor

      protected void setInvertedMotor(int index, boolean inverted)
      This method inverts direction of a given motor in the drive train.
      Parameters:
      index - specifies the index in the motors array.
      inverted - specifies true if inverting motor direction.

    • setStallVelocityThreshold

      protected void setStallVelocityThreshold(double stallVelThreshold)
      This method is called by the subclass to set the stall detection velocity threshold value.
      Parameters:
      stallVelThreshold - specifies the stall detection velocity threshold value.

    • isStalled

      public boolean isStalled(double stallTime)
      This method determines if the drive base is stalled. A drive base is in stalled state when there is power applied to the wheels and odometry has no movement for the given period of time.
      Parameters:
      stallTime - specifies the stall time in seconds.
      Returns:
      true if the drive base is in stalled state, false otherwise.

    • setBrakeMode

      public void setBrakeMode(boolean enabled)
      This method enables/disables brake mode of the drive base.
      Parameters:
      enabled - specifies true to enable brake mode, false to disable it.

    • stop

      public void stop(String owner)
      This methods stops the drive base.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.

    • stop

      public void stop()
      This methods stops the drive base.

    • tankDrive

      public void tankDrive(String owner, double leftPower, double rightPower, double driveTime, TrcEvent event)
      This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      leftPower - specifies left power value.
      rightPower - specifies right power value.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • tankDrive

      public void tankDrive(String owner, double leftPower, double rightPower)
      This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      leftPower - specifies left power value.
      rightPower - specifies right power value.

    • tankDrive

      public void tankDrive(double leftPower, double rightPower, double driveTime, TrcEvent event)
      This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
      Parameters:
      leftPower - specifies left power value.
      rightPower - specifies right power value.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • tankDrive

      public void tankDrive(double leftPower, double rightPower)
      This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
      Parameters:
      leftPower - specifies left power value.
      rightPower - specifies right power value.

    • tankDrive

      public void tankDrive(double leftPower, double rightPower, boolean inverted)
      This method implements tank drive where leftPower controls the left motors and right power controls the right motors.
      Parameters:
      leftPower - specifies left power value.
      rightPower - specifies right power value.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).

    • curveDrive

      public void curveDrive(String owner, double magnitude, double curve, boolean inverted, double driveTime, TrcEvent event)
      This method drives the motors at "magnitude" and "curve". Both magnitude and curve are -1.0 to +1.0 values, where 0.0 represents stopped and not turning. curve less than 0 will turn left and curve greater than 0 will turn right. The algorithm for steering provides a constant turn radius for any normal speed range, both forward and backward. Increasing sensitivity causes sharper turns for fixed values of curve.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      magnitude - specifies the speed setting for the outside wheel in a turn, forward or backwards, +1 to -1.
      curve - specifies the rate of turn, constant for different forward speeds. Set curve less than 0 for left turn or curve greater than 0 for right turn. Set curve = e^(-r/w) to get a turn radius r for wheel base w of your robot. Conversely, turn radius r = -ln(curve)*w for a given value of curve and wheel base w.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • curveDrive

      public void curveDrive(String owner, double magnitude, double curve, boolean inverted)
      This method drives the motors at "magnitude" and "curve". Both magnitude and curve are -1.0 to +1.0 values, where 0.0 represents stopped and not turning. curve less than 0 will turn left and curve greater than 0 will turn right. The algorithm for steering provides a constant turn radius for any normal speed range, both forward and backward. Increasing sensitivity causes sharper turns for fixed values of curve.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      magnitude - specifies the speed setting for the outside wheel in a turn, forward or backwards, +1 to -1.
      curve - specifies the rate of turn, constant for different forward speeds. Set curve less than 0 for left turn or curve greater than 0 for right turn. Set curve = e^(-r/w) to get a turn radius r for wheel base w of your robot. Conversely, turn radius r = -ln(curve)*w for a given value of curve and wheel base w.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).

    • curveDrive

      public void curveDrive(double magnitude, double curve, boolean inverted, double driveTime, TrcEvent event)
      This method drives the motors at "magnitude" and "curve". Both magnitude and curve are -1.0 to +1.0 values, where 0.0 represents stopped and not turning. curve less than 0 will turn left and curve greater than 0 will turn right. The algorithm for steering provides a constant turn radius for any normal speed range, both forward and backward. Increasing sensitivity causes sharper turns for fixed values of curve.
      Parameters:
      magnitude - specifies the speed setting for the outside wheel in a turn, forward or backwards, +1 to -1.
      curve - specifies the rate of turn, constant for different forward speeds. Set curve less than 0 for left turn or curve greater than 0 for right turn. Set curve = e^(-r/w) to get a turn radius r for wheel base w of your robot. Conversely, turn radius r = -ln(curve)*w for a given value of curve and wheel base w.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • curveDrive

      public void curveDrive(double magnitude, double curve, double driveTime, TrcEvent event)
      This method drives the motors with the given magnitude and curve values.
      Parameters:
      magnitude - specifies the magnitude value.
      curve - specifies the curve value.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • curveDrive

      public void curveDrive(double magnitude, double curve)
      This method drives the motors with the given magnitude and curve values.
      Parameters:
      magnitude - specifies the magnitude value.
      curve - specifies the curve value.

    • arcadeDrive

      public void arcadeDrive(String owner, double drivePower, double turnPower, boolean inverted, double driveTime, TrcEvent event)
      This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      drivePower - specifies the drive power value.
      turnPower - specifies the turn power value.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • arcadeDrive

      public void arcadeDrive(String owner, double drivePower, double turnPower, boolean inverted)
      This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      drivePower - specifies the drive power value.
      turnPower - specifies the turn power value.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).

    • arcadeDrive

      public void arcadeDrive(double drivePower, double turnPower, boolean inverted, double driveTime, TrcEvent event)
      This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
      Parameters:
      drivePower - specifies the drive power value.
      turnPower - specifies the turn power value.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • arcadeDrive

      public void arcadeDrive(double drivePower, double turnPower, double driveTime, TrcEvent event)
      This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
      Parameters:
      drivePower - specifies the drive power value.
      turnPower - specifies the turn power value.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • arcadeDrive

      public void arcadeDrive(double drivePower, double turnPower)
      This method implements arcade drive where drivePower controls how fast the robot goes in the y-axis and turnPower controls how fast it will turn.
      Parameters:
      drivePower - specifies the drive power value.
      turnPower - specifies the turn power value.

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation, boolean inverted, double gyroAngle, double driveTime, TrcEvent event)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain. Subclasses that supports holonomic drive should override this method.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      gyroAngle - specifies the current gyro heading. Use this to drive by the field reference frame.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation, boolean inverted, double gyroAngle)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain. Subclasses that supports holonomic drive should override this method.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      gyroAngle - specifies the current gyro heading. Use this to drive by the field reference frame.

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation, boolean inverted, double driveTime, TrcEvent event)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation, boolean inverted)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.

    • holonomicDrive

      public void holonomicDrive(double x, double y, double rotation, boolean inverted, double driveTime, TrcEvent event)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive

      public void holonomicDrive(double x, double y, double rotation, boolean inverted)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation, double gyroAngle, double driveTime, TrcEvent event)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      gyroAngle - specifies the current gyro heading. Use this to drive by the field reference frame.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation, double gyroAngle)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      gyroAngle - specifies the current gyro heading. Use this to drive by the field reference frame.

    • holonomicDrive

      public void holonomicDrive(double x, double y, double rotation, double gyroAngle, double driveTime, TrcEvent event)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      gyroAngle - specifies the current gyro heading. Use this to drive by the field reference frame.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive

      public void holonomicDrive(String owner, double x, double y, double rotation, double driveTime, TrcEvent event)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive

      public void holonomicDrive(double x, double y, double rotation, double driveTime, TrcEvent event)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive

      public void holonomicDrive(double x, double y, double rotation)
      This method implements holonomic drive where x controls how fast the robot will go in the x direction, and y controls how fast the robot will go in the y direction. Rotation controls how fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
      Parameters:
      x - specifies the x power.
      y - specifies the y power.
      rotation - specifies the rotating power.

    • holonomicDrive_Polar

      public void holonomicDrive_Polar(String owner, double magnitude, double direction, double rotation, boolean inverted, double driveTime, TrcEvent event)
      This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      magnitude - specifies the magnitude combining x and y axes.
      direction - specifies the direction in degrees. 0 is forward. Positive is clockwise.
      rotation - specifies the rotation power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive_Polar

      public void holonomicDrive_Polar(double magnitude, double direction, double rotation, boolean inverted, double driveTime, TrcEvent event)
      This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
      Parameters:
      magnitude - specifies the magnitude combining x and y axes.
      direction - specifies the direction in degrees.
      rotation - specifies the rotation power.
      inverted - specifies true to invert control (i.e. robot front becomes robot back).
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive_Polar

      public void holonomicDrive_Polar(String owner, double magnitude, double direction, double rotation, double gyroAngle, double driveTime, TrcEvent event)
      This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      magnitude - specifies the magnitude combining x and y axes.
      direction - specifies the direction in degrees.
      rotation - specifies the rotation power.
      gyroAngle - specifies the current gyro heading. Use this to drive by the field reference frame.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive_Polar

      public void holonomicDrive_Polar(double magnitude, double direction, double rotation, double gyroAngle, double driveTime, TrcEvent event)
      This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
      Parameters:
      magnitude - specifies the magnitude combining x and y axes.
      direction - specifies the direction in degrees. 0 is forward. Positive is clockwise.
      rotation - specifies the rotation power.
      gyroAngle - specifies the current gyro heading. Use this to drive by the field reference frame.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive_Polar

      public void holonomicDrive_Polar(double magnitude, double direction, double rotation, double gyroAngle)
      This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
      Parameters:
      magnitude - specifies the magnitude combining x and y axes.
      direction - specifies the direction in degrees. 0 is forward. Positive is clockwise.
      rotation - specifies the rotation power.
      gyroAngle - specifies the current gyro heading. Use this to drive by the field reference frame.

    • holonomicDrive_Polar

      public void holonomicDrive_Polar(String owner, double magnitude, double direction, double rotation, double driveTime, TrcEvent event)
      This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
      Parameters:
      owner - specifies the ID string of the caller for checking ownership, can be null if caller is not ownership aware.
      magnitude - specifies the magnitude combining x and y axes.
      direction - specifies the direction in degrees.
      rotation - specifies the rotation power.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive_Polar

      public void holonomicDrive_Polar(double magnitude, double direction, double rotation, double driveTime, TrcEvent event)
      This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
      Parameters:
      magnitude - specifies the magnitude combining x and y axes.
      direction - specifies the direction in degrees. 0 is forward. Positive is clockwise.
      rotation - specifies the rotation power.
      driveTime - specifies the amount of time in seconds after which the drive base will stop.
      event - specifies the event to signal when driveTime has expired, can be null if not provided.

    • holonomicDrive_Polar

      public void holonomicDrive_Polar(double magnitude, double direction, double rotation)
      This method implements holonomic drive where magnitude controls how fast the robot will go in the given direction and how fast it will rotate.
      Parameters:
      magnitude - specifies the magnitude combining x and y axes.
      direction - specifies the direction in degrees. 0 is forward. Positive is clockwise.
      rotation - specifies the rotation power.

    • odometryTask

      private void odometryTask(TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop)
      This method is called periodically to update the drive base odometry (position and velocity of both x, y and angle).
      Parameters:
      taskType - specifies the type of task being run.
      runMode - specifies the competition mode that is about to end (e.g. Autonomous, TeleOp, Test).
      slowPeriodicLoop - specifies true if it is running the slow periodic loop on the main robot thread, false otherwise.

    • stopTask

      private void stopTask(TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop)
      This method is called when the competition mode is about to end to stop the drive base.
      Parameters:
      taskType - specifies the type of task being run.
      runMode - specifies the competition mode that is about to end (e.g. Autonomous, TeleOp, Test).
      slowPeriodicLoop - specifies true if it is running the slow periodic loop on the main robot thread, false otherwise.

    • synchronizeOdometries

      private void synchronizeOdometries(TrcOdometrySensor.Odometry[] odometries)
      This method synchronizes the odometries of all the drive base motors. Since all motors are read at different times, the delay may cause inconsistencies on the odometry data which will affect the accuracy of the drive base odometry calculation. We will make all motors based off of the timestamp of the last motor and their position data will be interpolated accordingly. In effect, we are fast forwarding the other motors to sync with the latest timestamp and predicting their positions with their velocity info.
      Parameters:
      odometries - specifies the array of odometries of all drive base motors.

    • updateOdometry

      private void updateOdometry(TrcDriveBase.Odometry delta, double angle)
      This method updates the current robot odometry with the delta either using 0 or 1st order dynamics depending on the value of USE_CURVED_PATH. If true, use a curved path (with nonzero curvature) otherwise model path as a bunch of straight lines. The curved path is more accurate.
      Parameters:
      delta - specifies the odometry delta since the last update.
      angle - specifies the robot angle in the last update.