should reset for reset pose

Team334 · Mar 16, 2024 · 495d42c · 495d42c
1 parent e6a2c39
commit 495d42c
Showing 1 changed file with 11 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -28,10 +28,12 @@
 public class VisionSubsystem extends SubsystemBase {
   private final LimelightHelper _limelight = LimelightHelper.getInstance();
 
-  private final MedianFilter _xFilter = new MedianFilter(20);
-  private final MedianFilter _yFilter = new MedianFilter(20);
+  private final MedianFilter _xFilter = new MedianFilter(10); // TODO: was changed
+  private final MedianFilter _yFilter = new MedianFilter(10);
   private final MedianFilter _yawFilter = new MedianFilter(20);
 
+  private boolean _shouldResetPose = true;
+
   // private double[] _botpose = new double[6];
 
   /** Creates a new VisionSubsystem. */
@@ -113,16 +115,17 @@ public Optional<Pose2d> resetPose() {
     if (botpose.isEmpty()) return Optional.empty();
     // if (botpose.get()[7] < 2) return Optional.empty(); // tag count?
 
-    // center speaker tag
-    if (isApriltagVisible(UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_TAG_RED : FieldConstants.SPEAKER_TAG_BLUE)) {
-      return getBotpose();
-    }
+    int centerTag = UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_TAG_RED : FieldConstants.SPEAKER_TAG_BLUE;
+    int offsetTag = UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_TAG_RED_OFF : FieldConstants.SPEAKER_TAG_BLUE_OFF; 
 
-    // off-center speaker tag
-    if (isApriltagVisible(UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_TAG_RED_OFF : FieldConstants.SPEAKER_TAG_BLUE_OFF)) {
+    if (isApriltagVisible(centerTag) || isApriltagVisible(offsetTag)) {
+      if (!_shouldResetPose) return Optional.empty();
+      _shouldResetPose = false;
       return getBotpose();
     }
 
+    _shouldResetPose = true;
+
     return Optional.empty();
   }