null checkers + shuffleboard repositioning

robototes · Mar 22, 2024 · 1e98f5c · 1e98f5c
1 parent 1712878
commit 1e98f5c
Show file tree

Hide file tree

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