Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025

src/main/java/frc/robot/subsystems/Limelight3.java

@@ -69,6 +69,17 @@ public class Limelight3 extends SubsystemBase {
   public void Forme(){
     pipeline.setNumber(0);
   }
+  public double Calcule(double x1, double x2, double y1, double y2, double angle)
+  {
+    if (x1 > 4)
+    {
+      return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90;
+    }
+    else
+    {
+        return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90;
+    }
+  }
   @Override
   public void periodic() {
     // This method will be called once per scheduler run