small pose estimator test

Team334 · Mar 30, 2024 · 685ce9e · 685ce9e
1 parent 7dc236a
commit 685ce9e
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -184,7 +184,7 @@ public static class FieldConstants {
     public static final double SHOOTER_SLOW_THRESHOLD = 2;
 
     public static final double TAG_DISTANCE_THRESHOLD = 3.5;
-    public static final double SINGLE_TAG_DISTANCE_THRESHOLD = 2;
+    public static final double SINGLE_TAG_DISTANCE_THRESHOLD = 1.5;
 
     public static final int SPEAKER_TAG_BLUE = 7;
     public static final int SPEAKER_TAG_RED = 4;

diff --git a/src/main/java/frc/robot/commands/auto/AutonShoot.java b/src/main/java/frc/robot/commands/auto/AutonShoot.java
@@ -39,7 +39,7 @@ public AutonShoot(
       new ParallelCommandGroup(
         new SpinShooter(shooter, ShooterState.SHOOT, true).andThen(new WaitUntilCommand(shooter::isRevved)),
         new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1).onlyIf(() -> !intake.hasNoteAuton()),
-        new AutoAim(swerve, shooter, elevator, leds).withTimeout(1)
+        new AutoAim(swerve, shooter, elevator, leds)
       ),
 
       new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(1),

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -217,7 +217,7 @@ private void updateBotpose() {
       }
 
       // one tag, closer distance, estimated pose is inaccurate
-      else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference <= 3) {
+      else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference <= 1.5) {
         xyStds = 0.65;
       }