Removed duplicate implementations of isFacingSpeaker()

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

@@ -355,6 +355,7 @@ public static class ShooterConstants {
   }
 
   public static final class AutoShootConstants {
+    public static final double speakerEdgeTolerance = Units.inchesToMeters(2.0);
     // (distance, angle)
     // This isn't being used, but the data could still be important at some point
     public static final Point2D[] autoShootArmAngles =

src/main/java/frc/robot/commands/LookAheadSOTM.java

@@ -19,13 +19,13 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.AutoShootConstants;
-import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Swerve;
 import frc.robot.util.AllianceUtil;
+import frc.robot.util.AutoShootMathUtil;
 import frc.robot.util.MovingShotData;
 import frc.robot.util.ShooterState;
 import java.util.function.DoubleSupplier;
@@ -137,27 +137,6 @@ public void initialize() {
     recalculateSetpoint = true;
   }
 
-  private boolean isFacingSpeaker(Pose2d robotPose, Translation2d virtualGoalLocation) {
-    if (Math.abs(robotPose.getRotation().getCos()) == 0.0) {
-      return false;
-    }
-    // y = m(goalx) + b
-    // b = roboty - m(robotx)
-    double slope = Math.tan(robotPose.getRotation().getRadians());
-    if (Double.isInfinite(slope) || Double.isNaN(slope)) {
-      return false;
-    }
-
-    double b = robotPose.getY() - slope * robotPose.getX();
-
-    double yIntersect = slope * virtualGoalLocation.getX() + b;
-
-    double upperBound = virtualGoalLocation.getY() + FieldConstants.speakerWidth / 2;
-    double lowerBound = virtualGoalLocation.getY() - FieldConstants.speakerWidth / 2;
-
-    return yIntersect >= lowerBound && yIntersect <= upperBound;
-  }
-
   private MovingShotData updateCurrentState(
       MovingShotData currentState,
       Pose2d robotPose,
@@ -300,7 +279,8 @@ public void execute() {
 
     SmartDashboard.putNumber("Auto Shoot/Drive Angle Error", driveAngleError.getDegrees());
 
-    boolean facingSpeaker = isFacingSpeaker(robotPose, currentState.virtualGoalLocation);
+    boolean facingSpeaker =
+        AutoShootMathUtil.isFacingSpeaker(robotPose, currentState.virtualGoalLocation);
 
     SmartDashboard.putBoolean("Auto Shoot/Facing Speaker", facingSpeaker);
 

src/main/java/frc/robot/commands/ShootWhileMoving.java

@@ -19,13 +19,13 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.AutoShootConstants;
-import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Swerve;
 import frc.robot.util.AllianceUtil;
+import frc.robot.util.AutoShootMathUtil;
 import frc.robot.util.ShooterState;
 import java.util.function.DoubleSupplier;
 
@@ -125,27 +125,6 @@ public void initialize() {
     swerve.setIgnoreArducam(true);
   }
 
-  private boolean isFacingSpeaker(Pose2d robotPose, Translation2d virtualGoalLocation) {
-    if (Math.abs(robotPose.getRotation().getCos()) == 0.0) {
-      return false;
-    }
-    // y = m(goalx) + b
-    // b = roboty - m(robotx)
-    double slope = Math.tan(robotPose.getRotation().getRadians());
-    if (Double.isInfinite(slope) || Double.isNaN(slope)) {
-      return false;
-    }
-
-    double b = robotPose.getY() - slope * robotPose.getX();
-
-    double yIntersect = slope * virtualGoalLocation.getX() + b;
-
-    double upperBound = virtualGoalLocation.getY() + FieldConstants.speakerWidth / 2;
-    double lowerBound = virtualGoalLocation.getY() - FieldConstants.speakerWidth / 2;
-
-    return yIntersect >= lowerBound && yIntersect <= upperBound;
-  }
-
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
@@ -256,7 +235,7 @@ public void execute() {
 
     SmartDashboard.putNumber("Auto Shoot/Drive Angle Error", driveAngleError.getDegrees());
 
-    boolean facingSpeaker = isFacingSpeaker(robotPose, virtualGoalLocation);
+    boolean facingSpeaker = AutoShootMathUtil.isFacingSpeaker(robotPose, virtualGoalLocation);
 
     SmartDashboard.putBoolean("Auto Shoot/Facing Speaker", facingSpeaker);
 

src/main/java/frc/robot/util/AutoShootMathUtil.java

@@ -0,0 +1,33 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.Constants.AutoShootConstants;
+import frc.robot.Constants.FieldConstants;
+
+public class AutoShootMathUtil {
+  public static boolean isFacingSpeaker(Pose2d robotPose, Translation2d virtualGoalLocation) {
+    if (Math.abs(robotPose.getRotation().getCos()) == 0.0) {
+      return false;
+    }
+    // y = m(goalx) + b
+    // b = roboty - m(robotx)
+    double slope = Math.tan(robotPose.getRotation().getRadians());
+    if (Double.isInfinite(slope) || Double.isNaN(slope)) {
+      return false;
+    }
+
+    double b = robotPose.getY() - slope * robotPose.getX();
+
+    double yIntersect = slope * virtualGoalLocation.getX() + b;
+
+    double upperBound =
+        virtualGoalLocation.getY()
+            + (FieldConstants.speakerWidth - AutoShootConstants.speakerEdgeTolerance) / 2;
+    double lowerBound =
+        virtualGoalLocation.getY()
+            - (FieldConstants.speakerWidth - AutoShootConstants.speakerEdgeTolerance) / 2;
+
+    return yIntersect >= lowerBound && yIntersect <= upperBound;
+  }
+}