From 4ca98cfd48ba5d4b1ee3028f48229e01c111a9f2 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Sun, 25 Aug 2024 14:46:06 -0500 Subject: [PATCH] Added vision estimation. Signed-off-by: thenetworkgrinch --- .../swervedrive/SwerveSubsystem.java | 36 +++++++++- .../robot/subsystems/swervedrive/Vision.java | 70 ++++++------------- 2 files changed, 56 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index cdc06acd..c0679eb4 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/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. * diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 74aee6f1..9b821591 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/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 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: *
    @@ -256,7 +269,7 @@ private Optional filterPose(Optional 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 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 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 targets = new ArrayList(); 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 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);