Pos_estimation_gb

src/main/java/frc/robot/subsystems/Camera.java

@@ -4,6 +4,14 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import java.io.IOException;
+import java.util.Optional;
 
 public class Camera extends SubsystemBase {
 
@@ -18,9 +26,17 @@ public class Camera extends SubsystemBase {
     private long tagId;
     private double distanceCm;
     private double angleOffset;
+    private AprilTagFieldLayout fieldLayout;
+    private Pose2d estimatedPose;
 
     public Camera() {
         table = NetworkTableInstance.getDefault().getTable("datatable");
+        try {
+            fieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025ReefscapeAndyMark.m_resourceFile);
+        } catch (IOException e) {
+            // Handle exception
+            System.err.println("Failed to load AprilTag field layout: " + e.getMessage());
+        }
     }
 
     @Override
@@ -43,6 +59,42 @@ public void periodic() {
         SmartDashboard.putNumber("AprilTag ID", tagId);
         SmartDashboard.putNumber("Distance (cm)", distanceCm);
         SmartDashboard.putNumber("Angle Offset", angleOffset);
+
+        if (lock && tagId > 0) {
+            Optional<Pose3d> tagPose = fieldLayout.getTagPose((int)tagId);
+            if (tagPose.isPresent()) {
+                // Calculate robot position using AprilTag data
+                double focalLength = 320.0; // Example value, needs calibration
+                double tagSize = 0.166; // Tag size in meters
+
+                // Calculate distance using the data from the Raspberry Pi
+                double distance = distanceCm / 100.0; // Convert cm to meters
+
+                // Calculate angle based on the angleOffset from the Pi
+                double angleRad = Math.toRadians(angleOffset);
+
+                // Create the transform
+                Transform3d cameraToTag = new Transform3d(
+                    distance, 0, 0,
+                    new Rotation3d(0, 0, angleRad)
+                );
+
+                // Camera position on robot - adjust for your robot
+                Transform3d robotToCamera = new Transform3d(
+                    0.25, 0, 0.5,
+                    new Rotation3d()
+                );
+
+                // Calculate robot pose
+                Pose3d cameraPose = tagPose.get().transformBy(cameraToTag.inverse());
+                Pose3d robotPose = cameraPose.transformBy(robotToCamera.inverse());
+                estimatedPose = robotPose.toPose2d();
+
+                SmartDashboard.putNumber("Pose X", estimatedPose.getX());
+                SmartDashboard.putNumber("Pose Y", estimatedPose.getY());
+                SmartDashboard.putNumber("Pose Angle", estimatedPose.getRotation().getDegrees());
+            }
+        }
     }
 
     public long getX() {
@@ -76,4 +128,8 @@ public double getDistanceCm() {
     public double getAngleOffset() {
         return angleOffset;
     }
+
+    public Pose2d getEstimatedPose() {
+        return estimatedPose;
+    }
 }