Merge branch 'CSnair/LED'

src/main/java/frc/robot/BuildConstants.java

@@ -13,5 +13,6 @@ public final class BuildConstants {
   public static final long BUILD_UNIX_TIME = 1708320243051L;
   public static final int DIRTY = 1;
 
-  private BuildConstants() {}
+  private BuildConstants() {
+  }
 }

src/main/java/frc/robot/Constants.java

@@ -16,11 +16,15 @@
 import edu.wpi.first.math.util.Units;
 
 /**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be declared
+ * The Constants class provides a convenient place for teams to hold robot-wide
+ * numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants
+ * should be declared
  * globally (i.e. public static). Do not put anything functional in this class.
  *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * <p>
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
@@ -38,8 +42,7 @@ public static class SwerveConstants {
     public static final double MAX_LINEAR_SPEED = 5.56;
     public static final double TRACK_WIDTH_X = Units.inchesToMeters(26.0);
     public static final double TRACK_WIDTH_Y = Units.inchesToMeters(26.0);
-    public static final double DRIVE_BASE_RADIUS =
-        Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
+    public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
     public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
   }
 
@@ -83,7 +86,22 @@ public static class ElevatorConstants {
     public static final double PIVOT_THRESHOLD = 0;
     public static final double EXTENDER_THRESHOLD = 0;
 
-    public static final double[] PIVOT_PID = {0, 0, 0};
-    public static final double[] EXTENDER_PID = {0, 0, 0};
+    public static final double[] PIVOT_PID = { 0, 0, 0 };
+    public static final double[] EXTENDER_PID = { 0, 0, 0 };
+  }
+
+  public static class LEDConstants {
+    public static final double COLOR_BLUE = 0.87;
+    public static final double COLOR_RED = 0.61;
+    public static final double COLOR_YELLOW = 0.66;
+    public static final double COLOR_VIOLET = 0.91;
+  }
+
+  public static enum LED_STATE {
+    BLUE,
+    RED,
+    YELLOW,
+    VIOLET,
+    OFF
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -43,148 +43,161 @@
 import frc.robot.subsystems.intake.IntakeRollerIOSim;
 import frc.robot.subsystems.intake.IntakeRollerIOSparkFlex;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.LED.LED;
+import frc.robot.subsystems.LED.LED_IOSpark;
 
 /**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in
+ * the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of
+ * the robot (including
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  // Subsystems
-  private final Drive drive;
-  private final Intake intake;
-  private final Shooter shooter;
-  public final Elevator elevator;
+    // Subsystems
+    private final Drive drive;
+    private final Intake intake;
+    private final Shooter shooter;
+    public final Elevator elevator;
+    private final LED led;
 
-  private final CommandXboxController controller = new CommandXboxController(0);
-  private final LoggedDashboardChooser<Command> autoChooser;
+    private final CommandXboxController controller = new CommandXboxController(0);
+    private final LoggedDashboardChooser<Command> autoChooser;
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
-  public RobotContainer() {
-    switch (Constants.currentMode) {
-      case REAL:
-        drive =
-            new Drive(
-                new GyroIOPigeon2(),
-                new ModuleIOTalonFX(0),
-                new ModuleIOTalonFX(1),
-                new ModuleIOTalonFX(2),
-                new ModuleIOTalonFX(3));
-        intake = new Intake(new IntakeRollerIOSparkFlex(RobotMap.IntakeIDs.ROLLERS));
-        shooter =
-            new Shooter(
-                new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
-                new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
-                new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER));
-        elevator =
-            new Elevator(
-                new ElevatorPivotIOTalonFX(
-                    RobotMap.ElevatorIDs.PIVOT, RobotMap.ElevatorIDs.CANCODER),
-                new ElevatorExtenderIOTalonFX(
-                    RobotMap.ElevatorIDs.EXTENDERS[0], RobotMap.ElevatorIDs.EXTENDERS[1]));
-        break;
-      case REPLAY:
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim());
-        intake = new Intake(new IntakeRollerIOSim());
-        shooter = new Shooter(new FlywheelIOSim(), new FlywheelIOSim(), new FeederIOSim());
-        elevator = new Elevator(new ElevatorPivotIOSim(), new ElevatorExtenderIOSim());
-        break;
-      case SIM:
-        // Sim robot, instantiate physics sim IO implementations
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim());
-        intake = new Intake(new IntakeRollerIOSim());
-        shooter = new Shooter(new FlywheelIOSim(), new FlywheelIOSim(), new FeederIOSim());
-        elevator = new Elevator(new ElevatorPivotIOSim(), new ElevatorExtenderIOSim());
-        break;
+    /**
+     * The container for the robot. Contains subsystems, OI devices, and commands.
+     */
+    public RobotContainer() {
+        switch (Constants.currentMode) {
+            case REAL:
+                drive = new Drive(
+                        new GyroIOPigeon2(),
+                        new ModuleIOTalonFX(0),
+                        new ModuleIOTalonFX(1),
+                        new ModuleIOTalonFX(2),
+                        new ModuleIOTalonFX(3));
+                intake = new Intake(new IntakeRollerIOSparkFlex(RobotMap.IntakeIDs.ROLLERS));
+                shooter = new Shooter(
+                        new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
+                        new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
+                        new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER));
+                elevator = new Elevator(
+                        new ElevatorPivotIOTalonFX(
+                                RobotMap.ElevatorIDs.PIVOT, RobotMap.ElevatorIDs.CANCODER),
+                        new ElevatorExtenderIOTalonFX(
+                                RobotMap.ElevatorIDs.EXTENDERS[0], RobotMap.ElevatorIDs.EXTENDERS[1]));
+                led = new LED(new LED_IOSpark(1));
+                break;
+            case REPLAY:
+                drive = new Drive(
+                        new GyroIO() {
+                        },
+                        new ModuleIOSim(),
+                        new ModuleIOSim(),
+                        new ModuleIOSim(),
+                        new ModuleIOSim());
+                intake = new Intake(new IntakeRollerIOSim());
+                shooter = new Shooter(new FlywheelIOSim(), new FlywheelIOSim(), new FeederIOSim());
+                elevator = new Elevator(new ElevatorPivotIOSim(), new ElevatorExtenderIOSim());
+                led = new LED(new LED_IO());
+                break;
+            case SIM:
+                // Sim robot, instantiate physics sim IO implementations
+                drive = new Drive(
+                        new GyroIO() {
+                        },
+                        new ModuleIOSim(),
+                        new ModuleIOSim(),
+                        new ModuleIOSim(),
+                        new ModuleIOSim());
+                intake = new Intake(new IntakeRollerIOSim());
+                shooter = new Shooter(new FlywheelIOSim(), new FlywheelIOSim(), new FeederIOSim());
+                elevator = new Elevator(new ElevatorPivotIOSim(), new ElevatorExtenderIOSim());
+                led = new LED(new LED_IO());
+                break;
 
-      default:
-        // Replayed robot, disable IO implementations
-        intake = new Intake(new IntakeRollerIOSparkFlex(RobotMap.IntakeIDs.ROLLERS));
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {});
-        shooter =
-            new Shooter(
-                new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
-                new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
-                new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER));
-        elevator =
-            new Elevator(
-                new ElevatorPivotIOTalonFX(
-                    RobotMap.ElevatorIDs.PIVOT, RobotMap.ElevatorIDs.CANCODER),
-                new ElevatorExtenderIOTalonFX(
-                    RobotMap.ElevatorIDs.EXTENDERS[0], RobotMap.ElevatorIDs.EXTENDERS[1]));
-        break;
-    }
+            default:
+                // Replayed robot, disable IO implementations
+                intake = new Intake(new IntakeRollerIOSparkFlex(RobotMap.IntakeIDs.ROLLERS));
+                drive = new Drive(
+                        new GyroIO() {
+                        },
+                        new ModuleIO() {
+                        },
+                        new ModuleIO() {
+                        },
+                        new ModuleIO() {
+                        },
+                        new ModuleIO() {
+                        });
+                shooter = new Shooter(
+                        new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
+                        new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
+                        new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER));
+                elevator = new Elevator(
+                        new ElevatorPivotIOTalonFX(
+                                RobotMap.ElevatorIDs.PIVOT, RobotMap.ElevatorIDs.CANCODER),
+                        new ElevatorExtenderIOTalonFX(
+                                RobotMap.ElevatorIDs.EXTENDERS[0], RobotMap.ElevatorIDs.EXTENDERS[1]));
+                led = new LED(new LED_IO());
+                break;
+        }
 
