robotPoseSupplier) {
@@ -92,9 +91,8 @@ public void setHeadingControlledSupplier(BooleanSupplier headingControlledSuppli
this.headingControlledSupplier = headingControlledSupplier;
}
- public void setHasVisionEstimateSupplier(BooleanSupplier hasVisionEstimate, double debounceTime) {
+ public void setHasVisionEstimateSupplier(BooleanSupplier hasVisionEstimate) {
this.hasVisionEstimate = hasVisionEstimate;
- debouncer = new Debouncer(debounceTime, DebounceType.kFalling);
}
public void setSensorSuppliers(BooleanSupplier usingIntakeSensor, BooleanSupplier hasCoral) {
@@ -110,6 +108,10 @@ public void setSuperstructureAtGoal(BooleanSupplier superstructureAtGoal) {
this.superstructureAtGoal = superstructureAtGoal;
}
+ public void setIsInBox(BooleanSupplier isInBox) {
+ this.isInBox = isInBox;
+ }
+
public Field2d getField() {
return field;
}
@@ -120,20 +122,8 @@ public void periodic() {
if (poseSupplier != null) {
Pose2d pose = poseSupplier.get();
- SmartDashboard.putNumber("Heading Degrees", ((-pose.getRotation().getDegrees() + 360) % 360));
+ SmartDashboard.putNumber("Heading Degrees", -pose.getRotation().getDegrees());
field.setRobotPose(pose);
-
- // SmartDashboard.putNumber(
- // "Distance To Reef [Tag 8] [Test]",
- // Units.metersToInches(
- // Math.abs(
- // AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark)
- // .getTagPose(18)
- // .get()
- // .toPose2d()
- // .minus(pose)
- // .getX())
- // - DRIVE_CONFIG.bumperCornerToCorner().getX() / 2.0));
}
if (autoAlginPoseSupplier != null) {
@@ -159,8 +149,7 @@ public void periodic() {
}
if (hasVisionEstimate != null) {
- SmartDashboard.putBoolean(
- "Has Vision", debouncer.calculate(hasVisionEstimate.getAsBoolean()));
+ SmartDashboard.putBoolean("Has Vision", hasVisionEstimate.getAsBoolean());
}
if (usingIntakeSensor != null) {
@@ -178,5 +167,9 @@ public void periodic() {
if (hangValue != null) {
SmartDashboard.putNumber("Hang Value", hangValue.getAsDouble());
}
+
+ if (isInBox != null) {
+ SmartDashboard.putBoolean("Is In Box?", isInBox.getAsBoolean());
+ }
}
}
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index 85e34d9..7aff2fa 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -60,7 +60,10 @@ public PathConstraints getPathConstraints(double speedMultiplier) {
new Translation2d(Units.inchesToMeters(22.729228), Units.inchesToMeters(22.729228)),
new Translation2d(Units.inchesToMeters(35), Units.inchesToMeters(35)),
7.05968,
- 14.5);
+ 14.5
+ * (Constants.isDemoMode()
+ ? Constants.DEMO_SPEED_TRANSLATION_SPEED_COFICIENT
+ : 1.0));
case T_SHIRT_CANNON_CHASSIS -> new DriveConfig(
new Translation2d(Units.inchesToMeters(22.729226), Units.inchesToMeters(22.729226)),
new Translation2d(Units.inchesToMeters(25.729226), Units.inchesToMeters(25.729226)),
diff --git a/src/main/java/frc/robot/subsystems/hang/Hang.java b/src/main/java/frc/robot/subsystems/hang/Hang.java
index 2094f98..f84250c 100644
--- a/src/main/java/frc/robot/subsystems/hang/Hang.java
+++ b/src/main/java/frc/robot/subsystems/hang/Hang.java
@@ -38,7 +38,6 @@ public class Hang extends SubsystemBase {
public Hang(HangIO io) {
this.io = io;
io.setBrakeMode(true);
- io.setLimits(HangConstants.RELATIVE_MIN_ROTATIONS, HangConstants.RELATIVE_MAX_ROTATIONS);
}
@Override
@@ -60,7 +59,7 @@ public void periodic() {
measuredVisualizer.update(Rotation2d.fromRotations(inputs.positionRotations));
}
- private void setGoal(Rotation2d rotation) {
+ public void setGoal(Rotation2d rotation) {
runClosedLoop = true;
controller.setSetpoint(rotation.getRotations());
setpointVisualizer.update(rotation);
@@ -89,18 +88,6 @@ public double getRelativeRotations() {
return inputs.positionRotations;
}
- public Command stow() {
- return runOnce(() -> setGoal(HangConstants.STOWED_POSITION_ROTATIONS));
- }
-
- public Command deploy() {
- return runOnce(() -> setGoal(HangConstants.DEPLOY_POSITION_ROTATIONS));
- }
-
- public Command retract() {
- return runOnce(() -> setGoal(HangConstants.RETRACT_POSITION_ROTATIONS));
- }
-
public Command runSet(double speed) {
return runEnd(() -> io.runOpenLoop(speed), io::stop);
}
diff --git a/src/main/java/frc/robot/subsystems/hang/HangConstants.java b/src/main/java/frc/robot/subsystems/hang/HangConstants.java
index 510eff6..c03324d 100644
--- a/src/main/java/frc/robot/subsystems/hang/HangConstants.java
+++ b/src/main/java/frc/robot/subsystems/hang/HangConstants.java
@@ -1,6 +1,5 @@
package frc.robot.subsystems.hang;
-import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
import frc.robot.utility.records.PIDConstants;
@@ -27,12 +26,5 @@ public static record HangConfig(
public static final int MOTOR_CURRENT_LIMIT = 40;
public static final double GEAR_REDUCTION = 5 * 5 * 5;
- public static final Rotation2d STOWED_POSITION_ROTATIONS = Rotation2d.fromDegrees(0);
- public static final Rotation2d DEPLOY_POSITION_ROTATIONS = Rotation2d.fromDegrees(60);
- public static final Rotation2d RETRACT_POSITION_ROTATIONS = Rotation2d.fromDegrees(-30);
-
- public static final double RELATIVE_MIN_ROTATIONS = -1;
- public static final double RELATIVE_MAX_ROTATIONS = +1;
-
public static final double TOLERANCE = Units.degreesToRotations(0.3);
}
diff --git a/src/main/java/frc/robot/subsystems/led/BlinkenLEDPattern.java b/src/main/java/frc/robot/subsystems/led/BlinkenLEDPattern.java
new file mode 100644
index 0000000..eac75fe
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/BlinkenLEDPattern.java
@@ -0,0 +1,161 @@
+package frc.robot.subsystems.led;
+
+/**
+ * @brief LED patterns and solid colors for REV Blinkin
+ * @link https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf
+ */
+public enum BlinkenLEDPattern {
+ // Off
+ OFF(1995), // same as SolidColors.BLACK
+
+ // Fixed Palette - Rainbow
+ RAINBOW_PALETTE(1005),
+ PARTY_PALETTE(1015),
+ OCEAN_PALETTE(1025),
+ LAVA_PALETTE(1035),
+ FOREST_PALETTE(1045),
+
+ // Fixed Palette - Rainbow with Glitter
+ RAINBOW_WITH_GLITTER(1055),
+
+ // Fixed Palette - Confetti
+ CONFETTI(1065),
+
+ // Fixed Palette - Shot
+ SHOT_RED(1075),
+ SHOT_BLUE(1085),
+ SHOT_WHITE(1095),
+
+ // Fixed Palette - Sinelon
+ SINELON_RAINBOW(1105),
+ SINELON_PARTY(1115),
+ SINELON_OCEAN(1125),
+ SINELON_LAVA(1135),
+ SINELON_FOREST(1145),
+
+ // Fixed Palette - Beats per Minute
+ BPM_RAINBOW(1155),
+ BPM_PARTY(1165),
+ BPM_OCEAN(1175),
+ BPM_LAVA(1185),
+ BPM_FOREST(1195),
+
+ // Fixed Palette - Fire
+ FIRE_MEDIUM(1205),
+ FIRE_LARGE(1215),
+
+ // Fixed Palette - Twinkles
+ TWINKLES_RAINBOW(1225),
+ TWINKLES_PARTY(1235),
+ TWINKLES_OCEAN(1245),
+ TWINKLES_LAVA(1255),
+ TWINKLES_FOREST(1265),
+
+ // Fixed Palette - Color Waves
+ COLORWAVES_RAINBOW(1275),
+ COLORWAVES_PARTY(1285),
+ COLORWAVES_OCEAN(1295),
+ COLORWAVES_LAVA(1305),
+ COLORWAVES_FOREST(1315),
+
+ // Fixed Palette - Larson Scanner
+ LARSON_RED(1325),
+ LARSON_GRAY(1335),
+
+ // Fixed Palette - Light Chase
+ CHASE_RED(1345),
+ CHASE_BLUE(1355),
+ CHASE_GRAY(1365),
+
+ // Fixed Palette - Heartbeat
+ HEARTBEAT_RED(1375),
+ HEARTBEAT_BLUE(1385),
+ HEARTBEAT_WHITE(1395),
+ HEARTBEAT_GRAY(1405),
+
+ // Fixed Palette - Breath
+ BREATH_RED(1415),
+ BREATH_BLUE(1425),
+ BREATH_GRAY(1435),
+
+ // Fixed Palette - Strobe
+ STROBE_RED(1445),
+ STROBE_BLUE(1455),
+ STROBE_GOLD(1465),
+ STROBE_WHITE(1475),
+
+ // Color 1 Pattern
+ C1_BLEND_TO_BLACK(1485),
+ C1_LARSON(1495),
+ C1_CHASE(1505),
+ C1_HEARTBEAT_SLOW(1515),
+ C1_HEARTBEAT_MEDIUM(1525),
+ C1_HEARTBEAT_FAST(1535),
+ C1_BREATH_SLOW(1545),
+ C1_BREATH_FAST(1555),
+ C1_SHOT(1565),
+ C1_STROBE(1575),
+
+ // Color 2 Pattern
+ C2_BLEND_TO_BLACK(1585),
+ C2_LARSON(1595),
+ C2_CHASE(1605),
+ C2_HEARTBEAT_SLOW(1615),
+ C2_HEARTBEAT_MEDIUM(1625),
+ C2_HEARTBEAT_FAST(1635),
+ C2_BREATH_SLOW(1645),
+ C2_BREATH_FAST(1655),
+ C2_SHOT(1665),
+ C2_STROBE(1675),
+
+ // Color 1 & 2 Pattern
+ SPARKLE_1_ON_2(1685),
+ SPARKLE_2_ON_1(1695),
+ COLOR_GRADIENT_1_AND_2(1705),
+ COLOR_BPM_1_AND_2(1715),
+ BLEND_1_TO_2(1725),
+ BLEND_1_AND_2(1735),
+ COLOR_NO_BLEND_1_AND_2(1745),
+ COLOR_TWINKLES_1_AND_2(1755),
+ COLOR_WAVES_1_AND_2(1765),
+ COLOR_SINELON_1_AND_2(1775),
+
+ // Solid Colors
+ HOT_PINK(1785),
+ DARK_RED(1795),
+ RED(1805),
+ RED_ORANGE(1815),
+ ORANGE(1825),
+ GOLD(1835),
+ YELLOW(1845),
+ LAWN_GREEN(1855),
+ LIME(1865),
+ DARK_GREEN(1875),
+ GREEN(1885),
+ BLUE_GREEN(1895),
+ AQUA(1905),
+ SKY_BLUE(1915),
+ DARK_BLUE(1925),
+ BLUE(1935),
+ BLUE_VIOLET(1945),
+ VIOLET(1955),
+ WHITE(1965),
+ GRAY(1975),
+ DARK_GRAY(1985),
+ BLACK(1995);
+
+ private final int pulse;
+
+ BlinkenLEDPattern(int pulse) {
+ this.pulse = pulse;
+ }
+
+ public int getPulse() {
+ return pulse;
+ }
+
+ @Override
+ public String toString() {
+ return name() + " (" + pulse + "us)";
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDConstants.java b/src/main/java/frc/robot/subsystems/led/LEDConstants.java
index 9f91383..7035526 100644
--- a/src/main/java/frc/robot/subsystems/led/LEDConstants.java
+++ b/src/main/java/frc/robot/subsystems/led/LEDConstants.java
@@ -1,175 +1,22 @@
package frc.robot.subsystems.led;
-import frc.robot.Constants;
-
-/**
- * @brief Patterns taken from
- * @link https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf
- */
public class LEDConstants {
- public static final int DEFAULT = SolidColors.BLACK;
-
- public static final int[] PWM_PORTS =
- switch (Constants.getRobot()) {
- case COMP_BOT_2025 -> new int[] {0, 1};
- case T_SHIRT_CANNON_CHASSIS -> new int[] {0};
- case WOOD_BOT_TWO_2025 -> new int[] {0};
- default -> new int[] {};
- };
-
- public static final class FixedPalettePattern {
- public static final class Rainbow {
- public static final int RAINBOW_PALETTE = 1005;
- public static final int PARTY_PALETTE = 1015;
- public static final int OCEAN_PALETTE = 1025;
- public static final int LAVA_PALETTE = 1035;
- public static final int FOREST_PALETTE = 1045;
- }
-
- public static final class RainbowWithGlitter {
- public static final int RAINBOW_WITH_GLITTER = 1055;
- }
-
- public static final class Confetti {
- public static final int CONFETTI = 1065;
- }
- public static final class Shot {
- public static final int RED = 1075;
- public static final int BLUE = 1085;
- public static final int WHITE = 1095;
- }
-
- public static final class Sinelon {
- public static final int RAINBOW_PALETTE = 1105;
- public static final int PARTY_PALETTE = 1115;
- public static final int OCEAN_PALETTE = 1125;
- public static final int LAVA_PALETTE = 1135;
- public static final int FOREST_PALETTE = 1145;
- }
-
- public static final class BeatsPerMinute {
- public static final int RAINBOW_PALETTE = 1155;
- public static final int PARTY_PALETTE = 1165;
- public static final int OCEAN_PALETTE = 1175;
- public static final int LAVA_PALETTE = 1185;
- public static final int FOREST_PALETTE = 1195;
- }
+ public static final BlinkenLEDPattern DEFAULT_PATTERN = BlinkenLEDPattern.OFF;
- public static final class Fire {
- public static final int MEDIUM = 1205;
- public static final int LARGE = 1215;
- }
+ public enum BlinkenMode {
+ STRIP_5V(2125),
+ STRIP_12V(2145);
- public static final class Twinkles {
- public static final int RAINBOW_PALETTE = 1225;
- public static final int PARTY_PALETTE = 1235;
- public static final int OCEAN_PALETTE = 1245;
- public static final int LAVA_PALETTE = 1255;
- public static final int FOREST_PALETTE = 1265;
- }
+ public final int setupPulse;
- public static final class ColorWaves {
- public static final int RAINBOW_PALETTE = 1275;
- public static final int PARTY_PALETTE = 1285;
- public static final int OCEAN_PALETTE = 1295;
- public static final int LAVA_PALETTE = 1305;
- public static final int FOREST_PALETTE = 1315;
+ BlinkenMode(int setupPulse) {
+ this.setupPulse = setupPulse;
}
-
- public static final class LarsonScanner {
- public static final int RED = 1325;
- public static final int GRAY = 1335;
- }
-
- public static final class LightChase {
- public static final int RED = 1345;
- public static final int BLUE = 1355;
- public static final int GRAY = 1365;
- }
-
- public static final class Heartbeat {
- public static final int RED = 1375;
- public static final int BLUE = 1385;
- public static final int WHITE = 1395;
- public static final int GRAY = 1405;
- }
-
- public static final class Breath {
- public static final int RED = 1415;
- public static final int BLUE = 1425;
- public static final int GRAY = 1435;
- }
-
- public static final class Strobe {
- public static final int RED = 1445;
- public static final int BLUE = 1455;
- public static final int GOLD = 1465;
- public static final int WHITE = 1475;
- }
- }
-
- public static final class Color1Pattern {
- public static final int END_TO_END_BLEND_TO_BLACK = 1485;
- public static final int LARSON_SCANNER = 1495;
- public static final int LIGHT_CHASE = 1505;
- public static final int HEARTBEAT_SLOW = 1515;
- public static final int HEARTBEAT_MEDIUM = 1525;
- public static final int HEARTBEAT_FAST = 1535;
- public static final int BREATH_SLOW = 1545;
- public static final int BREATH_FAST = 1555;
- public static final int SHOT = 1565;
- public static final int STROBE = 1575;
}
- public static final class Color2Pattern {
- public static final int END_TO_END_BLEND_TO_BLACK = 1585;
- public static final int LARSON_SCANNER = 1595;
- public static final int LIGHT_CHASE = 1605;
- public static final int HEARTBEAT_SLOW = 1615;
- public static final int HEARTBEAT_MEDIUM = 1625;
- public static final int HEARTBEAT_FAST = 1635;
- public static final int BREATH_SLOW = 1645;
- public static final int BREATH_FAST = 1655;
- public static final int SHOT = 1665;
- public static final int STROBE = 1675;
- }
+ public record LEDConfig(int pwmChannel, BlinkenMode mode) {}
- public static final class Color1And2Pattern {
- public static final int SPARKLE_1_ON_2 = 1685;
- public static final int SPARKLE_2_ON_1 = 1695;
- public static final int COLOR_GRADIENT = 1705;
- public static final int BEATS_PER_MINUTE = 1715;
- public static final int END_TO_END_BLEND_1_TO_2 = 1725;
- public static final int END_TO_END_BLEND = 1735;
- public static final int COLOR_1_AND_2_NO_BLENDING = 1745;
- public static final int TWINKLES = 1755;
- public static final int COLOR_WAVES = 1765;
- public static final int SINELON = 1775;
- }
-
- public static final class SolidColors {
- public static final int HOT_PINK = 1785;
- public static final int DARK_RED = 1795;
- public static final int RED = 1805;
- public static final int RED_ORANGE = 1815;
- public static final int ORANGE = 1825;
- public static final int GOLD = 1835;
- public static final int YELLOW = 1845;
- public static final int LAWN_GREEN = 1855;
- public static final int LIME = 1865;
- public static final int DARK_GREEN = 1875;
- public static final int GREEN = 1885;
- public static final int BLUE_GREEN = 1895;
- public static final int AQUA = 1905;
- public static final int SKY_BLUE = 1915;
- public static final int DARK_BLUE = 1925;
- public static final int BLUE = 1935;
- public static final int BLUE_VIOLET = 1945;
- public static final int VIOLET = 1955;
- public static final int WHITE = 1965;
- public static final int GRAY = 1975;
- public static final int DARK_GRAY = 1985;
- public static final int BLACK = 1995;
- }
+ public static final LEDConfig LEDS_STRIP_2025_LEFT = new LEDConfig(0, BlinkenMode.STRIP_5V);
+ public static final LEDConfig LEDS_STRIP_2025_RIGHT = new LEDConfig(1, BlinkenMode.STRIP_5V);
}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDStripIO.java b/src/main/java/frc/robot/subsystems/led/LEDStripIO.java
new file mode 100644
index 0000000..c1d5c49
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/LEDStripIO.java
@@ -0,0 +1,22 @@
+package frc.robot.subsystems.led;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/** IO layer interface for april tag detection systems */
+public interface LEDStripIO {
+ @AutoLog
+ public static class LEDStripIOInputs {
+ int targetPulse;
+ int measuredPulse;
+ boolean runningSetup = false;
+
+ BlinkenLEDPattern requestedPattern;
+ }
+
+ /** Updates the set of loggable inputs. */
+ default void updateInputs(LEDStripIOInputs inputs) {}
+
+ public default void setPattern(BlinkenLEDPattern pattern) {}
+
+ public default void runSetup(boolean run) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDStripIOBlinken.java b/src/main/java/frc/robot/subsystems/led/LEDStripIOBlinken.java
new file mode 100644
index 0000000..60d5537
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/LEDStripIOBlinken.java
@@ -0,0 +1,45 @@
+package frc.robot.subsystems.led;
+
+import edu.wpi.first.wpilibj.PWM;
+import frc.robot.subsystems.led.LEDConstants.BlinkenMode;
+import frc.robot.subsystems.led.LEDConstants.LEDConfig;
+
+public class LEDStripIOBlinken implements LEDStripIO {
+
+ private final PWM pwm;
+ private final BlinkenMode mode;
+
+ private BlinkenLEDPattern pattern;
+ private boolean runSetup;
+
+ public LEDStripIOBlinken(LEDConfig config, BlinkenLEDPattern initialPattern) {
+ pwm = new PWM(config.pwmChannel());
+ mode = config.mode();
+ pattern = initialPattern;
+ }
+
+ @Override
+ public void updateInputs(LEDStripIOInputs inputs) {
+ inputs.runningSetup = runSetup;
+ inputs.requestedPattern = pattern;
+
+ inputs.targetPulse = inputs.requestedPattern.getPulse();
+ if (runSetup) {
+ inputs.targetPulse = mode.setupPulse;
+ }
+
+ pwm.setPulseTimeMicroseconds(inputs.targetPulse);
+
+ inputs.measuredPulse = pwm.getPulseTimeMicroseconds();
+ }
+
+ @Override
+ public void runSetup(boolean run) {
+ runSetup = run;
+ }
+
+ @Override
+ public void setPattern(BlinkenLEDPattern pattern) {
+ this.pattern = pattern;
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDStripIOSim.java b/src/main/java/frc/robot/subsystems/led/LEDStripIOSim.java
new file mode 100644
index 0000000..fda25e8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/LEDStripIOSim.java
@@ -0,0 +1,34 @@
+package frc.robot.subsystems.led;
+
+public class LEDStripIOSim implements LEDStripIO {
+
+ private BlinkenLEDPattern pattern;
+ private boolean runSetup;
+
+ public LEDStripIOSim(BlinkenLEDPattern initialPattern) {
+ pattern = initialPattern;
+ }
+
+ @Override
+ public void updateInputs(LEDStripIOInputs inputs) {
+ inputs.runningSetup = runSetup;
+ inputs.requestedPattern = pattern;
+
+ inputs.targetPulse = inputs.requestedPattern.getPulse();
+ if (runSetup) {
+ inputs.targetPulse = -1;
+ }
+
+ inputs.measuredPulse = inputs.targetPulse;
+ }
+
+ @Override
+ public void runSetup(boolean run) {
+ runSetup = run;
+ }
+
+ @Override
+ public void setPattern(BlinkenLEDPattern pattern) {
+ this.pattern = pattern;
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
index aa20629..e1f898f 100644
--- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
@@ -1,128 +1,75 @@
package frc.robot.subsystems.led;
+import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.PWM;
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.subsystems.led.LEDConstants.FixedPalettePattern;
-import frc.robot.subsystems.led.LEDConstants.SolidColors;
-import java.util.Optional;
+import java.util.Arrays;
+import java.util.function.Supplier;
import java.util.stream.Stream;
-import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
+/**
+ * Subsystem to manage multiple LED strips with Blinken LED Drivers.
+ *
+ * LEDs will retain their last set pattern until a new pattern is set, unless a default pattern
+ * is given though a default command.
+ */
public class LEDSubsystem extends SubsystemBase {
- private final Timer startupTimer = new Timer();
+ private LEDStripIO[] strips;
+ private LEDStripIOInputsAutoLogged[] inputs;
- // In future, this could be an IO layer
- private class LEDStrip {
- private final PWM pwm;
- private int pulse;
+ private final Debouncer setupDebouncer = new Debouncer(0.8);
- public LEDStrip(PWM pwmController, int initialPattern) {
- pwm = pwmController;
- pulse = initialPattern;
- }
-
- public void setPulse(int pulse) {
- this.pulse = pulse;
- }
-
- public void update() {
- pwm.setPulseTimeMicroseconds(pulse);
- }
- }
+ public LEDSubsystem(LEDStripIO... strips) {
+ this.strips = strips;
- private LEDStrip[] strips;
+ inputs =
+ Arrays.stream(strips)
+ .map(s -> new LEDStripIOInputsAutoLogged())
+ .toArray(LEDStripIOInputsAutoLogged[]::new);
- public LEDSubsystem(int... pwmPorts) {
-
- // Create all the strip objects
- strips = new LEDStrip[pwmPorts.length];
- for (int i = 0; i < pwmPorts.length; i++) {
- strips[i] = new LEDStrip(new PWM(pwmPorts[i]), LEDConstants.DEFAULT);
- }
+ Logger.recordOutput("led/numStrips", strips.length);
}
- private Optional alliance = Optional.empty();
-
@Override
public void periodic() {
+ boolean runSetup =
+ !setupDebouncer.calculate(DriverStation.isEnabled()) && DriverStation.isEnabled();
- if (DriverStation.isEnabled() && !startupTimer.isRunning()) {
- startupTimer.restart();
- } else if (startupTimer.hasElapsed(0.08)) {
- startupTimer.stop();
- }
-
- if (startupTimer.isRunning()) {
- applyAll(2125); // 5 volt mode
- } else {
- alliance = DriverStation.getAlliance();
-
- if (DriverStation.isEStopped()) {
- applyAll(SolidColors.GRAY);
- } else if (DriverStation.isDisabled()) {
- applyAll(SolidColors.BLUE, SolidColors.RED, SolidColors.WHITE);
- } else {
- applyAll(
- FixedPalettePattern.ColorWaves.OCEAN_PALETTE,
- FixedPalettePattern.ColorWaves.LAVA_PALETTE,
- FixedPalettePattern.ColorWaves.FOREST_PALETTE);
- }
+ for (int i = 0; i < strips.length; i++) {
+ strips[i].runSetup(runSetup);
+ strips[i].updateInputs(inputs[i]);
+ Logger.processInputs("LED/strip" + i, inputs[i]);
}
-
- // Update all the strips
- strips().forEach(LEDStrip::update);
-
- Logger.recordOutput("LED/pulses", strips().mapToInt(strip -> strip.pulse).toArray());
}
- /**
- * Apply a pattern to a specific LED strip
- *
- * @param strip The strip index to apply to. These are the same as the indices of LEDStrip objects
- * in the constructor
- * @param pattern The pattern to apply to the strip. See the below link for details, or use the
- * LEDPatterns constant.
- * @link https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf
- */
- public void apply(int strip, int pattern) {
- if (strip >= 0 && strip < strips.length) strips[strip].setPulse(pattern);
+ public Command applyColor(BlinkenLEDPattern color) {
+ return run(() -> set(color));
}
- /**
- * Apply a pattern to all LED strips
- *
- * @param pattern The pattern to apply to the strips. See the below link for details, or use the
- * LEDPatterns constant.
- * @link https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf
- */
- public void applyAll(int pattern) {
- for (LEDStrip strip : strips) {
- strip.setPulse(pattern);
- }
+ public Command applyColor(Supplier color) {
+ return run(() -> set(color.get()));
}
- @AutoLogOutput(key = "LED/hasSetUp")
- public boolean hasSetUp() {
- return !startupTimer.isRunning();
+ public Command applyColor(
+ BlinkenLEDPattern colorIfBlue,
+ BlinkenLEDPattern colorIfRed,
+ BlinkenLEDPattern colorIfUnknown) {
+ return applyColor(
+ () ->
+ DriverStation.getAlliance()
+ .map(a -> a == Alliance.Blue ? colorIfBlue : colorIfRed)
+ .orElse(colorIfUnknown));
}
- /**
- * Apply a pattern to all LED strips based on the alliance color
- *
- * @param blue The pattern to apply if the alliance color is blue
- * @param red The pattern to apply if the alliance color is red
- * @param backup The pattern to apply if the alliance color is unknown
- */
- private void applyAll(int blue, int red, int backup) {
- applyAll(alliance.map(a -> a == Alliance.Blue ? blue : red).orElse(backup));
+ public Command turnOff() {
+ return applyColor(BlinkenLEDPattern.OFF);
}
- private Stream strips() {
- return Stream.of(strips);
+ public void set(BlinkenLEDPattern pattern) {
+ Stream.of(strips).forEach(strip -> strip.setPattern(pattern));
}
}
diff --git a/src/main/java/frc/robot/subsystems/superstructure/wrist/Wrist.java b/src/main/java/frc/robot/subsystems/superstructure/wrist/Wrist.java
index e90d843..cde9f80 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/wrist/Wrist.java
+++ b/src/main/java/frc/robot/subsystems/superstructure/wrist/Wrist.java
@@ -56,19 +56,28 @@ public class Wrist extends SubsystemBase {
private final WristIO io;
private final WristIOInputsAutoLogged inputs = new WristIOInputsAutoLogged();
- private TrapezoidProfile profile;
- private TrapezoidProfile profileSlow;
- private BooleanSupplier slowModeSupplier = () -> false;
+ private final Alert motorConnectedAlert =
+ new Alert("Wrist motor disconnected!", Alert.AlertType.kError);
+
+ private final Debouncer disabledDebouncer = new Debouncer(3, DebounceType.kRising);
+
+ enum IdleModeControl {
+ BRAKE,
+ AUTO,
+ COAST
+ }
+
+ private IdleModeControl coastMode = IdleModeControl.BRAKE;
+ private boolean brakeModeEnabled = true;
private ArmFeedforward feedforward;
private State setpoint = null;
private Supplier goalSupplier = State::new;
- private final Alert motorConnectedAlert =
- new Alert("Wrist motor disconnected!", Alert.AlertType.kError);
-
- private final Debouncer disabledDebouncer = new Debouncer(3, DebounceType.kRising);
+ private TrapezoidProfile profile;
+ private TrapezoidProfile profileSlow;
+ private BooleanSupplier slowModeSupplier = () -> false;
/** Creates a new Wrist. */
public Wrist(WristIO io) {
@@ -79,7 +88,7 @@ public Wrist(WristIO io) {
new TrapezoidProfile(new Constraints(maxVelocitySlow.get(), maxAccelerationSlow.get()));
io.setPID(kP.get(), kI.get(), kD.get());
- io.setBrakeMode(true);
+ io.setBrakeMode(brakeModeEnabled);
this.feedforward = new ArmFeedforward(kS.get(), kG.get(), kV.get(), kA.get());
}
@@ -91,7 +100,45 @@ public void periodic() {
Logger.recordOutput("Wrist/slowMode", slowModeSupplier.getAsBoolean());
- io.setBrakeMode(!disabledDebouncer.calculate(DriverStation.isDisabled()));
+ LoggedTunableNumber.ifChanged(
+ hashCode(),
+ (values) -> {
+ io.setPID(values[0], values[1], values[2]);
+ },
+ kP,
+ kI,
+ kD);
+
+ LoggedTunableNumber.ifChanged(
+ hashCode(),
+ (values) -> feedforward = new ArmFeedforward(values[0], values[1], values[2], values[3]),
+ kS,
+ kG,
+ kV,
+ kA);
+
+ LoggedTunableNumber.ifChanged(
+ hashCode(),
+ (values) -> profile = new TrapezoidProfile(new Constraints(values[0], values[1])),
+ maxVelocity,
+ maxAcceleration);
+
+ LoggedTunableNumber.ifChanged(
+ hashCode(),
+ (values) -> profileSlow = new TrapezoidProfile(new Constraints(values[0], values[1])),
+ maxVelocitySlow,
+ maxAccelerationSlow);
+
+ brakeModeEnabled =
+ switch (coastMode) {
+ case BRAKE -> true;
+ case AUTO -> !disabledDebouncer.calculate(DriverStation.isDisabled());
+ case COAST -> false;
+ };
+
+ io.setBrakeMode(brakeModeEnabled);
+
+ Logger.recordOutput("Wrist/breakModeEnabled", brakeModeEnabled);
if (DriverStation.isEnabled()) {
Logger.recordOutput("Wrist/shouldRunProfiled", true);
@@ -128,35 +175,6 @@ public void periodic() {
setpoint = new State(inputs.positionRad, inputs.velocityRadPerSec);
}
- LoggedTunableNumber.ifChanged(
- hashCode(),
- (values) -> {
- io.setPID(values[0], values[1], values[2]);
- },
- kP,
- kI,
- kD);
-
- LoggedTunableNumber.ifChanged(
- hashCode(),
- (values) -> feedforward = new ArmFeedforward(values[0], values[1], values[2], values[3]),
- kS,
- kG,
- kV,
- kA);
-
- LoggedTunableNumber.ifChanged(
- hashCode(),
- (values) -> profile = new TrapezoidProfile(new Constraints(values[0], values[1])),
- maxVelocity,
- maxAcceleration);
-
- LoggedTunableNumber.ifChanged(
- hashCode(),
- (values) -> profileSlow = new TrapezoidProfile(new Constraints(values[0], values[1])),
- maxVelocitySlow,
- maxAccelerationSlow);
-
motorConnectedAlert.set(!inputs.motorConnected);
}
diff --git a/src/main/java/frc/robot/subsystems/superstructure/wrist/WristIOHardware.java b/src/main/java/frc/robot/subsystems/superstructure/wrist/WristIOHardware.java
index cc02db2..f6353e5 100644
--- a/src/main/java/frc/robot/subsystems/superstructure/wrist/WristIOHardware.java
+++ b/src/main/java/frc/robot/subsystems/superstructure/wrist/WristIOHardware.java
@@ -22,6 +22,10 @@
import frc.robot.utility.SparkUtil;
public class WristIOHardware implements WristIO {
+
+ private static final FeedbackSensor SENSOR_TYPE = FeedbackSensor.kAbsoluteEncoder;
+ // private final static FeedbackSensor SENSOR_TYPE = FeedbackSensor.kPrimaryEncoder;
+
private final SparkMax motor;
private final RelativeEncoder encoder;
private final SparkAbsoluteEncoder absEncoder;
@@ -52,7 +56,7 @@ public WristIOHardware(WristConfig config) {
.inverted(config.encoderInverted())
.zeroOffset(config.absoluteEncoderOffset())
.zeroCentered(true);
- motorConfig.closedLoop.pidf(0, 0, 0, 0).feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
+ motorConfig.closedLoop.pidf(0, 0, 0, 0).feedbackSensor(SENSOR_TYPE);
motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java
index 05f86d4..79a04fd 100644
--- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java
+++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java
@@ -1,5 +1,6 @@
package frc.robot.subsystems.vision;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.filter.Debouncer.DebounceType;
@@ -8,6 +9,8 @@
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.vision.Camera.AbsoluteTrackedTarget;
+import frc.robot.subsystems.vision.Camera.RelativeTrackedTarget;
import frc.robot.subsystems.vision.Camera.VisionResult;
import frc.robot.subsystems.vision.Camera.VisionResultStatus;
import java.util.ArrayList;
@@ -19,10 +22,6 @@
import org.littletonrobotics.junction.Logger;
public class AprilTagVision extends SubsystemBase {
-
- public static final boolean DO_SUMMARY_LOGGING = true;
- public static final boolean DO_CAMERA_LOGGING = true;
-
private final Camera[] cameras;
private final Debouncer debouncer = new Debouncer(0.2, DebounceType.kFalling);
@@ -32,6 +31,9 @@ public class AprilTagVision extends SubsystemBase {
private boolean hasVisionEstimate = false;
+ private AprilTagFieldLayout fieldTags = null;
+ private List simulatedTargets = new ArrayList<>();
+
public AprilTagVision(CameraIO... camerasIO) {
this.cameras = Arrays.stream(camerasIO).map(io -> new Camera(io)).toArray(Camera[]::new);
}
@@ -39,6 +41,14 @@ public AprilTagVision(CameraIO... camerasIO) {
@Override
public void periodic() {
+ AprilTagFieldLayout fieldTags = this.fieldTags;
+ for (SimControlledTarget target : simulatedTargets) {
+ fieldTags = target.createFieldWithTarget(fieldTags);
+ }
+ for (Camera camera : cameras) {
+ camera.setFieldTags(fieldTags);
+ }
+
// Run periodic for all cameras, as they are not real subsystems
for (Camera camera : cameras) {
camera.periodic();
@@ -56,21 +66,15 @@ public void periodic() {
for (Camera camera : cameras) {
String cameraRoot = root + "/" + camera.getCameraName();
- if (DO_CAMERA_LOGGING && camera.getResults().length == 0) {
- Logger.recordOutput(cameraRoot + "/tagsUsedPositions", new Pose3d[] {});
- Logger.recordOutput(cameraRoot + "/positionEstimate", new Pose3d[] {});
- Logger.recordOutput(cameraRoot + "/status", VisionResultStatus.NO_DATA);
- Logger.recordOutput(cameraRoot + "/success", false);
- }
+ Logger.recordOutput(cameraRoot + "/tagsUsedPositions", new Pose3d[] {});
+ Logger.recordOutput(cameraRoot + "/positionEstimate", new Pose3d[] {});
+ Logger.recordOutput(cameraRoot + "/status", VisionResultStatus.NO_DATA);
+ Logger.recordOutput(cameraRoot + "/success", false);
// Loop through all results that the camera has
for (VisionResult result : camera.getResults()) {
if (!result.hasNewData()) {
- Logger.recordOutput(cameraRoot + "/tagsUsedPositions", new Pose3d[] {});
- Logger.recordOutput(cameraRoot + "/positionEstimate", new Pose3d[] {});
- Logger.recordOutput(cameraRoot + "/status", VisionResultStatus.NO_DATA);
- Logger.recordOutput(cameraRoot + "/success", false);
continue;
}
@@ -84,24 +88,17 @@ public void periodic() {
hasVisionEstimate = hasVisionEstimate || visionEstimate.isSuccess();
- // Logging
- if (DO_CAMERA_LOGGING) {
- Logger.recordOutput(cameraRoot + "/tagsUsedPositions", result.tagPositionsOnField());
-
- Logger.recordOutput(cameraRoot + "/positionEstimate", visionEstimate.robotPose());
-
- Logger.recordOutput(cameraRoot + "/status", visionEstimate.status());
- Logger.recordOutput(cameraRoot + "/success", visionEstimate.status().isSuccess());
- }
+ Logger.recordOutput(cameraRoot + "/tagsUsedPositions", result.tagPositionsOnField());
+ Logger.recordOutput(cameraRoot + "/positionEstimate", visionEstimate.robotPose());
+ Logger.recordOutput(cameraRoot + "/status", visionEstimate.status());
+ Logger.recordOutput(cameraRoot + "/success", visionEstimate.status().isSuccess());
- if (DO_SUMMARY_LOGGING) {
- if (visionEstimate.isSuccess()) {
- robotPosesAccepted.add(visionEstimate.robotPose());
- } else {
- robotPosesRejected.add(visionEstimate.robotPose());
- }
- tagPoses.addAll(Arrays.asList(result.tagPositionsOnField()));
+ if (visionEstimate.isSuccess()) {
+ robotPosesAccepted.add(visionEstimate.robotPose());
+ } else {
+ robotPosesRejected.add(visionEstimate.robotPose());
}
+ tagPoses.addAll(Arrays.asList(result.tagPositionsOnField()));
for (Consumer consumer :
timestampRobotPoseEstimateConsumers) {
@@ -110,11 +107,25 @@ public void periodic() {
}
}
- if (DO_SUMMARY_LOGGING) {
- Logger.recordOutput(root + "/robotPosesAccepted", robotPosesAccepted.toArray(Pose3d[]::new));
- Logger.recordOutput(root + "/robotPosesRejected", robotPosesRejected.toArray(Pose3d[]::new));
- Logger.recordOutput(root + "/tagPoses", tagPoses.toArray(Pose3d[]::new));
- }
+ Logger.recordOutput(root + "/robotPosesAccepted", robotPosesAccepted.toArray(Pose3d[]::new));
+ Logger.recordOutput(root + "/robotPosesRejected", robotPosesRejected.toArray(Pose3d[]::new));
+ Logger.recordOutput(root + "/tagPoses", tagPoses.toArray(Pose3d[]::new));
+ }
+
+ public List getRelativeTrackedTargets() {
+ return Arrays.stream(cameras).flatMap(camera -> camera.getRelativeTargets().stream()).toList();
+ }
+
+ public List getAbsoluteTrackedTargets() {
+ return Arrays.stream(cameras).flatMap(camera -> camera.getAbsoluteTargets().stream()).toList();
+ }
+
+ public void setFieldTags(AprilTagFieldLayout fieldTags) {
+ this.fieldTags = fieldTags;
+ }
+
+ public void addSimulatedTarget(SimControlledTarget target) {
+ simulatedTargets.add(target);
}
/** Get whether or not the vision system has a valid estimate */
@@ -122,18 +133,19 @@ public boolean hasVisionEstimate() {
return hasVisionEstimate;
}
- public boolean hasVisionEstimateDebounce() {
+ public boolean hasStableVisionEstimate() {
return debouncer.calculate(hasVisionEstimate);
}
/**
* Set the last robot pose supplier for all cameras
*
+ * @param filter if robot pose filtering should be applied
* @param lastRobotPose the last robot pose supplier
*/
- public void setLastRobotPoseSupplier(Supplier lastRobotPose) {
+ public void filterBasedOnLastPose(boolean filter, Supplier lastRobotPose) {
for (Camera camera : cameras) {
- camera.setLastRobotPoseSupplier(lastRobotPose);
+ camera.filterBasedOnLastPose(filter, lastRobotPose);
}
}
diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java
index e544bb3..28b4c5c 100644
--- a/src/main/java/frc/robot/subsystems/vision/Camera.java
+++ b/src/main/java/frc/robot/subsystems/vision/Camera.java
@@ -1,18 +1,22 @@
package frc.robot.subsystems.vision;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Alert;
+import frc.robot.subsystems.vision.VisionConstants.CameraConfig;
import frc.robot.utility.tunable.LoggedTunableNumber;
import frc.robot.utility.tunable.LoggedTunableNumberFactory;
import java.util.Arrays;
import java.util.DoubleSummaryStatistics;
+import java.util.List;
import java.util.Optional;
import java.util.Set;
import java.util.function.Supplier;
@@ -43,6 +47,7 @@ public class Camera {
private final CameraIO io;
private final CameraIOInputsAutoLogged inputs = new CameraIOInputsAutoLogged();
+ private AprilTagFieldLayout aprilTagFieldLayout;
private Set tagsIdsOnField;
private VisionResult[] results = new VisionResult[0];
@@ -60,6 +65,31 @@ public static record VisionResult(
Matrix standardDeviation,
VisionResultStatus status) {}
+ public record RelativeTrackedTarget(
+ int id, Transform3d cameraToTarget, CameraConfig camera, double poseAmbiguity) {}
+
+ public record AbsoluteTrackedTarget(
+ int id,
+ Pose3d targetPose,
+ Pose3d cameraPose,
+ Transform3d cameraToTarget,
+ CameraConfig camera,
+ double poseAmbiguity,
+ Pose2d robotPose) {
+ public AbsoluteTrackedTarget(RelativeTrackedTarget relativeTarget, Pose2d robotPose) {
+ this(
+ relativeTarget.id,
+ new Pose3d(robotPose)
+ .plus(relativeTarget.camera.robotToCamera())
+ .plus(relativeTarget.cameraToTarget),
+ new Pose3d(robotPose).plus(relativeTarget.camera.robotToCamera()),
+ relativeTarget.cameraToTarget,
+ relativeTarget.camera,
+ relativeTarget.poseAmbiguity,
+ robotPose);
+ }
+ }
+
/**
* Create a new robot camera with IO layer
*
@@ -68,17 +98,21 @@ public static record VisionResult(
public Camera(CameraIO io) {
this.io = io;
- io.setAprilTagFieldLayout(VisionConstants.FIELD);
- this.tagsIdsOnField =
- VisionConstants.FIELD.getTags().stream().map((tag) -> tag.ID).collect(Collectors.toSet());
-
this.missingCameraAlert =
new Alert(String.format("Missing cameras %s", getCameraName()), Alert.AlertType.kWarning);
}
+ public void setFieldTags(AprilTagFieldLayout field) {
+ if (field != this.aprilTagFieldLayout) {
+ io.setAprilTagFieldLayout(field);
+ tagsIdsOnField = field.getTags().stream().map((tag) -> tag.ID).collect(Collectors.toSet());
+ this.aprilTagFieldLayout = field;
+ }
+ }
+
/** Get name of camera as specified by IO */
public String getCameraName() {
- return io.getCameraPosition() + " (" + io.getCameraName() + ")";
+ return io.getCameraConfig() + " (" + io.getCameraName() + ")";
}
/** Run periodic of module. Updates the set of loggable inputs, updating vision result. */
@@ -92,6 +126,13 @@ public void periodic() {
Pose3d[] tagPositionsOnField = getTagPositionsOnField(inputs.tagsUsed[i]);
if (inputs.hasNewData[i]) {
+ Matrix standardDeviations =
+ getStandardDeviations(tagPositionsOnField, inputs.estimatedRobotPose[i]);
+ VisionResultStatus status =
+ lastRobotPoseSupplier == null
+ ? getStatus(inputs.estimatedRobotPose[i], inputs.tagsUsed[i])
+ : getStatus(
+ inputs.estimatedRobotPose[i], inputs.tagsUsed[i], lastRobotPoseSupplier.get());
results[i] =
new VisionResult(
true,
@@ -99,13 +140,8 @@ public void periodic() {
inputs.timestampSecondFPGA[i],
inputs.tagsUsed[i],
tagPositionsOnField,
- getStandardDeviations(tagPositionsOnField, inputs.estimatedRobotPose[i]),
- lastRobotPoseSupplier == null
- ? getStatus(inputs.estimatedRobotPose[i], inputs.tagsUsed[i])
- : getStatus(
- inputs.estimatedRobotPose[i],
- inputs.tagsUsed[i],
- lastRobotPoseSupplier.get()));
+ standardDeviations,
+ status);
} else {
results[i] =
new VisionResult(
@@ -120,8 +156,17 @@ public void periodic() {
}
}
- public void setLastRobotPoseSupplier(Supplier lastRobotPose) {
- this.lastRobotPoseSupplier = lastRobotPose;
+ public List getAbsoluteTargets() {
+ return io.getAbsoluteTargets();
+ }
+
+ public List getRelativeTargets() {
+ return io.getRelativeTargets();
+ }
+
+ public void filterBasedOnLastPose(boolean filter, Supplier lastRobotPose) {
+ this.io.setLastPoseSupplier(lastRobotPose);
+ this.lastRobotPoseSupplier = filter ? lastRobotPose : null;
}
public VisionResult[] getResults() {
@@ -130,7 +175,7 @@ public VisionResult[] getResults() {
private Pose3d[] getTagPositionsOnField(int[] tagsUsed) {
return Arrays.stream(tagsUsed)
- .mapToObj(VisionConstants.FIELD::getTagPose)
+ .mapToObj(aprilTagFieldLayout::getTagPose)
.filter(Optional::isPresent)
.map(Optional::get)
.toArray(Pose3d[]::new);
@@ -206,8 +251,8 @@ private VisionResultStatus getStatus(Pose3d estimatedRobotPose, int[] tagsUsed)
if (estimatedRobotPose.getX() < 0
|| estimatedRobotPose.getY() < 0
- || estimatedRobotPose.getX() > VisionConstants.FIELD.getFieldLength()
- || estimatedRobotPose.getY() > VisionConstants.FIELD.getFieldWidth()) {
+ || estimatedRobotPose.getX() > aprilTagFieldLayout.getFieldLength()
+ || estimatedRobotPose.getY() > aprilTagFieldLayout.getFieldWidth()) {
return VisionResultStatus.INVALID_POSE_OUTSIDE_FIELD;
}
diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIO.java b/src/main/java/frc/robot/subsystems/vision/CameraIO.java
index 4afb333..b70ea6e 100644
--- a/src/main/java/frc/robot/subsystems/vision/CameraIO.java
+++ b/src/main/java/frc/robot/subsystems/vision/CameraIO.java
@@ -1,7 +1,13 @@
package frc.robot.subsystems.vision;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
+import frc.robot.subsystems.vision.Camera.AbsoluteTrackedTarget;
+import frc.robot.subsystems.vision.Camera.RelativeTrackedTarget;
+import frc.robot.subsystems.vision.VisionConstants.CameraConfig;
+import java.util.List;
+import java.util.function.Supplier;
import org.littletonrobotics.junction.AutoLog;
/** IO layer interface for april tag detection systems */
@@ -21,13 +27,23 @@ public static class CameraIOInputs {
/** Updates the set of loggable inputs. */
default void updateInputs(CameraIOInputs inputs) {}
+ default void setLastPoseSupplier(Supplier lastPoseSupplier) {}
+
+ default List getRelativeTargets() {
+ return List.of();
+ }
+
+ default List getAbsoluteTargets() {
+ return List.of();
+ }
+
/** Get name of io camera */
default String getCameraName() {
return "Camera";
}
- default String getCameraPosition() {
- return getCameraName();
+ default CameraConfig getCameraConfig() {
+ return null;
}
/** Set april tag field layout to use */
diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java
index 005bbd3..c63fe7d 100644
--- a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java
+++ b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java
@@ -1,10 +1,15 @@
package frc.robot.subsystems.vision;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
+import frc.robot.subsystems.vision.Camera.AbsoluteTrackedTarget;
+import frc.robot.subsystems.vision.Camera.RelativeTrackedTarget;
import frc.robot.subsystems.vision.VisionConstants.CameraConfig;
+import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
+import java.util.function.Supplier;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
@@ -17,10 +22,14 @@ public class CameraIOPhotonVision implements CameraIO {
private final PhotonCamera camera;
private final PhotonPoseEstimator photonPoseEstimator;
- private final String cameraPositionTitle;
+ private final CameraConfig config;
+
+ private List latestTargets = new ArrayList<>();
+
+ private Supplier lastPoseSupplier;
public CameraIOPhotonVision(CameraConfig config) {
- this.cameraPositionTitle = config.cameraPosition();
+ this.config = config;
// --- Setup Camera ---
camera = new PhotonCamera(config.cameraName());
@@ -38,7 +47,7 @@ public CameraIOPhotonVision(CameraConfig config) {
photonPoseEstimator =
new PhotonPoseEstimator(
- VisionConstants.FIELD,
+ VisionConstants.DEFAULT_FIELD,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
config.robotToCamera());
@@ -54,8 +63,13 @@ protected PhotonPoseEstimator getPhotonPoseEstimator() {
}
@Override
- public String getCameraPosition() {
- return cameraPositionTitle;
+ public void setLastPoseSupplier(Supplier robotPoseSupplier) {
+ this.lastPoseSupplier = robotPoseSupplier;
+ }
+
+ @Override
+ public CameraConfig getCameraConfig() {
+ return config;
}
@Override
@@ -75,9 +89,18 @@ public void updateInputs(CameraIOInputs inputs) {
inputs.updatesReceived = pipelineResults.size();
+ if (lastPoseSupplier != null) {
+ photonPoseEstimator.setLastPose(lastPoseSupplier.get());
+ }
+
+ this.latestTargets.clear();
+
for (int i = 0; i < pipelineResults.size(); i++) {
+ PhotonPipelineResult cameraResult = pipelineResults.get(i);
Optional estimatedRobotPoseOptional =
- photonPoseEstimator.update(pipelineResults.get(i));
+ photonPoseEstimator.update(cameraResult);
+
+ this.latestTargets.addAll(cameraResult.getTargets());
if (estimatedRobotPoseOptional.isPresent()) {
@@ -106,6 +129,27 @@ public void updateInputs(CameraIOInputs inputs) {
inputs.connected = camera.isConnected();
}
+ @Override
+ public List getRelativeTargets() {
+ return latestTargets.stream()
+ .map(
+ t ->
+ new RelativeTrackedTarget(
+ t.getFiducialId(), t.getBestCameraToTarget(), config, t.getPoseAmbiguity()))
+ .toList();
+ }
+
+ @Override
+ public List getAbsoluteTargets() {
+ if (lastPoseSupplier == null) {
+ throw new IllegalStateException(
+ "Robot pose supplier not set for absolute target calculation");
+ }
+ return getRelativeTargets().stream()
+ .map(r -> new AbsoluteTrackedTarget(r, lastPoseSupplier.get()))
+ .toList();
+ }
+
@Override
public String getCameraName() {
return camera.getName();
diff --git a/src/main/java/frc/robot/subsystems/vision/SimControlledTarget.java b/src/main/java/frc/robot/subsystems/vision/SimControlledTarget.java
new file mode 100644
index 0000000..4f35f51
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/SimControlledTarget.java
@@ -0,0 +1,87 @@
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.XboxController;
+import frc.robot.utility.JoystickUtil;
+import frc.robot.utility.VirtualSubsystem;
+import java.util.List;
+import org.littletonrobotics.junction.Logger;
+
+public class SimControlledTarget extends VirtualSubsystem {
+
+ private static final double TRANSLATION_SPEED = 0.03;
+ private static final double TRIGGER_SPEED = 0.03;
+ private static final double ROTATION_SPEED = 0.1;
+ private static final double BUTTON_ROTATION_SPEED = 0.1;
+
+ private final XboxController controller;
+
+ private final int tagId;
+
+ private Pose3d tagPose;
+ private boolean tagHidden = true;
+
+ public SimControlledTarget(int tagID, Pose3d startingPose, XboxController controller) {
+ this.tagId = tagID;
+ this.controller = controller;
+
+ this.tagPose = startingPose;
+ }
+
+ @Override
+ public void periodic() {
+
+ tagHidden = controller.getAButton();
+
+ tagPose =
+ new Pose3d(
+ new Translation3d(
+ tagPose.getX()
+ - JoystickUtil.applyDeadband(controller.getLeftY()) * TRANSLATION_SPEED,
+ tagPose.getY()
+ - JoystickUtil.applyDeadband(controller.getLeftX()) * TRANSLATION_SPEED,
+ tagPose.getZ()
+ - controller.getLeftTriggerAxis() * TRIGGER_SPEED
+ + controller.getRightTriggerAxis() * TRIGGER_SPEED),
+ new Rotation3d(
+ MathUtil.angleModulus(
+ tagPose.getRotation().getX() * (controller.getPOV() == 0 ? 0 : 1)
+ - (controller.getPOV() == 270 ? BUTTON_ROTATION_SPEED : 0)
+ + (controller.getPOV() == 90 ? BUTTON_ROTATION_SPEED : 0)),
+ MathUtil.angleModulus(
+ tagPose.getRotation().getY()
+ + JoystickUtil.applyDeadband(controller.getRightY()) * ROTATION_SPEED),
+ MathUtil.angleModulus(
+ tagPose.getRotation().getZ()
+ - JoystickUtil.applyDeadband(controller.getRightX()) * ROTATION_SPEED)));
+
+ Logger.recordOutput("SimControlledTarget/" + tagId + "/TagPose", tagPose);
+ Logger.recordOutput("SimControlledTarget/" + tagId + "/TagId", tagId);
+ }
+
+ public AprilTagFieldLayout createFieldWithTarget(AprilTagFieldLayout initialField) {
+
+ List tags = initialField.getTags();
+
+ tags.removeIf(tag -> tag.ID == tagId);
+ if (!tagHidden) {
+ tags.add(new AprilTag(tagId, tagPose));
+ }
+
+ return new AprilTagFieldLayout(
+ tags, initialField.getFieldLength(), initialField.getFieldWidth());
+ }
+
+ public int getTagId() {
+ return tagId;
+ }
+
+ public Pose3d getTagPose() {
+ return tagPose;
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
index e03ab5e..2ee4fc5 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -6,12 +6,17 @@
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
+import java.util.List;
public class VisionConstants {
// --- Vision Config ---
- public static final AprilTagFieldLayout FIELD =
- AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+ public static final AprilTagFieldLayout DEFAULT_FIELD =
+ AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+
+ public static final AprilTagFieldLayout BLANK_FIELD =
+ new AprilTagFieldLayout(
+ List.of(), DEFAULT_FIELD.getFieldLength(), DEFAULT_FIELD.getFieldWidth());
// Set cameraName on PhotonVision web interface. Edit camera name from camera type to camera
// position. To find robotToCamera, measure the distance from the camera to the center of the
@@ -19,20 +24,31 @@ public class VisionConstants {
// Docs: https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
- public record CameraConfig(String cameraName, String cameraPosition, Transform3d robotToCamera) {}
+ enum CameraPositions {
+ FRONT,
+ LEFT,
+ RIGHT,
+ FRONT_LEFT,
+ FRONT_RIGHT,
+ BACK_LEFT,
+ BACK_RIGHT
+ }
+
+ public record CameraConfig(
+ String cameraName, CameraPositions cameraPosition, Transform3d robotToCamera) {}
public static final CameraConfig SIM_FRONT_CAMERA =
new CameraConfig(
"frontCam",
- "front",
+ CameraPositions.FRONT,
new Transform3d(
new Translation3d(Units.inchesToMeters(27.5 / 2.0 + 1.0), 0, Units.inchesToMeters(6)),
new Rotation3d(0, Units.degreesToRadians(0), 0)));
- public static final CameraConfig WOODV2_LEFT_CAMERA =
+ public static final CameraConfig WOOD_V2_LEFT_CAMERA =
new CameraConfig(
"leftCamera",
- "left",
+ CameraPositions.LEFT,
new Transform3d(
new Translation3d(0, Units.inchesToMeters(27.5 / 2.0 - 0.5), 0),
new Rotation3d(0, Units.degreesToRadians(0), Units.degreesToRadians(90))));
@@ -40,7 +56,7 @@ public record CameraConfig(String cameraName, String cameraPosition, Transform3d
public static final CameraConfig WOODV2_RIGHT_CAMERA =
new CameraConfig(
"rightCamera",
- "right",
+ CameraPositions.RIGHT,
new Transform3d(
new Translation3d(
0, -Units.inchesToMeters(27.5 / 2.0 + 1.0), Units.inchesToMeters(3)),
@@ -61,7 +77,7 @@ public record CameraConfig(String cameraName, String cameraPosition, Transform3d
public static final CameraConfig COMP_FRONT_LEFT_CAMERA =
new CameraConfig(
"plzwork",
- "Front Left",
+ CameraPositions.FRONT_LEFT,
new Transform3d(
new Translation3d(+CAMERA_OFFSET_X, +CAMERA_OFFSET_Y, FRONT_CAMERA_OFFSET_Z),
new Rotation3d(0, -FRONT_CAMERA_PITCH, +FRONT_CAMERA_YAW)));
@@ -69,7 +85,7 @@ public record CameraConfig(String cameraName, String cameraPosition, Transform3d
public static final CameraConfig COMP_FRONT_RIGHT_CAMERA =
new CameraConfig(
"FrontRight8032",
- "Front Right",
+ CameraPositions.FRONT_RIGHT,
new Transform3d(
new Translation3d(+CAMERA_OFFSET_X, -CAMERA_OFFSET_Y, FRONT_CAMERA_OFFSET_Z),
new Rotation3d(0, -FRONT_CAMERA_PITCH, -FRONT_CAMERA_YAW)));
@@ -77,7 +93,7 @@ public record CameraConfig(String cameraName, String cameraPosition, Transform3d
public static final CameraConfig COMP_BACK_LEFT_CAMERA =
new CameraConfig(
"jarreaucam",
- "Back Left",
+ CameraPositions.BACK_LEFT,
new Transform3d(
new Translation3d(-CAMERA_OFFSET_X, +CAMERA_OFFSET_Y, BACK_CAMERA_OFFSET_Z),
new Rotation3d(0, -BACK_CAMERA_PITCH, Units.degreesToRadians(180 + 32.46))));
@@ -85,7 +101,7 @@ public record CameraConfig(String cameraName, String cameraPosition, Transform3d
public static final CameraConfig COMP_BACK_RIGHT_CAMERA =
new CameraConfig(
"BackRightCamera8032",
- "Back Right",
+ CameraPositions.BACK_RIGHT,
new Transform3d(
new Translation3d(-CAMERA_OFFSET_X, -CAMERA_OFFSET_Y, BACK_CAMERA_OFFSET_Z),
new Rotation3d(0, -BACK_CAMERA_PITCH, Units.degreesToRadians(180 - 30))));
diff --git a/src/main/java/frc/robot/subsystems/visualizer/ObjectVisualizer.java b/src/main/java/frc/robot/subsystems/visualizer/ObjectVisualizer.java
index 83e512d..baaaa7c 100644
--- a/src/main/java/frc/robot/subsystems/visualizer/ObjectVisualizer.java
+++ b/src/main/java/frc/robot/subsystems/visualizer/ObjectVisualizer.java
@@ -134,8 +134,11 @@ public Command placeHeldItemOnNearestWithInterpolation(
List targets, double maxDistance, double seconds) {
return defer(
() -> {
- Optional target = findHeldItemPlacementTarget(targets, maxDistance);
- return placeHeldItemWithInterpolation(target.get(), seconds).onlyIf(target::isPresent);
+ Optional nearestTarget = findHeldItemPlacementTarget(targets, maxDistance);
+ if (nearestTarget.isPresent()) {
+ return placeHeldItemWithInterpolation(nearestTarget.get(), seconds);
+ }
+ return runOnce(this::ejectHeldItem);
});
}