Added vision estimation.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -45,6 +45,10 @@
 public class SwerveSubsystem extends SubsystemBase
 {
 
+  /**
+   * PhotonVision class to keep an accurate odometry.
+   */
+  private Vision vision;
   /**
    * Swerve drive object.
    */
@@ -71,7 +75,7 @@ public SwerveSubsystem(File directory)
     //  The encoder resolution per motor revolution is 1 per motor revolution.
     double driveConversionFactor = SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(4), 6.75);
     System.out.println("\"conversionFactors\": {");
-    System.out.println("\t\"angle\": {\"factor\": " + angleConversionFactor + " },") ;
+    System.out.println("\t\"angle\": {\"factor\": " + angleConversionFactor + " },");
     System.out.println("\t\"drive\": {\"factor\": " + driveConversionFactor + " }");
     System.out.println("}");
 
@@ -102,6 +106,23 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig
     swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED);
   }
 
+  /**
+   * Setup the photon vision class.
+   */
+  public void setupPhotonVision()
+  {
+    vision = new Vision(swerveDrive::getPose, swerveDrive.field);
+    vision.updatePoseEstimation(swerveDrive);
+  }
+
+  /**
+   * Update the pose estimation with vision data.
+   */
+  public void updatePoseWithVision()
+  {
+    vision.updatePoseEstimation(swerveDrive);
+  }
+
   /**
    * Setup AutoBuilder for PathPlanner.
    */
@@ -253,7 +274,7 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat
     return run(() -> {
 
       Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(),
-                                                                                translationY.getAsDouble()), 0.8);
+                                                                                 translationY.getAsDouble()), 0.8);
 
       // Make the robot move
       driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(),
@@ -418,6 +439,17 @@ public Pose2d getPose()
     return swerveDrive.getPose();
   }
 
+  /**
+   * Get the pose while updating with vision readings.
+   *
+   * @return The robots pose with the vision estimates in place.
+   */
+  public Pose2d getVisionPose()
+  {
+    vision.updatePoseEstimation(swerveDrive);
+    return swerveDrive.getPose();
+  }
+
   /**
    * Set chassis speeds with closed-loop velocity control.
    *

src/main/java/frc/robot/subsystems/swervedrive/Vision.java

@@ -162,7 +162,7 @@ public void addToVisionSim(VisionSystemSim systemSim)
   /**
    * Photon Vision Simulation
    */
-  private VisionSystemSim visionSim;
+  public VisionSystemSim visionSim;
   /**
    * Photon Vision camera properties simulation.
    */
@@ -202,6 +202,19 @@ public Vision(Supplier<Pose2d> currentPose, Field2d field)
   }
 
 
+  /**
+   * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses.
+   *
+   * @param swerveDrive {@link SwerveDrive} instance.
+   */
+  public void updatePoseEstimation(SwerveDrive swerveDrive)
+  {
+    for (EstimatedRobotPose i : getEstimatedGlobalPose())
+    {
+      swerveDrive.addVisionMeasurement(i.estimatedPose.toPose2d(), i.timestampSeconds);
+    }
+  }
+
   /**
    * generates the estimated robot pose. Returns empty if:
    * <ul>
@@ -256,7 +269,7 @@ private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pos
       }
 
       //est pose is very far from recorded robot pose
-      if (getDistanceFromPose(pose.get().estimatedPose.toPose2d()) > 1)
+      if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1)
       {
         longDistangePoseEstimationCount++;
 
@@ -286,44 +299,6 @@ public PhotonPipelineResult getLatestResult(Cameras camera)
     return Robot.isReal() ? camera.camera.getLatestResult() : camera.cameraSim.getCamera().getLatestResult();
   }
 
-  /**
-   * Check if the latest result of a given Camera has any April Tags in view.
-   *
-   * @param camera Camera to check.
-   * @return If the given Camera has any April Tags
-   */
-  public boolean hasTargets(Cameras camera)
-  {
-    return getLatestResult(camera).hasTargets();
-  }
-
-  /**
-   * Get the distance from the pose to the current pose.
-   *
-   * @param pose Pose to check.
-   * @return Distance to the pose.
-   */
-  public double getDistanceFromPose(Pose2d pose)
-  {
-    return PhotonUtils.getDistanceToPose(currentPose.get(), pose);
-  }
-
-  /**
-   * Get AprilTag pose.
-   *
-   * @param id AprilTagID
-   * @return Pose of Tag.
-   */
-  public Pose2d getTagPose(int id)
-  {
-    Optional<Pose3d> tag = fieldLayout.getTagPose(id);
-    if (tag.isPresent())
-    {
-      return tag.get().toPose2d();
-    }
-    return null;
-  }
-
   /**
    * Get distance of the robot from the AprilTag pose.
    *
@@ -333,11 +308,7 @@ public Pose2d getTagPose(int id)
   public double getDistanceFromAprilTag(int id)
   {
     Optional<Pose3d> tag = fieldLayout.getTagPose(id);
-    if (tag.isPresent())
-    {
-      return getDistanceFromPose(tag.get().toPose2d());
-    }
-    return -1;
+    return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0);
   }
 
   /**
@@ -402,7 +373,7 @@ public void updateVisionField()
     List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>();
     for (Cameras c : Cameras.values())
     {
-      if (hasTargets(c))
+      if (getLatestResult(c).hasTargets())
       {
         targets.addAll(getLatestResult(c).targets);
       }
@@ -411,8 +382,11 @@ public void updateVisionField()
     List<Pose2d> poses = new ArrayList<>();
     for (PhotonTrackedTarget target : targets)
     {
-      Pose2d targetPose = getTagPose(target.getFiducialId());
-      poses.add(targetPose);
+      if (fieldLayout.getTagPose(target.getFiducialId()).isPresent())
+      {
+        Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d();
+        poses.add(targetPose);
+      }
     }
 
     field2d.getObject("tracked targets").setPoses(poses);