From c4134ef71393ac10b6857531a08f9a325a9f9d85 Mon Sep 17 00:00:00 2001
From: Team 2648 <team2648@coldlightalchemist.com>
Date: Fri, 21 Mar 2025 19:06:45 -0400
Subject: [PATCH] I AM STUPID MAN

---
 src/main/java/frc/robot/subsystems/Manipulator.java | 12 ++++++++++++
 src/main/java/frc/robot/subsystems/Vision.java      |  8 ++++----
 2 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java
index 8cbb6ed..f91beef 100644
--- a/src/main/java/frc/robot/subsystems/Manipulator.java
+++ b/src/main/java/frc/robot/subsystems/Manipulator.java
@@ -73,6 +73,18 @@ public class Manipulator extends SubsystemBase {
             );
         }).unless(() -> !coralBeamBreak.get())
         .until(() -> !coralBeamBreak.get());
+        /*
+        return run(() -> {
+            if(getCoralBeamBreak()) {
+                manipulatorMotor.set(
+                    speed.getAsDouble()
+                );
+            } else {
+                manipulatorMotor.set(
+                    speed.getAsDouble()
+                );
+            }
+        */
     }
 
     public Command retractCommand(DoubleSupplier retractSpeed){
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
index 1f3ae87..8868c38 100644
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -84,9 +84,9 @@ public class Vision{
 
        // System.out.println(gyroBuffer.getSample(timestamp));
 
-        double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-Units.degreesToRadians( camPose[1]) + Units.degreesToRadians(ty));
+        double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-camPose[1] + Units.degreesToRadians(ty));
 
-        Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(Units.degreesToRadians(camPose[2])).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
+        Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
 
         Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
         Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
@@ -97,8 +97,8 @@ public class Vision{
             .getTranslation();
         Pose2d robotPose = new Pose2d(
             fieldToCameraTranslation,
-            new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(Units.degreesToRadians( camPose[2]))))
-            .transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(Units.degreesToRadians(camPose[2]))), Pose2d.kZero));
+            new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(camPose[2])))
+            .transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(camPose[2])), Pose2d.kZero));
 
         robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));