From 865d3748b6a5c1db64fe21374c20274da1dea012 Mon Sep 17 00:00:00 2001 From: Kevin Yu Date: Tue, 7 Mar 2023 22:41:02 -0800 Subject: [PATCH] Re-add swerve to motor test command --- src/main/java/frc/robot/RobotContainer.java | 7 ++-- .../commands/pretest/MotorTestCommand.java | 32 +++++++++++++++---- 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a971460..23148842 100644 --- 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 index 62159ea5..f1005065 100644 --- 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)) ); } }