From 5d205ea7b30ffd9dfe33f0bb8e85db2d74203e69 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Thu, 6 Nov 2025 18:43:33 -0500 Subject: [PATCH 1/8] updated camera positions to the 2025 bot ones and added note to check CAN IDs --- .../java/frc/robot/subsystems/drive/DriveConstants.java | 1 + .../java/frc/robot/subsystems/vision/VisionConstants.java | 8 ++++---- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 1db554a..5c0e3cf 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -86,6 +86,7 @@ public class DriveConstants { // Device CAN IDs public static final int gyroCanId = 0; + //TODO: check CAN IDs for correctness public static final int backLeftDriveCanId = 10; public static final int backRightDriveCanId = 18; public static final int frontRightDriveCanId = 20; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 1afe97c..5dbc850 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -32,13 +32,13 @@ public class VisionConstants { // Robot to camera transforms public static Transform3d robotToFrontRightCamera = - new Transform3d(0.286, -0.332, 0.361, new Rotation3d(0, 0, 0.768)); + new Transform3d(0.248, -0.318, 0.513, new Rotation3d(0.0, 0.0, 0.0)); public static Transform3d robotToFrontLeftCamera = - new Transform3d(0.292, 0.316, 0.361, new Rotation3d(0, 0, 5.498)); + new Transform3d(0.222, 0.331, 0.513, new Rotation3d(0.0, 0, Math.PI / 2.0)); public static Transform3d robotToBackRightCamera = - new Transform3d(-0.259, -0.346, 0.628, new Rotation3d(0, 0, 4.311)); + new Transform3d(-0.375, -0.331, 0.513, new Rotation3d(0.0, 0.0, 3.0 * Math.PI / 2.0)); public static Transform3d robotToBackLeftCamera = - new Transform3d(-0.252, 0.341, 0.628, new Rotation3d(0, 0, 1.972)); + new Transform3d(-0.401, 0.318, 0.513, new Rotation3d(0.0, 0.0, Math.PI)); public static Distance minRobotWidth = Inches.of(36.875); From d42b9854e0cf6ff20466b3b5388e451baf52c34a Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 18 Nov 2025 18:29:16 -0500 Subject: [PATCH 2/8] changed camera names to fit 2025 chassis --- .../java/frc/robot/subsystems/drive/DriveConstants.java | 1 - .../java/frc/robot/subsystems/vision/VisionConstants.java | 8 ++++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 5c0e3cf..1db554a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -86,7 +86,6 @@ public class DriveConstants { // Device CAN IDs public static final int gyroCanId = 0; - //TODO: check CAN IDs for correctness public static final int backLeftDriveCanId = 10; public static final int backRightDriveCanId = 18; public static final int frontRightDriveCanId = 20; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 5dbc850..e76395a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -25,10 +25,10 @@ public class VisionConstants { public static AprilTagFields defaultAprilTagFieldLayout = AprilTagFields.k2025ReefscapeAndyMark; // Camera names, must match names configured on coprocessor - public static String cameraFrontRightName = "OV2311_TH_4"; - public static String cameraFrontLeftName = "OV2311_TH_3"; - public static String cameraBackRightName = "OV2311_TH_2"; - public static String cameraBackLeftName = "OV2311_TH_1"; + public static String cameraFrontRightName = "OV2311_TH_8"; + public static String cameraFrontLeftName = "OV2311_TH_5"; + public static String cameraBackRightName = "OV2311_TH_6"; + public static String cameraBackLeftName = "OV2311_TH_7"; // Robot to camera transforms public static Transform3d robotToFrontRightCamera = From 2e78d4dae1435b01a8936b9d9171f2e2b347ab6b Mon Sep 17 00:00:00 2001 From: BenGamer3 Date: Thu, 11 Dec 2025 19:07:04 -0500 Subject: [PATCH 3/8] advanced object detection --- .../frc/robot/subsystems/vision/VisionIO.java | 12 +++++++++--- .../subsystems/vision/VisionIOPhotonVision.java | 16 ++++++++++++---- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 1ebf25b..4a4035e 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -9,20 +9,26 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Optional; import org.littletonrobotics.junction.AutoLog; public interface VisionIO { @AutoLog public static class VisionIOInputs { public boolean connected = false; - public TargetObservation latestTargetObservation = - new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + public Optional latestTargetObservation = Optional.empty(); public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; } /** Represents the angle to a simple target, not used for pose estimation. */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + public static record TargetObservation( + Rotation2d yaw, + Rotation2d pitch, + Rotation2d skew, + double area, + float confidince, + int objectID) {} /** Represents a robot pose sample used for pose estimation. */ public static record PoseObservation( diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 7566750..ce36e7d 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -13,6 +13,7 @@ import java.util.HashSet; import java.util.LinkedList; import java.util.List; +import java.util.Optional; import java.util.Set; import org.photonvision.PhotonCamera; @@ -39,15 +40,22 @@ public void updateInputs(VisionIOInputs inputs) { // Read new camera observations Set tagIds = new HashSet<>(); List poseObservations = new LinkedList<>(); + for (var result : camera.getAllUnreadResults()) { // Update latest target observation if (result.hasTargets()) { + var bestTarget = result.getBestTarget(); inputs.latestTargetObservation = - new TargetObservation( - Rotation2d.fromDegrees(result.getBestTarget().getYaw()), - Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + Optional.of( + new TargetObservation( + Rotation2d.fromDegrees(bestTarget.getYaw()), + Rotation2d.fromDegrees(bestTarget.getPitch()), + Rotation2d.fromDegrees(bestTarget.getSkew()), + bestTarget.getArea(), + bestTarget.getDetectedObjectConfidence(), + bestTarget.getDetectedObjectClassID())); } else { - inputs.latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + inputs.latestTargetObservation = Optional.empty(); } // Add pose observation From a8b0abd0057beda580c8cc59ad3a0c71ffd94507 Mon Sep 17 00:00:00 2001 From: BenGamer3 Date: Thu, 11 Dec 2025 19:24:40 -0500 Subject: [PATCH 4/8] reworked vision --- .../frc/robot/subsystems/vision/Vision.java | 4 ++-- .../frc/robot/subsystems/vision/VisionIO.java | 4 ++-- .../vision/VisionIOPhotonVision.java | 19 +++++++++---------- 3 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 119c6cc..71fb3be 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -67,12 +67,12 @@ public Vision(VisionConsumer consumer, Supplier poseSupplier, VisionIO.. } /** - * Returns the X angle to the best target, which can be used for simple servoing with vision. + * Returns the yaw angle to the best target, which can be used for simple servoing with vision. * * @param cameraIndex The index of the camera to use. */ public Rotation2d getTargetX(int cameraIndex) { - return inputs[cameraIndex].latestTargetObservation.tx(); + return inputs[cameraIndex].latestTargetObservation.yaw(); } @Override diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 4a4035e..b59bca2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -9,14 +9,14 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import java.util.Optional; import org.littletonrobotics.junction.AutoLog; public interface VisionIO { @AutoLog public static class VisionIOInputs { public boolean connected = false; - public Optional latestTargetObservation = Optional.empty(); + public TargetObservation latestTargetObservation = + new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, 0, 0); public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index ce36e7d..7137ca6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -13,7 +13,6 @@ import java.util.HashSet; import java.util.LinkedList; import java.util.List; -import java.util.Optional; import java.util.Set; import org.photonvision.PhotonCamera; @@ -46,16 +45,16 @@ public void updateInputs(VisionIOInputs inputs) { if (result.hasTargets()) { var bestTarget = result.getBestTarget(); inputs.latestTargetObservation = - Optional.of( - new TargetObservation( - Rotation2d.fromDegrees(bestTarget.getYaw()), - Rotation2d.fromDegrees(bestTarget.getPitch()), - Rotation2d.fromDegrees(bestTarget.getSkew()), - bestTarget.getArea(), - bestTarget.getDetectedObjectConfidence(), - bestTarget.getDetectedObjectClassID())); + new TargetObservation( + Rotation2d.fromDegrees(bestTarget.getYaw()), + Rotation2d.fromDegrees(bestTarget.getPitch()), + Rotation2d.fromDegrees(bestTarget.getSkew()), + bestTarget.getArea(), + bestTarget.getDetectedObjectConfidence(), + bestTarget.getDetectedObjectClassID()); } else { - inputs.latestTargetObservation = Optional.empty(); + inputs.latestTargetObservation = + new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, 0, 0); } // Add pose observation From 140fe0ae863fcecb2585a1d1f05b292b2a236004 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 11 Dec 2025 19:47:52 -0500 Subject: [PATCH 5/8] match 2025 cameras, use object id -1 when no object detected --- src/main/java/frc/robot/subsystems/vision/VisionIO.java | 4 ++-- .../frc/robot/subsystems/vision/VisionIOPhotonVision.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index b59bca2..8a9c403 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -16,7 +16,7 @@ public interface VisionIO { public static class VisionIOInputs { public boolean connected = false; public TargetObservation latestTargetObservation = - new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, 0, 0); + new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, -1, -1); public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; } @@ -27,7 +27,7 @@ public static record TargetObservation( Rotation2d pitch, Rotation2d skew, double area, - float confidince, + float confidence, int objectID) {} /** Represents a robot pose sample used for pose estimation. */ diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 7137ca6..165ed26 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -54,7 +54,7 @@ public void updateInputs(VisionIOInputs inputs) { bestTarget.getDetectedObjectClassID()); } else { inputs.latestTargetObservation = - new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, 0, 0); + new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, -1, -1); } // Add pose observation From a84290af4af8fa8adb899a35be040607d6d5d12e Mon Sep 17 00:00:00 2001 From: BenGamer3 Date: Thu, 11 Dec 2025 20:04:22 -0500 Subject: [PATCH 6/8] added stuff --- src/main/java/frc/robot/Robot.java | 9 ++++- .../frc/robot/commands/DriveCommands.java | 40 +++++++++++++++++++ 2 files changed, 48 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 01c9a4a..caf891a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -328,8 +328,15 @@ public void bindXboxDriver(int port) { drive) .ignoringDisable(true)); + // Point at target while A button is held + xboxDriver + .a() + .whileTrue( + DriveCommands.pointAtTarget( + drive, () -> vision.getTargetX(2), allianceSelector::fieldRotated)); + // Drive 1m forward while A button is held - xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1))); + // xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1))); // Align with pose, approaching in correct orientation from 1 m away // xboxDriver diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 047e30b..ab92223 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -231,6 +231,46 @@ public static Command joystickDriveAtFixedOrientation( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + /** + * Field relative drive command using joystick for linear control and PID for angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, or controlling + * absolute rotation with a joystick. + */ + public static Command pointAtTarget( + Drive drive, Supplier yawErrorSupplier, BooleanSupplier fieldRotatedSupplier) { + + // Create PID controller + ProfiledPIDController angleController = + new ProfiledPIDController( + ANGLE_KP, + 0.0, + ANGLE_KD, + new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // Construct command + return Commands.run( + () -> { + + // Calculate angular speed + double omega = angleController.calculate(yawErrorSupplier.get().getRadians(), 0); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = new ChassisSpeeds(0, 0, omega); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + fieldRotatedSupplier.getAsBoolean() + ? drive.getRotation().plus(Rotation2d.kPi) + : drive.getRotation())); + }, + drive) + .withName("Point At Target") + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + } + /** * Measures the velocity feedforward constants for the drive motors. * From 157d1073f3b24b4c4c70397c62e194eaa4189879 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 11 Dec 2025 20:08:40 -0500 Subject: [PATCH 7/8] dampen swerve azimuth --- src/main/java/frc/robot/subsystems/drive/DriveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 1db554a..bd8715b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -146,7 +146,7 @@ public class DriveConstants { new Slot0Configs() .withKP(300) .withKI(0) - .withKD(0.5) + .withKD(1.5) .withKS(0.1) .withKV(1.91) .withKA(0) From 4705d4fe3ac89db8672f03ac11fe40d2b1de122b Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 11 Dec 2025 20:34:49 -0500 Subject: [PATCH 8/8] use cam0 for object detection, reset yaw controller correctly --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/commands/DriveCommands.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index caf891a..d3f1237 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -333,7 +333,7 @@ public void bindXboxDriver(int port) { .a() .whileTrue( DriveCommands.pointAtTarget( - drive, () -> vision.getTargetX(2), allianceSelector::fieldRotated)); + drive, () -> vision.getTargetX(0), allianceSelector::fieldRotated)); // Drive 1m forward while A button is held // xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1))); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index ab92223..35f49fe 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -268,7 +268,7 @@ public static Command pointAtTarget( .withName("Point At Target") // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + .beforeStarting(() -> angleController.reset(yawErrorSupplier.get().getRadians())); } /**