From 1f5f004df9fd76cf507fe14a92ff56ba7e75634b Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Mon, 9 Mar 2026 18:26:31 -0700 Subject: [PATCH 01/12] add led subsystem, add vision checks, link vision checks to leds --- src/main/java/frc/robot/RobotContainer.java | 5 +++++ .../frc/robot/subsystems/LEDSubsystem.java | 15 +++++++++++++- .../subsystems/vision/LocalizationCamera.java | 4 ++++ .../subsystems/vision/VisionSubsystem.java | 20 +++++++++++++++++++ 4 files changed, 43 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0003d60..faf8e70d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.subsystems.scorer.ShooterSubsystem; import frc.robot.subsystems.LaserCANSensorBase; import frc.robot.subsystems.RobotCommandFactory; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.PivotConstants; import frc.robot.Constants.RollerConstants; @@ -102,6 +103,7 @@ public enum RobotMode { public final IndexerSubsystem m_indexer = new IndexerSubsystem(); public final PivotSubsystem m_pivot = new PivotSubsystem(); public final ColumnSubsystem m_column = new ColumnSubsystem(); + public final LEDSubsystem m_led = new LEDSubsystem(); // Sensors private final LaserCANSensorBase m_intakeSensor = new LaserCANSensorBase( @@ -144,6 +146,9 @@ public RobotContainer() { } setRobotMode(RobotMode.NEUTRAL); + + // Vision LED indicator + m_led.setDefaultCommand(m_led.runVisionStatus(() -> m_vision.isVisionHealthy())); // Configure auto builder createNamedCommands(); diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index d4d87503..0640e0e8 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -37,7 +37,8 @@ public LEDSubsystem() { // Set the default command to turn the strip off, otherwise the last colors written by // the last command to run will continue to be displayed. // Note: Other default patterns could be used instead! - setDefaultCommand((runScrollingYellowBlueCommand()).withName("LED/default colors")); + // default command has been set in robot container + // setDefaultCommand((runScrollingYellowBlueCommand()).withName("LED/default colors")); } @@ -111,4 +112,16 @@ public Command runScrollingYellowBlueCommand() { LEDPattern pattern = base.scrollAtRelativeSpeed(Percent.per(Second).of(100)); return runPattern(pattern).withName("LED/scrolling yellow-blue"); } + + public Command runVisionStatus(java.util.function.BooleanSupplier visionHealthySupplier) { + LEDPattern base = LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kYellow, Color.kBlue); + LEDPattern pattern = base.scrollAtRelativeSpeed(Percent.per(Second).of(100)); + + return run(() -> { + if (visionHealthySupplier.getAsBoolean()) + pattern.applyTo(m_ledBuffer); + else + LEDPattern.solid(Color.kRed).applyTo(m_ledBuffer); + }).withName("LED/vision status"); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java index b01bf411..6e9597de 100644 --- a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java @@ -83,6 +83,10 @@ public Field2d getEstField(){ return m_estPoseField; } + public boolean isConnected() { + return m_camera.isConnected(); + } + // Toggles "Raw Video Mode" for single camera // NOTE: PhotonVision method for raw video mode is setDriverMode // setDriverMode(true) = raw video feed, setDriverMode(false) = normal AprilTag processing diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index d4103011..2e5b54a4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -108,6 +108,26 @@ public Command toggleRawVideoModeCommand() { }); } + public boolean areCamerasConnected() { + for (LocalizationCamera cam : cameras) + if (!cam.isConnected()) + return false; + return true; + } + + public boolean isVisionUpdating() { + double currentTime = Timer.getFPGATimestamp(); + return (currentTime - m_lastTimestamp) < 0.5; + } + + public boolean hasValidTargets() { + return !allValidReadings.isEmpty(); + } + + public boolean isVisionHealthy() { + return areCamerasConnected() && isVisionUpdating() && hasValidTargets(); + } + @Override public void periodic() { // clear allValidReadings at the beginning of every loop From 7e174ccc147f7f07f542a4e7b329f672ce48c237 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Mon, 9 Mar 2026 18:45:18 -0700 Subject: [PATCH 02/12] add check for est pose flickering --- .../subsystems/vision/VisionSubsystem.java | 36 ++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 2e5b54a4..2ea77a85 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -64,6 +64,10 @@ public class VisionSubsystem extends SubsystemBase { private boolean m_rawVideoModeEnabled = false; // true = raw video feed, false = normal AprilTag processing + private Pose2d m_lastVisionPose = null; + + private double m_lastVisionPoseTimestamp = 0.0; + /** Creates a new Vision Subsystem. */ public VisionSubsystem() { cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D)); @@ -124,8 +128,38 @@ public boolean hasValidTargets() { return !allValidReadings.isEmpty(); } + public boolean isVisionPoseStable() { + List readings = getValidCameraReadings(); + + //if there are no valid readings, then theres nothing to compare to + if (readings.isEmpty()) { + return true; + } + + Pose2d currentPose = readings.get(0).robotPose().estimatedPose.toPose2d(); + double timestamp = readings.get(0).robotPose().timestampSeconds; + + //if we have never stored a previous vision pose before, then assume the est pose is stable + if (m_lastVisionPose == null) { + m_lastVisionPose = currentPose; + m_lastVisionPoseTimestamp = timestamp; + return true; + } + + //calculate how far + how much time has passed since the robot appears to have moved between frames. + double distanceJump = currentPose.getTranslation().getDistance(m_lastVisionPose.getTranslation()); + double dt = timestamp - m_lastVisionPoseTimestamp; + + //update stored pose so that next time the new current pose will be compared against this frame + m_lastVisionPose = currentPose; + m_lastVisionPoseTimestamp = timestamp; + + // if robot seems like it's jumping > 1 meter in < 0.1s → probably est pose is flickering too much + return !(distanceJump > 1.0 && dt < 0.1); + } + public boolean isVisionHealthy() { - return areCamerasConnected() && isVisionUpdating() && hasValidTargets(); + return areCamerasConnected() && isVisionUpdating() && hasValidTargets() && isVisionPoseStable(); } @Override From 0bdeb4b0268627462afd9b9570e0520b824ac5a3 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Tue, 10 Mar 2026 11:32:42 -0700 Subject: [PATCH 03/12] update areCamerasConnected() check to see if multiple cameras are disconnected --- src/main/java/frc/robot/Constants.java | 3 +++ .../java/frc/robot/subsystems/vision/VisionSubsystem.java | 6 +++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1da829d..416cdb7c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -454,6 +454,9 @@ public static class VisionConstants { public static final double ROBOT_TO_CAM1_YAW; public static final Transform3d ROBOT_TO_CAM1_3D; + //is camera connected check: + public static final int NUM_CAMERAS_DISCONNECTED; + // Camera 2 position - robot-specific public static final double ROBOT_TO_CAM2_X; public static final double ROBOT_TO_CAM2_Y; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 2ea77a85..1899b68f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -113,9 +113,13 @@ public Command toggleRawVideoModeCommand() { } public boolean areCamerasConnected() { - for (LocalizationCamera cam : cameras) + int notConnectedCounter = 0; + for (LocalizationCamera cam : cameras){ if (!cam.isConnected()) + notConnectedCounter += 1; + if (notConnectedCounter > VisionConstants.MIN_NUM_CAMERAS_DISCONNECTED) return false; + } return true; } From 5dc67b739af77fb47993de4d238586350b1f60d8 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Tue, 10 Mar 2026 11:33:06 -0700 Subject: [PATCH 04/12] Squashed commit of the following: commit 13f0e79a3cf78e4f45681145d2fe6055fee429e8 Author: zoe z <121075323+amzoeee@users.noreply.github.com> Date: Tue Mar 10 11:18:08 2026 -0700 test displace fuel + run column backwards in intake (#104) * update displace fuel timing and position * add intake command commit 8d48f710f6ac7f0a39a3bbbc6db6f796b15514e2 Author: zoe z <121075323+amzoeee@users.noreply.github.com> Date: Tue Mar 10 11:17:48 2026 -0700 organize shoot commands (+ fix shootToHub) (#98) * fix whitespace + comments * use parallel group of sequences in shoottohub * add command names * fix shootWithoutDistance javadoc * remove snapToHub timeout? commit 7bc05cf79b889a8c3e7dc64d6231a42f1dd555f8 Author: zoe z <121075323+amzoeee@users.noreply.github.com> Date: Tue Mar 10 09:22:22 2026 -0700 add snapForBump command (#97) * add snapForBump * remove unused import * use snap to angle for snap for bump * fix snap to angle logging statement * add names to drivetrain snap to X * fix comments commit d8c257cc44f5698e4e8c7c07754889268c76b011 Author: aishahnazar Date: Mon Mar 9 19:26:14 2026 -0700 add front camera (#99) * add front camera * update constants for front camera * remove comments --------- Co-authored-by: zoe --- src/main/java/frc/robot/Constants.java | 40 +++++++++-- .../robot/subsystems/RobotCommandFactory.java | 71 ++++++++++++------- .../drivetrain/DrivetrainCommandFactory.java | 28 +++++++- .../subsystems/vision/VisionSubsystem.java | 1 + 4 files changed, 106 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 416cdb7c..2bbd3d38 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -182,13 +182,13 @@ public static class PivotConstants { // Pivot positions public static double STORE_POSITION_DEGREES = 10; - public static double DISPLACE_FUEL_POSITION_DEGREES = 35; + public static double DISPLACE_FUEL_POSITION_DEGREES = 25; public static double DEPLOY_POSITION_DEGREES = 55; // Timing values for displace fuel command public static final double DISPLACE_FUEL_UP_TIMEOUT = 1; // TODO: tune public static final double DISPLACE_FUEL_DOWN_TIMEOUT = 1; - public static final double DISPLACE_FUEL_DELAY = 1.5; // time between repeats of displace fuel command + public static final double DISPLACE_FUEL_DELAY = 0; // time between repeats of displace fuel command // Max manual volts @@ -444,6 +444,7 @@ public static class VisionConstants { // DEFAULT CONSTANTS (robot specific constants are below) public static final String CAMERA1_NAME; public static final String CAMERA2_NAME; + public static final String CAMERA3_NAME; // Camera 1 position - robot-specific because camera mounting may differ public static final double ROBOT_TO_CAM1_X; @@ -466,6 +467,15 @@ public static class VisionConstants { public static final double ROBOT_TO_CAM2_YAW; public static final Transform3d ROBOT_TO_CAM2_3D; + // Camera 3 position - robot-specific + public static final double ROBOT_TO_CAM3_X; + public static final double ROBOT_TO_CAM3_Y; + public static final double ROBOT_TO_CAM3_Z; + public static final double ROBOT_TO_CAM3_ROLL; + public static final double ROBOT_TO_CAM3_PITCH; + public static final double ROBOT_TO_CAM3_YAW; + public static final Transform3d ROBOT_TO_CAM3_3D; + static { switch (RobotConfig.getRobot()) { case CLEO: @@ -473,6 +483,7 @@ public static class VisionConstants { CAMERA1_NAME = "right_camera"; CAMERA2_NAME = "left_camera"; + CAMERA3_NAME = "front_camera"; // Cleo camera positions ROBOT_TO_CAM1_X = Units.inchesToMeters(-21.0/2+2.0); @@ -480,14 +491,22 @@ public static class VisionConstants { ROBOT_TO_CAM1_Z = Units.inchesToMeters(8.3); ROBOT_TO_CAM1_ROLL = 0; ROBOT_TO_CAM1_PITCH = Units.degreesToRadians(-20); - ROBOT_TO_CAM1_YAW = Units.degreesToRadians(135)-0.12; // estimated + ROBOT_TO_CAM1_YAW = Units.degreesToRadians(135)-0.12; ROBOT_TO_CAM2_X = Units.inchesToMeters(-21.0/2+2.25); ROBOT_TO_CAM2_Y = Units.inchesToMeters(33.0/2-6.5); ROBOT_TO_CAM2_Z = Units.inchesToMeters(8.5); ROBOT_TO_CAM2_ROLL = 0; ROBOT_TO_CAM2_PITCH = Units.degreesToRadians(-20); - ROBOT_TO_CAM2_YAW = Units.degreesToRadians(-135)+0.14; // estimated + ROBOT_TO_CAM2_YAW = Units.degreesToRadians(-135)+0.14; + + ROBOT_TO_CAM3_X = Units.inchesToMeters(21/2-0.75); + ROBOT_TO_CAM3_Y = Units.inchesToMeters(-33.0/2+2); + ROBOT_TO_CAM3_Z = Units.inchesToMeters(29); + ROBOT_TO_CAM3_ROLL = 0; + ROBOT_TO_CAM3_PITCH = Units.degreesToRadians(-30); + ROBOT_TO_CAM3_YAW = Units.degreesToRadians(0); + break; case CERIDWEN: @@ -495,6 +514,7 @@ public static class VisionConstants { CAMERA1_NAME = "camera1"; CAMERA2_NAME = "camera2"; + CAMERA3_NAME = "camera3"; ROBOT_TO_CAM1_X = Units.inchesToMeters(2); ROBOT_TO_CAM1_Y = Units.inchesToMeters(-7); @@ -509,6 +529,13 @@ public static class VisionConstants { ROBOT_TO_CAM2_ROLL = 0; ROBOT_TO_CAM2_PITCH = 0; ROBOT_TO_CAM2_YAW = 0; + + ROBOT_TO_CAM3_X = 0; + ROBOT_TO_CAM3_Y = 0; + ROBOT_TO_CAM3_Z = 0; + ROBOT_TO_CAM3_ROLL = 0; + ROBOT_TO_CAM3_PITCH = 0; + ROBOT_TO_CAM3_YAW = 0; break; default: @@ -525,6 +552,11 @@ public static class VisionConstants { new Translation3d(ROBOT_TO_CAM2_X, ROBOT_TO_CAM2_Y, ROBOT_TO_CAM2_Z), new Rotation3d(ROBOT_TO_CAM2_ROLL, ROBOT_TO_CAM2_PITCH, ROBOT_TO_CAM2_YAW) ); + + ROBOT_TO_CAM3_3D = new Transform3d( + new Translation3d(ROBOT_TO_CAM3_X, ROBOT_TO_CAM3_Y, ROBOT_TO_CAM3_Z), + new Rotation3d(ROBOT_TO_CAM3_ROLL, ROBOT_TO_CAM3_PITCH, ROBOT_TO_CAM3_YAW) + ); } } diff --git a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java index 6f4ee9b1..cc1d2a71 100644 --- a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java @@ -132,8 +132,17 @@ public Command runColumnBackCommand() { return m_column.velocityCommand(() -> -m_columnVelocitySupplier.get()).withName("runColumnBack"); } + public Command intake() { + return Commands.parallel( + m_roller.velocityCommand(m_rollerVelocitySupplier), + m_indexer.velocityCommand(m_indexerVelocitySupplier), + m_column.velocityCommand(() -> -m_columnVelocitySupplier.get()) + ).withName("intake"); + } + // combos public Command runIntakeRollersCommand() { + // return m_roller.velocityCommand(m_rollerVelocitySupplier); return Commands.parallel( m_roller.velocityCommand(m_rollerVelocitySupplier), m_indexer.velocityCommand(m_indexerVelocitySupplier) @@ -215,20 +224,28 @@ public Command shootByDistanceTestCommand() { * @return Command that shoots with given velocity suppliers */ public Command shootToHubCommand(Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier) { - return Commands.sequence( - new WaitCommand(3) // timeout and just shoot after 3 seconds - .until(m_drivetrainCommandFactory.atAngleTrigger(() -> HubCalculations.angleToHub(m_drivetrain.getState().Pose))), - Commands.parallel( - m_shooter.shooterVelocityCommand(shooterSupplier), // run shooter at given velocity - Commands.sequence( // column: - m_column.offCommand() // wait until - .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity - m_column.velocityCommand(columnSupplier)), - Commands.sequence( // indexer: - m_indexer.offCommand() // wait until - .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity - m_indexer.velocityCommand(indexerSupplier))) - ); + + return Commands.parallel( + // shooter + Commands.sequence( + new WaitCommand(10000) // wait until (no timeout) + .until(m_drivetrainCommandFactory.atAngleTrigger(() -> HubCalculations.angleToHub(m_drivetrain.getState().Pose))), // facing hub + m_shooter.shooterVelocityCommand(shooterSupplier)), // run shooter at given velocity + + // column + Commands.sequence( + m_column.offCommand() // wait until + .until(m_shooter.atTargetVelocityTrigger(shooterSupplier) // shooter at target velocity + .and(m_drivetrainCommandFactory.atAngleTrigger(() -> HubCalculations.angleToHub(m_drivetrain.getState().Pose)))), // and facing hub + m_column.velocityCommand(columnSupplier)), + + // indexer + Commands.sequence( // indexer: + m_indexer.offCommand() // wait until + .until(m_shooter.atTargetVelocityTrigger(shooterSupplier) // shooter at target velocity + .and(m_drivetrainCommandFactory.atAngleTrigger(() -> HubCalculations.angleToHub(m_drivetrain.getState().Pose)))), // and facing hub + m_indexer.velocityCommand(indexerSupplier)) + ).withName("shootToHub"); } public Command shootToHubCommandWithDisplacement(Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier) { @@ -239,7 +256,7 @@ public Command shootToHubCommandWithDisplacement(Supplier shooterSupplie .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity new WaitCommand(2), // wait 2 seconds for some of the fuel to be shot out m_pivot.repeatingDisplaceFuelCommand()) - ); + ).withName("shootToHubWithDisplacement"); } public Command snapToHubCommand(Supplier joystickValsSupplier) { @@ -306,7 +323,8 @@ public Command shootCommand(Supplier shooterSupplier, Supplier c Commands.sequence( // indexer: m_indexer.offCommand() // wait until .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity - m_indexer.velocityCommand(indexerSupplier))); + m_indexer.velocityCommand(indexerSupplier)) + ).withName("shootCommand"); } /** @@ -327,17 +345,13 @@ public Command shootCommandWithDisplacement(Supplier shooterSupplier, Su .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity new WaitCommand(2), // wait 2 seconds for some of the fuel to be shot out m_pivot.repeatingDisplaceFuelCommand()) - ); + ).withName("shootCommandWithDisplacement"); } /** - * Command that shoots with shooter, column, indexer velocity supplier - * Simultaneously runs the shooter, then runs column and indexer **once the drivetrain is at the correct angle** - * - * @param shooterSupplier Supplier for shooter velocity - * @param columnSupplier Supplier for column velocity - * @param indexerSupplier Supplier for indexer velocity - * @return Command that shoots with given velocity suppliers + * Command that shoots based on set[mechanism]Velocity() + * Simultaneously runs the shooter, then runs column, indexer and roller + * @return Command that shoots with set[mechanism]Velocity() */ public Command shootWithoutDistance() { return Commands.parallel( @@ -347,7 +361,6 @@ public Command shootWithoutDistance() { m_column.offCommand() // wait until .until(m_shooter.atTargetVelocityTrigger(m_shooterVelocitySupplier)), // shooter at target velocity m_column.velocityCommand(m_columnVelocitySupplier)), - Commands.sequence( // roller: m_roller.offCommand() // wait until .until(m_shooter.atTargetVelocityTrigger(m_shooterVelocitySupplier)), // shooter at target velocity @@ -355,8 +368,12 @@ public Command shootWithoutDistance() { Commands.sequence( // indexer: m_indexer.offCommand() // wait until .until(m_shooter.atTargetVelocityTrigger(m_shooterVelocitySupplier)), // shooter at target velocity - m_indexer.velocityCommand(m_indexerVelocitySupplier))); - + m_indexer.velocityCommand(m_indexerVelocitySupplier)) + ).withName("shootWithoutDistance"); + } + + public Command shootWithoutDistanceWithDisplacement() { + return Commands.parallel(shootWithoutDistance(), m_pivot.repeatingDisplaceFuelCommand()); } // HELPER FUNCTIONS diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java index efffaf2f..43cf103f 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java @@ -18,7 +18,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.Constants.DrivetrainConstants; +import frc.robot.FieldConstants; import frc.robot.RobotContainer.JoystickVals; +import frc.robot.utils.AllianceFlipUtil; public class DrivetrainCommandFactory { /* Setting up bindings for necessary control of the swerve drive platform */ @@ -84,8 +86,8 @@ public Command snapToAngle(Supplier translationSupplier, Rotation2 // Drives the robot while automatically rotating to face a specified rotation2d public Command snapToAngle(Supplier translationSupplier, Supplier angleSupplier) { return m_drivetrain.getCommandFromRequest(() -> { - SmartDashboard.putNumber("drivetrain/snap to angle", angleSupplier.get().getDegrees()); targetAngle = angleSupplier.get(); + SmartDashboard.putNumber("drivetrain/snapToAngle/targetAngle", targetAngle.getDegrees()); JoystickVals translation = translationSupplier.get(); boolean slowmode = slowmodeSupplier.getAsBoolean(); @@ -94,7 +96,7 @@ public Command snapToAngle(Supplier translationSupplier, Supplier< return m_driveFacingAngle.withVelocityX(-shapedValues.y() * DrivetrainConstants.MAX_TRANSLATION_SPEED) // Drive forward with negative Y (forward) .withVelocityY(-shapedValues.x() * DrivetrainConstants.MAX_TRANSLATION_SPEED) // Drive left with negative X (left) .withTargetDirection(targetAngle); - }); + }).withName("snapToAngle"); } /** @@ -136,7 +138,27 @@ public Command snapToTarget(Supplier translationSupplier, Supplier return m_driveFacingAngle.withVelocityX(-shapedValues.y() * DrivetrainConstants.MAX_TRANSLATION_SPEED) // Drive forward with negative Y (forward) .withVelocityY(-shapedValues.x() * DrivetrainConstants.MAX_TRANSLATION_SPEED) // Drive left with negative X (left) .withTargetDirection(angleToTarget); - }); + }).withName("snapToTarget"); + } + + public Command snapForBump(Supplier translationSupplier) { + return snapToAngle(translationSupplier, () -> getBumpAngle(m_drivetrain.getState().Pose)).withName("snapForBump"); + } + + /** + * Determines the field-relative heading the robot should use to face the bump. + * @param robotPose The robot's current field pose + * @return The field-relative heading the robot should face + */ + private Rotation2d getBumpAngle(Pose2d robotPose) { + Pose2d blueAlliancePose = AllianceFlipUtil.apply(robotPose); // Normalize to the blue-alliance perspective to check with blue hub + Rotation2d blueAllianceHeading; + if (blueAlliancePose.getX() < FieldConstants.LinesVertical.hubCenter) { // If in alliance zone + blueAllianceHeading = Rotation2d.fromDegrees(0); // Face away from the driver station. + } else { + blueAllianceHeading = Rotation2d.fromDegrees(180); // Face toward the driver station. + } + return AllianceFlipUtil.apply(blueAllianceHeading); // Convert heading back for the current alliance } public void setHeadingController(){ diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 1899b68f..2e92d35b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -72,6 +72,7 @@ public class VisionSubsystem extends SubsystemBase { public VisionSubsystem() { cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D)); cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D)); + cameras.add(new LocalizationCamera(VisionConstants.CAMERA3_NAME, VisionConstants.ROBOT_TO_CAM3_3D)); } public List getValidCameraReadings(){ From 4a5c79e7045f3ab590563b471c5d4761a3921987 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Tue, 10 Mar 2026 11:33:39 -0700 Subject: [PATCH 05/12] Revert "Squashed commit of the following:" This reverts commit 5dc67b739af77fb47993de4d238586350b1f60d8. --- src/main/java/frc/robot/Constants.java | 40 ++--------- .../robot/subsystems/RobotCommandFactory.java | 71 +++++++------------ .../drivetrain/DrivetrainCommandFactory.java | 28 +------- .../subsystems/vision/VisionSubsystem.java | 1 - 4 files changed, 34 insertions(+), 106 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2bbd3d38..416cdb7c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -182,13 +182,13 @@ public static class PivotConstants { // Pivot positions public static double STORE_POSITION_DEGREES = 10; - public static double DISPLACE_FUEL_POSITION_DEGREES = 25; + public static double DISPLACE_FUEL_POSITION_DEGREES = 35; public static double DEPLOY_POSITION_DEGREES = 55; // Timing values for displace fuel command public static final double DISPLACE_FUEL_UP_TIMEOUT = 1; // TODO: tune public static final double DISPLACE_FUEL_DOWN_TIMEOUT = 1; - public static final double DISPLACE_FUEL_DELAY = 0; // time between repeats of displace fuel command + public static final double DISPLACE_FUEL_DELAY = 1.5; // time between repeats of displace fuel command // Max manual volts @@ -444,7 +444,6 @@ public static class VisionConstants { // DEFAULT CONSTANTS (robot specific constants are below) public static final String CAMERA1_NAME; public static final String CAMERA2_NAME; - public static final String CAMERA3_NAME; // Camera 1 position - robot-specific because camera mounting may differ public static final double ROBOT_TO_CAM1_X; @@ -467,15 +466,6 @@ public static class VisionConstants { public static final double ROBOT_TO_CAM2_YAW; public static final Transform3d ROBOT_TO_CAM2_3D; - // Camera 3 position - robot-specific - public static final double ROBOT_TO_CAM3_X; - public static final double ROBOT_TO_CAM3_Y; - public static final double ROBOT_TO_CAM3_Z; - public static final double ROBOT_TO_CAM3_ROLL; - public static final double ROBOT_TO_CAM3_PITCH; - public static final double ROBOT_TO_CAM3_YAW; - public static final Transform3d ROBOT_TO_CAM3_3D; - static { switch (RobotConfig.getRobot()) { case CLEO: @@ -483,7 +473,6 @@ public static class VisionConstants { CAMERA1_NAME = "right_camera"; CAMERA2_NAME = "left_camera"; - CAMERA3_NAME = "front_camera"; // Cleo camera positions ROBOT_TO_CAM1_X = Units.inchesToMeters(-21.0/2+2.0); @@ -491,22 +480,14 @@ public static class VisionConstants { ROBOT_TO_CAM1_Z = Units.inchesToMeters(8.3); ROBOT_TO_CAM1_ROLL = 0; ROBOT_TO_CAM1_PITCH = Units.degreesToRadians(-20); - ROBOT_TO_CAM1_YAW = Units.degreesToRadians(135)-0.12; + ROBOT_TO_CAM1_YAW = Units.degreesToRadians(135)-0.12; // estimated ROBOT_TO_CAM2_X = Units.inchesToMeters(-21.0/2+2.25); ROBOT_TO_CAM2_Y = Units.inchesToMeters(33.0/2-6.5); ROBOT_TO_CAM2_Z = Units.inchesToMeters(8.5); ROBOT_TO_CAM2_ROLL = 0; ROBOT_TO_CAM2_PITCH = Units.degreesToRadians(-20); - ROBOT_TO_CAM2_YAW = Units.degreesToRadians(-135)+0.14; - - ROBOT_TO_CAM3_X = Units.inchesToMeters(21/2-0.75); - ROBOT_TO_CAM3_Y = Units.inchesToMeters(-33.0/2+2); - ROBOT_TO_CAM3_Z = Units.inchesToMeters(29); - ROBOT_TO_CAM3_ROLL = 0; - ROBOT_TO_CAM3_PITCH = Units.degreesToRadians(-30); - ROBOT_TO_CAM3_YAW = Units.degreesToRadians(0); - + ROBOT_TO_CAM2_YAW = Units.degreesToRadians(-135)+0.14; // estimated break; case CERIDWEN: @@ -514,7 +495,6 @@ public static class VisionConstants { CAMERA1_NAME = "camera1"; CAMERA2_NAME = "camera2"; - CAMERA3_NAME = "camera3"; ROBOT_TO_CAM1_X = Units.inchesToMeters(2); ROBOT_TO_CAM1_Y = Units.inchesToMeters(-7); @@ -529,13 +509,6 @@ public static class VisionConstants { ROBOT_TO_CAM2_ROLL = 0; ROBOT_TO_CAM2_PITCH = 0; ROBOT_TO_CAM2_YAW = 0; - - ROBOT_TO_CAM3_X = 0; - ROBOT_TO_CAM3_Y = 0; - ROBOT_TO_CAM3_Z = 0; - ROBOT_TO_CAM3_ROLL = 0; - ROBOT_TO_CAM3_PITCH = 0; - ROBOT_TO_CAM3_YAW = 0; break; default: @@ -552,11 +525,6 @@ public static class VisionConstants { new Translation3d(ROBOT_TO_CAM2_X, ROBOT_TO_CAM2_Y, ROBOT_TO_CAM2_Z), new Rotation3d(ROBOT_TO_CAM2_ROLL, ROBOT_TO_CAM2_PITCH, ROBOT_TO_CAM2_YAW) ); - - ROBOT_TO_CAM3_3D = new Transform3d( - new Translation3d(ROBOT_TO_CAM3_X, ROBOT_TO_CAM3_Y, ROBOT_TO_CAM3_Z), - new Rotation3d(ROBOT_TO_CAM3_ROLL, ROBOT_TO_CAM3_PITCH, ROBOT_TO_CAM3_YAW) - ); } } diff --git a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java index cc1d2a71..6f4ee9b1 100644 --- a/src/main/java/frc/robot/subsystems/RobotCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/RobotCommandFactory.java @@ -132,17 +132,8 @@ public Command runColumnBackCommand() { return m_column.velocityCommand(() -> -m_columnVelocitySupplier.get()).withName("runColumnBack"); } - public Command intake() { - return Commands.parallel( - m_roller.velocityCommand(m_rollerVelocitySupplier), - m_indexer.velocityCommand(m_indexerVelocitySupplier), - m_column.velocityCommand(() -> -m_columnVelocitySupplier.get()) - ).withName("intake"); - } - // combos public Command runIntakeRollersCommand() { - // return m_roller.velocityCommand(m_rollerVelocitySupplier); return Commands.parallel( m_roller.velocityCommand(m_rollerVelocitySupplier), m_indexer.velocityCommand(m_indexerVelocitySupplier) @@ -224,28 +215,20 @@ public Command shootByDistanceTestCommand() { * @return Command that shoots with given velocity suppliers */ public Command shootToHubCommand(Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier) { - - return Commands.parallel( - // shooter - Commands.sequence( - new WaitCommand(10000) // wait until (no timeout) - .until(m_drivetrainCommandFactory.atAngleTrigger(() -> HubCalculations.angleToHub(m_drivetrain.getState().Pose))), // facing hub - m_shooter.shooterVelocityCommand(shooterSupplier)), // run shooter at given velocity - - // column - Commands.sequence( - m_column.offCommand() // wait until - .until(m_shooter.atTargetVelocityTrigger(shooterSupplier) // shooter at target velocity - .and(m_drivetrainCommandFactory.atAngleTrigger(() -> HubCalculations.angleToHub(m_drivetrain.getState().Pose)))), // and facing hub - m_column.velocityCommand(columnSupplier)), - - // indexer - Commands.sequence( // indexer: - m_indexer.offCommand() // wait until - .until(m_shooter.atTargetVelocityTrigger(shooterSupplier) // shooter at target velocity - .and(m_drivetrainCommandFactory.atAngleTrigger(() -> HubCalculations.angleToHub(m_drivetrain.getState().Pose)))), // and facing hub - m_indexer.velocityCommand(indexerSupplier)) - ).withName("shootToHub"); + return Commands.sequence( + new WaitCommand(3) // timeout and just shoot after 3 seconds + .until(m_drivetrainCommandFactory.atAngleTrigger(() -> HubCalculations.angleToHub(m_drivetrain.getState().Pose))), + Commands.parallel( + m_shooter.shooterVelocityCommand(shooterSupplier), // run shooter at given velocity + Commands.sequence( // column: + m_column.offCommand() // wait until + .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity + m_column.velocityCommand(columnSupplier)), + Commands.sequence( // indexer: + m_indexer.offCommand() // wait until + .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity + m_indexer.velocityCommand(indexerSupplier))) + ); } public Command shootToHubCommandWithDisplacement(Supplier shooterSupplier, Supplier columnSupplier, Supplier indexerSupplier) { @@ -256,7 +239,7 @@ public Command shootToHubCommandWithDisplacement(Supplier shooterSupplie .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity new WaitCommand(2), // wait 2 seconds for some of the fuel to be shot out m_pivot.repeatingDisplaceFuelCommand()) - ).withName("shootToHubWithDisplacement"); + ); } public Command snapToHubCommand(Supplier joystickValsSupplier) { @@ -323,8 +306,7 @@ public Command shootCommand(Supplier shooterSupplier, Supplier c Commands.sequence( // indexer: m_indexer.offCommand() // wait until .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity - m_indexer.velocityCommand(indexerSupplier)) - ).withName("shootCommand"); + m_indexer.velocityCommand(indexerSupplier))); } /** @@ -345,13 +327,17 @@ public Command shootCommandWithDisplacement(Supplier shooterSupplier, Su .until(m_shooter.atTargetVelocityTrigger(shooterSupplier)), // shooter at target velocity new WaitCommand(2), // wait 2 seconds for some of the fuel to be shot out m_pivot.repeatingDisplaceFuelCommand()) - ).withName("shootCommandWithDisplacement"); + ); } /** - * Command that shoots based on set[mechanism]Velocity() - * Simultaneously runs the shooter, then runs column, indexer and roller - * @return Command that shoots with set[mechanism]Velocity() + * Command that shoots with shooter, column, indexer velocity supplier + * Simultaneously runs the shooter, then runs column and indexer **once the drivetrain is at the correct angle** + * + * @param shooterSupplier Supplier for shooter velocity + * @param columnSupplier Supplier for column velocity + * @param indexerSupplier Supplier for indexer velocity + * @return Command that shoots with given velocity suppliers */ public Command shootWithoutDistance() { return Commands.parallel( @@ -361,6 +347,7 @@ public Command shootWithoutDistance() { m_column.offCommand() // wait until .until(m_shooter.atTargetVelocityTrigger(m_shooterVelocitySupplier)), // shooter at target velocity m_column.velocityCommand(m_columnVelocitySupplier)), + Commands.sequence( // roller: m_roller.offCommand() // wait until .until(m_shooter.atTargetVelocityTrigger(m_shooterVelocitySupplier)), // shooter at target velocity @@ -368,12 +355,8 @@ public Command shootWithoutDistance() { Commands.sequence( // indexer: m_indexer.offCommand() // wait until .until(m_shooter.atTargetVelocityTrigger(m_shooterVelocitySupplier)), // shooter at target velocity - m_indexer.velocityCommand(m_indexerVelocitySupplier)) - ).withName("shootWithoutDistance"); - } - - public Command shootWithoutDistanceWithDisplacement() { - return Commands.parallel(shootWithoutDistance(), m_pivot.repeatingDisplaceFuelCommand()); + m_indexer.velocityCommand(m_indexerVelocitySupplier))); + } // HELPER FUNCTIONS diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java index 43cf103f..efffaf2f 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java @@ -18,9 +18,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.Constants.DrivetrainConstants; -import frc.robot.FieldConstants; import frc.robot.RobotContainer.JoystickVals; -import frc.robot.utils.AllianceFlipUtil; public class DrivetrainCommandFactory { /* Setting up bindings for necessary control of the swerve drive platform */ @@ -86,8 +84,8 @@ public Command snapToAngle(Supplier translationSupplier, Rotation2 // Drives the robot while automatically rotating to face a specified rotation2d public Command snapToAngle(Supplier translationSupplier, Supplier angleSupplier) { return m_drivetrain.getCommandFromRequest(() -> { + SmartDashboard.putNumber("drivetrain/snap to angle", angleSupplier.get().getDegrees()); targetAngle = angleSupplier.get(); - SmartDashboard.putNumber("drivetrain/snapToAngle/targetAngle", targetAngle.getDegrees()); JoystickVals translation = translationSupplier.get(); boolean slowmode = slowmodeSupplier.getAsBoolean(); @@ -96,7 +94,7 @@ public Command snapToAngle(Supplier translationSupplier, Supplier< return m_driveFacingAngle.withVelocityX(-shapedValues.y() * DrivetrainConstants.MAX_TRANSLATION_SPEED) // Drive forward with negative Y (forward) .withVelocityY(-shapedValues.x() * DrivetrainConstants.MAX_TRANSLATION_SPEED) // Drive left with negative X (left) .withTargetDirection(targetAngle); - }).withName("snapToAngle"); + }); } /** @@ -138,27 +136,7 @@ public Command snapToTarget(Supplier translationSupplier, Supplier return m_driveFacingAngle.withVelocityX(-shapedValues.y() * DrivetrainConstants.MAX_TRANSLATION_SPEED) // Drive forward with negative Y (forward) .withVelocityY(-shapedValues.x() * DrivetrainConstants.MAX_TRANSLATION_SPEED) // Drive left with negative X (left) .withTargetDirection(angleToTarget); - }).withName("snapToTarget"); - } - - public Command snapForBump(Supplier translationSupplier) { - return snapToAngle(translationSupplier, () -> getBumpAngle(m_drivetrain.getState().Pose)).withName("snapForBump"); - } - - /** - * Determines the field-relative heading the robot should use to face the bump. - * @param robotPose The robot's current field pose - * @return The field-relative heading the robot should face - */ - private Rotation2d getBumpAngle(Pose2d robotPose) { - Pose2d blueAlliancePose = AllianceFlipUtil.apply(robotPose); // Normalize to the blue-alliance perspective to check with blue hub - Rotation2d blueAllianceHeading; - if (blueAlliancePose.getX() < FieldConstants.LinesVertical.hubCenter) { // If in alliance zone - blueAllianceHeading = Rotation2d.fromDegrees(0); // Face away from the driver station. - } else { - blueAllianceHeading = Rotation2d.fromDegrees(180); // Face toward the driver station. - } - return AllianceFlipUtil.apply(blueAllianceHeading); // Convert heading back for the current alliance + }); } public void setHeadingController(){ diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 2e92d35b..1899b68f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -72,7 +72,6 @@ public class VisionSubsystem extends SubsystemBase { public VisionSubsystem() { cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D)); cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D)); - cameras.add(new LocalizationCamera(VisionConstants.CAMERA3_NAME, VisionConstants.ROBOT_TO_CAM3_3D)); } public List getValidCameraReadings(){ From 77a17094262a7405da8d603dddb68f28d86ce7f8 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Tue, 10 Mar 2026 11:34:06 -0700 Subject: [PATCH 06/12] initialize constant --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 416cdb7c..f5d22857 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -455,7 +455,7 @@ public static class VisionConstants { public static final Transform3d ROBOT_TO_CAM1_3D; //is camera connected check: - public static final int NUM_CAMERAS_DISCONNECTED; + public static final int MIN_NUM_CAMERAS_DISCONNECTED = 2; // Camera 2 position - robot-specific public static final double ROBOT_TO_CAM2_X; From efa2b86100849ec94b436c46114168d1d50f767b Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Wed, 11 Mar 2026 15:26:38 -0700 Subject: [PATCH 07/12] replace isVisionPoseStable() with isEstPoseJumpy() --- .../subsystems/vision/VisionSubsystem.java | 42 +++++++------------ 1 file changed, 14 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 1899b68f..61aaac0a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -64,9 +64,7 @@ public class VisionSubsystem extends SubsystemBase { private boolean m_rawVideoModeEnabled = false; // true = raw video feed, false = normal AprilTag processing - private Pose2d m_lastVisionPose = null; - - private double m_lastVisionPoseTimestamp = 0.0; + private ArrayList m_lastEstPoses = new ArrayList<>(); /** Creates a new Vision Subsystem. */ public VisionSubsystem() { @@ -132,38 +130,26 @@ public boolean hasValidTargets() { return !allValidReadings.isEmpty(); } - public boolean isVisionPoseStable() { - List readings = getValidCameraReadings(); - - //if there are no valid readings, then theres nothing to compare to - if (readings.isEmpty()) { + public boolean isEstPoseJumpy() { + if (m_lastEstPoses.size() < VisionConstants.NUM_LAST_EST_POSES) { return true; } - - Pose2d currentPose = readings.get(0).robotPose().estimatedPose.toPose2d(); - double timestamp = readings.get(0).robotPose().timestampSeconds; - - //if we have never stored a previous vision pose before, then assume the est pose is stable - if (m_lastVisionPose == null) { - m_lastVisionPose = currentPose; - m_lastVisionPoseTimestamp = timestamp; - return true; + + double totalDistance = 0; + + for (int i = 0; i < m_lastEstPoses.size() - 1; i++) { + // add distance between ith pose and i+1th pose + totalDistance += Math.abs(m_lastEstPoses.get(i).minus(m_lastEstPoses.get(i + 1)).getTranslation().getNorm()); } - //calculate how far + how much time has passed since the robot appears to have moved between frames. - double distanceJump = currentPose.getTranslation().getDistance(m_lastVisionPose.getTranslation()); - double dt = timestamp - m_lastVisionPoseTimestamp; - - //update stored pose so that next time the new current pose will be compared against this frame - m_lastVisionPose = currentPose; - m_lastVisionPoseTimestamp = timestamp; - - // if robot seems like it's jumping > 1 meter in < 0.1s → probably est pose is flickering too much - return !(distanceJump > 1.0 && dt < 0.1); + double avgDist = totalDistance / m_lastEstPoses.size(); + SmartDashboard.putNumber("vision/avgDistBetweenLastEstPoses", avgDist); + + return avgDist > VisionConstants.MAX_AVG_DIST_BETWEEN_LAST_EST_POSES; } public boolean isVisionHealthy() { - return areCamerasConnected() && isVisionUpdating() && hasValidTargets() && isVisionPoseStable(); + return areCamerasConnected() && isVisionUpdating() && hasValidTargets() && isEstPoseJumpy(); } @Override From 9acbcdd2a154a7d04caf560c151f160ea163c647 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Thu, 12 Mar 2026 11:37:59 -0700 Subject: [PATCH 08/12] update logging name --- src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 61aaac0a..9a3a214f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -143,7 +143,7 @@ public boolean isEstPoseJumpy() { } double avgDist = totalDistance / m_lastEstPoses.size(); - SmartDashboard.putNumber("vision/avgDistBetweenLastEstPoses", avgDist); + SmartDashboard.putNumber("vision/" + getName() + "/avgDistBetweenLastEstPoses", avgDist); return avgDist > VisionConstants.MAX_AVG_DIST_BETWEEN_LAST_EST_POSES; } From ef66a55bbdc094efde642a1a047d37d50d81177e Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Thu, 12 Mar 2026 12:30:49 -0700 Subject: [PATCH 09/12] remove magic numbers --- src/main/java/frc/robot/Constants.java | 8 +++++--- .../java/frc/robot/subsystems/vision/VisionSubsystem.java | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f5d22857..bef62555 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -454,9 +454,6 @@ public static class VisionConstants { public static final double ROBOT_TO_CAM1_YAW; public static final Transform3d ROBOT_TO_CAM1_3D; - //is camera connected check: - public static final int MIN_NUM_CAMERAS_DISCONNECTED = 2; - // Camera 2 position - robot-specific public static final double ROBOT_TO_CAM2_X; public static final double ROBOT_TO_CAM2_Y; @@ -466,6 +463,11 @@ public static class VisionConstants { public static final double ROBOT_TO_CAM2_YAW; public static final Transform3d ROBOT_TO_CAM2_3D; + //is camera connected check: + public static final int MIN_NUM_CAMERAS_DISCONNECTED = 2; + + //is vision updating check: + public static final double MAX_TIME_BETWEEN_UPDATES = 0.5; static { switch (RobotConfig.getRobot()) { case CLEO: diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 9a3a214f..356fb697 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -123,7 +123,7 @@ public boolean areCamerasConnected() { public boolean isVisionUpdating() { double currentTime = Timer.getFPGATimestamp(); - return (currentTime - m_lastTimestamp) < 0.5; + return (currentTime - m_lastTimestamp) < VisionConstants.MAX_TIME_BETWEEN_UPDATES; } public boolean hasValidTargets() { From 00c9030ab5605fc903b022e0536dc53b5e794850 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Thu, 12 Mar 2026 12:32:45 -0700 Subject: [PATCH 10/12] remove hasValidTargets() --- .../java/frc/robot/subsystems/vision/VisionSubsystem.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 356fb697..6257a74d 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -126,10 +126,6 @@ public boolean isVisionUpdating() { return (currentTime - m_lastTimestamp) < VisionConstants.MAX_TIME_BETWEEN_UPDATES; } - public boolean hasValidTargets() { - return !allValidReadings.isEmpty(); - } - public boolean isEstPoseJumpy() { if (m_lastEstPoses.size() < VisionConstants.NUM_LAST_EST_POSES) { return true; @@ -149,7 +145,7 @@ public boolean isEstPoseJumpy() { } public boolean isVisionHealthy() { - return areCamerasConnected() && isVisionUpdating() && hasValidTargets() && isEstPoseJumpy(); + return areCamerasConnected() && isVisionUpdating() && isEstPoseJumpy(); } @Override From b364f8a44387cd91e5e475c6ad8bfc795a3ab498 Mon Sep 17 00:00:00 2001 From: aishahnazar Date: Thu, 12 Mar 2026 23:28:47 -0700 Subject: [PATCH 11/12] replace runVisionStatus command with an event trigger --- src/main/java/frc/robot/RobotContainer.java | 5 ++++- src/main/java/frc/robot/subsystems/LEDSubsystem.java | 11 ----------- 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index faf8e70d..088e4cc7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -148,7 +148,10 @@ public RobotContainer() { setRobotMode(RobotMode.NEUTRAL); // Vision LED indicator - m_led.setDefaultCommand(m_led.runVisionStatus(() -> m_vision.isVisionHealthy())); + m_led.setDefaultCommand(m_led.runScrollingYellowBlueCommand()); + Trigger isVisionHealthy = new Trigger(() -> m_vision.isVisionHealthy()); + isVisionHealthy.whileTrue(m_led.runScrollingYellowBlueCommand()); + isVisionHealthy.whileFalse(m_led.runSolidRed()); // Configure auto builder createNamedCommands(); diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 0640e0e8..939a657c 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -113,15 +113,4 @@ public Command runScrollingYellowBlueCommand() { return runPattern(pattern).withName("LED/scrolling yellow-blue"); } - public Command runVisionStatus(java.util.function.BooleanSupplier visionHealthySupplier) { - LEDPattern base = LEDPattern.gradient(LEDPattern.GradientType.kDiscontinuous, Color.kYellow, Color.kBlue); - LEDPattern pattern = base.scrollAtRelativeSpeed(Percent.per(Second).of(100)); - - return run(() -> { - if (visionHealthySupplier.getAsBoolean()) - pattern.applyTo(m_ledBuffer); - else - LEDPattern.solid(Color.kRed).applyTo(m_ledBuffer); - }).withName("LED/vision status"); - } } From 2eede35f947be4f95f95adf2d68afa7e748e12bc Mon Sep 17 00:00:00 2001 From: zoe Date: Thu, 26 Mar 2026 17:28:57 -0700 Subject: [PATCH 12/12] update constants for leds --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 69453a8b..7b636186 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -662,8 +662,8 @@ public static class VisionConstants { } public static class LEDConstants { // placeholder constants - public static final int KPORT = 0; - public static final int KLENGTH = 60; + public static final int KPORT = 9; + public static final int KLENGTH = 3*30; public static final double BLINK_TIME = 1; // in seconds for after intake/outtake }