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.