diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 621064f..cc89419 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,7 +7,6 @@ package frc.robot; -import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.vision.VisionConstants.*; import com.ctre.phoenix6.CANBus; @@ -15,7 +14,6 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; @@ -24,13 +22,12 @@ import frc.robot.operator.OperatorIntent; import frc.robot.state.MatchState; import frc.robot.subsystems.drive.Drive; -import frc.robot.util.GoalBehavior; -import frc.robot.util.SubsystemBehavior; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.launcher.LauncherBehavior; import frc.robot.subsystems.launcher.LauncherIOSim; import frc.robot.subsystems.launcher.LauncherIOTalonFX; import frc.robot.subsystems.launcher.LauncherSubsystem; @@ -38,6 +35,8 @@ import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOLimelight; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; +import frc.robot.util.GoalBehavior; +import frc.robot.util.SubsystemBehavior; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -155,6 +154,7 @@ public RobotContainer() { // Create goal behaviors (wires operator intent → robot goals) new RobotGoalsBehavior(robotGoals); + new LauncherBehavior(launcher); // TODO (students): Create subsystem behaviors here, e.g.: // new LauncherBehavior(launcher); @@ -162,7 +162,7 @@ public RobotContainer() { // Configure all behaviors GoalBehavior.configureAll(operatorIntent); - SubsystemBehavior.configureAll(robotGoals, matchState); + SubsystemBehavior.configureAll(robotGoals, matchState, launcher); // Configure the button bindings configureButtonBindings(); @@ -193,20 +193,6 @@ private void configureButtonBindings() { // () -> -controller.getLeftX(), // () -> Rotation2d.kZero)); - controller - .b() - .onTrue( - new InstantCommand( - () -> { - launcher.setLaunchSpeed(Volts.of(2)); - launcher.setIndexerSpeed(Volts.of(2)); - })) - .onFalse( - new InstantCommand( - () -> { - launcher.stop(); - })); - // Switch to X pattern when X button is pressed controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); diff --git a/src/main/java/frc/robot/goals/RobotGoal.java b/src/main/java/frc/robot/goals/RobotGoal.java index 36226c3..7711d00 100644 --- a/src/main/java/frc/robot/goals/RobotGoal.java +++ b/src/main/java/frc/robot/goals/RobotGoal.java @@ -1,11 +1,12 @@ package frc.robot.goals; /** - * Enum representing what the robot is currently trying to accomplish. - * These are high-level goals, not hardware states. + * Enum representing what the robot is currently trying to accomplish. These are high-level goals, + * not hardware states. * *

TODO (students): Add your robot's goals here (e.g., LAUNCHING, INTAKING) */ public enum RobotGoal { - IDLE + IDLE, + LAUNCHING } diff --git a/src/main/java/frc/robot/goals/RobotGoalEvents.java b/src/main/java/frc/robot/goals/RobotGoalEvents.java index e9c6920..12f8dc2 100644 --- a/src/main/java/frc/robot/goals/RobotGoalEvents.java +++ b/src/main/java/frc/robot/goals/RobotGoalEvents.java @@ -3,11 +3,13 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * Interface exposing robot goal triggers for behaviors to react to. - * Behaviors should only depend on this interface, not on RobotGoals directly. + * Interface exposing robot goal triggers for behaviors to react to. Behaviors should only depend on + * this interface, not on RobotGoals directly. * *

TODO (students): Add triggers for each goal in RobotGoal enum */ public interface RobotGoalEvents { - Trigger isIdle(); + Trigger isIdleTrigger(); + + Trigger isLaunchingTrigger(); } diff --git a/src/main/java/frc/robot/goals/RobotGoals.java b/src/main/java/frc/robot/goals/RobotGoals.java index 5d91cfe..e2121eb 100644 --- a/src/main/java/frc/robot/goals/RobotGoals.java +++ b/src/main/java/frc/robot/goals/RobotGoals.java @@ -9,31 +9,36 @@ /** * Central robot state: what we're doing (goal). * - * Can be set by: - * - Teleop via RobotGoalsBehavior reacting to OperatorIntent - * - Autonomous via direct command calls + *

Can be set by: - Teleop via RobotGoalsBehavior reacting to OperatorIntent - Autonomous via + * direct command calls * *

TODO (students): Add goal triggers for each value in RobotGoal enum */ public class RobotGoals extends VirtualSubsystem implements RobotGoalEvents { - private final EnumState currentGoal = new EnumState<>("RobotGoals/Goal", RobotGoal.IDLE); + private final EnumState currentGoal = + new EnumState<>("RobotGoals/Goal", RobotGoal.IDLE); - public RobotGoals() {} + public RobotGoals() {} - public Command setGoal(RobotGoal goal) { - return Commands.runOnce(() -> currentGoal.set(goal)); - } + public Command setGoalCommand(RobotGoal goal) { + return Commands.runOnce(() -> currentGoal.set(goal)); + } - @Override - public Trigger isIdle() { - return currentGoal.is(RobotGoal.IDLE); - } + @Override + public Trigger isIdleTrigger() { + return currentGoal.is(RobotGoal.IDLE); + } - public RobotGoal getCurrentGoal() { - return currentGoal.get(); - } + public RobotGoal getCurrentGoal() { + return currentGoal.get(); + } - @Override - public void periodic() {} + @Override + public void periodic() {} + + @Override + public Trigger isLaunchingTrigger() { + return currentGoal.is(RobotGoal.LAUNCHING); + } } diff --git a/src/main/java/frc/robot/goals/RobotGoalsBehavior.java b/src/main/java/frc/robot/goals/RobotGoalsBehavior.java index b7fd8a9..61cc4ca 100644 --- a/src/main/java/frc/robot/goals/RobotGoalsBehavior.java +++ b/src/main/java/frc/robot/goals/RobotGoalsBehavior.java @@ -6,24 +6,26 @@ /** * Wires operator button presses to robot goal state. * - * This is the teleop-specific logic. Autonomous bypasses this - * and calls RobotGoals.setGoal() directly. + *

This is the teleop-specific logic. Autonomous bypasses this and calls RobotGoals.setGoal() + * directly. * - *

TODO (students): Map your button intents to goals here - * Example: - * intent.wantsToScore().onTrue(goals.setGoal(RobotGoal.LAUNCHING)) - * .onFalse(goals.setGoal(RobotGoal.IDLE)); + *

TODO (students): Map your button intents to goals here Example: + * intent.wantsToScore().onTrue(goals.setGoal(RobotGoal.LAUNCHING)) + * .onFalse(goals.setGoal(RobotGoal.IDLE)); */ public class RobotGoalsBehavior extends GoalBehavior { - private RobotGoals goals; + private RobotGoals goals; - public RobotGoalsBehavior(RobotGoals goals) { - this.goals = goals; - } + public RobotGoalsBehavior(RobotGoals goals) { + this.goals = goals; + } - @Override - public void configure(OperatorIntentEvents intent) { - // TODO (students): Wire intent triggers to goal state changes - } + @Override + public void configure(OperatorIntentEvents intent) { + intent + .wantsToScoreTrigger() + .whileTrue(goals.setGoalCommand(RobotGoal.LAUNCHING)) + .whileFalse(goals.setGoalCommand(RobotGoal.IDLE)); + } } diff --git a/src/main/java/frc/robot/operator/OperatorIntent.java b/src/main/java/frc/robot/operator/OperatorIntent.java index 2326133..2fab593 100644 --- a/src/main/java/frc/robot/operator/OperatorIntent.java +++ b/src/main/java/frc/robot/operator/OperatorIntent.java @@ -6,28 +6,33 @@ /** * Interprets driver controller inputs into intent triggers. * - * This class has NO state - it just wraps controller buttons - * and provides semantic meaning (wantsToScore vs rightTrigger). + *

This class has NO state - it just wraps controller buttons and provides semantic meaning + * (wantsToScore vs rightTrigger). * - * State lives in RobotGoals. This class just says what the - * operator is physically doing right now. + *

State lives in RobotGoals. This class just says what the operator is physically doing right + * now. * *

TODO (students): Map your controller buttons to semantic intents here */ public class OperatorIntent implements OperatorIntentEvents { - private final CommandXboxController driver; + private final CommandXboxController driver; - public OperatorIntent(int driverPort) { - this.driver = new CommandXboxController(driverPort); - } + public OperatorIntent(int driverPort) { + this.driver = new CommandXboxController(driverPort); + } - @Override - public Trigger wantsToScore() { - return driver.rightTrigger(0.5); - } + @Override + public Trigger wantsToScoreTrigger() { + return driver.rightTrigger(0.5); + } - public CommandXboxController getDriver() { - return driver; - } + public CommandXboxController getDriver() { + return driver; + } + + @Override + public Trigger wantsToOutake() { + return driver.b(); + } } diff --git a/src/main/java/frc/robot/operator/OperatorIntentEvents.java b/src/main/java/frc/robot/operator/OperatorIntentEvents.java index 2cdd6ca..8c83bb4 100644 --- a/src/main/java/frc/robot/operator/OperatorIntentEvents.java +++ b/src/main/java/frc/robot/operator/OperatorIntentEvents.java @@ -3,11 +3,13 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * Interface exposing only the triggers that behaviors might need from OperatorIntent. - * Keeps the contract minimal - behaviors shouldn't know about raw button inputs. + * Interface exposing only the triggers that behaviors might need from OperatorIntent. Keeps the + * contract minimal - behaviors shouldn't know about raw button inputs. * *

TODO (students): Add intent triggers for your robot's actions */ public interface OperatorIntentEvents { - Trigger wantsToScore(); + Trigger wantsToScoreTrigger(); + + Trigger wantsToOutake(); } diff --git a/src/main/java/frc/robot/state/MatchState.java b/src/main/java/frc/robot/state/MatchState.java index dbe6bd5..9a0809e 100644 --- a/src/main/java/frc/robot/state/MatchState.java +++ b/src/main/java/frc/robot/state/MatchState.java @@ -5,33 +5,33 @@ import frc.robot.util.VirtualSubsystem; /** - * Exposes match phase state as triggers for behaviors to react to. - * Wraps DriverStation calls into a reactive interface. + * Exposes match phase state as triggers for behaviors to react to. Wraps DriverStation calls into a + * reactive interface. */ public class MatchState extends VirtualSubsystem implements MatchStateEvents { - @Override - public Trigger isDisabled() { - return new Trigger(DriverStation::isDisabled); - } + @Override + public Trigger isDisabled() { + return new Trigger(DriverStation::isDisabled); + } - @Override - public Trigger isAutonomous() { - return new Trigger(DriverStation::isAutonomousEnabled); - } + @Override + public Trigger isAutonomous() { + return new Trigger(DriverStation::isAutonomousEnabled); + } - @Override - public Trigger isTeleop() { - return new Trigger(DriverStation::isTeleopEnabled); - } + @Override + public Trigger isTeleop() { + return new Trigger(DriverStation::isTeleopEnabled); + } - @Override - public Trigger isEnabled() { - return new Trigger(DriverStation::isEnabled); - } + @Override + public Trigger isEnabled() { + return new Trigger(DriverStation::isEnabled); + } - @Override - public void periodic() { - // No periodic updates needed - triggers poll DriverStation directly - } + @Override + public void periodic() { + // No periodic updates needed - triggers poll DriverStation directly + } } diff --git a/src/main/java/frc/robot/state/MatchStateEvents.java b/src/main/java/frc/robot/state/MatchStateEvents.java index dcfbc09..a95afd6 100644 --- a/src/main/java/frc/robot/state/MatchStateEvents.java +++ b/src/main/java/frc/robot/state/MatchStateEvents.java @@ -3,20 +3,20 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * Interface exposing match phase state as triggers for behaviors to react to. - * Provides triggers for disabled, autonomous, teleop, and enabled states. + * Interface exposing match phase state as triggers for behaviors to react to. Provides triggers for + * disabled, autonomous, teleop, and enabled states. */ public interface MatchStateEvents { - /** Trigger when robot is disabled */ - Trigger isDisabled(); + /** Trigger when robot is disabled */ + Trigger isDisabled(); - /** Trigger when robot is in autonomous mode */ - Trigger isAutonomous(); + /** Trigger when robot is in autonomous mode */ + Trigger isAutonomous(); - /** Trigger when robot is in teleop mode */ - Trigger isTeleop(); + /** Trigger when robot is in teleop mode */ + Trigger isTeleop(); - /** Trigger when robot is enabled (auto OR teleop) */ - Trigger isEnabled(); + /** Trigger when robot is enabled (auto OR teleop) */ + Trigger isEnabled(); } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherBehavior.java b/src/main/java/frc/robot/subsystems/launcher/LauncherBehavior.java new file mode 100644 index 0000000..a7ec305 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherBehavior.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.launcher; + +import frc.robot.goals.RobotGoalEvents; +import frc.robot.state.MatchStateEvents; +import frc.robot.util.SubsystemBehavior; + +public class LauncherBehavior extends SubsystemBehavior { + + private final LauncherSubsystem launcher; + + public LauncherBehavior(LauncherSubsystem launcher) { + this.launcher = launcher; + } + + @Override + public void configure( + RobotGoalEvents goals, MatchStateEvents matchState, LauncherEvents launcherState) { + goals.isLaunchingTrigger().whileTrue(this.launcher.launchCommand()); + goals.isIdleTrigger().whileTrue(this.launcher.idleCommand()); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherEvents.java b/src/main/java/frc/robot/subsystems/launcher/LauncherEvents.java new file mode 100644 index 0000000..7bd5bf3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherEvents.java @@ -0,0 +1,9 @@ +package frc.robot.subsystems.launcher; + +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public interface LauncherEvents { + public Trigger isIdleTrigger(); + + public Trigger isLaunchingTrigger(); +} diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherState.java b/src/main/java/frc/robot/subsystems/launcher/LauncherState.java new file mode 100644 index 0000000..4651ff5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherState.java @@ -0,0 +1,6 @@ +package frc.robot.subsystems.launcher; + +public enum LauncherState { + IDLE, + LAUNCHING +} diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java b/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java index 7d6ac4d..cc7999d 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java @@ -8,14 +8,20 @@ import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.util.EnumState; import org.littletonrobotics.junction.Logger; /** Sets the controless the launcher and endexer */ -public class LauncherSubsystem extends SubsystemBase { +public class LauncherSubsystem extends SubsystemBase implements LauncherEvents { /** Creates a new ExampleSubsystem. */ private LauncherIO m_IO; + private final EnumState currentGoal = + new EnumState<>("Launcher/States", LauncherState.IDLE); + private LauncherInputsAutoLogged logged = new LauncherInputsAutoLogged(); public LauncherSubsystem(LauncherIO IO) { @@ -44,6 +50,22 @@ public void setIndexerSpeed(Voltage speed) { m_IO.setIndexerTarget(speed); } + public Command launchCommand() { + return runOnce( + () -> { + setIndexerSpeed(Volts.of(2)); + setLaunchSpeed(Volts.of(2)); + }); + } + + public Command idleCommand() { + return runOnce( + () -> { + setIndexerSpeed(Volts.of(0)); + setLaunchSpeed(Volts.of(0)); + }); + } + public void stop() { m_IO.stop(); } @@ -53,4 +75,14 @@ public void periodic() { m_IO.updateInputs(logged); Logger.processInputs("RobotState/Launcher", logged); } + + @Override + public Trigger isIdleTrigger() { + return currentGoal.is(LauncherState.IDLE); + } + + @Override + public Trigger isLaunchingTrigger() { + return currentGoal.is(LauncherState.LAUNCHING); + } } diff --git a/src/main/java/frc/robot/util/Behavior.java b/src/main/java/frc/robot/util/Behavior.java index 5b77573..6a2806d 100644 --- a/src/main/java/frc/robot/util/Behavior.java +++ b/src/main/java/frc/robot/util/Behavior.java @@ -4,13 +4,14 @@ import java.util.List; /** - * Base class for all behaviors in the reactive architecture. - * Behaviors are self-registering and configure trigger-based reactions. + * Base class for all behaviors in the reactive architecture. Behaviors are self-registering and + * configure trigger-based reactions. * *

This is the foundation for both: + * *

* *

All behaviors auto-register when constructed and can be configured in batch. @@ -19,19 +20,19 @@ */ public abstract class Behavior> { - private static final List> allBehaviors = new ArrayList<>(); + private static final List> allBehaviors = new ArrayList<>(); - protected Behavior() { - allBehaviors.add(this); - } + protected Behavior() { + allBehaviors.add(this); + } - /** Get all registered behaviors of any type. */ - public static List> getAllBehaviors() { - return new ArrayList<>(allBehaviors); - } + /** Get all registered behaviors of any type. */ + public static List> getAllBehaviors() { + return new ArrayList<>(allBehaviors); + } - /** Clear all registered behaviors (useful for testing). */ - public static void clearAll() { - allBehaviors.clear(); - } + /** Clear all registered behaviors (useful for testing). */ + public static void clearAll() { + allBehaviors.clear(); + } } diff --git a/src/main/java/frc/robot/util/EnumState.java b/src/main/java/frc/robot/util/EnumState.java index 2385880..115ee7a 100644 --- a/src/main/java/frc/robot/util/EnumState.java +++ b/src/main/java/frc/robot/util/EnumState.java @@ -6,10 +6,11 @@ import org.littletonrobotics.junction.Logger; /** - * A utility class that automatically generates Triggers from any enum. - * Makes state management dead simple for FRC subsystems. + * A utility class that automatically generates Triggers from any enum. Makes state management dead + * simple for FRC subsystems. * *

Basic Usage:

+ * * 
{@code
  * // 1. Define your states as an enum
  * public enum IntakeState { IDLE, INTAKING, HOLDING, EJECTING }
@@ -28,6 +29,7 @@
  * }
* *

With Automatic Logging:

+ * * 
{@code
  * // Add a name and it logs automatically to AdvantageKit!
  * private final EnumState state =
@@ -35,6 +37,7 @@
  * }
* *

In RobotContainer Bindings:

+ * * 
{@code
  * // These triggers work just like any other WPILib Trigger
  * intake.state.is(HOLDING)
@@ -50,251 +53,259 @@
  */
 public class EnumState> {
 
-    private E currentState;
-    private E previousState;
-    private final String name;
-    private final Class enumClass;
-    private final EnumMap triggerCache;
-
-    /**
-     * Create a new EnumState with the given initial state.
-     *
-     * 
Example:
-     * 
{@code
-     * private final EnumState state = new EnumState<>(ShooterState.IDLE);
-     * }

-     *
-     * @param initialState The starting state
-     */
-    public EnumState(E initialState) {
-        this(null, initialState);
-    }
+  private E currentState;
+  private E previousState;
+  private final String name;
+  private final Class enumClass;
+  private final EnumMap triggerCache;
 
-    /**
-     * Create a new EnumState with automatic AdvantageKit logging.
-     *
-     * 
The state will be logged to "YourName/State" whenever it changes.
-     *
-     * 
Example:
-     * 
{@code
-     * // Logs to "Intake/State" in AdvantageKit
-     * private final EnumState state =
-     *     new EnumState<>("Intake", IntakeState.IDLE);
-     * }

-     *
-     * @param name The name for logging (e.g., "Intake", "Shooter")
-     * @param initialState The starting state
-     */
-    @SuppressWarnings("unchecked")
-    public EnumState(String name, E initialState) {
-        this.currentState = initialState;
-        this.previousState = initialState;
-        this.name = name;
-        this.enumClass = (Class) initialState.getClass();
-        this.triggerCache = new EnumMap<>(enumClass);
+  /**
+   * Create a new EnumState with the given initial state.
+   *
+   * 
Example:
+   *
+   * 
{@code
+   * private final EnumState state = new EnumState<>(ShooterState.IDLE);
+   * }

+   *
+   * @param initialState The starting state
+   */
+  public EnumState(E initialState) {
+    this(null, initialState);
+  }
 
-        // Pre-create triggers for every enum value
-        // This way .is() always returns the same Trigger instance
-        for (E value : enumClass.getEnumConstants()) {
-            triggerCache.put(value, new Trigger(() -> currentState == value));
-        }
+  /**
+   * Create a new EnumState with automatic AdvantageKit logging.
+   *
+   * 
The state will be logged to "YourName/State" whenever it changes.
+   *
+   * 
Example:
+   *
+   * 
{@code
+   * // Logs to "Intake/State" in AdvantageKit
+   * private final EnumState state =
+   *     new EnumState<>("Intake", IntakeState.IDLE);
+   * }

+   *
+   * @param name The name for logging (e.g., "Intake", "Shooter")
+   * @param initialState The starting state
+   */
+  @SuppressWarnings("unchecked")
+  public EnumState(String name, E initialState) {
+    this.currentState = initialState;
+    this.previousState = initialState;
+    this.name = name;
+    this.enumClass = (Class) initialState.getClass();
+    this.triggerCache = new EnumMap<>(enumClass);
 
-        // Log initial state
-        log();
+    // Pre-create triggers for every enum value
+    // This way .is() always returns the same Trigger instance
+    for (E value : enumClass.getEnumConstants()) {
+      triggerCache.put(value, new Trigger(() -> currentState == value));
     }
 
-    // ==================== CORE METHODS ====================
+    // Log initial state
+    log();
+  }
 
-    /**
-     * Get a Trigger that is TRUE when in the specified state.
-     *
-     * 
This is the main method you'll use! Example:
-     * 
{@code
-     * // In RobotContainer:
-     * intake.state.is(IntakeState.HOLDING)
-     *     .onTrue(flashLEDsCommand());
-     *
-     * // Combine with other triggers:
-     * driver.a()
-     *     .and(intake.state.is(HOLDING))
-     *     .and(shooter.state.is(READY))
-     *     .onTrue(shootCommand());
-     * }

-     *
-     * @param state The state to check for
-     * @return A Trigger that is true when currentState equals the given state
-     */
-    public Trigger is(E state) {
-        return triggerCache.get(state);
-    }
+  // ==================== CORE METHODS ====================
 
-    /**
-     * Change to a new state.
-     *
-     * 
If logging is enabled, automatically logs the change to AdvantageKit.
-     *
-     * 
Example:
-     * 
{@code
-     * public Command intakeCommand() {
-     *     return startEnd(
-     *         () -> state.set(IntakeState.INTAKING),
-     *         () -> state.set(IntakeState.IDLE)
-     *     );
-     * }
-     * }

-     *
-     * @param newState The state to change to
-     */
-    public void set(E newState) {
-        if (currentState != newState) {
-            previousState = currentState;
-            currentState = newState;
-            log();
-        }
-    }
+  /**
+   * Get a Trigger that is TRUE when in the specified state.
+   *
+   * 
This is the main method you'll use! Example:
+   *
+   * 
{@code
+   * // In RobotContainer:
+   * intake.state.is(IntakeState.HOLDING)
+   *     .onTrue(flashLEDsCommand());
+   *
+   * // Combine with other triggers:
+   * driver.a()
+   *     .and(intake.state.is(HOLDING))
+   *     .and(shooter.state.is(READY))
+   *     .onTrue(shootCommand());
+   * }

+   *
+   * @param state The state to check for
+   * @return A Trigger that is true when currentState equals the given state
+   */
+  public Trigger is(E state) {
+    return triggerCache.get(state);
+  }
 
-    /**
-     * Get the current state.
-     *
-     * 
Example:
-     * 
{@code
-     * if (state.get() == IntakeState.HOLDING) {
-     *     // Do something
-     * }
-     * }

-     *
-     * @return The current state
-     */
-    public E get() {
-        return currentState;
+  /**
+   * Change to a new state.
+   *
+   * 
If logging is enabled, automatically logs the change to AdvantageKit.
+   *
+   * 
Example:
+   *
+   * 
{@code
+   * public Command intakeCommand() {
+   *     return startEnd(
+   *         () -> state.set(IntakeState.INTAKING),
+   *         () -> state.set(IntakeState.IDLE)
+   *     );
+   * }
+   * }

+   *
+   * @param newState The state to change to
+   */
+  public void set(E newState) {
+    if (currentState != newState) {
+      previousState = currentState;
+      currentState = newState;
+      log();
     }
+  }
+
+  /**
+   * Get the current state.
+   *
+   * 
Example:
+   *
+   * 
{@code
+   * if (state.get() == IntakeState.HOLDING) {
+   *     // Do something
+   * }
+   * }

+   *
+   * @return The current state
+   */
+  public E get() {
+    return currentState;
+  }
 
-    // ==================== CONVENIENCE METHODS ====================
+  // ==================== CONVENIENCE METHODS ====================
 
-    /**
-     * Get a Trigger that is TRUE when in ANY of the specified states.
-     *
-     * 
Example:
-     * 
{@code
-     * // True when IDLE or HOLDING (not actively moving)
-     * state.isAnyOf(IntakeState.IDLE, IntakeState.HOLDING)
-     *     .whileTrue(allowShootingCommand());
-     * }

-     *
-     * @param states The states to check for (varargs - pass as many as you want)
-     * @return A Trigger that is true when in any of the given states
-     */
-    @SafeVarargs
-    public final Trigger isAnyOf(E... states) {
-        return new Trigger(() -> {
-            for (E state : states) {
-                if (currentState == state) {
-                    return true;
-                }
+  /**
+   * Get a Trigger that is TRUE when in ANY of the specified states.
+   *
+   * 
Example:
+   *
+   * 
{@code
+   * // True when IDLE or HOLDING (not actively moving)
+   * state.isAnyOf(IntakeState.IDLE, IntakeState.HOLDING)
+   *     .whileTrue(allowShootingCommand());
+   * }

+   *
+   * @param states The states to check for (varargs - pass as many as you want)
+   * @return A Trigger that is true when in any of the given states
+   */
+  @SafeVarargs
+  public final Trigger isAnyOf(E... states) {
+    return new Trigger(
+        () -> {
+          for (E state : states) {
+            if (currentState == state) {
+              return true;
             }
-            return false;
+          }
+          return false;
         });
-    }
+  }
 
-    /**
-     * Get a Trigger that is TRUE when NOT in the specified state.
-     *
-     * 
Example:
-     * 
{@code
-     * // True when not idle (doing something)
-     * state.isNot(IntakeState.IDLE)
-     *     .whileTrue(runMotorCommand());
-     * }

-     *
-     * @param state The state to check against
-     * @return A Trigger that is true when NOT in the given state
-     */
-    public Trigger isNot(E state) {
-        return triggerCache.get(state).negate();
-    }
+  /**
+   * Get a Trigger that is TRUE when NOT in the specified state.
+   *
+   * 
Example:
+   *
+   * 
{@code
+   * // True when not idle (doing something)
+   * state.isNot(IntakeState.IDLE)
+   *     .whileTrue(runMotorCommand());
+   * }

+   *
+   * @param state The state to check against
+   * @return A Trigger that is true when NOT in the given state
+   */
+  public Trigger isNot(E state) {
+    return triggerCache.get(state).negate();
+  }
 
-    /**
-     * Get a Trigger that is TRUE when NOT in any of the specified states.
-     *
-     * 
Example:
-     * 
{@code
-     * // True when not IDLE and not HOLDING
-     * state.isNoneOf(IntakeState.IDLE, IntakeState.HOLDING)
-     *     .whileTrue(busyIndicatorCommand());
-     * }

-     *
-     * @param states The states to exclude
-     * @return A Trigger that is true when NOT in any of the given states
-     */
-    @SafeVarargs
-    public final Trigger isNoneOf(E... states) {
-        return isAnyOf(states).negate();
-    }
+  /**
+   * Get a Trigger that is TRUE when NOT in any of the specified states.
+   *
+   * 
Example:
+   *
+   * 
{@code
+   * // True when not IDLE and not HOLDING
+   * state.isNoneOf(IntakeState.IDLE, IntakeState.HOLDING)
+   *     .whileTrue(busyIndicatorCommand());
+   * }

+   *
+   * @param states The states to exclude
+   * @return A Trigger that is true when NOT in any of the given states
+   */
+  @SafeVarargs
+  public final Trigger isNoneOf(E... states) {
+    return isAnyOf(states).negate();
+  }
 
-    /**
-     * Get the previous state (what we were in before the last change).
-     *
-     * 
Useful for detecting transitions:
-     * 
{@code
-     * if (state.get() == HOLDING && state.getPrevious() == INTAKING) {
-     *     // We just finished intaking and now we're holding
-     * }
-     * }

-     *
-     * @return The state we were in before the current state
-     */
-    public E getPrevious() {
-        return previousState;
-    }
+  /**
+   * Get the previous state (what we were in before the last change).
+   *
+   * 
Useful for detecting transitions:
+   *
+   * 
{@code
+   * if (state.get() == HOLDING && state.getPrevious() == INTAKING) {
+   *     // We just finished intaking and now we're holding
+   * }
+   * }

+   *
+   * @return The state we were in before the current state
+   */
+  public E getPrevious() {
+    return previousState;
+  }
 
-    // ==================== UTILITY METHODS ====================
+  // ==================== UTILITY METHODS ====================
 
-    /**
-     * Get all available triggers as a map.
-     *
-     * 
Useful for debugging or dashboard integration.
-     *
-     * @return Map from enum value to its corresponding Trigger
-     */
-    public Map triggers() {
-        return new EnumMap<>(triggerCache);
-    }
+  /**
+   * Get all available triggers as a map.
+   *
+   * 
Useful for debugging or dashboard integration.
+   *
+   * @return Map from enum value to its corresponding Trigger
+   */
+  public Map triggers() {
+    return new EnumMap<>(triggerCache);
+  }
 
-    /**
-     * Get all possible states for this EnumState.
-     *
-     * @return Array of all enum constants
-     */
-    public E[] getAllStates() {
-        return enumClass.getEnumConstants();
-    }
+  /**
+   * Get all possible states for this EnumState.
+   *
+   * @return Array of all enum constants
+   */
+  public E[] getAllStates() {
+    return enumClass.getEnumConstants();
+  }
 
-    /**
-     * Get the name used for logging.
-     *
-     * @return The logging name, or null if logging is disabled
-     */
-    public String getName() {
-        return name;
-    }
+  /**
+   * Get the name used for logging.
+   *
+   * @return The logging name, or null if logging is disabled
+   */
+  public String getName() {
+    return name;
+  }
 
-    /**
-     * Manually trigger a log update.
-     *
-     * 
Usually not needed since set() logs automatically,
-     * but can be called in periodic() if desired.
-     */
-    public void log() {
-        if (name != null) {
-            Logger.recordOutput(name + "/State", currentState.name());
-        }
+  /**
+   * Manually trigger a log update.
+   *
+   * 
Usually not needed since set() logs automatically, but can be called in periodic() if
+   * desired.
+   */
+  public void log() {
+    if (name != null) {
+      Logger.recordOutput(name + "/State", currentState.name());
     }
+  }
 
-    /**
-     * Returns the current state name as a string.
-     */
-    @Override
-    public String toString() {
-        return currentState.name();
-    }
+  /** Returns the current state name as a string. */
+  @Override
+  public String toString() {
+    return currentState.name();
+  }
 }
diff --git a/src/main/java/frc/robot/util/Gains.java b/src/main/java/frc/robot/util/Gains.java
index 110f79b..d487ce0 100644
--- a/src/main/java/frc/robot/util/Gains.java
+++ b/src/main/java/frc/robot/util/Gains.java
@@ -15,134 +15,134 @@
  * }
*/ public class Gains { - public final double kP; - public final double kI; - public final double kD; - public final double kS; - public final double kG; - public final double kV; - public final double kA; - - // Motion Magic parameters - public final double kMMV; - public final double kMMA; - public final double kMMJ; - public final double kMMEV; - public final double kMMEA; - - private Gains( - double kP, - double kI, - double kD, - double kS, - double kG, - double kV, - double kA, - double kMMV, - double kMMA, - double kMMJ, - double kMMEV, - double kMMEA) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kS = kS; - this.kG = kG; - this.kV = kV; - this.kA = kA; - - this.kMMV = kMMV; - this.kMMA = kMMA; - this.kMMJ = kMMJ; - this.kMMEA = kMMEA; - this.kMMEV = kMMEV; + public final double kP; + public final double kI; + public final double kD; + public final double kS; + public final double kG; + public final double kV; + public final double kA; + + // Motion Magic parameters + public final double kMMV; + public final double kMMA; + public final double kMMJ; + public final double kMMEV; + public final double kMMEA; + + private Gains( + double kP, + double kI, + double kD, + double kS, + double kG, + double kV, + double kA, + double kMMV, + double kMMA, + double kMMJ, + double kMMEV, + double kMMEA) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kS = kS; + this.kG = kG; + this.kV = kV; + this.kA = kA; + + this.kMMV = kMMV; + this.kMMA = kMMA; + this.kMMJ = kMMJ; + this.kMMEA = kMMEA; + this.kMMEV = kMMEV; + } + + public static Gains getEmpty() { + return new Gains(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + } + + public static Builder builder() { + return new Builder(); + } + + public static class Builder { + private double kP = 0; + private double kI = 0; + private double kD = 0; + private double kS = 0; + private double kG = 0; + private double kV = 0; + private double kA = 0; + + private double kMMV = 0; + private double kMMA = 0; + private double kMMJ = 0; + private double kMMEV = 0; + private double kMMEA = 0; + + public Builder kP(double kP) { + this.kP = kP; + return this; } - public static Gains getEmpty() { - return new Gains(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + public Builder kI(double kI) { + this.kI = kI; + return this; } - public static Builder builder() { - return new Builder(); + public Builder kD(double kD) { + this.kD = kD; + return this; } - public static class Builder { - private double kP = 0; - private double kI = 0; - private double kD = 0; - private double kS = 0; - private double kG = 0; - private double kV = 0; - private double kA = 0; - - private double kMMV = 0; - private double kMMA = 0; - private double kMMJ = 0; - private double kMMEV = 0; - private double kMMEA = 0; - - public Builder kP(double kP) { - this.kP = kP; - return this; - } - - public Builder kI(double kI) { - this.kI = kI; - return this; - } - - public Builder kD(double kD) { - this.kD = kD; - return this; - } - - public Builder kS(double kS) { - this.kS = kS; - return this; - } - - public Builder kG(double kG) { - this.kG = kG; - return this; - } - - public Builder kV(double kV) { - this.kV = kV; - return this; - } - - public Builder kA(double kA) { - this.kA = kA; - return this; - } - - public Builder kMMV(double kMMV) { - this.kMMV = kMMV; - return this; - } - - public Builder kMMA(double kMMA) { - this.kMMA = kMMA; - return this; - } - - public Builder kMMJ(double kMMJ) { - this.kMMJ = kMMJ; - return this; - } - - public Builder kMMEV(double kMMEV) { - this.kMMEV = kMMEV; - return this; - } - - public Builder kMMEA(double kMMEA) { - this.kMMEA = kMMEA; - return this; - } - - public Gains build() { - return new Gains(kP, kI, kD, kS, kG, kV, kA, kMMV, kMMA, kMMJ, kMMEV, kMMEA); - } + public Builder kS(double kS) { + this.kS = kS; + return this; } + + public Builder kG(double kG) { + this.kG = kG; + return this; + } + + public Builder kV(double kV) { + this.kV = kV; + return this; + } + + public Builder kA(double kA) { + this.kA = kA; + return this; + } + + public Builder kMMV(double kMMV) { + this.kMMV = kMMV; + return this; + } + + public Builder kMMA(double kMMA) { + this.kMMA = kMMA; + return this; + } + + public Builder kMMJ(double kMMJ) { + this.kMMJ = kMMJ; + return this; + } + + public Builder kMMEV(double kMMEV) { + this.kMMEV = kMMEV; + return this; + } + + public Builder kMMEA(double kMMEA) { + this.kMMEA = kMMEA; + return this; + } + + public Gains build() { + return new Gains(kP, kI, kD, kS, kG, kV, kA, kMMV, kMMA, kMMJ, kMMEV, kMMEA); + } + } } diff --git a/src/main/java/frc/robot/util/GoalBehavior.java b/src/main/java/frc/robot/util/GoalBehavior.java index 06611e9..13347c0 100644 --- a/src/main/java/frc/robot/util/GoalBehavior.java +++ b/src/main/java/frc/robot/util/GoalBehavior.java @@ -5,13 +5,14 @@ import java.util.List; /** - * Base class for goal-level behaviors that react to operator intent. - * These behaviors translate operator intent into robot goal state changes. + * Base class for goal-level behaviors that react to operator intent. These behaviors translate + * operator intent into robot goal state changes. * - *

Goal behaviors listen to the UI layer (OperatorIntent) and drive the - * API layer (RobotGoals state). + *

Goal behaviors listen to the UI layer (OperatorIntent) and drive the API layer (RobotGoals + * state). * *

Usage: + * * 

{@code
  * public class RobotGoalsBehavior extends GoalBehavior {
  *     private final RobotGoals goals;
@@ -32,30 +33,30 @@
  */
 public abstract class GoalBehavior extends Behavior {
 
-    private static final List goalBehaviors = new ArrayList<>();
+  private static final List goalBehaviors = new ArrayList<>();
 
-    protected GoalBehavior() {
-        super();
-        goalBehaviors.add(this);
-    }
+  protected GoalBehavior() {
+    super();
+    goalBehaviors.add(this);
+  }
 
-    /**
-     * Configure all registered goal behaviors.
-     *
-     * @param intent The operator intent to react to
-     * @param goals The goal events interface (for reading current goals if needed)
-     */
-    public static void configureAll(OperatorIntentEvents intent) {
-        for (GoalBehavior behavior : goalBehaviors) {
-            behavior.configure(intent);
-        }
+  /**
+   * Configure all registered goal behaviors.
+   *
+   * @param intent The operator intent to react to
+   * @param goals The goal events interface (for reading current goals if needed)
+   */
+  public static void configureAll(OperatorIntentEvents intent) {
+    for (GoalBehavior behavior : goalBehaviors) {
+      behavior.configure(intent);
     }
+  }
 
-    /**
-     * Configure this goal behavior's trigger bindings.
-     *
-     * @param intent The operator intent to react to
-     * @param goals The goal events interface (for reading current goals if needed)
-     */
-    public abstract void configure(OperatorIntentEvents intent);
+  /**
+   * Configure this goal behavior's trigger bindings.
+   *
+   * @param intent The operator intent to react to
+   * @param goals The goal events interface (for reading current goals if needed)
+   */
+  public abstract void configure(OperatorIntentEvents intent);
 }
diff --git a/src/main/java/frc/robot/util/LoggedTunableGainsBuilder.java b/src/main/java/frc/robot/util/LoggedTunableGainsBuilder.java
index 7359ee1..d79402d 100644
--- a/src/main/java/frc/robot/util/LoggedTunableGainsBuilder.java
+++ b/src/main/java/frc/robot/util/LoggedTunableGainsBuilder.java
@@ -28,88 +28,88 @@
  * }
*/ public class LoggedTunableGainsBuilder { - private LoggedTunableNumber kP; - private LoggedTunableNumber kI; - private LoggedTunableNumber kD; - private LoggedTunableNumber kS; - private LoggedTunableNumber kG; - private LoggedTunableNumber kV; - private LoggedTunableNumber kA; + private LoggedTunableNumber kP; + private LoggedTunableNumber kI; + private LoggedTunableNumber kD; + private LoggedTunableNumber kS; + private LoggedTunableNumber kG; + private LoggedTunableNumber kV; + private LoggedTunableNumber kA; - private LoggedTunableNumber kMMV; - private LoggedTunableNumber kMMA; - private LoggedTunableNumber kMMJ; - private LoggedTunableNumber kMMEV; - private LoggedTunableNumber kMMEA; + private LoggedTunableNumber kMMV; + private LoggedTunableNumber kMMA; + private LoggedTunableNumber kMMJ; + private LoggedTunableNumber kMMEV; + private LoggedTunableNumber kMMEA; - public LoggedTunableGainsBuilder( - String key, - double kP, - double kI, - double kD, - double kS, - double kG, - double kV, - double kA, - double kMMV, - double kMMA, - double kMMJ, - double kMMEV, - double kMMEA) { - this.kP = new LoggedTunableNumber(key + "kP", kP); - this.kI = new LoggedTunableNumber(key + "kI", kI); - this.kD = new LoggedTunableNumber(key + "kD", kD); - this.kS = new LoggedTunableNumber(key + "kS", kS); - this.kG = new LoggedTunableNumber(key + "kG", kG); - this.kV = new LoggedTunableNumber(key + "kV", kV); - this.kA = new LoggedTunableNumber(key + "kA", kA); - this.kMMV = new LoggedTunableNumber(key + "kMMV", kMMV); - this.kMMA = new LoggedTunableNumber(key + "kMMA", kMMA); - this.kMMJ = new LoggedTunableNumber(key + "kMMJ", kMMJ); - this.kMMEV = new LoggedTunableNumber(key + "kMMEV", kMMEV); - this.kMMEA = new LoggedTunableNumber(key + "kMMEA", kMMEA); - } + public LoggedTunableGainsBuilder( + String key, + double kP, + double kI, + double kD, + double kS, + double kG, + double kV, + double kA, + double kMMV, + double kMMA, + double kMMJ, + double kMMEV, + double kMMEA) { + this.kP = new LoggedTunableNumber(key + "kP", kP); + this.kI = new LoggedTunableNumber(key + "kI", kI); + this.kD = new LoggedTunableNumber(key + "kD", kD); + this.kS = new LoggedTunableNumber(key + "kS", kS); + this.kG = new LoggedTunableNumber(key + "kG", kG); + this.kV = new LoggedTunableNumber(key + "kV", kV); + this.kA = new LoggedTunableNumber(key + "kA", kA); + this.kMMV = new LoggedTunableNumber(key + "kMMV", kMMV); + this.kMMA = new LoggedTunableNumber(key + "kMMA", kMMA); + this.kMMJ = new LoggedTunableNumber(key + "kMMJ", kMMJ); + this.kMMEV = new LoggedTunableNumber(key + "kMMEV", kMMEV); + this.kMMEA = new LoggedTunableNumber(key + "kMMEA", kMMEA); + } - /** - * Calls the consumer with the current gains if any gain value has changed since last check. - * - * @param gainsConsumer Callback to apply the new gains (e.g., update motor configuration) - */ - public void ifGainsHaveChanged(Consumer gainsConsumer) { - LoggedTunableNumber.ifChanged( - hashCode(), - () -> { - gainsConsumer.accept(build()); - }, - kP, - kI, - kD, - kS, - kG, - kV, - kA, - kMMV, - kMMA, - kMMJ, - kMMEV, - kMMEA); - } + /** + * Calls the consumer with the current gains if any gain value has changed since last check. + * + * @param gainsConsumer Callback to apply the new gains (e.g., update motor configuration) + */ + public void ifGainsHaveChanged(Consumer gainsConsumer) { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + gainsConsumer.accept(build()); + }, + kP, + kI, + kD, + kS, + kG, + kV, + kA, + kMMV, + kMMA, + kMMJ, + kMMEV, + kMMEA); + } - /** Build the current Gains from all tunable values. */ - public Gains build() { - return Gains.builder() - .kP(kP.get()) - .kI(kI.get()) - .kD(kD.get()) - .kS(kS.get()) - .kG(kG.get()) - .kV(kV.get()) - .kA(kA.get()) - .kMMV(kMMV.get()) - .kMMA(kMMA.get()) - .kMMJ(kMMJ.get()) - .kMMEV(kMMEV.get()) - .kMMEA(kMMEA.get()) - .build(); - } + /** Build the current Gains from all tunable values. */ + public Gains build() { + return Gains.builder() + .kP(kP.get()) + .kI(kI.get()) + .kD(kD.get()) + .kS(kS.get()) + .kG(kG.get()) + .kV(kV.get()) + .kA(kA.get()) + .kMMV(kMMV.get()) + .kMMA(kMMA.get()) + .kMMJ(kMMJ.get()) + .kMMEV(kMMEV.get()) + .kMMEA(kMMEA.get()) + .build(); + } } diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 37e1696..3e516e2 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -23,137 +23,136 @@ * value not in dashboard. */ public class LoggedTunableNumber implements DoubleSupplier { - private static final String tableKey = "/SmartDashboard/TunableNumbers"; - private static final boolean tuningMode = Constants.tuningMode; - - private static List initializedKeys = new ArrayList<>(); - private static int collisionCount = 0; - - private final String key; - private boolean hasDefault = false; - private double defaultValue; - private LoggedNetworkNumber dashboardNumber; - private Map lastHasChangedValues = new HashMap<>(); - - /** - * Create a new LoggedTunableNumber - * - * @param dashboardKey Key on dashboard - */ - public LoggedTunableNumber(String dashboardKey) { - this.key = getNoCollisionKey(tableKey + "/" + dashboardKey); + private static final String tableKey = "/SmartDashboard/TunableNumbers"; + private static final boolean tuningMode = Constants.tuningMode; + + private static List initializedKeys = new ArrayList<>(); + private static int collisionCount = 0; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = getNoCollisionKey(tableKey + "/" + dashboardKey); + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (tuningMode) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } } - - /** - * Create a new LoggedTunableNumber with the default value - * - * @param dashboardKey Key on dashboard - * @param defaultValue Default value - */ - public LoggedTunableNumber(String dashboardKey, double defaultValue) { - this(dashboardKey); - initDefault(defaultValue); + } + + /** + * Ensures the given tunable number key does not already exist. If it does, appends as many + * numbers as are necessary to make it unique, and reports a warning. + */ + public static String getNoCollisionKey(String key) { + String newKey = key; + int appendedNumber = 0; + + while (initializedKeys.contains(newKey)) { + appendedNumber += 1; + collisionCount += 1; + newKey = "%s.%03d".formatted(key, appendedNumber); } - /** - * Set the default value of the number. The default value can only be set once. - * - * @param defaultValue The default value - */ - public void initDefault(double defaultValue) { - if (!hasDefault) { - hasDefault = true; - this.defaultValue = defaultValue; - if (tuningMode) { - dashboardNumber = new LoggedNetworkNumber(key, defaultValue); - } - } - } + if (appendedNumber != 0) { + DriverStation.reportWarning( + "%n!WARNING! - LoggedTunableNumber collision detected at key \"%s\"%nNew Key: \"%s\"" + .formatted(key, newKey), + true); - /** - * Ensures the given tunable number key does not already exist. If it does, appends as many - * numbers as are necessary to make it unique, and reports a warning. - */ - public static String getNoCollisionKey(String key) { - String newKey = key; - int appendedNumber = 0; - - while (initializedKeys.contains(newKey)) { - appendedNumber += 1; - collisionCount += 1; - newKey = "%s.%03d".formatted(key, appendedNumber); - } - - if (appendedNumber != 0) { - DriverStation.reportWarning( - "%n!WARNING! - LoggedTunableNumber collision detected at key \"%s\"%nNew Key: \"%s\"" - .formatted(key, newKey), - true); - - System.out.println(">>> - - - - - COLLISION REPORT - - - - - <<<"); - System.out.println("%d LoggedTubableNumber Collisions".formatted(collisionCount)); - } - - initializedKeys.add(newKey); - return newKey; + System.out.println(">>> - - - - - COLLISION REPORT - - - - - <<<"); + System.out.println("%d LoggedTubableNumber Collisions".formatted(collisionCount)); } - /** - * Get the current value, from dashboard if available and in tuning mode. - * - * @return The current value - */ - public double get() { - if (!hasDefault) { - return 0.0; - } else { - return tuningMode ? dashboardNumber.get() : defaultValue; - } + initializedKeys.add(newKey); + return newKey; + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return tuningMode ? dashboardNumber.get() : defaultValue; } - - /** - * Checks whether the number has changed since our last check - * - * @param id Unique identifier for the caller to avoid conflicts when shared between multiple - * objects. Recommended approach is to pass the result of "hashCode()" - * @return True if the number has changed since the last time this method was called, false - * otherwise. - */ - public boolean hasChanged(int id) { - double currentValue = get(); - Double lastValue = lastHasChangedValues.get(id); - if (lastValue == null || currentValue != lastValue) { - lastHasChangedValues.put(id, currentValue); - return true; - } - - return false; + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; } - /** - * Runs action if any of the tunableNumbers have changed - * - * @param id Unique identifier for the caller to avoid conflicts when shared between multiple - * objects. Recommended approach is to pass the result of "hashCode()" - * @param action Callback to run when any of the tunable numbers have changed. Access tunable - * numbers in order inputted in method - * @param tunableNumbers All tunable numbers to check - */ - public static void ifChanged( - int id, Consumer action, LoggedTunableNumber... tunableNumbers) { - if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { - action.accept( - Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); - } + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); } + } - /** Runs action if any of the tunableNumbers have changed */ - public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { - ifChanged(id, values -> action.run(), tunableNumbers); - } + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } - @Override - public double getAsDouble() { - return get(); - } + @Override + public double getAsDouble() { + return get(); + } } diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java index 8f6a143..bb6e0b8 100644 --- a/src/main/java/frc/robot/util/PhoenixUtil.java +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -35,55 +35,55 @@ */ public class PhoenixUtil { - /** - * Attempts to run the command until no error is produced. - * - * @param maxAttempts Maximum number of attempts before giving up - * @param command The configuration command to run (should return StatusCode) - * @param device Optional device for better error reporting - * @return The final StatusCode (OK if successful, error code otherwise) - */ - public static StatusCode tryUntilOk( - int maxAttempts, Supplier command, Optional device) { - StatusCode error = StatusCode.NotFound; - for (int i = 0; i < maxAttempts; i++) { - error = command.get(); - if (error.isOK()) break; - DriverStation.reportWarning( - String.format( - "Unable to configure device %s: %s", - device.isPresent() ? device.get().getDeviceID() : "?", error.toString()), - true); - } - return error; + /** + * Attempts to run the command until no error is produced. + * + * @param maxAttempts Maximum number of attempts before giving up + * @param command The configuration command to run (should return StatusCode) + * @param device Optional device for better error reporting + * @return The final StatusCode (OK if successful, error code otherwise) + */ + public static StatusCode tryUntilOk( + int maxAttempts, Supplier command, Optional device) { + StatusCode error = StatusCode.NotFound; + for (int i = 0; i < maxAttempts; i++) { + error = command.get(); + if (error.isOK()) break; + DriverStation.reportWarning( + String.format( + "Unable to configure device %s: %s", + device.isPresent() ? device.get().getDeviceID() : "?", error.toString()), + true); } + return error; + } - /** - * Attempts to run the command until no error is produced (without device info for errors). - * - * @param maxAttempts Maximum number of attempts before giving up - * @param command The configuration command to run - * @return The final StatusCode - */ - public static StatusCode tryUntilOk(int maxAttempts, Supplier command) { - return tryUntilOk(maxAttempts, command, Optional.empty()); - } + /** + * Attempts to run the command until no error is produced (without device info for errors). + * + * @param maxAttempts Maximum number of attempts before giving up + * @param command The configuration command to run + * @return The final StatusCode + */ + public static StatusCode tryUntilOk(int maxAttempts, Supplier command) { + return tryUntilOk(maxAttempts, command, Optional.empty()); + } - /** - * Get position from controller without explicit casting. Handles cases where the position - * configuration may not be available yet due to delayed configurations. - * - * @param motor The motor to get position from - * @param defaultPosition Default value if position not available - * @return The position from the controller, or defaultPosition if not available - */ - public static double getPositionFromController(ParentDevice motor, double defaultPosition) { - Map map = motor.getAppliedControl().getControlInfo(); - String positionString = map.get("Position"); - double position = defaultPosition; - if (positionString != null) { - position = Double.valueOf(positionString); - } - return position; + /** + * Get position from controller without explicit casting. Handles cases where the position + * configuration may not be available yet due to delayed configurations. + * + * @param motor The motor to get position from + * @param defaultPosition Default value if position not available + * @return The position from the controller, or defaultPosition if not available + */ + public static double getPositionFromController(ParentDevice motor, double defaultPosition) { + Map map = motor.getAppliedControl().getControlInfo(); + String positionString = map.get("Position"); + double position = defaultPosition; + if (positionString != null) { + position = Double.valueOf(positionString); } + return position; + } } diff --git a/src/main/java/frc/robot/util/SubsystemBehavior.java b/src/main/java/frc/robot/util/SubsystemBehavior.java index 3ef5539..08e7ebb 100644 --- a/src/main/java/frc/robot/util/SubsystemBehavior.java +++ b/src/main/java/frc/robot/util/SubsystemBehavior.java @@ -2,17 +2,19 @@ import frc.robot.goals.RobotGoalEvents; import frc.robot.state.MatchStateEvents; +import frc.robot.subsystems.launcher.LauncherEvents; import java.util.ArrayList; import java.util.List; /** - * Base class for hardware subsystem behaviors that react to robot goals. - * These behaviors translate robot goals into hardware subsystem state changes. + * Base class for hardware subsystem behaviors that react to robot goals. These behaviors translate + * robot goals into hardware subsystem state changes. * - *

Subsystem behaviors listen to the API layer (RobotGoalEvents) and drive - * the hardware layer (Launcher, Drive, etc.). + *

Subsystem behaviors listen to the API layer (RobotGoalEvents) and drive the hardware layer + * (Launcher, Drive, etc.). * *

Usage: + * * 

{@code
  * public class LauncherBehavior extends SubsystemBehavior {
  *     private final LauncherSubsystem launcher;
@@ -29,35 +31,39 @@
  * }
  * }
* - *

TODO (students): Add additional subsystem event interfaces to the signature - * as you create them (e.g., LauncherEvents, DriveEvents) + *

TODO (students): Add additional subsystem event interfaces to the signature as you create them + * (e.g., LauncherEvents, DriveEvents) */ public abstract class SubsystemBehavior extends Behavior { - private static final List subsystemBehaviors = new ArrayList<>(); + private static final List subsystemBehaviors = new ArrayList<>(); - protected SubsystemBehavior() { - super(); - subsystemBehaviors.add(this); - } + protected SubsystemBehavior() { + super(); + subsystemBehaviors.add(this); + } - /** - * Configure all registered subsystem behaviors. - * - * @param goals Robot goal events to react to - * @param matchState Match phase events (disabled, auto, teleop) - */ - public static void configureAll(RobotGoalEvents goals, MatchStateEvents matchState) { - for (SubsystemBehavior behavior : subsystemBehaviors) { - behavior.configure(goals, matchState); - } + /** + * Configure all registered subsystem behaviors. + * + * @param goals Robot goal events to react to + * @param matchState Match phase events (disabled, auto, teleop) + * @param launcher + */ + public static void configureAll( + RobotGoalEvents goals, MatchStateEvents matchState, LauncherEvents launcher) { + for (SubsystemBehavior behavior : subsystemBehaviors) { + behavior.configure(goals, matchState, launcher); } + } - /** - * Configure this subsystem behavior's trigger bindings. - * - * @param goals Robot goal events to react to - * @param matchState Match phase events (disabled, auto, teleop) - */ - public abstract void configure(RobotGoalEvents goals, MatchStateEvents matchState); + /** + * Configure this subsystem behavior's trigger bindings. + * + * @param goals Robot goal events to react to + * @param matchState Match phase events (disabled, auto, teleop) + * @param launcher + */ + public abstract void configure( + RobotGoalEvents goals, MatchStateEvents matchState, LauncherEvents launcher); } diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 80b6e20..22378fd 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -4,17 +4,17 @@ import java.util.List; public abstract class VirtualSubsystem { - private static List subsystems = new ArrayList<>(); + private static List subsystems = new ArrayList<>(); - public VirtualSubsystem() { - subsystems.add(this); - } + public VirtualSubsystem() { + subsystems.add(this); + } - public static void periodicAll() { - for (VirtualSubsystem subsystem : subsystems) { - subsystem.periodic(); - } + public static void periodicAll() { + for (VirtualSubsystem subsystem : subsystems) { + subsystem.periodic(); } + } - public abstract void periodic(); + public abstract void periodic(); }