-    // Set up auto routines
-    autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+        // Set up auto routines
+        autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
 
-    configureButtonBindings();
-  }
+        configureButtonBindings();
+    }
 
-  /**
-   * Use this method to define your button->command mappings. Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
-   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-   */
-  private void configureButtonBindings() {
-    drive.setDefaultCommand(
-        DriveCommands.joystickDrive(
-            drive,
-            () -> -controller.getLeftY(),
-            () -> -controller.getLeftX(),
-            () -> -controller.getRightX()));
-    controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
-    controller
-        .b()
-        .onTrue(
-            Commands.runOnce(
-                    () ->
-                        drive.setPose(
-                            new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
-                    drive)
-                .ignoringDisable(true));
+    /**
+     * Use this method to define your button->command mappings. Buttons can be
+     * created by
+     * instantiating a {@link GenericHID} or one of its subclasses ({@link
+     * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing
+     * it to a {@link
+     * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
+    private void configureButtonBindings() {
+        drive.setDefaultCommand(
+                DriveCommands.joystickDrive(
+                        drive,
+                        () -> -controller.getLeftY(),
+                        () -> -controller.getLeftX(),
+                        () -> -controller.getRightX()));
+        controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+        controller
+                .b()
+                .onTrue(
+                        Commands.runOnce(
+                                () -> drive.setPose(
+                                        new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
+                                drive)
+                                .ignoringDisable(true));
 
-    // Schedule `exampleMethodCommand` when the Xbox controller's B button is
-    // pressed,
-    // cancelling on release.
+        // Schedule `exampleMethodCommand` when the Xbox controller's B button is
+        // pressed,
+        // cancelling on release.
 
-    controller.a().onTrue(new InstantCommand(() -> intake.runRollers(3)));
-    controller.a().onFalse(new InstantCommand(() -> intake.stopRollers()));
+        controller.a().onTrue(new InstantCommand(() -> intake.runRollers(3)));
+        controller.a().onFalse(new InstantCommand(() -> intake.stopRollers()));
 
-    controller.x().onTrue(new InstantCommand(() -> shooter.setShooterVelocitys(3000, 3000)));
-    controller.x().onFalse(new InstantCommand(() -> shooter.stopShooterMotors()));
+        controller.x().onTrue(new InstantCommand(() -> shooter.setShooterVelocitys(3000, 3000)));
+        controller.x().onFalse(new InstantCommand(() -> shooter.stopShooterMotors()));
 
-    controller.leftBumper().onTrue(Commands.run(() -> elevator.setExtenderGoal(1), elevator));
-    controller.rightBumper().onTrue(Commands.run(() -> elevator.setPivotGoal(Math.PI), elevator));
-    controller.y().onTrue(Commands.run(() -> elevator.setPivotGoal(0), elevator));
-  }
+        controller.leftBumper().onTrue(Commands.run(() -> elevator.setExtenderGoal(1), elevator));
+        controller.rightBumper().onTrue(Commands.run(() -> elevator.setPivotGoal(Math.PI), elevator));
+        controller.y().onTrue(Commands.run(() -> elevator.setPivotGoal(0), elevator));
+    }
 
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
-  public Command getAutonomousCommand() {
-    return autoChooser.get();
-  }
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+        return autoChooser.get();
+    }
 }

src/main/java/frc/robot/subsystems/LED/LED.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.LED;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.LED_STATE;
+
+import org.littletonrobotics.junction.Logger;
+
+/** Add your docs here. */
+public class LED extends SubsystemBase {
+  private final LED_IO led;
+  private final LED_IOInputsAutoLogged lInputs = new LED_IOInputsAutoLogged();
+
+  public LED(LED_IO led) {
+    this.led = led;
+  }
+
+  @Override
+  public void periodic() {
+    led.updateInputs(lInputs);
+
+    setColor(lInputs.ledState);
+
+    Logger.processInputs("LED Inputs", lInputs);
+  }
+
+  public void noBumpersPressed() {
+    led.noBumpersPressed();
+  }
+
+  public void setColor(LED_STATE state) {
+    led.setColor(state);
+    Logger.recordOutput("Set State", state);
+  }
+}