Scoring believes drive is ready if drive is aligned OR if distance to…

… goal is below a threshold
team401 · Oct 23, 2024 · d8fd9a3 · d8fd9a3
1 parent d76bebe
commit d8fd9a3
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java
@@ -52,6 +52,8 @@ public class ScoringConstants {
     public static final double aimAngleMarginRotations = Units.degreesToRotations(1);
     public static final double aimAngleVelocityMargin = 2.0;
 
+    public static final double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value
+
     public static final double intakeAngleToleranceRotations = 0.05; // Todo: tune this value
 
     public static final double aimerAmpPositionRot = 0.14;

diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java
@@ -245,7 +245,9 @@ private void prime() {
                                 < aimerAngleTolerance.getValue(distanceToGoal)
                         && Math.abs(aimerInputs.aimVelocityErrorRotPerSec)
                                 < ScoringConstants.aimAngleVelocityMargin;
-        boolean driveReady = driveAlignedSupplier.get();
+        boolean driveReady =
+                (driveAlignedSupplier.get()
+                        || distanceToGoal < ScoringConstants.minDistanceAlignmentNeeded);
         boolean fieldLocationReady = true;
 
         if (!DriverStation.getAlliance().isPresent()) {