Drive Pose is not used as back if no vision reported

src/main/java/frc/robot/config/RobotConfig.java

@@ -54,6 +54,10 @@ public static class DriveConstants {
     public static double slewRateLimiterX = 3;
     public static double slewRateLimiterY = 3;
     public static double slewRateLimiterAngle = 3;
+
+    public static double blueSpeakerX = 0.14;
+    public static double speakerY = 5.53;
+    public static double redSpeakerX = 16.54-0.14;
   }
 
   public static class ArmConstants {

src/main/java/frc/robot/config/RobotConfigPhoenix.java

@@ -40,7 +40,7 @@ public RobotConfigPhoenix() {
                 new Translation3d(-Units.inchesToMeters(10.75), 0, Units.inchesToMeters(8)),
                 new Rotation3d(0, Units.degreesToRadians(-33), Units.degreesToRadians(180)))));
 
-    VisionConstants.visionDistanceOffsetInMeters = -0.2;
+    VisionConstants.visionDistanceOffsetInMeters = 0;
     vision = new VisionSubsystem(cameras, AprilTagFields.k2024Crescendo.loadAprilTagLayoutField());
 
     if (Robot.isSimulation()) {

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -1,5 +1,7 @@
 package frc.robot.subsystems.drive;
 
+import java.util.Optional;
+
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -72,4 +74,8 @@ public default void lockPose() {}
 
   public default void addVisionMeasurement(
       Pose2d robotPose, double timestamp, Matrix<N3, N1> visionMeasurementStdDevs) {}
+
+  public default Optional<Double> getDistanceFromSpeaker() {
+    return Optional.empty();
+  }
 }

src/main/java/frc/robot/subsystems/drive/DriveIO.java

@@ -1,6 +1,9 @@
 package frc.robot.subsystems.drive;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.config.RobotConfig.DriveConstants;
+import frc.robot.util.DevilBotState;
+
 import org.littletonrobotics.junction.AutoLog;
 import swervelib.SwerveDrive;
 
@@ -11,6 +14,7 @@ public static class DriveIOInputs {
     public double poseX = 0.0;
     public double poseY = 0.0;
     public double poseRotInDegrees = 0.0;
+    public double distanceFromSpeaker = 0;
   }
 
   /** Updates the set of loggable inputs. */
@@ -19,6 +23,11 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.poseX = inputs.pose.getTranslation().getX();
     inputs.poseY = inputs.pose.getTranslation().getY();
     inputs.poseRotInDegrees = inputs.pose.getRotation().getDegrees();
+    if(!DevilBotState.isRedAlliance()){
+      inputs.distanceFromSpeaker = Math.sqrt(Math.pow(inputs.poseX - DriveConstants.blueSpeakerX,2)+Math.pow(inputs.poseY - DriveConstants.speakerY,2));
+    } else{
+      inputs.distanceFromSpeaker = Math.sqrt(Math.pow(inputs.poseX - DriveConstants.redSpeakerX,2)+Math.pow(inputs.poseY - DriveConstants.speakerY,2));
+    }                                                           ;
   }
 
   // Other methods for controlling the drive subsystem...

src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java

@@ -15,6 +15,8 @@
 import frc.robot.config.RobotConfig.DriveConstants;
 import frc.robot.util.DevilBotState;
 import java.io.File;
+import java.util.Optional;
+
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import swervelib.SwerveDrive;
@@ -157,4 +159,8 @@ public void addVisionMeasurement(
 
     swerveDrive.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs);
   }
+
+  public Optional<Double> getDistanceFromSpeaker() {
+    return Optional.of(inputs.distanceFromSpeaker);
+  }
 }

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

@@ -184,6 +184,14 @@ public static Optional<Double> getArmAngleToTarget() {
                 RobotConfig.instance.getArmAngleFromDistance(distanceToTarget.get());
 
             return armAngle;
+          } else {
+            distanceToTarget =RobotConfig.drive.getDistanceFromSpeaker();
+            if (distanceToTarget.isPresent()) {
+              Optional<Double> armAngle =
+                  RobotConfig.instance.getArmAngleFromDistance(distanceToTarget.get());
+
+              return armAngle;
+            }
           }
           return Optional.empty();