RM_Detect_Opposing_Alliance_Zone

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

@@ -173,4 +173,8 @@ public enum Mode {
     public static final double MAX_HUB_DISTANCE = 5.18;
     public static final double MIN_HUB_DISTANCE = 1.42;
     public static final double COMPUTATED_DISTANCE_OFFSET = 2;
+    public static final double RED_SHUTTLING_TARGET_X_POSITION = 13.8;
+    public static final double BLUE_SHUTTLING_TARGET_X_POSITION = 2.8;
+    public static final double SHUTTLING_TARGET_LOWER_Y_POSITION = 1.6;
+    public static final double SHUTTLING_TARGET_HIGHER_Y_POSITION = 6.5;
 }

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

@@ -31,8 +31,6 @@ public class ControllerSubsystem extends SubsystemBase {
             Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero);
     private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION,
             Constants.RED_HUB_Y_POSITION, Rotation2d.kZero);
-    private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero);
-
     private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX";
     private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY";
     private static final String MANUAL_POSE_R_KEY = "controller/ManualPoseRotation";
@@ -57,9 +55,9 @@ public class ControllerSubsystem extends SubsystemBase {
             14.0);
     private static final PoseControlProfile RED_HUB_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 32.0, 230.0,
             14.0);
-    private static final PoseControlProfile RED_SHUTTLE_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 16.0, 90.0,
+    private static final PoseControlProfile RED_SHUTTLE_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 20.0, 90.0,
             -14.0);
-    private static final PoseControlProfile BLUE_SHUTTLE_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 16.0, 90.0,
+    private static final PoseControlProfile BLUE_SHUTTLE_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 20.0, 90.0,
             -14.0);
 
     private final SwerveSubsystem drivebase;
@@ -202,9 +200,9 @@ private void useShotTargets(ShotTargets shotTargets) {
 
     private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose) {
         double computedDistanceMeters = calculateDistanceMeters(state, robotPose, profile.targetPose);
-        double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile);
-        double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
-        double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile);
+        double anglerAngleDegrees = calculateAnglerAngleDegrees(state, computedDistanceMeters, profile);
+        double shooterVelocity = calculateShooterVelocity(state, computedDistanceMeters, profile);
+        double turretAngleDegrees = calculateTurretAngleDegrees(state, robotPose, profile);
         return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true,
                 true);
     }
@@ -244,8 +242,8 @@ private double calculateFlightTime(double computedDistanceMeters) {
         return 0.208*computedDistanceMeters + 0.647;
     }
 
-    private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) {
-        if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
+    private double calculateAnglerAngleDegrees(ShootState state, double computedDistanceMeters, PoseControlProfile profile) {
+        if (state == ShootState.SHOOTING_HUB) {
             double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET;
             return 0.169 * distance * distance
                     - 1.73 * distance
@@ -254,21 +252,29 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo
         return profile.defaultAnglerAngleDegrees;
     }
 
-    private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
-        if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
-            double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET;
+    private double calculateShooterVelocity(ShootState state, double computedDistanceMeters, PoseControlProfile profile) {
+        double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET;
+        if (state == ShootState.SHOOTING_HUB) {
             return (8.46 * distance * distance
                     - 237 * distance
                     - 1380);
+        }else if(state == ShootState.SHUTTLING){
+            //Essentially random values needs to be tuned
+            return (-0.31 * distance * distance
+                    -30.7 * distance
+                    - 3838.7);
         }
         return profile.defaultShooterVelocityRpm;
     }
 
-    private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) {
-        return Math.floor(
-                Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(),
-                        robotPose.getRotation().getRadians(),
-                        Robot.allianceColor().get() == DriverStation.Alliance.Blue)));
+    private double calculateTurretAngleDegrees(ShootState state, Pose2d robotPose, PoseControlProfile profile) {
+        if(state == ShootState.SHOOTING_HUB || state == ShootState.SHUTTLING){
+            return Math.floor(
+                    Math.toDegrees(TurretCalculations.calculateTurretAngle(state,robotPose.getX(), robotPose.getY(),
+                            robotPose.getRotation().getRadians(),
+                            Robot.allianceColor().get() == DriverStation.Alliance.Blue)));
+        }
+            return profile.defaultTurretAngleDegrees;
     }
 
     // Getters for all the subsystems to set posistion.
@@ -348,4 +354,11 @@ private PoseControlProfile(
             this.defaultTurretAngleDegrees = defaultTurretAngleDegrees;
         }
     }
+
+    public boolean isOnOpposingAllianceSide() {
+        return switch (Robot.allianceColor().get()) {
+            case Red -> getRobotPose().getX() < 4.6116;
+            case Blue -> getRobotPose().getX() > 11.9014;
+        };
+    }
 }

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

@@ -6,7 +6,9 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.constants.Constants;
 import frc.robot.constants.GameConstants;
+import frc.robot.constants.enums.ShootingState.ShootState;
 
 public class TurretCalculations {
 
@@ -32,7 +34,7 @@ public class TurretCalculations {
     // all positions are in meters
     // robotRotation is in radians
     // Returns - turret angle in radians
-    public static double calculateTurretAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) {
+    public static double calculateTurretAngle(ShootState state, double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) {
 
         // Get turret offset from robot center
         double turretOffsetX = GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
@@ -48,25 +50,30 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do
 
         double hubPosX;
         double hubPosY;
-
+    if(state == ShootState.SHOOTING_HUB) {
         if (isBlueAlliance) {
             // hub position determined by which alliance robot is on
-            hubPosX = GameConstants.BLUE_HUB_X_POSITION;
-            hubPosY = GameConstants.BLUE_HUB_Y_POSITION;
+            hubPosX = Constants.BLUE_HUB_X_POSITION;
+            hubPosY = Constants.BLUE_HUB_Y_POSITION;
         } else {
-            hubPosX = GameConstants.RED_HUB_X_POSITION;
-            hubPosY = GameConstants.RED_HUB_Y_POSITION;
+            hubPosX = Constants.RED_HUB_X_POSITION;
+            hubPosY = Constants.RED_HUB_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 hub
-         * and the x-distance between the robot and the hub, 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(hubPosY - turretPosY, hubPosX - turretPosX);
+        }else{
+            //Sets the hubPosX to be either on the blue or red side 
+            //Sets the hubPoseY to be lower or upper depending on the Y position of the robot
+            hubPosX = isBlueAlliance ? Constants.BLUE_HUB_X_POSITION : Constants.RED_HUB_X_POSITION;
+            hubPosY = robotPosY > (Constants.FIELD_WIDTH / 2) ? Constants.SHUTTLING_TARGET_HIGHER_Y_POSITION : Constants.SHUTTLING_TARGET_LOWER_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 hub
+             * and the x-distance between the robot and the hub, 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(hubPosY - turretPosY, hubPosX - turretPosX);
 
         /*
          * Adjusts the pan angle to account for the robot's current rotation. We subtract the