Calculate speaker angle concept

src/main/java/frc/robot/Constants.java

@@ -12,6 +12,7 @@
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Distance;
@@ -82,8 +83,9 @@ public static class ShooterConstants {
   }
 
   public static class TurretConstants {
-    public static final int DEVICE_ID_ROT_CONTROL_LEADER = 5;
-    public static final int DEVICE_ID_ROT_CONTROL_FOLLOWER = 6;
+    public static final int DEVICE_ID_TURRET_ROT_CONTROL_LEADER = 5;
+    public static final int DEVICE_ID_TURRET_ROT_CONTROL_FOLLOWER = 6;
+    public static final Translation2d SPEAKER_POSITION = new Translation2d(1, 1);
 
   }
 
@@ -100,5 +102,6 @@ public static class ElevatorClimbConstants {
 
   public static class TrapAmpShooterConstants {
     public static final int DEVICE_ID_TRAP_AMP_SHOOTER = 10;
+
   }
 }

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

@@ -1,7 +1,32 @@
 package frc.robot.subsystems;
 
+import static frc.robot.Constants.TurretConstants.DEVICE_ID_TURRET_ROT_CONTROL_FOLLOWER;
+import static frc.robot.Constants.TurretConstants.DEVICE_ID_TURRET_ROT_CONTROL_LEADER;
+import static frc.robot.Constants.TurretConstants.SPEAKER_POSITION;
+import static java.lang.Math.atan2;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class TurretSubsystem extends SubsystemBase { 
-
+  private final TalonFX rotLeaderMotor = new TalonFX(DEVICE_ID_TURRET_ROT_CONTROL_LEADER);
+  private final TalonFX rotFollowerMotor = new TalonFX(DEVICE_ID_TURRET_ROT_CONTROL_FOLLOWER);\
+  private MotorController leaderMotorController;
+  private MotorController followerMotorController;
+
+  public TurretSubsystem() {
+
+  }
+
+  public double getSpeakerAngle() {
+    Translation2d currentBotPosition = new Translation2d(0, 0);
+    double dx = currentBotPosition.getX() - SPEAKER_POSITION.getX();
+    double dy = currentBotPosition.getY() - SPEAKER_POSITION.getY();
+
+    return atan2(dy, dx);
+  }
+
 }