Class TankDrive
- java.lang.Object
-
- edu.wpi.first.wpilibj2.command.SubsystemBase
-
- com.alumiboti5590.eop.subsystems.drivetrain.tank.TankDrive
-
- All Implemented Interfaces:
edu.wpi.first.util.sendable.Sendable
,edu.wpi.first.wpilibj2.command.Subsystem
- Direct Known Subclasses:
SparkMaxBrushlessTankDrive
,TalonSRXBrushedTankDrive
public abstract class TankDrive extends edu.wpi.first.wpilibj2.command.SubsystemBase
TankDrive abstracts out the control methods needed for a Tank Drive chassis subsystem, where each side of the robot acts as it's own system, similar to a tank - hence the name. For the most part, this class just wraps the DifferentialDrive and a few other pieces nicely. You will have to use one of the more concrete implementations to configure the motors and other components correctly.
-
-
Nested Class Summary
Nested Classes Modifier and Type Class Description static class
TankDrive.TeleopDriveType
TeleopDriveType is an enumeration that allows for selecting the drive type during the tele-operated period for a tank drive chassis.
-
Field Summary
Fields Modifier and Type Field Description static TankDrive.TeleopDriveType
DEFAULT_TELEOP_DRIVE_TYPE
The default drive type to use if one is not selected in the SmartDashboard
-
Constructor Summary
Constructors Constructor Description TankDrive()
-
Method Summary
All Methods Instance Methods Abstract Methods Concrete Methods Modifier and Type Method Description protected void
configureDifferentialDrive(edu.wpi.first.wpilibj.motorcontrol.MotorController leftLeader, edu.wpi.first.wpilibj.motorcontrol.MotorController rightLeader)
void
curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace, boolean squareInputs)
Given a movement speed and a rotational speed, move the robot.edu.wpi.first.wpilibj.smartdashboard.SendableChooser<TankDrive.TeleopDriveType>
getTeleopDriveTypeSendableChooser()
Returns the SendableChooser needed to pick the desired drive type from the SmartDashboard / Shuffleboardabstract void
resetOdometry()
Reset the encoders and gyroscope readings.abstract void
setAccelerationRampRate(double maxAccRate)
Sets the ramp rate for open loop control modes.abstract void
setCurrentLimit(int currentInAmps)
Sets the current limit in Amps.TankDrive
setCurvatureQuickTurnSupplier(java.util.function.BooleanSupplier quickTurnSupplier)
Set if the robot should be allowed to 'quick turn' in curvature modeTankDrive
setCurvatureSpeedSupplier(java.util.function.DoubleSupplier speedProvider)
Set the speed supplier for curvature drive modeTankDrive
setCurvatureSteeringSupplier(java.util.function.DoubleSupplier steeringProvider)
Set the rotation supplier for curvature drive modeTankDrive
setInvertCurvatureSteering(boolean invertSteeringInput)
Sets if the curvature steering input should be invertedabstract void
setSidePIDGains(PIDGains gains)
Set the PID controller gains for both sides of the tank drive.TankDrive
setSquareTeleopInputs(boolean squareTeleopInputs)
If configured - which it is by default - then the speed inputs will be squared meaning less sensitivity at lower levels.TankDrive
setTankDriveLeftSpeedSupplier(java.util.function.DoubleSupplier speedProvider)
Set the speed supplier for the left-side of the tank drive controlTankDrive
setTankDriveRightSpeedSupplier(java.util.function.DoubleSupplier speedProvider)
Set the speed supplier for the right-side of the tank drive controlTankDrive
setTeleopDriveType(TankDrive.TeleopDriveType driveType)
Set the drive type to use in Tele-operated mode by the drivervoid
stop()
Stops both sides of the tank drive when called.void
tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs)
Given a speed for the left and right side, respectively, set the motor speeds as given.void
teleopOpenDrive()
The main method to control the drivetrain during teleop.-
Methods inherited from class edu.wpi.first.wpilibj2.command.SubsystemBase
addChild, getName, getSubsystem, initSendable, setName, setSubsystem
-
-
-
-
Field Detail
-
DEFAULT_TELEOP_DRIVE_TYPE
public static final TankDrive.TeleopDriveType DEFAULT_TELEOP_DRIVE_TYPE
The default drive type to use if one is not selected in the SmartDashboard
-
-
Method Detail
-
setCurvatureSpeedSupplier
public TankDrive setCurvatureSpeedSupplier(java.util.function.DoubleSupplier speedProvider)
Set the speed supplier for curvature drive mode- Parameters:
speedProvider
- a supplier that is used to control the speed in curvature mode- Returns:
- The TankDrive instance to be a part of a call chain
-
setCurvatureSteeringSupplier
public TankDrive setCurvatureSteeringSupplier(java.util.function.DoubleSupplier steeringProvider)
Set the rotation supplier for curvature drive mode- Parameters:
steeringProvider
- a supplier that is used to control the rotation in curvature mode- Returns:
- The TankDrive instance to be a part of a call chain
-
setCurvatureQuickTurnSupplier
public TankDrive setCurvatureQuickTurnSupplier(java.util.function.BooleanSupplier quickTurnSupplier)
Set if the robot should be allowed to 'quick turn' in curvature mode- Parameters:
quickTurnSupplier
- a supplier that is used to control 'quick turn' toggling in curvature mode- Returns:
- The TankDrive instance to be a part of a call chain
-
setTankDriveLeftSpeedSupplier
public TankDrive setTankDriveLeftSpeedSupplier(java.util.function.DoubleSupplier speedProvider)
Set the speed supplier for the left-side of the tank drive control- Parameters:
speedProvider
- a supplier that is used for driving the left-side in tank drive- Returns:
- The TankDrive instance to be a part of a call chain
-
setTankDriveRightSpeedSupplier
public TankDrive setTankDriveRightSpeedSupplier(java.util.function.DoubleSupplier speedProvider)
Set the speed supplier for the right-side of the tank drive control- Parameters:
speedProvider
- a supplier that is used for driving the right-side in tank drive- Returns:
- The TankDrive instance to be a part of a call chain
-
setInvertCurvatureSteering
public TankDrive setInvertCurvatureSteering(boolean invertSteeringInput)
Sets if the curvature steering input should be inverted- Parameters:
invertSteeringInput
- If the curvature steering input should be inverted- Returns:
- The TankDrive instance to be a part of a call chain
-
setSquareTeleopInputs
public TankDrive setSquareTeleopInputs(boolean squareTeleopInputs)
If configured - which it is by default - then the speed inputs will be squared meaning less sensitivity at lower levels.- Parameters:
squareTeleopInputs
- If the speed inputs should be squared- Returns:
- The TankDrive instance to be a part of a call chain
-
setTeleopDriveType
public TankDrive setTeleopDriveType(TankDrive.TeleopDriveType driveType)
Set the drive type to use in Tele-operated mode by the driver- Parameters:
driveType
- The drive type to use- Returns:
- The TankDrive instance to be a part of a call chain
-
getTeleopDriveTypeSendableChooser
public edu.wpi.first.wpilibj.smartdashboard.SendableChooser<TankDrive.TeleopDriveType> getTeleopDriveTypeSendableChooser()
Returns the SendableChooser needed to pick the desired drive type from the SmartDashboard / Shuffleboard- Returns:
- The SendableChooser that can be sent to the SmartDashboard
-
setAccelerationRampRate
public abstract void setAccelerationRampRate(double maxAccRate)
Sets the ramp rate for open loop control modes.This is the maximum rate at which the motor controller's output is allowed to change.
- Parameters:
maxAccRate
- Time in seconds to go from 0 to full throttle.
-
setCurrentLimit
public abstract void setCurrentLimit(int currentInAmps)
Sets the current limit in Amps.The motor controller will reduce the controller voltage output to avoid surpassing this limit. This limit is enabled by default and used for brushless only. This limit is highly recommended when brushless motors, but it also helps to avoid brownouts in any situation.
- Parameters:
currentInAmps
- The current limit in Amps.
-
setSidePIDGains
public abstract void setSidePIDGains(PIDGains gains)
Set the PID controller gains for both sides of the tank drive. Note that the actual effect will be determined by the motors used (Neos + Spark Max's have a lot more control because of their built-in encoder) but it tries to be fairly equal enough for a drivetrain to work.- Parameters:
gains
- desired PID gains
-
teleopOpenDrive
public void teleopOpenDrive()
The main method to control the drivetrain during teleop. It controls the chassis based on the current teleopDriveType and it requires the correct suppliers to be defined to fetch controller values.
-
curvatureDrive
public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace, boolean squareInputs)
Given a movement speed and a rotational speed, move the robot. 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.- Parameters:
xSpeed
- The robot's speed along the X axis [-1.0..1.0]. Forward is positive.zRotation
- The normalized curvature [-1.0..1.0]. Counterclockwise is positive.allowTurnInPlace
- If set, overrides constant-curvature turning for turn-in-place maneuvers. zRotation will control turning rate instead of curvature.squareInputs
- If set, decreases the input sensitivity at low speeds.
-
tankDrive
public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs)
Given a speed for the left and right side, respectively, set the motor speeds as given. This is the more difficult control mode for a drivetrain.- 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.
-
stop
public void stop()
Stops both sides of the tank drive when called.
-
resetOdometry
public abstract void resetOdometry()
Reset the encoders and gyroscope readings.
-
configureDifferentialDrive
protected void configureDifferentialDrive(edu.wpi.first.wpilibj.motorcontrol.MotorController leftLeader, edu.wpi.first.wpilibj.motorcontrol.MotorController rightLeader)
-
-