From 80cf8dc1be327bc0c85e8a275356f97b2dbffdc8 Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Fri, 25 Oct 2024 18:04:04 -0400 Subject: [PATCH] Intake and lights --- .../java/org/team340/robot/Constants.java | 2 + .../org/team340/robot/RobotContainer.java | 23 +-- .../org/team340/robot/subsystems/Intake.java | 2 +- .../org/team340/robot/subsystems/Lights.java | 131 ++++++++++++++++++ 4 files changed, 147 insertions(+), 11 deletions(-) create mode 100644 src/main/java/org/team340/robot/subsystems/Lights.java diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index 4d46854..8a3e5d3 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -46,6 +46,8 @@ public static final class RobotMap { public static final int kPivotLimit = 8; public static final int kNoteDetector = 9; + + public static final int kLights = 9; } public static final class FieldConstants { diff --git a/src/main/java/org/team340/robot/RobotContainer.java b/src/main/java/org/team340/robot/RobotContainer.java index e76f976..3fb3790 100644 --- a/src/main/java/org/team340/robot/RobotContainer.java +++ b/src/main/java/org/team340/robot/RobotContainer.java @@ -28,18 +28,19 @@ @Logged public final class RobotContainer { - private Controller driver; - private Controller coDriver; + private final Controller driver; + private final Controller coDriver; - public Amplifier amplifier; - public Feeder feeder; - public Intake intake; - public Pivot pivot; - public Shooter shooter; - public Swerve swerve; + public final Amplifier amplifier; + public final Feeder feeder; + public final Intake intake; + // public Lights lights; + public final Pivot pivot; + public final Shooter shooter; + public final Swerve swerve; - public Routines routines; - public Autos autos; + public final Routines routines; + public final Autos autos; /** * Entry to initializing subsystems and command execution. @@ -53,6 +54,7 @@ public RobotContainer() { amplifier = new Amplifier(); feeder = new Feeder(); intake = new Intake(); + // lights = new Lights(); pivot = new Pivot(); shooter = new Shooter(); swerve = new Swerve(); @@ -75,6 +77,7 @@ private void configBindings() { // Set default commands. amplifier.setDefaultCommand(amplifier.apply(AmplifierPosition.kRetract)); intake.setDefaultCommand(intake.apply(IntakeState.kRetract)); + // lights.setDefaultCommand(lights.run(feeder::hasNote)); pivot.setDefaultCommand(pivot.apply(PivotPosition.kDown)); shooter.setDefaultCommand(shooter.idle(swerve::getSpeakerDistance)); swerve.setDefaultCommand(swerve.drive(driver::getLeftX, driver::getLeftY, driver::getTriggerDifference)); diff --git a/src/main/java/org/team340/robot/subsystems/Intake.java b/src/main/java/org/team340/robot/subsystems/Intake.java index 539a482..ca64f8d 100644 --- a/src/main/java/org/team340/robot/subsystems/Intake.java +++ b/src/main/java/org/team340/robot/subsystems/Intake.java @@ -109,7 +109,7 @@ public Intake() { .setPositionConversionFactor(Math.PI) .setVelocityConversionFactor(Math.PI) .setInverted(true) - .setZeroOffset(2.876) + .setZeroOffset(0.236) .apply(pivotMotor, pivotEncoder); new SparkPIDControllerConfig().setPID(1.5, 0.0027, 0.0).setIZone(0.1).apply(pivotMotor, pivotPID); diff --git a/src/main/java/org/team340/robot/subsystems/Lights.java b/src/main/java/org/team340/robot/subsystems/Lights.java new file mode 100644 index 0000000..1eb90a0 --- /dev/null +++ b/src/main/java/org/team340/robot/subsystems/Lights.java @@ -0,0 +1,131 @@ +package org.team340.robot.subsystems; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; +import org.team340.lib.util.Alliance; +import org.team340.lib.util.GRRSubsystem; +import org.team340.lib.util.Math2; +import org.team340.robot.Constants.RobotMap; + +public class Lights extends GRRSubsystem { + + private static final int kLength = 56; + + private final AddressableLED lights; + private final AddressableLEDBuffer buffer; + private final Timer timer; + + public Lights() { + lights = new AddressableLED(RobotMap.kLights); + buffer = new AddressableLEDBuffer(kLength); + timer = new Timer(); + + lights.setLength(buffer.getLength()); + apply(); + lights.start(); + timer.start(); + } + + /** + * Applies the buffer to the LED strip. + */ + private void apply() { + lights.setData(buffer); + } + + /** + * Modifies the buffer to be a single color. + * @param r Red value from {@code 0} to {@code 255}. + * @param g Green value from {@code 0} to {@code 255}. + * @param b Blue value from {@code 0} to {@code 255}. + */ + private void set(int r, int g, int b) { + for (int i = 0; i < buffer.getLength(); i++) { + buffer.setRGB(i, r, g, b); + } + } + + /** + * Modifies the buffer with values mirrored across the center of the LED strip. + * @param i The index of the buffer to modify. + * @param r Red value from {@code 0} to {@code 255}. + * @param g Green value from {@code 0} to {@code 255}. + * @param b Blue value from {@code 0} to {@code 255}. + */ + private void setMirrored(int i, int r, int g, int b) { + buffer.setRGB(i, r, g, b); + buffer.setRGB(buffer.getLength() - i - 1, r, g, b); + } + + /** + * Runs the lights. + */ + public Command run(Supplier hasNote) { + int[] state = new int[buffer.getLength()]; + for (int i = 0; i < state.length; i++) { + state[i] = 0; + } + + return commandBuilder() + .onExecute(() -> { + if (DriverStation.isTeleopEnabled()) { + if (hasNote.get()) { + double time = timer.get(); + int v = time % 1.0 > 0.5 ? 255 : 0; + for (int i = 0; i < buffer.getLength() / 2; i++) { + setMirrored(i, 0, v, 0); + } + } else { + int v = (int) (((Math.cos(timer.get() * 8.5) + 1.0) * 87.5) + 25.0); + if (Alliance.isBlue()) { + set(0, 0, v); + } else { + set(v, 0, 0); + } + } + } else if (DriverStation.isAutonomousEnabled()) { + for (int i = 0; i < buffer.getLength() / 2; i++) { + state[i] = (int) Math.max( + 0.0, + state[i] - Math2.random((0.5 + (i / (buffer.getLength() * 0.25))) * 70.0) + 4.0 + ); + } + for (int i = (buffer.getLength() / 2) - 1; i >= 2; i--) { + state[i] = (state[i - 1] + state[i - 2] + state[i - 2]) / 3; + } + if (Math.random() < 0.5) { + int i = (int) Math2.random(8.0); + state[i] = (int) (state[i] + Math2.random(160.0, 255.0)); + } + for (int i = 0; i < buffer.getLength() / 2; i++) { + int heat = (int) ((state[i] / 255.0) * 191.0); + int ramp = (heat & 63) << 2; + if (heat > 180) { + setMirrored(i, 255, 255, ramp); + } else if (heat > 60) { + setMirrored(i, 255, ramp, 0); + } else { + setMirrored(i, ramp, 0, 0); + } + } + } else if (DriverStation.isDisabled()) { + if (Alliance.isBlue()) set(0, 0, 150); + else set(150, 0, 0); + } else { + set(0, 0, 0); + } + + apply(); + }) + .onEnd(() -> { + set(0, 0, 0); + apply(); + }) + .ignoringDisable(true) + .withName("lights.defaultCommand()"); + } +}