From 1b3d8815f32e8349ed54bfd27e97513f9745a3ee Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Fri, 10 Jan 2025 17:43:23 -0500 Subject: [PATCH 1/9] setup files --- .../java/org/sciborgs1155/robot/Ports.java | 4 ++++ .../sciborgs1155/robot/led/LedConstants.java | 5 +++++ .../org/sciborgs1155/robot/led/LedStrip.java | 18 ++++++++++++++++++ 3 files changed, 27 insertions(+) create mode 100644 src/main/java/org/sciborgs1155/robot/led/LedConstants.java create mode 100644 src/main/java/org/sciborgs1155/robot/led/LedStrip.java diff --git a/src/main/java/org/sciborgs1155/robot/Ports.java b/src/main/java/org/sciborgs1155/robot/Ports.java index 18a0b5a..cb9e0d2 100644 --- a/src/main/java/org/sciborgs1155/robot/Ports.java +++ b/src/main/java/org/sciborgs1155/robot/Ports.java @@ -25,4 +25,8 @@ public static final class Scoral { public static final int BOTTOM_ROLLER = 22; public static final int BEAMBREAK = 23; } + + public static final class LEDs { + public static final int LED_PORT = 1; + } } diff --git a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java new file mode 100644 index 0000000..e1a48ac --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java @@ -0,0 +1,5 @@ +package org.sciborgs1155.robot.led; + +public class LedConstants { + +} diff --git a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java new file mode 100644 index 0000000..7b3ab3e --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java @@ -0,0 +1,18 @@ +package org.sciborgs1155.robot.led; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import monologue.Logged; + +import static org.sciborgs1155.robot.Ports.LEDs.*; + +public class LedStrip extends SubsystemBase implements Logged, AutoCloseable{ + + private final AddressableLED led = new AddressableLED(LED_PORT); + + @Override + public void close() throws Exception { + led.close(); + } + +} From 23bf496ed5454016b0bb7befd55e232b321f28ae Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Sat, 11 Jan 2025 09:46:38 -0500 Subject: [PATCH 2/9] basic methods and rainbow --- simgui.json | 8 ++++ .../java/org/sciborgs1155/robot/Robot.java | 4 ++ .../sciborgs1155/robot/led/LedConstants.java | 13 ++++++- .../org/sciborgs1155/robot/led/LedStrip.java | 39 +++++++++++++++---- 4 files changed, 55 insertions(+), 9 deletions(-) diff --git a/simgui.json b/simgui.json index 640332e..b7fb899 100644 --- a/simgui.json +++ b/simgui.json @@ -1,5 +1,10 @@ { "HALProvider": { + "Addressable LEDs": { + "window": { + "visible": true + } + }, "Other Devices": { "window": { "visible": false @@ -9,6 +14,8 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/Faults/Active Faults": "Alerts", + "/Faults/Total Faults": "Alerts", "/Robot/autos": "String Chooser", "/Robot/drive/field2d": "Field2d", "/Robot/drive/frontLeft/driveFeedback": "PIDController", @@ -21,6 +28,7 @@ "/Robot/drive/rearRight/turnFeedback": "PIDController", "/Robot/drive/rotationController": "PIDController", "/Robot/drive/translationController": "ProfiledPIDController", + "/SmartDashboard/Alerts": "Alerts", "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d", "/SmartDashboard/drive dynamic backward": "Command", diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index c069140..3df79e3 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -32,6 +32,7 @@ import org.sciborgs1155.robot.Ports.OI; import org.sciborgs1155.robot.commands.Autos; import org.sciborgs1155.robot.drive.Drive; +import org.sciborgs1155.robot.led.LedStrip; import org.sciborgs1155.robot.vision.Vision; /** @@ -50,6 +51,7 @@ public class Robot extends CommandRobot implements Logged { // SUBSYSTEMS private final Drive drive = Drive.create(); private final Vision vision = Vision.create(); + private final LedStrip led = new LedStrip(); // COMMANDS @Log.NT private final SendableChooser autos = Autos.configureAutos(drive); @@ -139,6 +141,8 @@ private void configureBindings() { .onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER)) .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER)); + driver.x().onTrue(led.rainbow()); + // TODO: Add any additional bindings. } diff --git a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java index e1a48ac..0d7bce3 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java @@ -1,5 +1,16 @@ package org.sciborgs1155.robot.led; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.LEDPattern; + public class LedConstants { - + // Lengt6h of the LED Strip. + public static final int LED_LENGTH = 64; + public static final Distance LED_SPACING = Meters.of(0.01); + + public static final LEDPattern RAINBOW = + LEDPattern.rainbow(200, 200).scrollAtAbsoluteSpeed(MetersPerSecond.of(0.25), LED_SPACING); } diff --git a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java index 7b3ab3e..d596640 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java @@ -1,18 +1,41 @@ package org.sciborgs1155.robot.led; +import static org.sciborgs1155.robot.Ports.LEDs.*; +import static org.sciborgs1155.robot.led.LedConstants.*; + import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import monologue.Logged; -import static org.sciborgs1155.robot.Ports.LEDs.*; +public class LedStrip extends SubsystemBase implements Logged, AutoCloseable { + + private final AddressableLED led = new AddressableLED(LED_PORT); + private final AddressableLEDBuffer buffer; + + public LedStrip() { + led.setLength(LED_LENGTH); + buffer = new AddressableLEDBuffer(LED_LENGTH); + led.setData(buffer); + led.start(); + } -public class LedStrip extends SubsystemBase implements Logged, AutoCloseable{ + public Command rainbow() { + return set(RAINBOW); + } - private final AddressableLED led = new AddressableLED(LED_PORT); + public Command set(LEDPattern pattern) { + return run( + () -> { + pattern.applyTo(buffer); + led.setData(buffer); + }); + } - @Override - public void close() throws Exception { - led.close(); - } - + @Override + public void close() throws Exception { + led.close(); + } } From a2cfe09a149b6795f2ad564518268b6b84fcf0b3 Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Sat, 11 Jan 2025 12:10:07 -0500 Subject: [PATCH 3/9] some more led commands --- simgui.json | 3 +++ .../java/org/sciborgs1155/robot/Robot.java | 11 ++++++++++ .../sciborgs1155/robot/led/LedConstants.java | 12 +++++++++- .../org/sciborgs1155/robot/led/LedStrip.java | 22 +++++++++++++++++++ 4 files changed, 47 insertions(+), 1 deletion(-) diff --git a/simgui.json b/simgui.json index b7fb899..feeaf6a 100644 --- a/simgui.json +++ b/simgui.json @@ -1,6 +1,9 @@ { "HALProvider": { "Addressable LEDs": { + "0": { + "columns": 64 + }, "window": { "visible": true } diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 3df79e3..dd8c78d 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -143,6 +143,17 @@ private void configureBindings() { driver.x().onTrue(led.rainbow()); + driver + .y() + .onTrue( + led.elevator( + () -> + Math.abs( + driver.getLeftX() * driver.getLeftY() + + driver.getLeftX() + + driver.getLeftY()))); + driver.a().onTrue(led.music()); + // TODO: Add any additional bindings. } diff --git a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java index 0d7bce3..8420d81 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java @@ -2,9 +2,12 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.util.Color; public class LedConstants { // Lengt6h of the LED Strip. @@ -12,5 +15,12 @@ public class LedConstants { public static final Distance LED_SPACING = Meters.of(0.01); public static final LEDPattern RAINBOW = - LEDPattern.rainbow(200, 200).scrollAtAbsoluteSpeed(MetersPerSecond.of(0.25), LED_SPACING); + LEDPattern.rainbow(225, 225).scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING); + public static final LEDPattern MUSIC_60_BPM = + LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow) + .mask( + LEDPattern.progressMaskLayer( + () -> + Math.sin(RobotController.getMeasureTime().in(Seconds) * Math.PI * 2) / 3 + + 0.5)); } diff --git a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java index d596640..362737c 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java @@ -6,8 +6,10 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; import monologue.Logged; public class LedStrip extends SubsystemBase implements Logged, AutoCloseable { @@ -22,10 +24,30 @@ public LedStrip() { led.start(); } + /** Rainbow LEDs. Very cool. */ public Command rainbow() { return set(RAINBOW); } + /** + * A gradient of green to yellow LEDs, with an applied mask of how much the elevator is raised. + * + * @param percent A double supplier that supplies the elevator's percent raised. + */ + public Command elevator(DoubleSupplier percent) { + return set( + LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow) + .mask(LEDPattern.progressMaskLayer(percent))); + } + + /** + * A gradient of green to yellow LEDs, moving at 60 bpm, which synchronizes with many songs, such + * as common 120 bpm songs today. + */ + public Command music() { + return set(MUSIC_60_BPM); + } + public Command set(LEDPattern pattern) { return run( () -> { From 6b801e3e9d64bd074c691862f16af3af01627273 Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Sat, 11 Jan 2025 16:03:28 -0500 Subject: [PATCH 4/9] adress some comments --- src/main/java/org/sciborgs1155/robot/Robot.java | 13 ------------- .../org/sciborgs1155/robot/led/LedConstants.java | 6 +++++- .../java/org/sciborgs1155/robot/led/LedStrip.java | 7 ++----- 3 files changed, 7 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index dd8c78d..85a1d3f 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -141,19 +141,6 @@ private void configureBindings() { .onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER)) .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER)); - driver.x().onTrue(led.rainbow()); - - driver - .y() - .onTrue( - led.elevator( - () -> - Math.abs( - driver.getLeftX() * driver.getLeftY() - + driver.getLeftX() - + driver.getLeftY()))); - driver.a().onTrue(led.music()); - // TODO: Add any additional bindings. } diff --git a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java index 8420d81..fd00b31 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java @@ -10,12 +10,16 @@ import edu.wpi.first.wpilibj.util.Color; public class LedConstants { - // Lengt6h of the LED Strip. + // The length of the LED Strip. public static final int LED_LENGTH = 64; + // The distance between two LEDs on the LED Strip. public static final Distance LED_SPACING = Meters.of(0.01); + // A rainbow, scrolling at 0.5 m/s. public static final LEDPattern RAINBOW = LEDPattern.rainbow(225, 225).scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING); + + // A maksed graident that rises and falls at 60 BPM. public static final LEDPattern MUSIC_60_BPM = LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow) .mask( diff --git a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java index 362737c..91828a8 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java @@ -34,16 +34,13 @@ public Command rainbow() { * * @param percent A double supplier that supplies the elevator's percent raised. */ - public Command elevator(DoubleSupplier percent) { + public Command elevatorLED(DoubleSupplier percent) { return set( LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow) .mask(LEDPattern.progressMaskLayer(percent))); } - /** - * A gradient of green to yellow LEDs, moving at 60 bpm, which synchronizes with many songs, such - * as common 120 bpm songs today. - */ + /** A gradient of green to yellow LEDs, moving at 60 bpm, which synchronizes with many song. */ public Command music() { return set(MUSIC_60_BPM); } From d475d15506efc8c14f10e62afa8afcf50d452535 Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Mon, 13 Jan 2025 15:58:22 -0500 Subject: [PATCH 5/9] merge main into here (merge conflicts aaadsakskdf;aksl) --- simgui-ds.json | 9 +- simgui.json | 58 ++++++++- .../org/sciborgs1155/lib/FaultLogger.java | 3 + .../org/sciborgs1155/robot/Constants.java | 17 +++ .../java/org/sciborgs1155/robot/Ports.java | 11 ++ .../java/org/sciborgs1155/robot/Robot.java | 11 +- .../sciborgs1155/robot/elevator/Elevator.java | 113 ++++++++++++++++++ .../robot/elevator/ElevatorConstants.java | 35 ++++++ .../robot/elevator/ElevatorIO.java | 9 ++ .../robot/elevator/ElevatorVisualizer.java | 28 +++++ .../robot/elevator/NoElevator.java | 20 ++++ .../robot/elevator/RealElevator.java | 48 ++++++++ .../robot/elevator/SimElevator.java | 46 +++++++ .../org/sciborgs1155/robot/hopper/Hopper.java | 68 +++++++++++ .../robot/hopper/HopperConstants.java | 5 + .../sciborgs1155/robot/hopper/HopperIO.java | 7 ++ .../sciborgs1155/robot/hopper/NoHopper.java | 14 +++ .../sciborgs1155/robot/hopper/RealHopper.java | 45 +++++++ .../org/sciborgs1155/robot/ElevatorTest.java | 46 +++++++ .../org/sciborgs1155/robot/RobotTest.java | 2 +- 20 files changed, 582 insertions(+), 13 deletions(-) create mode 100644 src/main/java/org/sciborgs1155/robot/elevator/Elevator.java create mode 100644 src/main/java/org/sciborgs1155/robot/elevator/ElevatorConstants.java create mode 100644 src/main/java/org/sciborgs1155/robot/elevator/ElevatorIO.java create mode 100644 src/main/java/org/sciborgs1155/robot/elevator/ElevatorVisualizer.java create mode 100644 src/main/java/org/sciborgs1155/robot/elevator/NoElevator.java create mode 100644 src/main/java/org/sciborgs1155/robot/elevator/RealElevator.java create mode 100644 src/main/java/org/sciborgs1155/robot/elevator/SimElevator.java create mode 100644 src/main/java/org/sciborgs1155/robot/hopper/Hopper.java create mode 100644 src/main/java/org/sciborgs1155/robot/hopper/HopperConstants.java create mode 100644 src/main/java/org/sciborgs1155/robot/hopper/HopperIO.java create mode 100644 src/main/java/org/sciborgs1155/robot/hopper/NoHopper.java create mode 100644 src/main/java/org/sciborgs1155/robot/hopper/RealHopper.java create mode 100644 src/test/java/org/sciborgs1155/robot/ElevatorTest.java diff --git a/simgui-ds.json b/simgui-ds.json index e3207aa..7806f73 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -9,11 +9,6 @@ "visible": true } }, - "System Joysticks": { - "window": { - "visible": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ @@ -106,7 +101,9 @@ } ], "robotJoysticks": [ - {}, + { + "guid": "Keyboard1" + }, { "guid": "Keyboard0" } diff --git a/simgui.json b/simgui.json index feeaf6a..5de6fe8 100644 --- a/simgui.json +++ b/simgui.json @@ -31,6 +31,9 @@ "/Robot/drive/rearRight/turnFeedback": "PIDController", "/Robot/drive/rotationController": "PIDController", "/Robot/drive/translationController": "ProfiledPIDController", + "/Robot/elevator/measurement/mech": "Mechanism2d", + "/Robot/elevator/pid": "ProfiledPIDController", + "/Robot/elevator/setpoint/mech": "Mechanism2d", "/SmartDashboard/Alerts": "Alerts", "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d", @@ -79,6 +82,16 @@ "window": { "visible": true } + }, + "/Robot/elevator/measurement/mech": { + "window": { + "visible": true + } + }, + "/Robot/elevator/setpoint/mech": { + "window": { + "visible": true + } } } }, @@ -88,9 +101,6 @@ }, "transitory": { "Robot": { - "autos": { - "open": true - }, "drive": { "Pose2d##v_/Robot/drive/getPose": { "Rotation2d##v_rotation": { @@ -160,7 +170,9 @@ "open": true }, "open": true - }, + } + }, + "elevator": { "open": true }, "open": true, @@ -175,5 +187,43 @@ }, "NetworkTables Info": { "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 278, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/Robot/elevator/positionSetpoint" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/Robot/elevator/position" + } + ] + } + ], + "window": { + "name": "Elevator" + } + } } } diff --git a/src/main/java/org/sciborgs1155/lib/FaultLogger.java b/src/main/java/org/sciborgs1155/lib/FaultLogger.java index 89497a8..a332add 100644 --- a/src/main/java/org/sciborgs1155/lib/FaultLogger.java +++ b/src/main/java/org/sciborgs1155/lib/FaultLogger.java @@ -329,6 +329,9 @@ public static void register(PhotonCamera camera) { */ public static void register(TalonFX talon) { int id = talon.getDeviceID(); + + register(() -> !talon.isConnected(), "Talon ID: " + id, "disconnected", FaultType.ERROR); + BiConsumer, String> regFault = (f, d) -> register(() -> f.getValue(), "Talon ID: " + id, d, FaultType.ERROR); diff --git a/src/main/java/org/sciborgs1155/robot/Constants.java b/src/main/java/org/sciborgs1155/robot/Constants.java index f60e54f..c71a75c 100644 --- a/src/main/java/org/sciborgs1155/robot/Constants.java +++ b/src/main/java/org/sciborgs1155/robot/Constants.java @@ -60,5 +60,22 @@ public static boolean inField(Pose3d pose) { && pose.getY() > 0 && pose.getY() < Field.WIDTH.in(Meters)); } + + public enum Level { + L1(.3), + L2(.7), + L3(1), + L4(1.5); + + public final double height; + + Level(double height) { + this.height = height; + } + + public double getHeight() { + return this.height; + } + } } } diff --git a/src/main/java/org/sciborgs1155/robot/Ports.java b/src/main/java/org/sciborgs1155/robot/Ports.java index cb9e0d2..2a6b860 100644 --- a/src/main/java/org/sciborgs1155/robot/Ports.java +++ b/src/main/java/org/sciborgs1155/robot/Ports.java @@ -20,6 +20,11 @@ public static final class Drive { public static final int REAR_RIGHT_TURNING = 17; } + public static final class Elevator { + public static final int LEADER = -1; + public static final int FOLLOWER = -1; + } + public static final class Scoral { public static final int TOP_ROLLER = 21; public static final int BOTTOM_ROLLER = 22; @@ -29,4 +34,10 @@ public static final class Scoral { public static final class LEDs { public static final int LED_PORT = 1; } + + public static final class Hopper { + public static final int LEFT_MOTOR = -1; + public static final int RIGHT_MOTOR = -1; + public static final int BEAMBREAK = -1; + } } diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 85a1d3f..80ec850 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -5,8 +5,8 @@ import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.*; -import static org.sciborgs1155.robot.Constants.DEADBAND; -import static org.sciborgs1155.robot.Constants.PERIOD; +import static org.sciborgs1155.robot.Constants.*; +import static org.sciborgs1155.robot.Constants.Field.*; import static org.sciborgs1155.robot.drive.DriveConstants.*; import edu.wpi.first.wpilibj.DataLogManager; @@ -33,6 +33,7 @@ import org.sciborgs1155.robot.commands.Autos; import org.sciborgs1155.robot.drive.Drive; import org.sciborgs1155.robot.led.LedStrip; +import org.sciborgs1155.robot.elevator.Elevator; import org.sciborgs1155.robot.vision.Vision; /** @@ -52,6 +53,7 @@ public class Robot extends CommandRobot implements Logged { private final Drive drive = Drive.create(); private final Vision vision = Vision.create(); private final LedStrip led = new LedStrip(); + private final Elevator elevator = Elevator.create(); // COMMANDS @Log.NT private final SendableChooser autos = Autos.configureAutos(drive); @@ -96,6 +98,11 @@ private void configureGameBehavior() { /** Configures trigger -> command bindings. */ private void configureBindings() { + operator.a().onTrue(elevator.scoreLevel(Level.L1)); + operator.b().onTrue(elevator.scoreLevel(Level.L2)); + operator.x().onTrue(elevator.scoreLevel(Level.L3)); + operator.y().onTrue(elevator.scoreLevel(Level.L4)); + // x and y are switched: we use joystick Y axis to control field x motion InputStream x = InputStream.of(driver::getLeftY).negate(); InputStream y = InputStream.of(driver::getLeftX).negate(); diff --git a/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java b/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java new file mode 100644 index 0000000..974b23d --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java @@ -0,0 +1,113 @@ +package org.sciborgs1155.robot.elevator; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static org.sciborgs1155.robot.Constants.*; +import static org.sciborgs1155.robot.Constants.Field.*; +import static org.sciborgs1155.robot.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import monologue.Annotations.Log; +import monologue.Logged; +import org.sciborgs1155.robot.Constants.Field.Level; +import org.sciborgs1155.robot.Robot; + +public class Elevator extends SubsystemBase implements Logged, AutoCloseable { + public static Elevator create() { + return Robot.isReal() ? new Elevator(new RealElevator()) : new Elevator(new SimElevator()); + } + + public static Elevator none() { + return new Elevator(new NoElevator()); + } + + private final ElevatorIO hardware; + + // private final SysIdRoutine sysIdRoutine; + + @Log.NT + private final ProfiledPIDController pid = + new ProfiledPIDController( + kP, + kI, + kD, + new TrapezoidProfile.Constraints( + MAX_VELOCITY.in(MetersPerSecond), MAX_ACCEL.in(MetersPerSecondPerSecond))); + + @Log.NT private final ElevatorFeedforward ff = new ElevatorFeedforward(kS, kG, kV, kA); + + @Log.NT + private final ElevatorVisualizer setpoint = new ElevatorVisualizer(new Color8Bit(0, 0, 255)); + + @Log.NT + private final ElevatorVisualizer measurement = new ElevatorVisualizer(new Color8Bit(255, 0, 0)); + + public Elevator(ElevatorIO hardware) { + this.hardware = hardware; + setDefaultCommand(retract()); + // sysIdRoutine = + // new SysIdRoutine( + // new SysIdRoutine.Config(), + // new SysIdRoutine.Mechanism(v -> pivot.setVoltage(v.in(Volts)), null, this)); + } + + public Command extend() { + return run(() -> update(MAX_HEIGHT.in(Meters))); + } + + public Command retract() { + return run(() -> update(MIN_HEIGHT.in(Meters))); + } + + public Command scoreLevel(Level level) { + return run(() -> update(level.getHeight())); + } + + @Log.NT + public double position() { + return hardware.position(); + } + + @Log.NT + public double velocity() { + return hardware.velocity(); + } + + @Log.NT + public double positionSetpoint() { + return pid.getSetpoint().position; + } + + @Log.NT + public double velocitySetpoint() { + return pid.getSetpoint().velocity; + } + + private void update(double position) { + position = MathUtil.clamp(position, MIN_HEIGHT.in(Meters), MAX_HEIGHT.in(Meters)); + + double lastVelocity = pid.getSetpoint().velocity; + double feedback = pid.calculate(hardware.position(), position); + double feedforward = ff.calculateWithVelocities(lastVelocity, pid.getSetpoint().velocity); + + hardware.setVoltage(feedforward + feedback); + } + + @Override + public void periodic() { + setpoint.setLength(positionSetpoint()); + measurement.setLength(position()); + } + + @Override + public void close() throws Exception { + hardware.close(); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/elevator/ElevatorConstants.java b/src/main/java/org/sciborgs1155/robot/elevator/ElevatorConstants.java new file mode 100644 index 0000000..990182e --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/elevator/ElevatorConstants.java @@ -0,0 +1,35 @@ +package org.sciborgs1155.robot.elevator; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Kilograms; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; + +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Mass; + +public class ElevatorConstants { + public static final double kP = 10; + public static final double kI = 0; + public static final double kD = 10; + public static final double kS = .1; + public static final double kG = 1; + public static final double kV = 1; + public static final double kA = 0; + + public static final Distance MIN_HEIGHT = Meters.of(.2); + public static final Distance MAX_HEIGHT = Meters.of(2); + + public static final LinearVelocity MAX_VELOCITY = MetersPerSecond.of(1); + public static final LinearAcceleration MAX_ACCEL = MetersPerSecondPerSecond.of(1); + + public static final Mass WEIGHT = Kilograms.of(20); + public static final Distance DRUM_RADIUS = Meters.of(.1); + public static final double GEARING = 20 / 1; + public static final Distance SPROCKET_RADIUS = Inches.of(1); + public static final Distance SPROCKET_CIRCUMFRENCE = SPROCKET_RADIUS.times(2 * Math.PI); + public static final double CONVERSION_FACTOR = SPROCKET_CIRCUMFRENCE.in(Meters) * 2 / GEARING; +} diff --git a/src/main/java/org/sciborgs1155/robot/elevator/ElevatorIO.java b/src/main/java/org/sciborgs1155/robot/elevator/ElevatorIO.java new file mode 100644 index 0000000..76d7977 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/elevator/ElevatorIO.java @@ -0,0 +1,9 @@ +package org.sciborgs1155.robot.elevator; + +public interface ElevatorIO extends AutoCloseable { + public void setVoltage(double voltage); + + public double position(); + + public double velocity(); +} diff --git a/src/main/java/org/sciborgs1155/robot/elevator/ElevatorVisualizer.java b/src/main/java/org/sciborgs1155/robot/elevator/ElevatorVisualizer.java new file mode 100644 index 0000000..316b438 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/elevator/ElevatorVisualizer.java @@ -0,0 +1,28 @@ +package org.sciborgs1155.robot.elevator; + +import static edu.wpi.first.units.Units.Meters; +import static org.sciborgs1155.robot.elevator.ElevatorConstants.MIN_HEIGHT; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color8Bit; +import monologue.Annotations.Log; +import monologue.Logged; + +public class ElevatorVisualizer implements Logged { + @Log.NT private final Mechanism2d mech; + private final MechanismLigament2d elevator; + + public ElevatorVisualizer(Color8Bit color) { + mech = new Mechanism2d(50, 50); + MechanismRoot2d chassis = mech.getRoot("chassis", 25, 10); + elevator = + chassis.append( + new MechanismLigament2d("elevator", MIN_HEIGHT.in(Meters) * 10, 90, 3, color)); + } + + public void setLength(double length) { + elevator.setLength(length * 10); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/elevator/NoElevator.java b/src/main/java/org/sciborgs1155/robot/elevator/NoElevator.java new file mode 100644 index 0000000..70eecdc --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/elevator/NoElevator.java @@ -0,0 +1,20 @@ +package org.sciborgs1155.robot.elevator; + +public class NoElevator implements ElevatorIO { + + @Override + public void setVoltage(double voltage) {} + + @Override + public double position() { + return 0; + } + + @Override + public double velocity() { + return 0; + } + + @Override + public void close() throws Exception {} +} diff --git a/src/main/java/org/sciborgs1155/robot/elevator/RealElevator.java b/src/main/java/org/sciborgs1155/robot/elevator/RealElevator.java new file mode 100644 index 0000000..5234fd0 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/elevator/RealElevator.java @@ -0,0 +1,48 @@ +package org.sciborgs1155.robot.elevator; + +import static org.sciborgs1155.robot.Ports.Elevator.*; +import static org.sciborgs1155.robot.elevator.ElevatorConstants.*; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import org.sciborgs1155.lib.TalonUtils; + +public class RealElevator implements ElevatorIO { + // idk how many there are + private final TalonFX leader = new TalonFX(LEADER); + private final TalonFX follower = new TalonFX(FOLLOWER); + + public RealElevator() { + TalonFXConfiguration drumConfig = new TalonFXConfiguration(); + follower.setControl(new Follower(LEADER, false)); + + drumConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + drumConfig.Feedback.SensorToMechanismRatio = CONVERSION_FACTOR; + + leader.getConfigurator().apply(drumConfig); + TalonUtils.addMotor(leader); + } + + @Override + public void setVoltage(double voltage) { + leader.setVoltage(voltage); + } + + @Override + public double position() { + return leader.getPosition().getValueAsDouble() * CONVERSION_FACTOR; + } + + @Override + public double velocity() { + return leader.getVelocity().getValueAsDouble() * CONVERSION_FACTOR; + } + + @Override + public void close() throws Exception { + leader.close(); + follower.close(); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/elevator/SimElevator.java b/src/main/java/org/sciborgs1155/robot/elevator/SimElevator.java new file mode 100644 index 0000000..1acf86b --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/elevator/SimElevator.java @@ -0,0 +1,46 @@ +package org.sciborgs1155.robot.elevator; + +import static edu.wpi.first.units.Units.Kilograms; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; +import static org.sciborgs1155.robot.elevator.ElevatorConstants.*; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import org.sciborgs1155.robot.Constants; + +public class SimElevator implements ElevatorIO { + private final ElevatorSim elevator; + + public SimElevator() { + elevator = + new ElevatorSim( + LinearSystemId.createElevatorSystem( + DCMotor.getKrakenX60(2), WEIGHT.in(Kilograms), DRUM_RADIUS.in(Meters), GEARING), + DCMotor.getKrakenX60(2), + MIN_HEIGHT.in(Meters), + MAX_HEIGHT.in(Meters), + true, + MIN_HEIGHT.in(Meters)); + } + + @Override + public void setVoltage(double voltage) { + elevator.setInputVoltage(voltage); + elevator.update(Constants.PERIOD.in(Seconds)); + } + + @Override + public double position() { + return elevator.getPositionMeters(); + } + + @Override + public double velocity() { + return elevator.getVelocityMetersPerSecond(); + } + + @Override + public void close() throws Exception {} +} diff --git a/src/main/java/org/sciborgs1155/robot/hopper/Hopper.java b/src/main/java/org/sciborgs1155/robot/hopper/Hopper.java new file mode 100644 index 0000000..b6f1b7c --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/hopper/Hopper.java @@ -0,0 +1,68 @@ +package org.sciborgs1155.robot.hopper; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import org.sciborgs1155.robot.Robot; + +public class Hopper extends SubsystemBase implements AutoCloseable { + /** Creates a real or non-existent hopper based on {@link Robot#isReal()}. */ + public static Hopper create() { + return Robot.isReal() ? new Hopper(new RealHopper()) : Hopper.none(); + } + + /** Creates a non-existent hopper. */ + public static Hopper none() { + return new Hopper(new NoHopper()); + } + + private final HopperIO hardware; + public final Trigger beambreakTrigger; + + public Hopper(HopperIO hardware) { + this.hardware = hardware; + this.beambreakTrigger = new Trigger(hardware::beambreak); + } + + /** + * Runs the motors of the hopper. + * + * @param power The power to set the hoppers motors to. + * @return A command to set the power of the hopper motors. + */ + public Command run(double power) { + return runOnce(() -> hardware.setPower(power)); + } + + /** + * Intakes corals from human player. + * + * @return A command to intake corals. + */ + public Command intake() { + return run(HopperConstants.INTAKE_POWER); // more logic later + } + + /** + * Outtake corals. + * + * @return A command to outtake corals. + */ + public Command outtake() { + return run(-HopperConstants.INTAKE_POWER); + } + + /** + * Stops the hopper. + * + * @return A command to stop the hopper. + */ + public Command stop() { + return run(0); + } + + @Override + public void close() throws Exception { + hardware.close(); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/hopper/HopperConstants.java b/src/main/java/org/sciborgs1155/robot/hopper/HopperConstants.java new file mode 100644 index 0000000..716e6c8 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/hopper/HopperConstants.java @@ -0,0 +1,5 @@ +package org.sciborgs1155.robot.hopper; + +public class HopperConstants { + public static final double INTAKE_POWER = 0.9; +} diff --git a/src/main/java/org/sciborgs1155/robot/hopper/HopperIO.java b/src/main/java/org/sciborgs1155/robot/hopper/HopperIO.java new file mode 100644 index 0000000..ba52207 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/hopper/HopperIO.java @@ -0,0 +1,7 @@ +package org.sciborgs1155.robot.hopper; + +public interface HopperIO extends AutoCloseable { + void setPower(double power); + + boolean beambreak(); +} diff --git a/src/main/java/org/sciborgs1155/robot/hopper/NoHopper.java b/src/main/java/org/sciborgs1155/robot/hopper/NoHopper.java new file mode 100644 index 0000000..7357488 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/hopper/NoHopper.java @@ -0,0 +1,14 @@ +package org.sciborgs1155.robot.hopper; + +public class NoHopper implements HopperIO { + @Override + public void setPower(double power) {} + + @Override + public boolean beambreak() { + return false; + } + + @Override + public void close() {} +} diff --git a/src/main/java/org/sciborgs1155/robot/hopper/RealHopper.java b/src/main/java/org/sciborgs1155/robot/hopper/RealHopper.java new file mode 100644 index 0000000..9773543 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/hopper/RealHopper.java @@ -0,0 +1,45 @@ +package org.sciborgs1155.robot.hopper; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DigitalInput; +import org.sciborgs1155.robot.Ports.Hopper; + +public class RealHopper implements HopperIO { + private final SparkMax leftMotor = new SparkMax(Hopper.LEFT_MOTOR, MotorType.kBrushless); + private final SparkMax rightMotor = new SparkMax(Hopper.RIGHT_MOTOR, MotorType.kBrushless); + private final DigitalInput beambreak = new DigitalInput(Hopper.BEAMBREAK); + + public RealHopper() { + SparkBaseConfig config = new SparkMaxConfig(); // prob more configs to do + config.inverted(true); + + rightMotor.configure( + config, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); // one of the motors are inverted so that the motors can + // intake/outtake right + } + + @Override + public void setPower(double power) { + leftMotor.set(power); + rightMotor.set(power); + } + + @Override + public boolean beambreak() { + return beambreak.get(); + } + + @Override + public void close() { + leftMotor.close(); + rightMotor.close(); + beambreak.close(); + } +} diff --git a/src/test/java/org/sciborgs1155/robot/ElevatorTest.java b/src/test/java/org/sciborgs1155/robot/ElevatorTest.java new file mode 100644 index 0000000..b045526 --- /dev/null +++ b/src/test/java/org/sciborgs1155/robot/ElevatorTest.java @@ -0,0 +1,46 @@ +package org.sciborgs1155.robot; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.sciborgs1155.lib.UnitTestingUtil.*; +import static org.sciborgs1155.robot.elevator.ElevatorConstants.*; + +import java.util.stream.Stream; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; +import org.sciborgs1155.robot.Constants.Field.Level; +import org.sciborgs1155.robot.elevator.Elevator; +import org.sciborgs1155.robot.elevator.SimElevator; + +public class ElevatorTest { + private Elevator elevator; + private final double DELTA = .05; + + @BeforeEach + public void initialize() { + setupTests(); + elevator = new Elevator(new SimElevator()); + } + + @AfterEach + public void destroy() throws Exception { + reset(elevator); + } + + @ParameterizedTest + @MethodSource("providePositionValues") + public void reachesPosition(Level level) { + run(elevator.scoreLevel(level), 200); + assertEquals(level.getHeight(), elevator.position(), DELTA); + } + + private static Stream providePositionValues() { + return Stream.of( + Arguments.of(Level.L1), + Arguments.of(Level.L2), + Arguments.of(Level.L3), + Arguments.of(Level.L4)); + } +} diff --git a/src/test/java/org/sciborgs1155/robot/RobotTest.java b/src/test/java/org/sciborgs1155/robot/RobotTest.java index 02e750a..d8a1de5 100644 --- a/src/test/java/org/sciborgs1155/robot/RobotTest.java +++ b/src/test/java/org/sciborgs1155/robot/RobotTest.java @@ -6,7 +6,7 @@ public class RobotTest { @Test - void initialize() throws Exception { + void setup() throws Exception { new Robot().close(); reset(); } From f56e6b9d51c0489f51aeaf8a3ead5329cdf9a3b9 Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Mon, 13 Jan 2025 16:27:30 -0500 Subject: [PATCH 6/9] some leds --- simgui.json | 2 +- .../java/org/sciborgs1155/robot/Ports.java | 2 +- .../java/org/sciborgs1155/robot/Robot.java | 4 +++- .../sciborgs1155/robot/led/LedConstants.java | 20 ++++++++++++++++++- .../org/sciborgs1155/robot/led/LedStrip.java | 16 +++++++++++++++ 5 files changed, 40 insertions(+), 4 deletions(-) diff --git a/simgui.json b/simgui.json index 5de6fe8..a0de544 100644 --- a/simgui.json +++ b/simgui.json @@ -198,7 +198,7 @@ 0.0, 0.8500000238418579 ], - "height": 278, + "height": 140, "series": [ { "color": [ diff --git a/src/main/java/org/sciborgs1155/robot/Ports.java b/src/main/java/org/sciborgs1155/robot/Ports.java index 2a6b860..7cb3a0f 100644 --- a/src/main/java/org/sciborgs1155/robot/Ports.java +++ b/src/main/java/org/sciborgs1155/robot/Ports.java @@ -34,7 +34,7 @@ public static final class Scoral { public static final class LEDs { public static final int LED_PORT = 1; } - + public static final class Hopper { public static final int LEFT_MOTOR = -1; public static final int RIGHT_MOTOR = -1; diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 80ec850..567737a 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -32,8 +32,8 @@ import org.sciborgs1155.robot.Ports.OI; import org.sciborgs1155.robot.commands.Autos; import org.sciborgs1155.robot.drive.Drive; -import org.sciborgs1155.robot.led.LedStrip; import org.sciborgs1155.robot.elevator.Elevator; +import org.sciborgs1155.robot.led.LedStrip; import org.sciborgs1155.robot.vision.Vision; /** @@ -136,8 +136,10 @@ private void configureBindings() { .rateLimit(MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second))); drive.setDefaultCommand(drive.drive(x, y, omega)); + led.setDefaultCommand(led.scrolling()); autonomous().whileTrue(Commands.defer(autos::getSelected, Set.of(drive)).asProxy()); + autonomous().whileTrue(led.autos()); test().whileTrue(systemsCheck()); diff --git a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java index fd00b31..1017034 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.util.Color; +import java.util.Map; public class LedConstants { // The length of the LED Strip. @@ -19,7 +20,7 @@ public class LedConstants { public static final LEDPattern RAINBOW = LEDPattern.rainbow(225, 225).scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING); - // A maksed graident that rises and falls at 60 BPM. + // A masked graident that rises and falls at 60 BPM. public static final LEDPattern MUSIC_60_BPM = LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow) .mask( @@ -27,4 +28,21 @@ public class LedConstants { () -> Math.sin(RobotController.getMeasureTime().in(Seconds) * Math.PI * 2) / 3 + 0.5)); + + // A masked gradient of yellow green that is scrolled through. + public static final LEDPattern SCROLLING = + LEDPattern.solid(Color.kGreenYellow) + .mask( + LEDPattern.steps(Map.of(0, Color.kWhite, 0.5, Color.kBlack)) + .scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING)); + + // A gradiant of red that is breathing with 2 seconds/cycle. + public static final LEDPattern BREATHE_RED = + LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kFirstRed, Color.kOrangeRed) + .breathe(Seconds.of(2)); + + // A gradiant of blue that is breathing with 2 seconds/cycle. + public static final LEDPattern BREATHE_BLUE = + LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kFirstBlue, Color.kAqua) + .breathe(Seconds.of(2)); } diff --git a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java index 91828a8..0756c8f 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java @@ -5,12 +5,14 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import monologue.Logged; +import org.sciborgs1155.robot.Constants; public class LedStrip extends SubsystemBase implements Logged, AutoCloseable { @@ -45,6 +47,20 @@ public Command music() { return set(MUSIC_60_BPM); } + /** A solid yellow green that is scrolled through. */ + public Command scrolling() { + return set(SCROLLING); + } + + /** A breathing gradient that matches the alliance colors. */ + public Command autos() { + if (Constants.alliance() == Alliance.Red) { + return set(BREATHE_RED); + } else { + return set(BREATHE_BLUE); + } + } + public Command set(LEDPattern pattern) { return run( () -> { From d5444ecdd48b6d7f90f06f4b023402536a6f5e28 Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Wed, 15 Jan 2025 21:17:11 -0500 Subject: [PATCH 7/9] address review --- .../java/org/sciborgs1155/robot/Robot.java | 4 +- .../sciborgs1155/robot/led/LedConstants.java | 38 +------------------ .../org/sciborgs1155/robot/led/LedStrip.java | 38 ++++++++++++++----- 3 files changed, 32 insertions(+), 48 deletions(-) diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 567737a..8f0d158 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -33,7 +33,7 @@ import org.sciborgs1155.robot.commands.Autos; import org.sciborgs1155.robot.drive.Drive; import org.sciborgs1155.robot.elevator.Elevator; -import org.sciborgs1155.robot.led.LedStrip; +import org.sciborgs1155.robot.led.LEDStrip; import org.sciborgs1155.robot.vision.Vision; /** @@ -52,7 +52,7 @@ public class Robot extends CommandRobot implements Logged { // SUBSYSTEMS private final Drive drive = Drive.create(); private final Vision vision = Vision.create(); - private final LedStrip led = new LedStrip(); + private final LEDStrip led = new LEDStrip(); private final Elevator elevator = Elevator.create(); // COMMANDS diff --git a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java index 1017034..92fce7f 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedConstants.java @@ -1,48 +1,12 @@ package org.sciborgs1155.robot.led; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.util.Color; -import java.util.Map; -public class LedConstants { +public class LEDConstants { // The length of the LED Strip. public static final int LED_LENGTH = 64; // The distance between two LEDs on the LED Strip. public static final Distance LED_SPACING = Meters.of(0.01); - - // A rainbow, scrolling at 0.5 m/s. - public static final LEDPattern RAINBOW = - LEDPattern.rainbow(225, 225).scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING); - - // A masked graident that rises and falls at 60 BPM. - public static final LEDPattern MUSIC_60_BPM = - LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow) - .mask( - LEDPattern.progressMaskLayer( - () -> - Math.sin(RobotController.getMeasureTime().in(Seconds) * Math.PI * 2) / 3 - + 0.5)); - - // A masked gradient of yellow green that is scrolled through. - public static final LEDPattern SCROLLING = - LEDPattern.solid(Color.kGreenYellow) - .mask( - LEDPattern.steps(Map.of(0, Color.kWhite, 0.5, Color.kBlack)) - .scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING)); - - // A gradiant of red that is breathing with 2 seconds/cycle. - public static final LEDPattern BREATHE_RED = - LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kFirstRed, Color.kOrangeRed) - .breathe(Seconds.of(2)); - - // A gradiant of blue that is breathing with 2 seconds/cycle. - public static final LEDPattern BREATHE_BLUE = - LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kFirstBlue, Color.kAqua) - .breathe(Seconds.of(2)); } diff --git a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java index 0756c8f..2d103cf 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java +++ b/src/main/java/org/sciborgs1155/robot/led/LedStrip.java @@ -1,34 +1,39 @@ package org.sciborgs1155.robot.led; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Seconds; import static org.sciborgs1155.robot.Ports.LEDs.*; -import static org.sciborgs1155.robot.led.LedConstants.*; +import static org.sciborgs1155.robot.led.LEDConstants.*; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Map; import java.util.function.DoubleSupplier; import monologue.Logged; import org.sciborgs1155.robot.Constants; -public class LedStrip extends SubsystemBase implements Logged, AutoCloseable { +public class LEDStrip extends SubsystemBase implements Logged, AutoCloseable { private final AddressableLED led = new AddressableLED(LED_PORT); private final AddressableLEDBuffer buffer; - public LedStrip() { + public LEDStrip() { led.setLength(LED_LENGTH); buffer = new AddressableLEDBuffer(LED_LENGTH); led.setData(buffer); led.start(); } - /** Rainbow LEDs. Very cool. */ + /** Rainbow LEDs, scrolling at 0.5 m/s. Very cool. */ public Command rainbow() { - return set(RAINBOW); + return set( + LEDPattern.rainbow(225, 225).scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING)); } /** @@ -44,20 +49,35 @@ public Command elevatorLED(DoubleSupplier percent) { /** A gradient of green to yellow LEDs, moving at 60 bpm, which synchronizes with many song. */ public Command music() { - return set(MUSIC_60_BPM); + return set( + LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kGreen, Color.kYellow) + .mask( + LEDPattern.progressMaskLayer( + () -> + Math.sin(RobotController.getMeasureTime().in(Seconds) * Math.PI * 2) / 3 + + 0.5))); } /** A solid yellow green that is scrolled through. */ public Command scrolling() { - return set(SCROLLING); + return set( + LEDPattern.solid(Color.kGreenYellow) + .mask( + LEDPattern.steps(Map.of(0, Color.kWhite, 0.5, Color.kBlack)) + .scrollAtAbsoluteSpeed(MetersPerSecond.of(0.5), LED_SPACING))); } /** A breathing gradient that matches the alliance colors. */ public Command autos() { if (Constants.alliance() == Alliance.Red) { - return set(BREATHE_RED); + return set( + LEDPattern.gradient( + LEDPattern.GradientType.kDiscontinuous, Color.kFirstRed, Color.kOrangeRed) + .breathe(Seconds.of(2))); } else { - return set(BREATHE_BLUE); + return set( + LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kFirstBlue, Color.kAqua) + .breathe(Seconds.of(2))); } } From 5589a9ebc92803b3f6e4534d69521bc15749aa59 Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Wed, 15 Jan 2025 21:24:46 -0500 Subject: [PATCH 8/9] git isnt detecting file renaming? --- src/main/java/org/sciborgs1155/robot/Robot.java | 4 ++-- .../robot/led/{LedConstants.java => LEDConstants_a.java} | 2 +- .../robot/led/{LedStrip.java => LEDStrip_a.java} | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) rename src/main/java/org/sciborgs1155/robot/led/{LedConstants.java => LEDConstants_a.java} (91%) rename src/main/java/org/sciborgs1155/robot/led/{LedStrip.java => LEDStrip_a.java} (95%) diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 8f0d158..2b11378 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -33,7 +33,7 @@ import org.sciborgs1155.robot.commands.Autos; import org.sciborgs1155.robot.drive.Drive; import org.sciborgs1155.robot.elevator.Elevator; -import org.sciborgs1155.robot.led.LEDStrip; +import org.sciborgs1155.robot.led.LEDStrip_a; import org.sciborgs1155.robot.vision.Vision; /** @@ -52,7 +52,7 @@ public class Robot extends CommandRobot implements Logged { // SUBSYSTEMS private final Drive drive = Drive.create(); private final Vision vision = Vision.create(); - private final LEDStrip led = new LEDStrip(); + private final LEDStrip_a led = new LEDStrip_a(); private final Elevator elevator = Elevator.create(); // COMMANDS diff --git a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java b/src/main/java/org/sciborgs1155/robot/led/LEDConstants_a.java similarity index 91% rename from src/main/java/org/sciborgs1155/robot/led/LedConstants.java rename to src/main/java/org/sciborgs1155/robot/led/LEDConstants_a.java index 92fce7f..2b64220 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedConstants.java +++ b/src/main/java/org/sciborgs1155/robot/led/LEDConstants_a.java @@ -4,7 +4,7 @@ import edu.wpi.first.units.measure.Distance; -public class LEDConstants { +public class LEDConstants_a { // The length of the LED Strip. public static final int LED_LENGTH = 64; // The distance between two LEDs on the LED Strip. diff --git a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java b/src/main/java/org/sciborgs1155/robot/led/LEDStrip_a.java similarity index 95% rename from src/main/java/org/sciborgs1155/robot/led/LedStrip.java rename to src/main/java/org/sciborgs1155/robot/led/LEDStrip_a.java index 2d103cf..7aa99a9 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LedStrip.java +++ b/src/main/java/org/sciborgs1155/robot/led/LEDStrip_a.java @@ -3,7 +3,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Seconds; import static org.sciborgs1155.robot.Ports.LEDs.*; -import static org.sciborgs1155.robot.led.LEDConstants.*; +import static org.sciborgs1155.robot.led.LEDConstants_a.*; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; @@ -18,12 +18,12 @@ import monologue.Logged; import org.sciborgs1155.robot.Constants; -public class LEDStrip extends SubsystemBase implements Logged, AutoCloseable { +public class LEDStrip_a extends SubsystemBase implements Logged, AutoCloseable { private final AddressableLED led = new AddressableLED(LED_PORT); private final AddressableLEDBuffer buffer; - public LEDStrip() { + public LEDStrip_a() { led.setLength(LED_LENGTH); buffer = new AddressableLEDBuffer(LED_LENGTH); led.setData(buffer); From 55dd550a0df8b143747fe8f2974df459b0a5d04b Mon Sep 17 00:00:00 2001 From: HenryLi-0 Date: Wed, 15 Jan 2025 21:25:13 -0500 Subject: [PATCH 9/9] should work now --- src/main/java/org/sciborgs1155/robot/Robot.java | 4 ++-- .../robot/led/{LEDConstants_a.java => LEDConstants.java} | 2 +- .../robot/led/{LEDStrip_a.java => LEDStrip.java} | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) rename src/main/java/org/sciborgs1155/robot/led/{LEDConstants_a.java => LEDConstants.java} (91%) rename src/main/java/org/sciborgs1155/robot/led/{LEDStrip_a.java => LEDStrip.java} (95%) diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 2b11378..8f0d158 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -33,7 +33,7 @@ import org.sciborgs1155.robot.commands.Autos; import org.sciborgs1155.robot.drive.Drive; import org.sciborgs1155.robot.elevator.Elevator; -import org.sciborgs1155.robot.led.LEDStrip_a; +import org.sciborgs1155.robot.led.LEDStrip; import org.sciborgs1155.robot.vision.Vision; /** @@ -52,7 +52,7 @@ public class Robot extends CommandRobot implements Logged { // SUBSYSTEMS private final Drive drive = Drive.create(); private final Vision vision = Vision.create(); - private final LEDStrip_a led = new LEDStrip_a(); + private final LEDStrip led = new LEDStrip(); private final Elevator elevator = Elevator.create(); // COMMANDS diff --git a/src/main/java/org/sciborgs1155/robot/led/LEDConstants_a.java b/src/main/java/org/sciborgs1155/robot/led/LEDConstants.java similarity index 91% rename from src/main/java/org/sciborgs1155/robot/led/LEDConstants_a.java rename to src/main/java/org/sciborgs1155/robot/led/LEDConstants.java index 2b64220..92fce7f 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LEDConstants_a.java +++ b/src/main/java/org/sciborgs1155/robot/led/LEDConstants.java @@ -4,7 +4,7 @@ import edu.wpi.first.units.measure.Distance; -public class LEDConstants_a { +public class LEDConstants { // The length of the LED Strip. public static final int LED_LENGTH = 64; // The distance between two LEDs on the LED Strip. diff --git a/src/main/java/org/sciborgs1155/robot/led/LEDStrip_a.java b/src/main/java/org/sciborgs1155/robot/led/LEDStrip.java similarity index 95% rename from src/main/java/org/sciborgs1155/robot/led/LEDStrip_a.java rename to src/main/java/org/sciborgs1155/robot/led/LEDStrip.java index 7aa99a9..2d103cf 100644 --- a/src/main/java/org/sciborgs1155/robot/led/LEDStrip_a.java +++ b/src/main/java/org/sciborgs1155/robot/led/LEDStrip.java @@ -3,7 +3,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Seconds; import static org.sciborgs1155.robot.Ports.LEDs.*; -import static org.sciborgs1155.robot.led.LEDConstants_a.*; +import static org.sciborgs1155.robot.led.LEDConstants.*; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; @@ -18,12 +18,12 @@ import monologue.Logged; import org.sciborgs1155.robot.Constants; -public class LEDStrip_a extends SubsystemBase implements Logged, AutoCloseable { +public class LEDStrip extends SubsystemBase implements Logged, AutoCloseable { private final AddressableLED led = new AddressableLED(LED_PORT); private final AddressableLEDBuffer buffer; - public LEDStrip_a() { + public LEDStrip() { led.setLength(LED_LENGTH); buffer = new AddressableLEDBuffer(LED_LENGTH); led.setData(buffer);