From 04586542812fdbef75887aa4f0c46c2227045c57 Mon Sep 17 00:00:00 2001
From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com>
Date: Mon, 10 Mar 2025 13:13:48 -0600
Subject: [PATCH] Fix vision noise?

---
 .../java/frc4388/robot/subsystems/Vision.java | 41 +++++++++----------
 1 file changed, 19 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java
index 47ca4f8..1b9ac39 100644
--- a/src/main/java/frc4388/robot/subsystems/Vision.java
+++ b/src/main/java/frc4388/robot/subsystems/Vision.java
@@ -111,20 +111,12 @@ public class Vision extends Subsystem {
         // cameras[0].
     }
 
-    public double rotations = 0;
+    public int rotations = 0;
 
     public void resetRotations(){
         rotations = 0;
     }
 
-    public void addRotation(){
-        rotations += 180;
-    }
-
-    public void subtractRotation(){
-        rotations -= 180;
-    }
-
     private Instant lastVisionTime = null;
 
 
@@ -178,27 +170,32 @@ public class Vision extends Subsystem {
 
         lastLatency = latency / cams;
 
-        if(isTagProcessed){
+        if(isTagProcessed || true){
             Instant now = Instant.now();
 
-            double curAngle = Yaw/cams;
+            // double curAngle = (Yaw/cams);
+            double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise
 
-            // if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) > 1){            
-            //     double lastAngle = lastVisionPose.getRotation().getDegrees();
 
-            //     if(lastAngle - curAngle >= 80){
-            //         addRotation();
-            //     }else if(lastAngle - curAngle <= -80){
-            //         subtractRotation();
-            //     }
-            // }
+            if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){            
+                double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360;
+
+                if(diff > 180){
+                    rotations -= 1;
+                }else if(diff < -180){
+                    rotations += 1;
+                }
+            }
 
 
 
-            // SmartDashboard.putNumber("curAngle", curAngle);
-            // SmartDashboard.putNumber("Rotations", rotations);
             
-            lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
+            // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle + rotations*360));
+            lastVisionPose = new Pose2d(0, 0, Rotation2d.fromDegrees(curAngle + rotations*360));
+
+            SmartDashboard.putNumber("curAngle", lastVisionPose.getRotation().getRadians());
+            SmartDashboard.putNumber("Rotations", rotations);
+
             lastVisionTime = now;
         }
     }