diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 618b884..fc22107 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,11 @@ public class RobotContainer { } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return drivetrain.tankDrive(() -> .5, () -> .5) + .withTimeout(2.5) + .andThen( + drivetrain.tankDrive(() -> -.4, () -> .4).withTimeout(1), + drivetrain.tankDrive(() -> .5, () -> .5).withTimeout(1.5) + ); } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index d193cc3..fc2d42e 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -37,4 +37,10 @@ public class Drivetrain extends SubsystemBase { dt.arcadeDrive(forwardBackward.getAsDouble(), leftRight.getAsDouble()); }); } + + public Command tankDrive(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) { + return run(() -> { + dt.tankDrive(leftSpeed.getAsDouble(), rightSpeed.getAsDouble()); + }); + } }