Implemented auto aim turret code into the subsystem based on shooting state

src/main/java/frc/robot/RobotContainer.java

@@ -72,6 +72,7 @@
 import frc.robot.apriltags.ApriltagReading;
 
 import java.io.File;
+import java.util.function.Supplier;
 
 import choreo.auto.AutoFactory;
 import choreo.auto.AutoRoutine;
@@ -331,6 +332,7 @@ private void configureBindings() {
                         Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
                         drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
                 }
+
         }
 
         public void putShuffleboardCommands() {

src/main/java/frc/robot/commands/turret/DefaultTurretControl.java

@@ -17,7 +17,7 @@ public DefaultTurretControl(TurretSubsystem turretSubsystem, ControllerSubsystem
 
     @Override
     public void execute() {
-        turretSubsystem.setAngle(controllerSubsystem.getTargetTurretAngleDegrees());
+        new SetTurretAngle(turretSubsystem, controllerSubsystem.getTargetTurretAngleDegrees());
     }
 
     @Override

src/main/java/frc/robot/commands/turret/SetTurretAngle.java

@@ -1,30 +1,39 @@
 package frc.robot.commands.turret;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.constants.GameConstants;
 import frc.robot.subsystems.TurretSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 
 /**
  * Runs the turret to the target angle
  */
 public class SetTurretAngle extends LoggableCommand {
-    private final TurretSubsystem turret;
-    private double targetAngle;
-
-
+        private final TurretSubsystem turret;
+        private double targetAngle;
+        private boolean turretInRange;
+
+        public SetTurretAngle(TurretSubsystem turret, double targetAngle) {
+            this.turret = turret;
+            this.targetAngle = targetAngle;
+            this.turretInRange = false;
+            addRequirements(turret);
+        }
 
-    public SetTurretAngle(TurretSubsystem turret, double targetAngle) {
-        this.turret = turret;
-        this.targetAngle = targetAngle;
-        addRequirements(turret);
-    }
-
-    @Override
-    public void initialize() {
-    }
-
-    @Override
-    public void execute() {
-        turret.setAngle(targetAngle);
+        @Override
+        public void initialize() {
+            if ((targetAngle > GameConstants.TURRET_LEFT_ANGLE + 5) &&
+             (targetAngle < GameConstants.TURRET_RIGHT_ANGLE - 5)) {
+                turretInRange = true;
+            }
+        }
+
+        @Override
+        public void execute() {
+            SmartDashboard.putBoolean("WITHIN_SHOOTING_RANGE", turretInRange);
+        if (turretInRange) {
+            turret.setAngle(targetAngle);
+        }
     }
 
     @Override

src/main/java/frc/robot/constants/GameConstants.java

@@ -57,6 +57,9 @@ public enum Mode {
     public static final double INITIAL_INTAKE_DEPLOYER_SPEED = 0.25;
     public static final double INITIAL_INTAKE_RETRACTION_SPEED = -0.1;
 
+    // Fixed Angles
+    public static final double FIXED_TURRET_ANGLE_1 = 0;
+    public static final double FIXED_TURRET_ANGLE_2 = 0;
 
     //Diags
     public static final double HOPPER_DIAGS_ENCODER = 1;
@@ -147,8 +150,10 @@ public enum Mode {
     public static final double BLUE_HUB_Y_POSITION = 4.0345;
     public static final double RED_HUB_X_POSITION = 11.9154;
     public static final double RED_HUB_Y_POSITION = 4.0345;
-    public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware
-    public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware
+    public static final double BLUE_SHUTTLE_X_POSITION = 3;
+    public static final double BLUE_SHUTTLE_Y_POSITION = 4.0345;
+    public static final double RED_SHUTTLE_X_POSITION = 3;
+    public static final double RED_SHUTTLE_Y_POSITION = 4.0345;
 
     // Shift timings
     public static final int SHIFT_1_START = 130;

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

@@ -5,13 +5,15 @@
 import frc.robot.RobotContainer;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.enums.ShootingState;
 import frc.robot.constants.enums.ShootingState.ShootState;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.math.TurretCalculations;
 
 public class ControllerSubsystem extends SubsystemBase {
 
@@ -36,9 +38,9 @@ public class ControllerSubsystem extends SubsystemBase {
     private static final ShotTargets STOPPED_TARGETS =
             new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, false);
     private static final ShotTargets FIXED_TARGETS =
-            new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true);
+            new ShotTargets(10.0, 120.0, 15.0, 0.0, true, true);
     private static final ShotTargets FIXED_2_TARGETS =
-            new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true);
+            new ShotTargets(22.0, 180.0, -15.0, 0.0, true, true);
 
     // Placeholder pose-driven profiles.
     private static final PoseControlProfile HUB_PROFILE =
@@ -179,11 +181,24 @@ private double calculateShooterVelocity(double computedDistanceMeters, PoseContr
         return profile.defaultShooterVelocityRpm;
     }
 
+    // calculates turret angle from calculations
     private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) {
-        // TODO: Replace with distance turret angle calculation.
-        return profile.defaultTurretAngleDegrees;
-    }
+
+        double targetAngle = 0;
+
+        if (getCurrentShootState() == ShootState.SHOOTING_HUB) {
+
+            targetAngle = TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(),
+                robotPose.getRotation().getRadians(), isBlue());
 
+        } else if (getCurrentShootState() == ShootState.SHUTTLING) {
+
+            targetAngle = TurretCalculations.calculateTurretShuttleAngle(robotPose.getX(), robotPose.getY(),
+                robotPose.getRotation().getRadians(), isBlue());
+        }
+        return targetAngle;
+    }
+
     //Getters for all the subsystems to set posistion.
     public double getTargetAnglerAngleDegrees() {
         return activeTargets.anglerAngleDegrees;
@@ -261,4 +276,16 @@ private PoseControlProfile(
             this.defaultTurretAngleDegrees = defaultTurretAngleDegrees;
         }
     }
+
+    /**
+     * Checks if the alliance is blue, defaults to false if alliance isn't
+     * available.
+     *
+     * @return true if the blue alliance, false if red. Defaults to false if none is
+     *         available.
+     */
+    private boolean isBlue() {
+        var alliance = DriverStation.getAlliance();
+        return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Blue : false;
+    }
 }

src/main/java/frc/robot/utils/math/LaunchCalculations.java

@@ -22,11 +22,6 @@ public static double calculateShooterVelocity(double robotPosX, double robotPosY
         // Target value -- what we're trying to find
         double shooterVelocity; // Initial velocity of the shooter -- related to flywheel speed
 
-        // calculates the position of the turret with respect to the origin using the robot center 
-        // and the constant distance between the robot center and the turret.
-        double turretPosX = robotPosX + GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-        double turretPosY = robotPosY + GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-
         // Given values -- from Constants file
         double hubHeight = GameConstants.HUB_HEIGHT; // height of the top of the hub
         double shooterHeight = GameConstants.SHOOTER_HEIGHT; // height of shooter
@@ -47,8 +42,8 @@ public static double calculateShooterVelocity(double robotPosX, double robotPosY
         }
 
         // Compute distance between hub and robot with Pythagorean Theorem
-        deltaDistance = Math.sqrt(Math.pow(hubPosY - turretPosY, 2) 
-        + Math.pow(hubPosX - turretPosX, 2));
+        deltaDistance = Math.sqrt(Math.pow(hubPosY - robotPosY, 2) 
+        + Math.pow(hubPosX - robotPosX, 2));
 
         // Compute the change in height between the shooter and the hub
         deltaHeight = hubHeight - shooterHeight;

src/main/java/frc/robot/utils/math/TurretCalculations.java

@@ -30,11 +30,6 @@ public class TurretCalculations {
     // robotRotation is the angle between the horizontal (by the alliance side chute) and the robot
     public static double calculateTurretAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) {
 
-        // calculates the position of the turret with respect to the origin using the robot center 
-        // and the constant distance between the robot center and the turret.
-        double turretPosX = robotPosX + GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-        double turretPosY = robotPosY + GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-
         double hubPosX;
         double hubPosY;
 
@@ -55,7 +50,43 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do
          * of the input numbers.
          * 
          */
-        double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX);
+        double panAngleUnadjusted = Math.atan2(hubPosY - robotPosY, hubPosX - robotPosX);
+
+        /*
+         * Adjusts the pan angle to account for the robot's current rotation. We subtract the
+         * angle of the robot's rotation from the unadjusted angle of the turret to find the 
+         * pan angle, which is the proper angle of the turret adjusted for the robot's rotation.
+         */
+        double panAngle = panAngleUnadjusted - robotRotation;
+
+        return panAngle;
+
+    }
+
+    // shuttlePosX and shuttlePosY are given values from the constants file -- gives the x and y positions of the shuttle site (in meters)
+    public static double calculateTurretShuttleAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) {
+
+        double shuttlePosX;
+        double shuttlePosY;
+
+        if (isBlueAlliance) {
+            // shuttle position determined by which alliance robot is on
+            shuttlePosX = GameConstants.BLUE_SHUTTLE_X_POSITION;
+            shuttlePosY = GameConstants.BLUE_SHUTTLE_Y_POSITION;
+        } else {
+            shuttlePosX = GameConstants.RED_SHUTTLE_X_POSITION;
+            shuttlePosY = GameConstants.RED_SHUTTLE_Y_POSITION;
+        }
+
+        /*
+         * This finds the unadjusted pan angle (assuming there is no robot rotation) using
+         * trigonometry. We take the arctangent of the y-distance beween the robot and the shuttle position
+         * and the x-distance between the robot and the shuttle position, giving us the unadjusted pan
+         * angle. The function atan2 ensures the sign of the angle is correct based on the signs
+         * of the input numbers.
+         * 
+         */
+        double panAngleUnadjusted = Math.atan2(shuttlePosY - robotPosY, shuttlePosX - robotPosX);
 
         /*
          * Adjusts the pan angle to account for the robot's current rotation. We subtract the