diff --git a/src/main/java/frc/robot/subsystems/turrettracker/TurretTracker.java b/src/main/java/frc/robot/subsystems/turrettracker/TurretTracker.java index 3bc8543..8db36c9 100644 --- a/src/main/java/frc/robot/subsystems/turrettracker/TurretTracker.java +++ b/src/main/java/frc/robot/subsystems/turrettracker/TurretTracker.java @@ -77,10 +77,19 @@ public class TurretTracker extends SubsystemBase { @Getter private boolean targetInRange = false; - // Distance from robot to the active target in meters. + // Horizontal distance from robot to the active target in meters (2D, X/Y only). + @Getter + private double horizontalDistanceMeters = 0.0; + + // 3D distance from turret to the target, accounting for height difference (meters). @Getter private double distanceToTargetMeters = 0.0; + // Elevation angle to the target in degrees (positive = upward, 0 = flat). + // In passing mode this is always 0 (flat lob trajectory). + @Getter + private double elevationAngleDegrees = 0.0; + // The currently resolved target position (hub center or passing target). @Getter private Translation2d activeTarget = new Translation2d(); @@ -92,7 +101,7 @@ public class TurretTracker extends SubsystemBase { // Visualization: AdvantageScope via StructPublisher private final StructPublisher aimPose3dPublisher; private final StructPublisher targetPose3dPublisher; - private final StructArrayPublisher aimLinePublisher; + private final StructArrayPublisher aimLinePublisher; public TurretTracker(final TurretTrackerContext context, final Drivetrain drivetrain) { this.context = requireNonNull(context, "TurretTrackerContext cannot be null"); @@ -129,7 +138,7 @@ public TurretTracker(final TurretTrackerContext context, final Drivetrain drivet this.targetPose3dPublisher = nti.getStructTopic("TurretTracker/TargetPose3d", Pose3d.struct).publish(); this.aimLinePublisher = - nti.getStructArrayTopic("TurretTracker/AimLine", Pose2d.struct).publish(); + nti.getStructArrayTopic("TurretTracker/AimLine", Pose3d.struct).publish(); // Register telemetry Telemetry.registerSubsystem(TELEMETRY_PREFIX, this::captureTelemetry); @@ -188,10 +197,25 @@ public void periodic() { // Resolve the active target based on tracking mode activeTarget = (trackingMode == TrackingMode.PASSING) ? computePassingTarget(robotPose, hubCenter) : hubCenter; - // Calculate distance to active target + // Calculate horizontal distance to active target (2D) double dx = activeTarget.getX() - robotPose.getX(); double dy = activeTarget.getY() - robotPose.getY(); - distanceToTargetMeters = Math.sqrt(dx * dx + dy * dy); + horizontalDistanceMeters = Math.sqrt(dx * dx + dy * dy); + + // Calculate height difference and 3D distance + double targetZ = (trackingMode == TrackingMode.SHOOTING) + ? context.getShootingTargetHeightMeters() + : context.getPassingTargetHeightMeters(); + double dz = targetZ - context.getTurretHeightMeters(); + distanceToTargetMeters = Math.sqrt(dx * dx + dy * dy + dz * dz); + + // Calculate elevation angle (positive = upward, 0 = flat) + // For passing mode, force flat (0°) since we lob over obstacles + if (trackingMode == TrackingMode.PASSING) { + elevationAngleDegrees = 0.0; + } else { + elevationAngleDegrees = Units.radiansToDegrees(Math.atan2(dz, horizontalDistanceMeters)); + } // Calculate field-relative angle from robot to active target double fieldAngleRad = Math.atan2(dy, dx); @@ -309,30 +333,41 @@ private void updateAdvantageScope(Pose2d robotPose, Translation2d hubCenter) { // Field-relative aim direction double aimFieldAngleRad = robotPose.getRotation().getRadians() + Units.degreesToRadians(turretAngleDegrees); - // Aim pose at robot position, pointed toward hub center + // Aim pose at robot position, pointed toward active target with elevation pitch + double elevPitchRad = Units.degreesToRadians(elevationAngleDegrees); Pose3d aimPose = new Pose3d( robotPose.getX(), robotPose.getY(), context.getTurretHeightMeters(), - new Rotation3d(0, 0, aimFieldAngleRad)); + new Rotation3d(0, -elevPitchRad, aimFieldAngleRad)); aimPose3dPublisher.set(aimPose); - // Hub center as a Pose3d (Z = turret height for visual alignment) - Pose3d targetPose = - new Pose3d(hubCenter.getX(), hubCenter.getY(), context.getTurretHeightMeters(), new Rotation3d()); + // Active target as a Pose3d at the actual target height + double activeTargetZ = (trackingMode == TrackingMode.SHOOTING) + ? context.getShootingTargetHeightMeters() + : context.getPassingTargetHeightMeters(); + Pose3d targetPose = new Pose3d(hubCenter.getX(), hubCenter.getY(), activeTargetZ, new Rotation3d()); targetPose3dPublisher.set(targetPose); - // Aim line: array of 2 Pose2d (start at robot, end at aim vector endpoint) - double endX = robotPose.getX() + context.getAimVectorLengthMeters() * Math.cos(aimFieldAngleRad); - double endY = robotPose.getY() + context.getAimVectorLengthMeters() * Math.sin(aimFieldAngleRad); - - Pose2d[] aimLine = new Pose2d[] { - robotPose, new Pose2d(endX, endY, new Rotation2d(aimFieldAngleRad)), + // Aim line: array of 2 Pose3d from turret to aim vector endpoint. + // In shooting mode the line pitches upward toward the hub intake height; + // in passing mode it stays flat (elevation = 0). + double turretZ = context.getTurretHeightMeters(); + double elevationRad = Units.degreesToRadians(elevationAngleDegrees); + double aimLength = context.getAimVectorLengthMeters(); + + // Horizontal projection of the aim vector (shortened by pitch) + double horizontalLength = aimLength * Math.cos(elevationRad); + double endX = robotPose.getX() + horizontalLength * Math.cos(aimFieldAngleRad); + double endY = robotPose.getY() + horizontalLength * Math.sin(aimFieldAngleRad); + double endZ = turretZ + aimLength * Math.sin(elevationRad); + + // Rotation3d: roll=0, pitch=-elevation (WPILib pitch is nose-down positive), yaw=aim heading + Rotation3d aimRot = new Rotation3d(0, -elevationRad, aimFieldAngleRad); + Pose3d[] aimLine = new Pose3d[] { + new Pose3d(robotPose.getX(), robotPose.getY(), turretZ, aimRot), new Pose3d(endX, endY, endZ, aimRot), }; aimLinePublisher.set(aimLine); - - // Also record for DataLog (AdvantageScope replay) - Telemetry.recordPoses(TELEMETRY_PREFIX + "/AimLine", aimLine, TelemetryLevel.MATCH); } private void captureTelemetry(String prefix) { @@ -340,12 +375,16 @@ private void captureTelemetry(String prefix) { Telemetry.record(prefix + "/AngleDeg", turretAngleDegrees, TelemetryLevel.MATCH); Telemetry.record(prefix + "/InRange", targetInRange, TelemetryLevel.MATCH); Telemetry.record(prefix + "/DistanceM", distanceToTargetMeters, TelemetryLevel.MATCH); + Telemetry.record(prefix + "/HorizontalDistM", horizontalDistanceMeters, TelemetryLevel.MATCH); + Telemetry.record(prefix + "/ElevationDeg", elevationAngleDegrees, TelemetryLevel.MATCH); Telemetry.record(prefix + "/Mode", trackingMode.name(), TelemetryLevel.MATCH); // Publish to NT for live dashboard Telemetry.publish(prefix + "/AngleDeg", turretAngleDegrees, TelemetryLevel.MATCH); Telemetry.publish(prefix + "/InRange", targetInRange, TelemetryLevel.MATCH); Telemetry.publish(prefix + "/DistanceM", distanceToTargetMeters, TelemetryLevel.MATCH); + Telemetry.publish(prefix + "/HorizontalDistM", horizontalDistanceMeters, TelemetryLevel.MATCH); + Telemetry.publish(prefix + "/ElevationDeg", elevationAngleDegrees, TelemetryLevel.MATCH); Telemetry.publish(prefix + "/Mode", trackingMode.name(), TelemetryLevel.MATCH); // LAB level - detailed tracking data @@ -358,7 +397,9 @@ private void captureTelemetry(String prefix) { String modeLabel = trackingMode == TrackingMode.PASSING ? "Passing" : "Hub Center"; String status = targetInRange - ? String.format("Tracking %s (%.1f deg, %.1fm)", modeLabel, turretAngleDegrees, distanceToTargetMeters) + ? String.format( + "Tracking %s (%.1f deg, %.1f elev, %.1fm)", + modeLabel, turretAngleDegrees, elevationAngleDegrees, distanceToTargetMeters) : String.format("Out of Range (%.1f deg)", rawAngleDegrees); Telemetry.publish(prefix + "/Status", status, TelemetryLevel.MATCH); } diff --git a/src/main/java/frc/robot/subsystems/turrettracker/TurretTrackerContext.java b/src/main/java/frc/robot/subsystems/turrettracker/TurretTrackerContext.java index 09feaf4..6971cbf 100644 --- a/src/main/java/frc/robot/subsystems/turrettracker/TurretTrackerContext.java +++ b/src/main/java/frc/robot/subsystems/turrettracker/TurretTrackerContext.java @@ -20,9 +20,26 @@ public class TurretTrackerContext { /** * Height of the turret above ground for 3D visualization (meters). + * 19 inches = 0.4826m. */ @Builder.Default - private final double turretHeightMeters = 0.5; + private final double turretHeightMeters = 0.4826; + + /** + * Height of the hub intake opening above ground (meters). + * 72 inches = 1.8288m. Used in shooting mode to compute elevation angle + * and 3D distance for motor speed derivation. + */ + @Builder.Default + private final double shootingTargetHeightMeters = 1.8288; + + /** + * Height of the passing target above ground (meters). + * Passing uses a lob trajectory, so elevation is computed as 0 (flat) + * rather than aiming down at the ground. + */ + @Builder.Default + private final double passingTargetHeightMeters = 0.0; /** * Length of the aim vector line drawn in visualizations (meters). diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index e3e43e8..820e13f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -5,7 +5,9 @@ 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.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -36,11 +38,11 @@ /** * Vision subsystem for AprilTag detection using PhotonVision. - * Manages three cameras (front-right, front-left, rear), provides pose estimation, - * supports simulation, and visualizes camera FOV cones. + * Manages four cameras (front-right, front-left, right-side, left-side), + * provides pose estimation, supports simulation, and visualizes camera FOV cones. * * Features: - * - Triple camera support (front-right, front-left, rear) + * - Quad camera support (front-right, front-left, right-side, left-side) * - PhotonPoseEstimator integration for robot localization * - Dynamic standard deviation calculation * - Full simulation support with VisionSystemSim @@ -63,24 +65,28 @@ public interface VisionMeasurementConsumer { private final VisionMeasurementConsumer visionMeasurementConsumer; private final PhotonCamera frontRightCamera; private final PhotonCamera frontLeftCamera; - private final PhotonCamera rearCamera; + private final PhotonCamera rightSideCamera; + private final PhotonCamera leftSideCamera; // Pose estimation private final AprilTagFieldLayout fieldLayout; private final PhotonPoseEstimator frontRightPoseEstimator; private final PhotonPoseEstimator frontLeftPoseEstimator; - private final PhotonPoseEstimator rearPoseEstimator; + private final PhotonPoseEstimator rightSidePoseEstimator; + private final PhotonPoseEstimator leftSidePoseEstimator; // Simulation (only created in simulation mode) private VisionSystemSim visionSim; private PhotonCameraSim frontRightCameraSim; private PhotonCameraSim frontLeftCameraSim; - private PhotonCameraSim rearCameraSim; + private PhotonCameraSim rightSideCameraSim; + private PhotonCameraSim leftSideCameraSim; // FOV visualization publishers (simulation only) - private StructArrayPublisher frontRightFovPublisher; - private StructArrayPublisher frontLeftFovPublisher; - private StructArrayPublisher rearFovPublisher; + private StructArrayPublisher frontRightFovPublisher; + private StructArrayPublisher frontLeftFovPublisher; + private StructArrayPublisher rightSideFovPublisher; + private StructArrayPublisher leftSideFovPublisher; private Mechanism2d cameraLayoutMech; /** @@ -102,7 +108,8 @@ public VisionSubsystem( // Initialize PhotonVision cameras this.frontRightCamera = new PhotonCamera(context.getFrontRightCameraName()); this.frontLeftCamera = new PhotonCamera(context.getFrontLeftCameraName()); - this.rearCamera = new PhotonCamera(context.getRearCameraName()); + this.rightSideCamera = new PhotonCamera(context.getRightSideCameraName()); + this.leftSideCamera = new PhotonCamera(context.getLeftSideCameraName()); // Load AprilTag field layout from WPILib this.fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); @@ -112,8 +119,10 @@ public VisionSubsystem( fieldLayout, context.getPoseEstimationStrategy(), context.getFrontRightCameraToRobot()); this.frontLeftPoseEstimator = new PhotonPoseEstimator( fieldLayout, context.getPoseEstimationStrategy(), context.getFrontLeftCameraToRobot()); - this.rearPoseEstimator = new PhotonPoseEstimator( - fieldLayout, context.getPoseEstimationStrategy(), context.getRearCameraToRobot()); + this.rightSidePoseEstimator = new PhotonPoseEstimator( + fieldLayout, context.getPoseEstimationStrategy(), context.getRightSideCameraToRobot()); + this.leftSidePoseEstimator = new PhotonPoseEstimator( + fieldLayout, context.getPoseEstimationStrategy(), context.getLeftSideCameraToRobot()); // Initialize simulation if enabled // NOTE: PhotonVision simulation is expensive (~96ms per loop) and causes "CommandScheduler @@ -132,7 +141,8 @@ public VisionSubsystem( Telemetry.publish("Vision/Status", "Initialized", TelemetryLevel.MATCH); Telemetry.publish("Vision/FrontRightCamera/Connected", false, TelemetryLevel.MATCH); Telemetry.publish("Vision/FrontLeftCamera/Connected", false, TelemetryLevel.MATCH); - Telemetry.publish("Vision/RearCamera/Connected", false, TelemetryLevel.MATCH); + Telemetry.publish("Vision/RightSideCamera/Connected", false, TelemetryLevel.MATCH); + Telemetry.publish("Vision/LeftSideCamera/Connected", false, TelemetryLevel.MATCH); } /** @@ -160,16 +170,25 @@ private void initializeSimulation() { frontLeftCameraSim.enableRawStream(false); frontLeftCameraSim.enableProcessedStream(false); - // Configure rear camera simulation - SimCameraProperties rearProps = createSimCameraProperties(); - rearCameraSim = new PhotonCameraSim(rearCamera, rearProps); - visionSim.addCamera(rearCameraSim, context.getRearCameraToRobot()); - rearCameraSim.enableDrawWireframe(true); + // Configure right-side camera simulation + SimCameraProperties rightSideProps = createSimCameraProperties(); + rightSideCameraSim = new PhotonCameraSim(rightSideCamera, rightSideProps); + visionSim.addCamera(rightSideCameraSim, context.getRightSideCameraToRobot()); + rightSideCameraSim.enableDrawWireframe(true); // Disable video streaming to avoid CameraServer handle issues - rearCameraSim.enableRawStream(false); - rearCameraSim.enableProcessedStream(false); + rightSideCameraSim.enableRawStream(false); + rightSideCameraSim.enableProcessedStream(false); + + // Configure left-side camera simulation + SimCameraProperties leftSideProps = createSimCameraProperties(); + leftSideCameraSim = new PhotonCameraSim(leftSideCamera, leftSideProps); + visionSim.addCamera(leftSideCameraSim, context.getLeftSideCameraToRobot()); + leftSideCameraSim.enableDrawWireframe(true); + // Disable video streaming to avoid CameraServer handle issues + leftSideCameraSim.enableRawStream(false); + leftSideCameraSim.enableProcessedStream(false); - Telemetry.publish("Vision/Simulation", "Active (3 cameras)", TelemetryLevel.LAB); + Telemetry.publish("Vision/Simulation", "Active (4 cameras)", TelemetryLevel.LAB); } /** @@ -197,14 +216,16 @@ private SimCameraProperties createSimCameraProperties() { private void initializeFovVisualization() { NetworkTableInstance nti = NetworkTableInstance.getDefault(); - frontRightFovPublisher = nti.getStructArrayTopic("Vision/FrontRight/FOVCone", Pose2d.struct) + frontRightFovPublisher = nti.getStructArrayTopic("Vision/FrontRight/FOVCone", Pose3d.struct) + .publish(); + frontLeftFovPublisher = nti.getStructArrayTopic("Vision/FrontLeft/FOVCone", Pose3d.struct) + .publish(); + rightSideFovPublisher = nti.getStructArrayTopic("Vision/RightSide/FOVCone", Pose3d.struct) .publish(); - frontLeftFovPublisher = nti.getStructArrayTopic("Vision/FrontLeft/FOVCone", Pose2d.struct) + leftSideFovPublisher = nti.getStructArrayTopic("Vision/LeftSide/FOVCone", Pose3d.struct) .publish(); - rearFovPublisher = - nti.getStructArrayTopic("Vision/Rear/FOVCone", Pose2d.struct).publish(); - // Mechanism2d: top-down camera layout (robot center, 3 directional lines) + // Mechanism2d: top-down camera layout (robot center, 4 directional lines) double mechSize = 100.0; cameraLayoutMech = new Mechanism2d(mechSize, mechSize); MechanismRoot2d center = cameraLayoutMech.getRoot("robotCenter", mechSize / 2.0, mechSize / 2.0); @@ -214,8 +235,10 @@ private void initializeFovVisualization() { center.append(new MechanismLigament2d("frontRightCam", 30, 90 - 30, 2, new Color8Bit(Color.kOrange))); // Front-left at yaw=+30deg: mechanism angle = 90 + 30 = 120 center.append(new MechanismLigament2d("frontLeftCam", 30, 90 + 30, 2, new Color8Bit(Color.kYellow))); - // Rear at yaw=180deg: mechanism angle = 90 + 180 = 270 - center.append(new MechanismLigament2d("rearCam", 30, 270, 2, new Color8Bit(Color.kCyan))); + // Right-side at yaw=-120deg: mechanism angle = 90 + (-120) = -30 + center.append(new MechanismLigament2d("rightSideCam", 30, -30, 2, new Color8Bit(Color.kCyan))); + // Left-side at yaw=+120deg: mechanism angle = 90 + 120 = 210 + center.append(new MechanismLigament2d("leftSideCam", 30, 210, 2, new Color8Bit(Color.kMagenta))); Telemetry.putData("Vision/CameraLayout", cameraLayoutMech); } @@ -227,11 +250,13 @@ private void updatePoseEstimation() { Pose2d currentPose = drivetrain.getPose2dEstimator(); frontRightPoseEstimator.setReferencePose(currentPose); frontLeftPoseEstimator.setReferencePose(currentPose); - rearPoseEstimator.setReferencePose(currentPose); + rightSidePoseEstimator.setReferencePose(currentPose); + leftSidePoseEstimator.setReferencePose(currentPose); processCamera(frontRightCamera, frontRightPoseEstimator, "FrontRight"); processCamera(frontLeftCamera, frontLeftPoseEstimator, "FrontLeft"); - processCamera(rearCamera, rearPoseEstimator, "Rear"); + processCamera(rightSideCamera, rightSidePoseEstimator, "RightSide"); + processCamera(leftSideCamera, leftSidePoseEstimator, "LeftSide"); } /** @@ -331,15 +356,18 @@ public void periodic() { boolean frontRightConnected = isSimulation || frontRightCamera.isConnected(); boolean frontLeftConnected = isSimulation || frontLeftCamera.isConnected(); - boolean rearConnected = isSimulation || rearCamera.isConnected(); + boolean rightSideConnected = isSimulation || rightSideCamera.isConnected(); + boolean leftSideConnected = isSimulation || leftSideCamera.isConnected(); Telemetry.publish("Vision/FrontRightCamera/Connected", frontRightConnected, TelemetryLevel.MATCH); Telemetry.publish("Vision/FrontLeftCamera/Connected", frontLeftConnected, TelemetryLevel.MATCH); - Telemetry.publish("Vision/RearCamera/Connected", rearConnected, TelemetryLevel.MATCH); + Telemetry.publish("Vision/RightSideCamera/Connected", rightSideConnected, TelemetryLevel.MATCH); + Telemetry.publish("Vision/LeftSideCamera/Connected", leftSideConnected, TelemetryLevel.MATCH); PhotonPipelineResult frontRightResult = frontRightCamera.getLatestResult(); PhotonPipelineResult frontLeftResult = frontLeftCamera.getLatestResult(); - PhotonPipelineResult rearResult = rearCamera.getLatestResult(); + PhotonPipelineResult rightSideResult = rightSideCamera.getLatestResult(); + PhotonPipelineResult leftSideResult = leftSideCamera.getLatestResult(); if (frontRightConnected && frontRightResult.hasTargets()) { processAndLogTargets("FrontRight", frontRightResult); @@ -355,15 +383,29 @@ public void periodic() { Telemetry.publish("Vision/FrontLeftCamera/DetectedTags", "None", TelemetryLevel.LAB); } - if (rearConnected && rearResult.hasTargets()) { - processAndLogTargets("Rear", rearResult); + if (rightSideConnected && rightSideResult.hasTargets()) { + processAndLogTargets("RightSide", rightSideResult); } else { - Telemetry.publish("Vision/RearCamera/TargetCount", 0, TelemetryLevel.MATCH); - Telemetry.publish("Vision/RearCamera/DetectedTags", "None", TelemetryLevel.LAB); + Telemetry.publish("Vision/RightSideCamera/TargetCount", 0, TelemetryLevel.MATCH); + Telemetry.publish("Vision/RightSideCamera/DetectedTags", "None", TelemetryLevel.LAB); + } + + if (leftSideConnected && leftSideResult.hasTargets()) { + processAndLogTargets("LeftSide", leftSideResult); + } else { + Telemetry.publish("Vision/LeftSideCamera/TargetCount", 0, TelemetryLevel.MATCH); + Telemetry.publish("Vision/LeftSideCamera/DetectedTags", "None", TelemetryLevel.LAB); } updateSystemStatus( - frontRightConnected, frontLeftConnected, rearConnected, frontRightResult, frontLeftResult, rearResult); + frontRightConnected, + frontLeftConnected, + rightSideConnected, + leftSideConnected, + frontRightResult, + frontLeftResult, + rightSideResult, + leftSideResult); updatePoseEstimation(); } @@ -382,8 +424,9 @@ public void simulationPeriodic() { /** * Computes field-relative FOV cone edges for each camera and publishes - * as Pose2d arrays for AdvantageScope 2D field overlay. - * Each FOV cone is a 3-point V shape: [left edge, camera position, right edge]. + * as Pose3d arrays for AdvantageScope 3D field overlay at the camera's + * mounted height. Each FOV cone is a 3-point V shape: + * [left edge, camera position, right edge]. */ private void updateFovVisualization(Pose2d robotPose) { double rayLength = context.getFovVisualizationRayLength(); @@ -392,15 +435,17 @@ private void updateFovVisualization(Pose2d robotPose) { publishCameraFov( frontRightFovPublisher, robotPose, context.getFrontRightCameraToRobot(), halfFovRad, rayLength); publishCameraFov(frontLeftFovPublisher, robotPose, context.getFrontLeftCameraToRobot(), halfFovRad, rayLength); - publishCameraFov(rearFovPublisher, robotPose, context.getRearCameraToRobot(), halfFovRad, rayLength); + publishCameraFov(rightSideFovPublisher, robotPose, context.getRightSideCameraToRobot(), halfFovRad, rayLength); + publishCameraFov(leftSideFovPublisher, robotPose, context.getLeftSideCameraToRobot(), halfFovRad, rayLength); } /** - * Publishes a single camera's FOV cone as a V-shaped Pose2d array. - * Projects the camera position and FOV edges onto the field coordinate system. + * Publishes a single camera's FOV cone as a V-shaped Pose3d array. + * Projects the camera position and FOV edges onto the field coordinate system + * at the camera's mounted Z height. */ private void publishCameraFov( - StructArrayPublisher publisher, + StructArrayPublisher publisher, Pose2d robotPose, Transform3d cameraToRobot, double halfFovRad, @@ -413,6 +458,7 @@ private void publishCameraFov( double sinH = Math.sin(robotHeading); double camX = robotPose.getX() + cameraToRobot.getX() * cosH - cameraToRobot.getY() * sinH; double camY = robotPose.getY() + cameraToRobot.getX() * sinH + cameraToRobot.getY() * cosH; + double camZ = cameraToRobot.getZ(); // Camera heading in field coordinates (robot heading + camera yaw) double cameraYaw = cameraToRobot.getRotation().getZ(); @@ -427,11 +473,15 @@ private void publishCameraFov( double rightX = camX + rayLength * Math.cos(rightAngle); double rightY = camY + rayLength * Math.sin(rightAngle); - Pose2d leftEdge = new Pose2d(leftX, leftY, new Rotation2d(leftAngle)); - Pose2d camPose = new Pose2d(camX, camY, new Rotation2d(camHeading)); - Pose2d rightEdge = new Pose2d(rightX, rightY, new Rotation2d(rightAngle)); + Rotation3d leftRot = new Rotation3d(0, 0, leftAngle); + Rotation3d camRot = new Rotation3d(0, 0, camHeading); + Rotation3d rightRot = new Rotation3d(0, 0, rightAngle); + + Pose3d leftEdge = new Pose3d(leftX, leftY, camZ, leftRot); + Pose3d camPose = new Pose3d(camX, camY, camZ, camRot); + Pose3d rightEdge = new Pose3d(rightX, rightY, camZ, rightRot); - publisher.set(new Pose2d[] {leftEdge, camPose, rightEdge}); + publisher.set(new Pose3d[] {leftEdge, camPose, rightEdge}); } /** @@ -473,35 +523,40 @@ private void processAndLogTargets(String cameraName, PhotonPipelineResult result } /** - * Updates overall system status telemetry for 3 cameras. + * Updates overall system status telemetry for 4 cameras. */ private void updateSystemStatus( boolean frontRightConnected, boolean frontLeftConnected, - boolean rearConnected, + boolean rightSideConnected, + boolean leftSideConnected, PhotonPipelineResult frontRightResult, PhotonPipelineResult frontLeftResult, - PhotonPipelineResult rearResult) { + PhotonPipelineResult rightSideResult, + PhotonPipelineResult leftSideResult) { int connectedCount = 0; if (frontRightConnected) connectedCount++; if (frontLeftConnected) connectedCount++; - if (rearConnected) connectedCount++; + if (rightSideConnected) connectedCount++; + if (leftSideConnected) connectedCount++; String status; if (connectedCount == 0) { status = "No Cameras Connected"; - } else if (connectedCount < 3) { + } else if (connectedCount < 4) { List offline = new ArrayList<>(); if (!frontRightConnected) offline.add("FrontRight"); if (!frontLeftConnected) offline.add("FrontLeft"); - if (!rearConnected) offline.add("Rear"); + if (!rightSideConnected) offline.add("RightSide"); + if (!leftSideConnected) offline.add("LeftSide"); status = String.join(", ", offline) + " Offline"; } else { List trackingCams = new ArrayList<>(); if (frontRightResult.hasTargets()) trackingCams.add("FR"); if (frontLeftResult.hasTargets()) trackingCams.add("FL"); - if (rearResult.hasTargets()) trackingCams.add("Rear"); + if (rightSideResult.hasTargets()) trackingCams.add("RS"); + if (leftSideResult.hasTargets()) trackingCams.add("LS"); if (trackingCams.isEmpty()) { status = "No Targets Detected"; @@ -519,8 +574,11 @@ private void updateSystemStatus( if (frontLeftConnected && frontLeftResult.hasTargets()) { totalTags += frontLeftResult.getTargets().size(); } - if (rearConnected && rearResult.hasTargets()) { - totalTags += rearResult.getTargets().size(); + if (rightSideConnected && rightSideResult.hasTargets()) { + totalTags += rightSideResult.getTargets().size(); + } + if (leftSideConnected && leftSideResult.hasTargets()) { + totalTags += leftSideResult.getTargets().size(); } Telemetry.publish("Vision/TotalTagsDetected", totalTags, TelemetryLevel.MATCH); } @@ -535,8 +593,12 @@ public PhotonPipelineResult getFrontLeftCameraResult() { return frontLeftCamera.getLatestResult(); } - public PhotonPipelineResult getRearCameraResult() { - return rearCamera.getLatestResult(); + public PhotonPipelineResult getRightSideCameraResult() { + return rightSideCamera.getLatestResult(); + } + + public PhotonPipelineResult getLeftSideCameraResult() { + return leftSideCamera.getLatestResult(); } public PhotonCamera getFrontRightCamera() { @@ -547,8 +609,12 @@ public PhotonCamera getFrontLeftCamera() { return frontLeftCamera; } - public PhotonCamera getRearCamera() { - return rearCamera; + public PhotonCamera getRightSideCamera() { + return rightSideCamera; + } + + public PhotonCamera getLeftSideCamera() { + return leftSideCamera; } public boolean isFrontRightCameraConnected() { @@ -559,8 +625,12 @@ public boolean isFrontLeftCameraConnected() { return frontLeftCamera.isConnected(); } - public boolean isRearCameraConnected() { - return rearCamera.isConnected(); + public boolean isRightSideCameraConnected() { + return rightSideCamera.isConnected(); + } + + public boolean isLeftSideCameraConnected() { + return leftSideCamera.isConnected(); } public int getFrontRightTargetCount() { @@ -573,8 +643,13 @@ public int getFrontLeftTargetCount() { return result.hasTargets() ? result.getTargets().size() : 0; } - public int getRearTargetCount() { - PhotonPipelineResult result = rearCamera.getLatestResult(); + public int getRightSideTargetCount() { + PhotonPipelineResult result = rightSideCamera.getLatestResult(); + return result.hasTargets() ? result.getTargets().size() : 0; + } + + public int getLeftSideTargetCount() { + PhotonPipelineResult result = leftSideCamera.getLatestResult(); return result.hasTargets() ? result.getTargets().size() : 0; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java index ba49c71..49165b0 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystemContext.java @@ -9,8 +9,8 @@ /** * Configuration context for the Vision subsystem using PhotonVision. - * Supports three cameras (front-right, front-left, rear) for AprilTag detection - * and localization, with FOV visualization in simulation. + * Supports four cameras (front-right, front-left, right-side, left-side) + * for AprilTag detection and localization, with FOV visualization in simulation. */ @Data @Builder @@ -29,10 +29,16 @@ public class VisionSubsystemContext { private final String frontLeftCameraName = "photonvision-front-left"; /** - * Network table name for the rear camera + * Network table name for the right-side camera */ @Builder.Default - private final String rearCameraName = "photonvision-rear"; + private final String rightSideCameraName = "photonvision-right-side"; + + /** + * Network table name for the left-side camera + */ + @Builder.Default + private final String leftSideCameraName = "photonvision-left-side"; /** * Whether to enable verbose logging to SmartDashboard @@ -49,32 +55,42 @@ public class VisionSubsystemContext { /** * Transform from robot center to front-right camera optical center. * Mounted on the front-right bumper corner, angled 30deg outward to the right. - * Position: X=+0.30m forward, Y=-0.25m right, Z=+0.25m up. + * Position: X=+0.30m forward, Y=-0.25m right, Z=+0.2286m up (9in). * Rotation: pitch=-15deg (tilted down), yaw=-30deg (angled right). */ @Builder.Default private final Transform3d frontRightCameraToRobot = new Transform3d( - new Translation3d(0.30, -0.25, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(-30))); + new Translation3d(0.30, -0.25, 0.2286), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(-30))); /** * Transform from robot center to front-left camera optical center. * Mounted on the front-left bumper corner, angled 30deg outward to the left. - * Position: X=+0.30m forward, Y=+0.25m left, Z=+0.25m up. + * Position: X=+0.30m forward, Y=+0.25m left, Z=+0.2286m up (9in). * Rotation: pitch=-15deg (tilted down), yaw=+30deg (angled left). */ @Builder.Default private final Transform3d frontLeftCameraToRobot = new Transform3d( - new Translation3d(0.30, 0.25, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(30))); + new Translation3d(0.30, 0.25, 0.2286), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(30))); + + /** + * Transform from robot center to right-side camera optical center. + * Adjacent to the front-right camera, angled 120deg to the right. + * Position: X=+0.30m forward, Y=-0.25m right, Z=+0.2286m up (9in). + * Rotation: pitch=-15deg (tilted down), yaw=-120deg. + */ + @Builder.Default + private final Transform3d rightSideCameraToRobot = new Transform3d( + new Translation3d(0.30, -0.25, 0.2286), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(-120))); /** - * Transform from robot center to rear camera optical center. - * Mounted centered on the rear of the robot, facing backward. - * Position: X=-0.30m backward, Y=0 centered, Z=+0.25m up. - * Rotation: pitch=-15deg (tilted down), yaw=180deg (facing backward). + * Transform from robot center to left-side camera optical center. + * Adjacent to the front-left camera, angled 120deg to the left. + * Position: X=+0.30m forward, Y=+0.25m left, Z=+0.2286m up (9in). + * Rotation: pitch=-15deg (tilted down), yaw=+120deg. */ @Builder.Default - private final Transform3d rearCameraToRobot = - new Transform3d(new Translation3d(-0.30, 0.0, 0.25), new Rotation3d(0, Math.toRadians(-15), Math.PI)); + private final Transform3d leftSideCameraToRobot = new Transform3d( + new Translation3d(0.30, 0.25, 0.2286), new Rotation3d(0, Math.toRadians(-15), Math.toRadians(120))); /** * Whether to enable simulation features (VisionSystemSim)