From 1e98f5c3a422d16dda782c6c42f251b02456cb7b Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Thu, 21 Mar 2024 18:55:58 -0700 Subject: [PATCH] null checkers + shuffleboard repositioning --- .../team2412/robot/util/AutonomousField.java | 1 - .../team2412/robot/util/MatchDashboard.java | 2 +- .../team2412/robot/util/auto/AutoLogic.java | 54 ++++++++++++------- .../robot/util/auto/ComplexAutoPaths.java | 17 +++--- 4 files changed, 47 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 98f4bddb..f5853231 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -113,7 +113,6 @@ public Pose2d getUpdatedPose(String autoName) { lastName = Optional.of(autoName); auto = PathPlannerAutos.getAuto(autoName); trajectories = auto.trajectories; - System.out.println("num trajectories: " + trajectories.size()); trajectoryIndex = 0; lastFPGATime = fpgaTime; lastTrajectoryTimeOffset = 0; diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index be8fbe5f..d8290074 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -20,6 +20,6 @@ public MatchDashboard(Subsystems s) { tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); - AutonomousField.configureShuffleboardTab(tab, 9, 0, "Available Auto Variants", r::addPeriodic); + AutonomousField.configureShuffleboardTab(tab, 7, 0, "Available Auto Variants", r::addPeriodic); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 1753a1fb..8c05dcfa 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -1,5 +1,7 @@ package frc.team2412.robot.util.auto; +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; @@ -172,32 +174,46 @@ public static void registerCommands() { // param: String commandName, Command command // Intake - NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, null)); NamedCommands.registerCommand( - "IntakeSensorOverride", new AllInSensorOverrideCommand(s.intakeSubsystem)); + "StopIntake", + (INTAKE_ENABLED ? new IntakeStopCommand(s.intakeSubsystem) : Commands.none())); + NamedCommands.registerCommand( + "Intake", (INTAKE_ENABLED ? new AllInCommand(s.intakeSubsystem, null) : Commands.none())); + NamedCommands.registerCommand( + "IntakeSensorOverride", + (INTAKE_ENABLED ? new AllInSensorOverrideCommand(s.intakeSubsystem) : Commands.none())); // Launcher NamedCommands.registerCommand( "VisionLaunch", - Commands.sequence( - new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls), - new FeederInCommand(s.intakeSubsystem))); + (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED + ? Commands.sequence( + new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls), + new FeederInCommand(s.intakeSubsystem)) + : Commands.none())); NamedCommands.registerCommand( "SubwooferLaunch", - new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.SUBWOOFER_AIM_ANGLE) - .andThen(new WaitCommand(1)) - .andThen(new FeederInCommand(s.intakeSubsystem).andThen(new WaitCommand(0.5)))); - NamedCommands.registerCommand("StopLaunch", new StopLauncherCommand(s.launcherSubsystem)); + (LAUNCHER_ENABLED && INTAKE_ENABLED + ? new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.SUBWOOFER_AIM_ANGLE) + .andThen(new WaitCommand(1)) + .andThen(new FeederInCommand(s.intakeSubsystem)) + .andThen(new WaitCommand(0.5)) + : Commands.none())); + NamedCommands.registerCommand( + "StopLaunch", + (LAUNCHER_ENABLED ? new StopLauncherCommand(s.launcherSubsystem) : Commands.none())); NamedCommands.registerCommand( "RetractPivot", - new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0)); // TODO: add retract angle + (LAUNCHER_ENABLED && INTAKE_ENABLED + ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0) + : Commands.none())); // TODO: add retract angle // Complex Autos NamedCommands.registerCommand("AutoLogicTest", ComplexAutoPaths.testAuto); + NamedCommands.registerCommand( "MidSpeakerCenterLineN3N2N1", ComplexAutoPaths.midSpeakerCenterLineN3N2N1); NamedCommands.registerCommand( @@ -239,11 +255,11 @@ public static void initShuffleBoard() { gameObjects.addOption(String.valueOf(i), i); } - tab.add("Starting Position", startPositionChooser).withPosition(7, 0).withSize(2, 1); - tab.add("Launch Type", isVision).withPosition(7, 1); - tab.add("Game Objects", gameObjects).withPosition(8, 1); - tab.add("Available Auto Variants", availableAutos).withPosition(7, 2).withSize(2, 1); - autoDelayEntry = tab.add("Auto Delay", 0).withPosition(7, 3).withSize(1, 1).getEntry(); + tab.add("Starting Position", startPositionChooser).withPosition(5, 0).withSize(2, 1); + tab.add("Launch Type", isVision).withPosition(5, 1); + tab.add("Game Objects", gameObjects).withPosition(6, 1); + tab.add("Available Auto Variants", availableAutos).withPosition(5, 2).withSize(2, 1); + autoDelayEntry = tab.add("Auto Delay", 0).withPosition(5, 3).withSize(1, 1).getEntry(); isVision.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); startPositionChooser.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); diff --git a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java index 7c8cca90..4c48ffa4 100644 --- a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java @@ -1,5 +1,6 @@ package frc.team2412.robot.util.auto; +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; import static frc.team2412.robot.util.auto.AutoLogic.*; import edu.wpi.first.wpilibj2.command.Command; @@ -157,17 +158,21 @@ public class ComplexAutoPaths { // new command getters public static final Command VisionLaunchCommand() { - return new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls); + return (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED + ? new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls) + : Commands.none()); } public static final Command SubwooferLaunchCommand() { - return new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.SUBWOOFER_AIM_ANGLE); + return (LAUNCHER_ENABLED + ? new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.SUBWOOFER_AIM_ANGLE) + : Commands.none()); } public static BooleanSupplier checkForTargets() { - return (s.limelightSubsystem != null ? s.limelightSubsystem::hasTargets : () -> true); + return (LIMELIGHT_ENABLED ? s.limelightSubsystem::hasTargets : () -> true); } }