Re-add swerve to motor test command

grt192 · Mar 8, 2023 · 865d374 · 865d374
1 parent 35a3c2d
commit 865d374
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -122,19 +122,20 @@ public RobotContainer() {
         superstructure = new Superstructure(rollerSubsystem, tiltedElevatorSubsystem, signalLEDSubsystem, switchableCamera);
 
         balancerCommand = new DefaultBalancerCommand(driveSubsystem);
-        testCommand = new MotorTestCommand(driveSubsystem, tiltedElevatorSubsystem, rollerSubsystem);
 
         // Configure button bindings
         configureDriveBindings();
         configureMechBindings();
 
-        // Add auton sequences to the chooser and add the chooser to shuffleboard
+        // Initialize auton and test commands
         autonChooser = new SendableChooser<>();
         autonChooser.setDefaultOption("Skip auton", new InstantCommand());
 
         if (driveSubsystem instanceof BaseSwerveSubsystem) {
             final BaseSwerveSubsystem swerveSubsystem = (BaseSwerveSubsystem) driveSubsystem;
 
+            testCommand = new MotorTestCommand(swerveSubsystem, tiltedElevatorSubsystem, rollerSubsystem);
+
             autonChooser.addOption("Red preloaded only", new PreloadedOnlyAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
             autonChooser.addOption("Red top auton (1-piece)", new TopOnePieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
             // autonChooser.addOption("Red top auton (2-piece)", new TopTwoPieceAutonSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem, true));
@@ -160,6 +161,8 @@ public RobotContainer() {
             // autonChooser.addOption("Box auton", new BoxAutonSequence(swerveSubsystem));
             // autonChooser.addOption("No-stopping box auton", new ContinuousBoxAutonSequence(swerveSubsystem));
             // autonChooser.addOption("GRT path", new GRTAutonSequence(swerveSubsystem));
+        } else {
+            testCommand = null;
         }
 
         shuffleboardTab.add("Auton", autonChooser)

diff --git a/src/main/java/frc/robot/commands/pretest/MotorTestCommand.java b/src/main/java/frc/robot/commands/pretest/MotorTestCommand.java
@@ -8,7 +8,6 @@
 import frc.robot.commands.grabber.RollerIntakeCommand;
 import frc.robot.commands.mover.TiltedElevatorCommand;
 import frc.robot.subsystems.RollerSubsystem;
-import frc.robot.subsystems.drivetrain.BaseDrivetrain;
 import frc.robot.subsystems.drivetrain.BaseSwerveSubsystem;
 import frc.robot.subsystems.tiltedelevator.ElevatorState;
 import frc.robot.subsystems.tiltedelevator.TiltedElevatorSubsystem;
@@ -20,12 +19,12 @@ public class MotorTestCommand extends SequentialCommandGroup {
     private static final double SWERVE_DRIVE_POWER = 0.5;
     private static final double SWERVE_DRIVE_TIME_SECS = 3;
 
-    public MotorTestCommand(BaseDrivetrain driveSubsystem, TiltedElevatorSubsystem tiltedElevatorSubsystem, RollerSubsystem rollerSubsystem) {
-        addRequirements(driveSubsystem, tiltedElevatorSubsystem, rollerSubsystem);
+    public MotorTestCommand(BaseSwerveSubsystem swerveSubsystem, TiltedElevatorSubsystem tiltedElevatorSubsystem, RollerSubsystem rollerSubsystem) {
+        addRequirements(swerveSubsystem, tiltedElevatorSubsystem, rollerSubsystem);
 
         addCommands(
             // Lock swerve
-            // new InstantCommand(swerveSubsystem::applyLock, swerveSubsystem),
+            new InstantCommand(swerveSubsystem::applyLock, swerveSubsystem),
 
             // Run tilted elevator to all heights
             // GROUND - CHUTE - INTAKE CONE - SUBSTATION - CUBEMID - CUBEHIGH - CONEMID - CONEHIGH - CONEMID 
@@ -58,8 +57,8 @@ public MotorTestCommand(BaseDrivetrain driveSubsystem, TiltedElevatorSubsystem t
             new WaitCommand(TEST_DELAY_SECS),
 
             // Test dropper command from `CONE_MID`
-            DropperChooserCommand.getSequence(driveSubsystem, rollerSubsystem, tiltedElevatorSubsystem),
-            new WaitCommand(MECH_DELAY_SECS)
+            DropperChooserCommand.getSequence(swerveSubsystem, rollerSubsystem, tiltedElevatorSubsystem),
+            new WaitCommand(MECH_DELAY_SECS),
 
             // // Sweep rollers from [-1.0, 1.0] over 2 seconds
             // new RollerSweepCommand(rollerSubsystem, 2.0),
@@ -68,6 +67,27 @@ public MotorTestCommand(BaseDrivetrain driveSubsystem, TiltedElevatorSubsystem t
             // // Open motor
             // new InstantCommand(rollerSubsystem::openMotor, rollerSubsystem),
             // new WaitCommand(MECH_DELAY_SECS),
+
+            // Test swerve with forward, back, left, and right powers, and turning counterclockwise and clockwise
+            new InstantCommand(() -> swerveSubsystem.setDrivePowers(SWERVE_DRIVE_POWER, 0, 0, true)),
+            new WaitCommand(SWERVE_DRIVE_TIME_SECS),
+
+            new InstantCommand(() -> swerveSubsystem.setDrivePowers(-SWERVE_DRIVE_POWER, 0, 0, true)),
+            new WaitCommand(SWERVE_DRIVE_TIME_SECS),
+
+            new InstantCommand(() -> swerveSubsystem.setDrivePowers(0, SWERVE_DRIVE_POWER, 0, true)),
+            new WaitCommand(SWERVE_DRIVE_TIME_SECS),
+
+            new InstantCommand(() -> swerveSubsystem.setDrivePowers(0, -SWERVE_DRIVE_POWER, 0, true)),
+            new WaitCommand(SWERVE_DRIVE_TIME_SECS),
+
+            new InstantCommand(() -> swerveSubsystem.setDrivePowers(0, 0, SWERVE_DRIVE_POWER, true)),
+            new WaitCommand(SWERVE_DRIVE_TIME_SECS),
+
+            new InstantCommand(() -> swerveSubsystem.setDrivePowers(0, 0, -SWERVE_DRIVE_POWER, true)),
+            new WaitCommand(SWERVE_DRIVE_TIME_SECS),
+
+            new InstantCommand(() -> swerveSubsystem.setDrivePowers(0, 0, 0, true))
         );
     }
 }