Working thing

2026-06-09 00:38:02 -06:00 · 2025-01-11 11:26:10 -07:00
parent 33629e9fdb
commit 84faad46c4
2 changed files with 5 additions and 5 deletions
@@ -106,12 +106,14 @@ public class Vision extends Subsystem {
        }

        lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
+        // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation()));
        // lastVisionPose = new Pose2d(
-        //     new Translation2d(lastVisionPose.getY(), lastVisionPose.getX()), 
-        //     lastVisionPose.getRotation()
+        //     lastVisionPose.getTranslation(), 
+        //     lastPhysOdomPose.getRotation()
        // );

        
+        
        field.setRobotPose(getPose2d());