From f2224bcd3f7894710dc0ae0f191cd3d94dab645d Mon Sep 17 00:00:00 2001 From: Brandonzx3 Date: Thu, 29 Aug 2024 00:09:48 -0500 Subject: [PATCH 1/2] update the vision simulation on Vision#updatePoseEstimation and check for real env before adding vision measurements --- .../java/frc/robot/subsystems/swervedrive/Vision.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 9b821591..b65d9d34 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -209,10 +209,13 @@ public Vision(Supplier currentPose, Field2d field) */ public void updatePoseEstimation(SwerveDrive swerveDrive) { - for (EstimatedRobotPose i : getEstimatedGlobalPose()) - { - swerveDrive.addVisionMeasurement(i.estimatedPose.toPose2d(), i.timestampSeconds); - } + ArrayList estimatedRobotPoses = getEstimatedGlobalPose(); + if(Robot.isReal()) { + for (EstimatedRobotPose i : estimatedRobotPoses) + { + swerveDrive.addVisionMeasurement(i.estimatedPose.toPose2d(), i.timestampSeconds); + } + } else visionSim.update(swerveDrive.getPose()); } /** From 1d9ac185cb76acaee86307869ed73dc85a2c1e7c Mon Sep 17 00:00:00 2001 From: Jason Kreidler Date: Fri, 30 Aug 2024 06:53:52 -0500 Subject: [PATCH 2/2] Added getAprilTagPose method to Vision --- .../robot/subsystems/swervedrive/Vision.java | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index b65d9d34..67c8d4d3 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -6,7 +6,9 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -395,5 +397,29 @@ public void updateVisionField() field2d.getObject("tracked targets").setPoses(poses); } + /** + * Calculates a target pose relative to an AprilTag on the field. + * + * @param aprilTag The ID of the AprilTag. + * @param xOffset The X offset in meters from the AprilTag's position, positive is away from the AprilTag. + * @param yOffset The Y offset in meters from the AprilTag's position, positive is to the right of the AprilTag + * regardless of alliance. + * @param rotOffset The rotation offset in degrees from the AprilTag's orientation. + * @return The target pose of the AprilTag. + */ + public static Pose2d getAprilTagPose(int aprilTag, Double xOffset, Double yOffset, Double rotOffset) + { + Optional aprilTagPose3d = + AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo).getTagPose(aprilTag); + + Pose2d aprilTagPose2d = aprilTagPose3d.get().toPose2d(); + + Transform2d aprilTagGoalTrans2d = new Transform2d(new Translation2d(xOffset, yOffset), + new Rotation2d(Math.toRadians(rotOffset))); + + Pose2d aprilTagTargetPose2d = aprilTagPose2d.transformBy(aprilTagGoalTrans2d); + + return aprilTagTargetPose2d; + } }