Sketchy limelight code

Author: PhilBurt627

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -272,7 +272,6 @@ public void periodic() {
       poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
 
        Pose2d visionPose = getRobotVisionPose();
-       Logger.recordOutput("Limelight/VisionPose", visionPose);
     //   if(visionPose != null){
     // //    poseEstimator.addVisionMeasurement(visionPose, sampleTimestamps[i]);
     //   }
@@ -283,7 +282,6 @@ public void periodic() {
   }
 
 
-
   private Pose2d getRobotVisionPose(){
 
     double[] limelightData = LimelightHelpers.getTargetPose_RobotSpace("limelight");
@@ -297,23 +295,60 @@ private Pose2d getRobotVisionPose(){
     FieldCoordinatePose2d actualTagCoord = new FieldCoordinatePose2d(actualTagCoordTemp);
 
     RelativeCoordinatePose2d aprilTagRelativePose = new RelativeCoordinatePose2d(Help.limelightCoordsToWPICoordsPose2d(limelightData));
-    RelativeCoordinatePose2d robotPoseRelativeToTag = new RelativeCoordinatePose2d(new Pose2d(
-      -aprilTagRelativePose.pose.getX(),
-      -aprilTagRelativePose.pose.getY(),
-      aprilTagRelativePose.pose.getRotation().rotateBy(new Rotation2d(Math.PI))
+    RelativeCoordinatePose2d robotPoseTagSpace = new RelativeCoordinatePose2d(new Pose2d(
+      aprilTagRelativePose.pose.getX(),
+      aprilTagRelativePose.pose.getY(),
+      aprilTagRelativePose.pose.getRotation()
     ));
 
-    FieldCoordinatePose2d robotCalculatedPoseFieldSpace = robotPoseRelativeToTag.toFieldSpace(actualTagCoord);
+
+    FieldCoordinatePose2d robotPoseField = robotPoseTagSpace.toFieldSpace(actualTagCoord);
+
+
+    Logger.recordOutput("Limelight/RobotCalculatedPose", robotPoseField.pose);
+    Logger.recordOutput("Limelight/RobotRelativeToAprilTag", robotPoseTagSpace.pose);
 
 
     Logger.recordOutput("Limelight/TagLookupPose", actualTagCoord.pose);
-    Logger.recordOutput("Limelight/CalculatedRobotPose", robotCalculatedPoseFieldSpace.pose);
 
 
-    return robotCalculatedPoseFieldSpace.pose;
+  return null;
 
   }
 
+  // private Pose2d getRobotVisionPose(){
+
+  //   double[] limelightData = LimelightHelpers.getTargetPose_RobotSpace("limelight");
+  //   int tagId = (int)LimelightHelpers.getFiducialID("limelight");
+
+  //   Optional<Pose3d> tagPose3d = AprilTagDataLoader.field.getTagPose(tagId);
+
+  //   if(tagPose3d.isEmpty()) return null;
+
+  //   Pose2d actualTagCoordTemp = new Pose2d(tagPose3d.get().getX(), tagPose3d.get().getY(), tagPose3d.get().getRotation().toRotation2d());
+  //   FieldCoordinatePose2d actualTagCoord = new FieldCoordinatePose2d(actualTagCoordTemp);
+
+  //   RelativeCoordinatePose2d aprilTagRelativePose = new RelativeCoordinatePose2d(Help.limelightCoordsToWPICoordsPose2d(limelightData));
+  //   RelativeCoordinatePose2d robotPoseRelativeToTag = new RelativeCoordinatePose2d(new Pose2d(
+  //     -aprilTagRelativePose.pose.getX(),
+  //     -aprilTagRelativePose.pose.getY(),
+  //     aprilTagRelativePose.pose.getRotation().rotateBy(new Rotation2d(Math.PI))
+  //   ));
+
+  //   Logger.recordOutput("Limelight/RobotRelativeToTag", robotPoseRelativeToTag.pose);
+
+
+  //   FieldCoordinatePose2d robotCalculatedPoseFieldSpace = robotPoseRelativeToTag.toFieldSpace(actualTagCoord);
+
+
+  //   Logger.recordOutput("Limelight/TagLookupPose", actualTagCoord.pose);
+  //   Logger.recordOutput("Limelight/CalculatedRobotPose", robotCalculatedPoseFieldSpace.pose);
+
+
+  //   return robotCalculatedPoseFieldSpace.pose;
+
+  // }
+