vision subsystem cleanup

src/main/java/frc/robot/Constants.java

@@ -189,8 +189,8 @@ 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 TAG_DISTANCE_RESET_THRESHOLD = 1;
+    public static final double CLOSE_TAG_DISTANCE_THRESHOLD = 3;
+    public static final double FAR_TAG_DISTANCE_THRESHOLD = 4;
 
     public static final int SPEAKER_TAG_BLUE = 7;
     public static final int SPEAKER_TAG_RED = 4;

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

@@ -155,10 +155,12 @@ private void configureBindings() {
 
     // TESTING ONLY!!!
     _driveController.circle().onTrue(Commands.runOnce(() -> {
-      Optional<Pose2d> pose = _visionSubsystem.getBotpose();
-      if (pose.isPresent()) _swerveSubsystem.resetPose(
-        new Pose2d(pose.get().getX(), pose.get().getY(), _swerveSubsystem.getHeading())
-      );
+      Optional<double[]> pose = _visionSubsystem.getBotposeBlue();
+
+      if (pose.isPresent()) {
+        Pose2d botpose = UtilFuncs.ToPose(pose.get());
+        _swerveSubsystem.resetPose(new Pose2d(botpose.getX(), botpose.getY(), _swerveSubsystem.getHeading()));
+      }
     }, _swerveSubsystem));
 
     _driveController.options().whileTrue(new NoteAlign(

src/main/java/frc/robot/commands/auto/AutoAim.java

@@ -6,9 +6,6 @@
 
 import java.util.function.DoubleSupplier;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.commands.elevator.SetElevator;
 import frc.robot.commands.shooter.SetShooter;

src/main/java/frc/robot/commands/shooter/OperateShooter.java

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.ShooterSubsystem.ShooterState;
 
 public class OperateShooter extends Command {
   private final ShooterSubsystem _shooter;

src/main/java/frc/robot/commands/shooter/SpinShooter.java

@@ -1,9 +1,7 @@
 /* Copyright (C) 2024 Team 334. All Rights Reserved.*/
 package frc.robot.commands.shooter;
 
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.commands.auto.AutonShoot;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShooterState;
 

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

@@ -8,8 +8,6 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;

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

@@ -1,8 +1,6 @@
 /* Copyright (C) 2024 Team 334. All Rights Reserved.*/
 package frc.robot.subsystems;
 
-import java.beans.beancontext.BeanContextProxy;
-import java.sql.Driver;
 import java.util.Optional;
 
 import com.ctre.phoenix6.Orchestra;
@@ -24,20 +22,13 @@
 import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.FieldConstants;
-import frc.robot.Constants.Physical;
-import frc.robot.Constants.Presets;
-import frc.robot.subsystems.ShooterSubsystem.ShooterState;
-import frc.robot.utils.BNO055;
 import frc.robot.utils.SwerveModule;
 import frc.robot.utils.UtilFuncs;
 
@@ -192,7 +183,7 @@ private void updateBotpose() {
       _backLeft.getPosition()
     });
 
-    Optional<double[]> visionBotpose = _visionSubsystem.getValidNTEntry();
+    Optional<double[]> visionBotpose = _visionSubsystem.getBotposeBlue();
 
     SmartDashboard.putBoolean("SEES TAG", false);
 
@@ -202,46 +193,30 @@ private void updateBotpose() {
       double[] botpose = visionBotpose.get();
 
       double tagDistance = botpose[9];
-      double tagCount = botpose[7];
 
       SmartDashboard.putNumber("DISTANCE TAGG", tagDistance);
-      SmartDashboard.putNumber("TAG COUNNTTT", tagCount);
 
-      double xyStds = 0.9;
+      double xyStds;
       double yawStd = 9999999;
 
-      // really good, low std devs
-      // if (tagCount >= 2) {
-      //   xyStds = 0.5;
-      // }
-
-      // one tag, good distance
-      if (tagDistance <= FieldConstants.TAG_DISTANCE_THRESHOLD) {
-        xyStds = 0.7;
+      // good distance
+      if (tagDistance <= FieldConstants.CLOSE_TAG_DISTANCE_THRESHOLD) {
+        xyStds = 0.65;
       }
 
-      // on tag, mid distance
-      else if (tagDistance <= 5) {
-        xyStds = 1;
+      // mid distance
+      else if (tagDistance <= FieldConstants.FAR_TAG_DISTANCE_THRESHOLD) {
+        xyStds = 0.95;
       }
 
       else {
         return;
       }
 
-      double botposeX = botpose[0];
-      double botposeY = botpose[1];
-      double botposeYaw = botpose[5];
-
       SmartDashboard.putNumber("STD", xyStds);
-
       System.out.println("UPDATING POSE");
 
-      _estimator.addVisionMeasurement(new Pose2d(
-        botposeX,
-        botposeY,
-        Rotation2d.fromDegrees(botposeYaw)
-      ), _visionSubsystem.getLatency(), VecBuilder.fill(xyStds, xyStds, yawStd));
+      _estimator.addVisionMeasurement(UtilFuncs.ToPose(botpose), _visionSubsystem.getLatency(), VecBuilder.fill(xyStds, xyStds, yawStd));
     }
   }
 
@@ -385,35 +360,6 @@ public Translation2d shotVector() {
     return distanceVec;
   }
 
-  /**
-   * Get the setpoint x and y angles as well as elevater height for auto-aim.
-   * 
-   * @return [xSpeakerAngle, ySpeakerAngle, elevatorHeight]
-   */
-  public double[] speakerSetpoints() {
-    double xSpeakerAngle;
-    double ySpeakerAngle;
-
-    // Pose3d speakerPose = UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_POSE_RED : FieldConstants.SPEAKER_POSE_BLUE;
-    Pose3d speakerPose = UtilFuncs.GetSpeakerPose();
-
-    Translation2d speakerTranslation = new Translation2d(speakerPose.getX(), speakerPose.getY());
-    Translation2d botTranslation = getPose().getTranslation();
-
-    Translation2d distanceVec = speakerTranslation.minus(botTranslation);
-
-    double elevatorHeight = Physical.ELEVATOR_MAX_SHOOT_HEIGHT + (distanceVec.getNorm() * Presets.ELEVATOR_HEIGHT_RATE); // TODO: get values and test
-
-    xSpeakerAngle = MathUtil.inputModulus(distanceVec.getAngle().getDegrees(), -180, 180);
-
-    double zDifference = speakerPose.getZ() - elevatorHeight;
-    ySpeakerAngle = Math.toDegrees(Math.atan(zDifference / distanceVec.getNorm()));
-
-    double[] offsets = {xSpeakerAngle, ySpeakerAngle, elevatorHeight - Physical.ELEVATOR_LOWEST_HEIGHT};
-
-    return offsets;
-  }
-
   /**
    * Get the calculated heading to aim the chassis at the speaker shot point.
    */

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

@@ -23,7 +23,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.FieldConstants;
-import frc.robot.utils.NoteSort;
 import frc.robot.utils.UtilFuncs;
 import frc.robot.utils.helpers.LimelightHelper;
 
@@ -36,18 +35,8 @@ public class VisionSubsystem extends SubsystemBase {
   private final LimelightHelper _main = new LimelightHelper("limelight-main");
   private final LimelightHelper _intake = new LimelightHelper("limelight-intake");
 
-
-  private final MedianFilter _xFilter = new MedianFilter(20); // TODO: change?
-  private final MedianFilter _yFilter = new MedianFilter(20);
-  private final MedianFilter _yawFilter = new MedianFilter(20);
-
-  private boolean _shouldResetPose = true;
-
-  // private double[] _botpose = new double[6];
-
   /** Creates a new VisionSubsystem. */
-  public VisionSubsystem() {
-  }
+  public VisionSubsystem() {}
 
   @Override
   public void periodic() {
@@ -60,7 +49,7 @@ public void periodic() {
   }
 
   /**
-   * Returns the latency from the last time data was sent from the limelight. This
+   * Returns the latency from the last time data was sent from the main limelight. This
    * should be used in the pose estimator.
    */
   public double getLatency() {
@@ -70,9 +59,9 @@ public double getLatency() {
     return Timer.getFPGATimestamp() - (tl / 1000.0) - (cl / 1000.0);
   }
 
-  /** Return a boolean for whether a note is seen. */
-  public boolean isNoteVisible() {
-    double tv = _intake.getEntry("tv").getDouble(0);
+  /** Return a boolean for whether a tag is seen by main cam. */
+  public boolean isApriltagVisible() {
+    double tv = _main.getEntry("tv").getDouble(0);
 
     if (tv == 0) {
       return false;
@@ -85,9 +74,9 @@ public boolean isNoteVisible() {
     return false;
   }
 
-  /** Return a boolean for whether a tag is seen. */
-  public boolean isApriltagVisible() {
-    double tv = _main.getEntry("tv").getDouble(0);
+  /** Return a boolean for whether a note is seen by intake cam. */
+  public boolean isNoteVisible() {
+    double tv = _intake.getEntry("tv").getDouble(0);
 
     if (tv == 0) {
       return false;
@@ -100,27 +89,14 @@ public boolean isApriltagVisible() {
     return false;
   }
 
-  /**
-   * Return a boolean for whether the desired tag is seen.
-   *
-   * @param ID
-   *            The id of the desired tag.
-   */
-  public boolean isApriltagVisible(int ID) {
-    if (!isApriltagVisible())
-      return false;
-
-    return _main.getTag(ID) != null;
-  }
-
   /**
    * Returns the NT "botpose_wpiblue" as an array from the limelight. 
    * 
    * @return NT "botpose_wpiblue" limelight topic.
    * 
    * @see Optional
    */
-  public Optional<double[]> getValidNTEntry() {
+  public Optional<double[]> getBotposeBlue() {
     if (!isApriltagVisible()) return Optional.empty();
 
     NetworkTableEntry botpose_entry = _main.getEntry("botpose_wpiblue");
@@ -132,80 +108,10 @@ public Optional<double[]> getValidNTEntry() {
   }
 
   /**
-   * If the limelight is in perfect condition with the apriltags to reset the robot's pose.
-   * 
-   * @return The pose to reset to (USE GYRO FOR HEADING).
-   */
-  public Optional<Pose2d> resetPose() {
-    Optional<double[]> botpose = getValidNTEntry();
-
-    if (botpose.isEmpty()) { _shouldResetPose = true; return Optional.empty(); }
-    // if (botpose.get()[7] < 2) return Optional.empty(); // tag count?
-
-    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; 
-
-    if (isApriltagVisible(centerTag) || isApriltagVisible(offsetTag)) {
-      if (!_shouldResetPose) return Optional.empty();
-      _shouldResetPose = false;
-      return getBotpose();
-    }
-
-    _shouldResetPose = true;
-
-    return Optional.empty();
-  }
-
-  /**
-   * Returns the "wpiblue" Pose2d of the robot from the limelight.
-   *
-   * @return An Optional of Pose2d which is necessary if no valid (at least 2 tags, good distance) data is found from
-   * the limelight.
+   * Returns the angles between the intake limelight and the note closest to it.
    * 
-   * @see Optional
+   * @return {xAngle, yAngle}
    */
-  public Optional<Pose2d> getBotpose() {
-    Optional<double[]> botpose = getValidNTEntry();
-    if (botpose.isEmpty()) return Optional.empty();
-
-    double[] botpose_array = botpose.get();
-
-    // double botposeX = _xFilter.calculate(botpose_array[0]); // to get rid of the weird origin outlier
-    // double botposeY = _yFilter.calculate(botpose_array[1]); // to get rid of the weird origin outlier
-    // double botposeYaw = _yawFilter.calculate(botpose_array[5]); // to get rid of weird 0 heading outlier
-    double botposeX = botpose_array[0];
-    double botposeY = botpose_array[1];
-    double botposeYaw = botpose_array[5];
-
-    Rotation2d botposeRotation = Rotation2d.fromDegrees(botposeYaw);
-
-    Pose2d botpose2D = new Pose2d(botposeX, botposeY, botposeRotation);
-
-    return Optional.of(botpose2D);
-  }
-
-  /**
-   * Return tx and ty angle offsets from a desired tag.
-   *
-   * @param ID
-   *            The id of the desired tag.
-   * @return A double array [tx, ty]. Null is returned if no tags are visible at
-   *         all.
-   */
-  public double[] tagAngleOffsets(int ID) {
-    if (!isApriltagVisible(ID))
-      return null;
-
-    JsonNode tag = _main.getTag(ID);
-
-    double tx = tag.get("tx").asDouble();
-    double ty = tag.get("ty").asDouble();
-
-    double[] angles = {tx, ty};
-
-    return angles;
-  }
-
   public Optional<double[]> getNoteAngles() {
     if (!isNoteVisible()) return Optional.empty();