switched to shot vector in utilfuncs

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

@@ -121,8 +121,7 @@ public RobotContainer() {
 
     configureBindings();
 
-    UtilFuncs.ShotDistance(_swerveSubsystem::speakerShotDistance);
-    UtilFuncs.ShootFast(_swerveSubsystem::speakerDistance);
+    UtilFuncs.ShotVector(_swerveSubsystem::shotVector);
 
     _autonChooser = AutoBuilder.buildAutoChooser();
 

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

@@ -57,7 +57,7 @@ public void periodic() {
    * Get the calculated height needed to aim at the speaker.
    */
   public double speakerHeight() {
-    double distance = UtilFuncs.ShotDistance();
+    double distance = UtilFuncs.ShotVector().getNorm();
 
     return 0;
   }

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

@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.FeedForward;
+import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.PID;
 import frc.robot.Constants.Speeds;
 import frc.robot.utils.UtilFuncs;
@@ -83,7 +84,7 @@ public void periodic() {
    * Get the calculated angle needed to aim at the speaker.
    */
   public double speakerAngle() {
-    double distance = UtilFuncs.ShotDistance();
+    double distance = UtilFuncs.ShotVector().getNorm();
 
     return 0;
   }
@@ -132,7 +133,7 @@ public void stopAngle() {
   public void setShooterState(ShooterState state) {
     switch (state) {
       case SHOOT:
-        if (UtilFuncs.ShootFast()) { 
+        if (UtilFuncs.ShotVector().getNorm() > FieldConstants.SHOOTER_SLOW_THRESHOLD) { 
           spinShooter(Speeds.SHOOTER_FAST_SPIN_SPEED); 
         } else {
           spinShooter(Speeds.SHOOTER_SLOW_SPIN_SPEED);

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

@@ -164,7 +164,7 @@ public void periodic() {
     SmartDashboard.putNumber("Gyro RAW", getHeadingRaw().getDegrees());
     SmartDashboard.putBoolean("Field Oriented", fieldOriented);
     SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime());
-    SmartDashboard.putNumber("Speaker Distance", speakerDistance());
+    SmartDashboard.putNumber("Shot Distance", shotVector().getNorm());
 
     // Update the bot's pose
     _estimator.update(getHeadingRaw(), new SwerveModulePosition[]{
@@ -319,25 +319,15 @@ public Rotation2d getHeadingRaw() {
    * 
    * (could be modified to shoot while moving)
    */
-  public double speakerShotDistance() {
-    return speakerDistance();
-  }
-
-  /**
-   * Get the distance of the chassis from the speaker shot point.
-   * 
-   * @return Norm of the distance vector.
-   */
-  public double speakerDistance() {
-    // Pose3d speakerPose = UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_POSE_RED : FieldConstants.SPEAKER_POSE_BLUE;
+  public Translation2d shotVector() {
     Pose3d speakerPose = UtilFuncs.GetSpeakerPose();
 
     Translation2d speakerTranslation = new Translation2d(speakerPose.getX(), speakerPose.getY());
     Translation2d botTranslation = getPose().getTranslation();
 
     Translation2d distanceVec = speakerTranslation.minus(botTranslation);
 
-    return distanceVec.getNorm();
+    return distanceVec;
   }
 
   /**

src/main/java/frc/robot/utils/UtilFuncs.java

@@ -1,27 +1,23 @@
 /* Copyright (C) 2024 Team 334. All Rights Reserved.*/
 package frc.robot.utils;
 
-import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
 import edu.wpi.first.math.geometry.Pose3d;
 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.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.utils.helpers.AllianceHelper;
 
 /** Any utility functions are here. */
 public final class UtilFuncs {
-  private static DoubleSupplier _shotDistanceSupplier;
-  private static DoubleSupplier _distanceSupplier;
-
   private static AprilTagFieldLayout _field;
+  private static Supplier<Translation2d> _shotVectorSupplier;
 
   /**
    * Loads the AprilTag field.
@@ -51,30 +47,20 @@ public static Pose3d GetSpeakerPose() {
     );
   }
 
-  public static void ShotDistance(DoubleSupplier distance) {
-    _shotDistanceSupplier = distance;
-  }
-
-  public static double ShotDistance() {
-    return _shotDistanceSupplier.getAsDouble();
-  }
-
   /**
-   * Supply the ShootFast function with a distance supplier.
+   * Supply UtilFuncs with a shot vector supplier.
    * 
-   * @param distance Distance supplier - chassis distance from speaker shot point.
+   * @param shotVectorSupplier A supplier that returns the vector from the chassis to the shot point.
    */
-  public static void ShootFast(DoubleSupplier distance) {
-    _distanceSupplier = distance;
+  public static void ShotVector(Supplier<Translation2d> shotVectorSupplier) {
+    _shotVectorSupplier = shotVectorSupplier;
   }
 
   /**
-   * Whether to shoot fast based on distance of bot from speaker shot point.
+   * The vector between the chassis pose and the specific shot point.
    */
-  public static boolean ShootFast() {
-    if (_distanceSupplier.getAsDouble() > FieldConstants.SHOOTER_SLOW_THRESHOLD) return true;
-
-    return false;
+  public static Translation2d ShotVector() {
+    return _shotVectorSupplier.get();
   }
 
   /**