From e852993d27bbc3f0bdfd3c4605c8294882f1c8c3 Mon Sep 17 00:00:00 2001
From: Arun Varadarajan <arunieboy@gmail.com>
Date: Mon, 1 Apr 2024 16:50:43 -0500
Subject: [PATCH] Added drive commands to test sequence

---
 src/main/java/frc/robot/Constants.java      |  1 +
 src/main/java/frc/robot/RobotContainer.java | 51 +++++++++++++++++++--
 2 files changed, 47 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index a3f357a..6d0e43e 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -101,6 +101,7 @@ public static class AutoNames {
     public static final Pair<String, String> RIGHT_FARDISRUPT_FARTOP_AUTO_NAME = new Pair<String, String>("Disrupt auto (amp side - source side)", "Right_FarDisrupt_FarTop");
     public static final Pair<String, String> SIDEWAYS_AUTO_NAME  = new Pair<String, String>("[Pose-estimation] Go sideways", "sideways auto");
     public static final Pair<String, String> TEST_180_FAR_PATH_AUTO_NAME  = new Pair<String, String>("[Pose-estimation] Go amp side and flip 180", "test 180 far path");
+    public static final double TEST_COMMAND_TIME = 5.0;
   }
 
   public static class Drive {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index eb9cf1f..f1ca205 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -423,15 +423,56 @@ public Command getAutonomousCommand() {
    */
   public Command getTestCommand() {
     return Commands.sequence(
-      intakeCommand().withTimeout(5),
+      Commands.print("Intaking..."),
+      intakeCommand().withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Moving shooter down..."),
       SHOOTER_SUBSYSTEM.shootManualCommand(
         new State(ShooterSubsystem.ZERO_FLYWHEEL_SPEED, Units.Radians.of(Constants.Shooter.ANGLE_CONFIG.getLowerLimit()))
-      ).withTimeout(5),
+      ).withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Moving shooter up..."),
       SHOOTER_SUBSYSTEM.shootManualCommand(
         new State(ShooterSubsystem.ZERO_FLYWHEEL_SPEED, Units.Radians.of(Constants.Shooter.ANGLE_CONFIG.getUpperLimit()))
-      ).withTimeout(5),
-      SHOOTER_SUBSYSTEM.scoreAmpCommand().withTimeout(5),
-      SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(5)
+      ).withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Scoring amp..."),
+      SHOOTER_SUBSYSTEM.scoreAmpCommand().withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Scoring speaker..."),
+      SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Driving forwards..."),
+      DRIVE_SUBSYSTEM.driveCommand(
+        () -> 0.1,
+        () -> 0.0,
+        () -> 0.0
+      ).withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Driving backwards..."),
+      DRIVE_SUBSYSTEM.driveCommand(
+        () -> -0.1,
+        () -> 0.0,
+        () -> 0.0
+      ).withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Driving left..."),
+      DRIVE_SUBSYSTEM.driveCommand(
+        () -> 0.0,
+        () -> 0.1,
+        () -> 0.0
+      ).withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Driving right..."),
+      DRIVE_SUBSYSTEM.driveCommand(
+        () -> 0.0,
+        () -> -0.1,
+        () -> 0.0
+      ).withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Rotating left..."),
+      DRIVE_SUBSYSTEM.driveCommand(
+        () -> 0.0,
+        () -> 0.0,
+        () -> 0.1
+      ).withTimeout(Constants.AutoNames.TEST_COMMAND_TIME),
+      Commands.print("Rotating right..."),
+      DRIVE_SUBSYSTEM.driveCommand(
+        () -> 0.0,
+        () -> 0.0,
+        () -> -0.1
+      ).withTimeout(Constants.AutoNames.TEST_COMMAND_TIME)
     );
   }