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

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

@@ -29,6 +29,7 @@
 import frc.robot.commands.angler.AimAngler;
 import frc.robot.commands.angler.RunAnglerToReverseLimit;
 import frc.robot.commands.shooter.SetShootingState;
+import frc.robot.commands.turret.AimTurret;
 import frc.robot.commands.turret.RunTurretToFwdLimit;
 import frc.robot.commands.turret.RunTurretToRevLimit;
 import frc.robot.commands.turret.SetTurretAngle;
@@ -63,6 +64,7 @@
 import frc.robot.apriltags.TCPApriltagIo;
 
 import java.io.File;
+import java.util.function.Supplier;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -224,6 +226,8 @@ private void configureBindings() {
                         Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
                         drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
                 }
+
+                turretSubsystem.setDefaultCommand(new AimTurret(turretSubsystem, drivebase::getPose, getShootingState()));
         }
 
         public void putShuffleboardCommands() {

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

@@ -0,0 +1,99 @@
+package frc.robot.commands.turret;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.constants.GameConstants;
+import frc.robot.constants.enums.ShootingState;
+import frc.robot.constants.enums.ShootingState.ShootState;
+import frc.robot.subsystems.TurretSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+import frc.robot.utils.math.TurretCalculations;
+import java.util.function.Supplier;
+
+/**
+ * Default command that aims the turret based on robot pose and shooting state.
+ *
+ */
+public class AimTurret extends LoggableCommand {
+
+    private final TurretSubsystem turret;
+    private final Supplier<Pose2d> poseSupplier;
+    private final ShootingState shootingState;
+    private final boolean isBlueAlliance;
+
+    public AimTurret(TurretSubsystem turret, Supplier<Pose2d> poseSupplier, ShootingState shootingState) {
+        this.turret = turret;
+        this.poseSupplier = poseSupplier;
+        this.shootingState = shootingState;
+        this.isBlueAlliance = isBlue();
+        addRequirements(turret);
+    }
+
+    @Override
+    public void execute() {
+        Pose2d robotPose = poseSupplier.get();
+        ShootState state = shootingState.getShootState();
+        handleState(state, robotPose);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    private void handleState(ShootState state, Pose2d robotPose) {
+        switch (state) {
+            case STOPPED:
+                turret.stopMotors();
+                // set to fixed turret angle at the various fixed states
+            case FIXED:
+                new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_1);
+            case FIXED_2:
+                new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_2);
+                // auto aim to hub
+            case SHOOTING_HUB:
+                AutoShoot(turret, robotPose);
+                // auto aim to shuttle site
+            case SHUTTLING:
+                ShuttleShoot(turret, robotPose);
+        }
+    }
+
+    /**
+     * Moves turret to position automatically calculated based on angle from the
+     * hub.
+     * 
+     * @param turret
+     * @param robotPose
+     */
+    private void AutoShoot(TurretSubsystem turret, Pose2d robotPose) {
+        double targetAngle = TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(),
+                robotPose.getRotation().getRadians(), isBlueAlliance);
+        new SetTurretAngle(turret, targetAngle);
+    }
+
+    /**
+     * Moves turret to shuttling position based on angle from shuttling position
+     * 
+     * @param turret
+     * @param robotPose
+     */
+    private void ShuttleShoot(TurretSubsystem turret, Pose2d robotPose) {
+        double targetAngle = TurretCalculations.calculateTurretShuttleAngle(robotPose.getX(), robotPose.getY(),
+                robotPose.getRotation().getRadians(), isBlueAlliance);
+        new SetTurretAngle(turret, targetAngle);
+    }
+
+    /**
+     * 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/constants/GameConstants.java

@@ -57,6 +57,9 @@ public enum Mode {
     public static final double INITIAL_INTAKE_DEPLOYER_SPEED = 10;
     public static final double INITIAL_INTAKE_RETRACTION_SPEED = -10;
 
+    // Fixed Angles
+    public static final double FIXED_TURRET_ANGLE_1 = 0;
+    public static final double FIXED_TURRET_ANGLE_2 = 0;
 
     //Timeouts
     public static final double SPIN_TIMEOUT = 5;
@@ -135,6 +138,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 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;
     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
 

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

@@ -68,4 +68,45 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do
 
     }
 
+    // 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) {
+
+        // 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 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 - turretPosY, shuttlePosX - turretPosX);
+
+        /*
+         * 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;
+
+    }
+
 }