Janky Boi

chopshop-166 · Feb 22, 2025 · 3bbdc69 · 3bbdc69
1 parent ed4e470
commit 3bbdc69
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -220,9 +220,12 @@ private void periodicMove(final double xSpeed, final double ySpeed, final double
             Transform2d reefLocation = vision.pickBestReefLocation(visionData.targets);
             Transform2d robotToBranch = vision
                     .adjustTranslationForBranch(reefLocation, Branch.LEFT_BRANCH);
-            Logger.recordOutput("Reef Location Transform2d", estimator.getEstimatedPosition().plus(reefLocation));
+            Pose2d robotPose = new Pose2d(estimator.getEstimatedPosition().getTranslation(),
+                    robotToBranch.getRotation().minus(Rotation2d.fromDegrees(180)));
+            Logger.recordOutput("Reef Location Transform2d", robotPose.plus(reefLocation));
             Logger.recordOutput("Robot To Branch Transform2d",
-                    estimator.getEstimatedPosition().plus(robotToBranch));
+                    robotPose.plus(robotToBranch));
+            Logger.recordOutput("Robot Pose", robotPose);
             targetPose = estimator.getEstimatedPosition().plus(robotToBranch);
         }
         if (targetBranch != Branch.NONE) {