From cc6c455475aa37b3ad2b26d1e97feed30e7f1918 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Fri, 18 Mar 2022 23:54:16 -0600
Subject: [PATCH] aarav good math

---
 .../robot/commands/ShooterCommands/AimToCenter.java  | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
index 38a8888..083eec7 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
@@ -60,6 +60,18 @@ public class AimToCenter extends CommandBase {
     return angle;
   }
 
+  public static double aaravAngleToCenter(double x, double y, double gyro) {
+    double exp = Math.toDegrees(Math.atan(y/x)) - gyro;
+    if (x > 0) { return exp; }
+    if (x < 0) { return (180 + exp); }
+
+    if (x == 0 && y > 0) { return (90 - gyro); }
+    if (x == 0 && y < 0) { return (-1 * gyro); }
+
+    System.out.println("Invalid case.");
+    return 0;
+  }
+
   /**
    * Checks if in deadzone.
    * @param angle Angle to check.