Added drive commands to test sequence

lasarobotics · Apr 1, 2024 · e852993 · e852993
1 parent 3d488c0
commit e852993
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 5 deletions.
diff --git 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
@@ -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)
     );
   }