feat: add triaging

flamingchickens1540 · Feb 6, 2024 · 102fd7b · 102fd7b
1 parent 2fba8a2
commit 102fd7b
Show file tree

Hide file tree

Showing 12 changed files with 182 additions and 86 deletions.
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -3,21 +3,21 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 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.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
 import org.team1540.robot2024.commands.FeedForwardCharacterization;
 import org.team1540.robot2024.commands.SwerveDriveCommand;
 import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
 import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.commands.indexer.IntakeCommand;
 import org.team1540.robot2024.commands.shooter.ShootSequence;
-import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
 import org.team1540.robot2024.subsystems.drive.*;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
@@ -28,6 +28,7 @@
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
 import org.team1540.robot2024.subsystems.led.Leds;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
 import org.team1540.robot2024.subsystems.shooter.*;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 import org.team1540.robot2024.subsystems.tramp.TrampIO;
@@ -64,7 +65,7 @@ public class RobotContainer {
     public final CommandXboxController copilot = new CommandXboxController(1);
 
     // Dashboard inputs
-//    public final LoggedDashboardChooser<Command> autoChooser;
+    public final LoggedDashboardChooser<Command> autoChooser;
 
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
@@ -75,29 +76,29 @@ public class RobotContainer {
      */
     public RobotContainer() {
         switch (Constants.currentMode) {
-//            case REAL:
-//                // Real robot, instantiate hardware IO implementations
-//                drivetrain =
-//                        new Drivetrain(
-//                                new GyroIOPigeon2(odometrySignalRefresher),
-//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
-//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
-//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
-//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
-//                tramp = new Tramp(new TrampIOSparkMax());
-//                shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
-//                elevator = new Elevator(new ElevatorIOTalonFX());
-//                indexer =
-//                        new Indexer(
-//                                new IndexerIOSparkMax()
-//                        );
-//                aprilTagVision = new AprilTagVision(
-//                        new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
-//                        new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
-//                        drivetrain::addVisionMeasurement,
-//                        () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
-//                        new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
-//                break;
+            case REAL:
+                // Real robot, instantiate hardware IO implementations
+                drivetrain =
+                        new Drivetrain(
+                                new GyroIOPigeon2(odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
+                tramp = new Tramp(new TrampIOSparkMax());
+                shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
+                elevator = new Elevator(new ElevatorIOTalonFX());
+                indexer =
+                        new Indexer(
+                                new IndexerIOSparkMax()
+                        );
+                aprilTagVision = new AprilTagVision(
+                        new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
+                        new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
+                        drivetrain::addVisionMeasurement,
+                        () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
+                        new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
+                break;
             case SIM:
                 // Sim robot, instantiate physics sim IO implementations
                 drivetrain =
@@ -150,22 +151,31 @@ public RobotContainer() {
 
 
         // Set up auto routines
-//        autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
-//
-//        // Set up FF characterization routines
-//        autoChooser.addOption(
-//                "Drive FF Characterization",
-//                new FeedForwardCharacterization(
-//                        drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
-//        autoChooser.addOption(
-//                "Flywheels FF Characterization",
-//                new FeedForwardCharacterization(
-//                        shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
+        autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+
+        // Set up FF characterization routines
+        autoChooser.addOption(
+                "Drive FF Characterization",
+                new FeedForwardCharacterization(
+                        drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
+        autoChooser.addOption(
+                "Flywheels FF Characterization",
+                new FeedForwardCharacterization(
+                        shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
 
         // Configure the button bindings
-//        configureButtonBindings();
+        configureButtonBindings();
+        configureLedBindings();
     }
 
+    private void configureLedBindings() {
+        leds.setFatalPattern(new LedPatternFlame());
+        new Trigger(DriverStation::isDSAttached)
+                .onTrue(Commands.runOnce(leds::clearFatalPattern)
+                            .ignoringDisable(true))
+                .onFalse(Commands.runOnce(() -> leds.setFatalPattern(new LedPatternFlame()))
+                            .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

diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java
@@ -0,0 +1,38 @@
+package org.team1540.robot2024.subsystems.led;
+
+import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow;
+
+
+public class LedTriager {
+    private final LedPattern[] patterns = new LedPattern[Leds.CRITICALITY_COUNT];
+    private final LedPattern defaultPattern = new LedPatternRainbow(1);
+    private boolean isNew = true;
+    public LedPattern getPattern() {
+        for (int i = patterns.length -1; i >= 0; i--) {
+            if (patterns[i] != null) {
+                return patterns[i];
+            }
+        }
+        return defaultPattern;
+    }
+
+    public boolean shouldRefresh() {
+        final boolean val = isNew || getPattern().isDynamic();
+        isNew = false;
+        return val;
+    }
+
+    public void clearPattern(Leds.PatternCriticality criticality) {
+        patterns[criticality.ordinal()] = null;
+        isNew = true;
+    }
+
+    public boolean addPattern(LedPattern pattern, Leds.PatternCriticality criticality) {
+        patterns[criticality.ordinal()] = pattern;
+        isNew = true;
+        return getPattern() == pattern;
+    }
+
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
@@ -4,16 +4,16 @@
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
 
 
 import static org.team1540.robot2024.Constants.LED_STRIP_PORT_PWM;
 
 public class Leds extends SubsystemBase {
-
     private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(Constants.LED_STRIP_LENGTH);
     private final AddressableLED strip = new AddressableLED(LED_STRIP_PORT_PWM);
     private final ZonedAddressableLEDBuffer[] buffers = new ZonedAddressableLEDBuffer[ZONE_COUNT];
-    private final LedPattern[] patterns = new LedPattern[ZONE_COUNT];
+    private final LedTriager[] patterns = new LedTriager[ZONE_COUNT];
 
     public Leds() {
         strip.setLength(ledBuffer.getLength());
@@ -22,29 +22,62 @@ public Leds() {
 
         buffers[Zone.ZONE1.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 40, false);
         buffers[Zone.ZONE2.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 40, 80, false);
+        for (int i = 0; i < ZONE_COUNT;i++) {
+            patterns[i] = new LedTriager();
+        }
     }
 
     @Override
     public void periodic() {
         for (int i = 0; i < ZONE_COUNT;i++) {
-            if (patterns[i].isDynamic()) {
-                patterns[i].apply(buffers[i]);
+            if (patterns[i].shouldRefresh()) {
+                patterns[i].getPattern().apply(buffers[i]);
             }
         }
         strip.setData(ledBuffer);
     }
 
+    public void setPattern(Zone zone, LedPattern pattern, PatternCriticality criticality) {
+        patterns[zone.ordinal()].addPattern(pattern, criticality);
+        pattern.setLength(buffers[zone.ordinal()].getLength());
+    }
+
     public void setPattern(Zone zone, LedPattern pattern) {
-        patterns[zone.ordinal()] = pattern;
-        if (!pattern.isDynamic()) {
-            pattern.apply(buffers[zone.ordinal()]);
+        setPattern(zone, pattern, PatternCriticality.INFO);
+    }
+
+    public void clearPattern(Zone zone, PatternCriticality criticality) {
+        patterns[zone.ordinal()].clearPattern(criticality);
+    }
+
+    public void setFatalPattern(LedPattern pattern) {
+        System.out.println("Setting criticality");
+        for (int i = 0; i<ZONE_COUNT;i++) {
+            patterns[i].addPattern(pattern, PatternCriticality.FATAL);
+            pattern.setLength(buffers[i].getLength());
         }
     }
 
+    public void clearFatalPattern() {
+        System.out.println("Clearing criticality");
+        for (int i = 0; i<ZONE_COUNT;i++) {
+            patterns[i].clearPattern(PatternCriticality.FATAL);
+        }
+
+    }
+
 
     private static final int ZONE_COUNT=Zone.values().length;
     public enum Zone {
         ZONE1,
         ZONE2;
     }
+    static final int CRITICALITY_COUNT=PatternCriticality.values().length;
+    public enum PatternCriticality {
+        DECORATION,
+        INFO,
+        CRITICAL,
+        FATAL
+    }
+
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
@@ -5,10 +5,10 @@
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 
-class ZonedAddressableLEDBuffer {
-    public final boolean isInverted;
-    public final int start;
-    public final int length;
+public class ZonedAddressableLEDBuffer {
+    private final boolean isInverted;
+    private final int start;
+    private final int length;
     private final AddressableLEDBuffer buffer;
 
     public ZonedAddressableLEDBuffer(AddressableLEDBuffer buffer, int start, int end, boolean isInverted) {

diff --git a/.../robot2024/subsystems/led/LedPattern.java → ...4/subsystems/led/patterns/LedPattern.java b/.../robot2024/subsystems/led/LedPattern.java → ...4/subsystems/led/patterns/LedPattern.java
@@ -1,6 +1,7 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
 
 import edu.wpi.first.wpilibj.util.Color;
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
 
 public abstract class LedPattern {
     private final boolean isDynamic;
@@ -9,11 +10,12 @@ protected LedPattern(boolean isDynamic) {
         this.isDynamic = isDynamic;
     }
 
-    final boolean isDynamic() {
+    public final boolean isDynamic() {
         return isDynamic;
     }
 
-    abstract void apply(ZonedAddressableLEDBuffer buffer);
+    public abstract void apply(ZonedAddressableLEDBuffer buffer);
+    public void setLength(int length) {}
 
     protected static int getHue(Color color) {
         final int red = (int) color.red * 255;

diff --git a/...4/subsystems/led/LedPatternBreathing.java → ...ems/led/patterns/LedPatternBreathing.java b/...4/subsystems/led/LedPatternBreathing.java → ...ems/led/patterns/LedPatternBreathing.java
@@ -1,6 +1,7 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
 
 import edu.wpi.first.wpilibj.util.Color;
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
 
 public class LedPatternBreathing extends LedPattern {
     private final int speed;
@@ -15,7 +16,7 @@ public LedPatternBreathing(int speed, Color color) {
     }
 
     @Override
-    void apply(ZonedAddressableLEDBuffer buffer) {
+    public void apply(ZonedAddressableLEDBuffer buffer) {
         if (saturation > 255) {
             saturation = 255;
             isReversed = !isReversed;

diff --git a/...t2024/subsystems/led/LedPatternFlame.java → ...systems/led/patterns/LedPatternFlame.java b/...t2024/subsystems/led/LedPatternFlame.java → ...systems/led/patterns/LedPatternFlame.java
@@ -1,16 +1,19 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
 
 
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
+
 import java.awt.*;
 import java.util.Random;
 
 public class LedPatternFlame extends LedPattern {
     private static final Random generator = new Random();
+    private static final boolean reverseDirection = true;
 
     private final int cooling;
     private final int sparking = 123;
-    private final boolean reverseDirection = true;
-    private final int[] temperatures = new int[40];
+
+    private int[] temperatures;
 
 
     public LedPatternFlame(int cooling) {
@@ -22,9 +25,13 @@ public LedPatternFlame() {
         this(62);
     }
 
+    @Override
+    public void setLength(int length) {
+        this.temperatures = new int[length];
+    }
 
     @Override
-    void apply(ZonedAddressableLEDBuffer buffer) {
+    public void apply(ZonedAddressableLEDBuffer buffer) {
 
         for (int i = 0; i < buffer.getLength(); i++) {
             temperatures[i] = bit8Subtraction(temperatures[i], getRandomInt(0, ((cooling * 10) / buffer.getLength()) + 2));