diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 6eaffbb0..20ef767d 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -3,13 +3,14 @@ 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; @@ -17,7 +18,6 @@ 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 autoChooser; + public final LoggedDashboardChooser 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 deleted file mode 100644 index a6d31925..00000000 --- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.team1540.robot2024.subsystems.led; - -import edu.wpi.first.wpilibj.RobotController; - -public class LedPatternRSLState extends LedPattern { - private LedPattern pattern1; - private LedPattern pattern2; - public LedPatternRSLState(LedPattern pattern1, LedPattern pattern2) { - super(true); - this.pattern1 = pattern1; - this.pattern2 = pattern2; - } - - @Override - void apply(ZonedAddressableLEDBuffer buffer) { - if (RobotController.getRSLState()) { - pattern1.apply(buffer); - } else { - pattern2.apply(buffer); - } - } -} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java new file mode 100644 index 00000000..4cacde1c --- /dev/null +++ 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 index bc2c9c2a..c20b589d 100644 --- 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 255) { saturation = 255; isReversed = !isReversed; diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java similarity index 84% rename from src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java index a0b20551..b79bb4a2 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java +++ b/src/main/java/org/team1540/robot2024/subsystems/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)); diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java new file mode 100644 index 00000000..36ac5ed7 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java @@ -0,0 +1,23 @@ +package org.team1540.robot2024.subsystems.led.patterns; + +import edu.wpi.first.wpilibj.RobotController; +import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer; + +public class LedPatternRSLState extends LedPattern { + private final LedPattern a; + private final LedPattern b; + public LedPatternRSLState(LedPattern a, LedPattern b) { + super(true); + this.a = a; + this.b = b; + } + + @Override + public void apply(ZonedAddressableLEDBuffer buffer) { + if (RobotController.getRSLState()) { + a.apply(buffer); + } else { + b.apply(buffer); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRainbow.java similarity index 71% rename from src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRainbow.java index 62b5162e..83abeab9 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRainbow.java @@ -1,4 +1,6 @@ -package org.team1540.robot2024.subsystems.led; +package org.team1540.robot2024.subsystems.led.patterns; + +import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer; public class LedPatternRainbow extends LedPattern { private final int speed; @@ -10,7 +12,7 @@ public LedPatternRainbow(int speed) { } @Override - void apply(ZonedAddressableLEDBuffer buffer) { + public void apply(ZonedAddressableLEDBuffer buffer) { for (int i = 0; i < buffer.getLength(); i++) { int hue = (initialHue + (i * 182 / buffer.getLength())) % 180; buffer.setHSV(i, hue, 255, 128); diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java similarity index 84% rename from src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java index f063d517..41f45c94 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.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 LedPatternWave extends LedPattern { private static final double degradation = 0.7; @@ -15,7 +16,7 @@ public LedPatternWave(Color color) { } @Override - void apply(ZonedAddressableLEDBuffer buffer) { + public void apply(ZonedAddressableLEDBuffer buffer) { for (int i = 0; i < buffer.getLength(); i++) { int distance = Math.abs(location - i); if (distance > buffer.getLength() / 2) { diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java similarity index 84% rename from src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java index 3863a7e9..7ff0a952 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java +++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.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; import java.util.function.BiConsumer; @@ -11,7 +12,7 @@ private SimpleLedPattern(BiConsumer applier) this.applier = applier; } - void apply(ZonedAddressableLEDBuffer buffer) { + public void apply(ZonedAddressableLEDBuffer buffer) { for (int i = 0; i < buffer.getLength(); i++) { this.applier.accept(buffer, i); }