From 56f3ccf0c56c231516fc6541c50d9991f3feb88c Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 16 Jan 2025 16:51:09 -0700 Subject: [PATCH] Add Reef Position Helper --- build.gradle | 2 +- src/main/java/frc4388/robot/Constants.java | 26 +++--- .../java/frc4388/robot/subsystems/Vision.java | 3 +- .../frc4388/utility/ReefPositionHelper.java | 88 +++++++++++++++++++ 4 files changed, 107 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc4388/utility/ReefPositionHelper.java diff --git a/build.gradle b/build.gradle index 302477e..689ff84 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8f7e30e..736af3a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,6 +46,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import frc4388.utility.CanDevice; @@ -301,16 +302,6 @@ public final class Constants { public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI)); - // public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - - // Test april tag field layout - public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( - Arrays.asList(new AprilTag[] { - new AprilTag(1, new Pose3d( - new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) - )), - }), 100, 100); // The standard deviations of our vision estimated poses, which affect correction rate // (Fake values. Experiment and determine estimation noise on an actual robot.) @@ -318,6 +309,21 @@ public final class Constants { public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } + public static final class FieldConstants { + public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2025Reefscape.loadAprilTagLayoutField(); + public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(13); + + + // Test april tag field layout + // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + // Arrays.asList(new AprilTag[] { + // new AprilTag(1, new Pose3d( + // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + // )), + // }), 100, 100); + + } + public static final class DriveConstants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index a32b0cd..d88a2d4 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.FieldConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.Status; import frc4388.utility.Subsystem; @@ -67,7 +68,7 @@ public class Vision extends Subsystem { this.camera = camera; SmartDashboard.putData(field); - photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); + photonEstimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java new file mode 100644 index 0000000..15bbf7c --- /dev/null +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -0,0 +1,88 @@ +package frc4388.utility; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc4388.robot.Constants.FieldConstants; + +public class ReefPositionHelper { + public enum Side { + LEFT, + RIGHT + } + + public static Pose2d[] RED_TAGS = { + FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(11).get().toPose2d() + }; + + public static Pose2d[] BLUE_TAGS = { + FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(), + FieldConstants.kTagLayout.getTagPose(22).get().toPose2d() + }; + + public static double distanceTo(Pose2d first, Pose2d second){ + return Math.sqrt( Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2)); + } + + + /* + * Function to loop through a list of tag locations to figure out closest one + */ + public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){ + if(locations.length <= 0) return new Pose2d(); + + Pose2d minPos = locations[0]; + double minDistance = distanceTo(position,minPos); + + for(int i = 1; i < locations.length; i++){ + double dist = distanceTo(locations[i],minPos); + if(dist < minDistance){ + minPos = locations[i]; + minDistance = dist; + } + } + + return minPos; + } + + /* + * Function to find closest tag location based on side + */ + public static Pose2d getNearestTag(Pose2d position) { + Optional ally = DriverStation.getAlliance(); + if (!ally.isPresent()) + return new Pose2d(); + if (ally.get() == Alliance.Red) + return getNearestTag(RED_TAGS, position); + if (ally.get() == Alliance.Blue) + return getNearestTag(BLUE_TAGS, position); + return new Pose2d(); + } + + public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) { + return getNearestTag(horisontalOffset(position, side == Side.LEFT ? -(xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET))); + } + + + public static Pose2d horisontalOffset(Pose2d oldPose, double xoffset){ + Translation2d oldTranslation = oldPose.getTranslation(); + Rotation2d rot = oldPose.getRotation(); + return new Pose2d(new Translation2d( + oldTranslation.getX() + rot.getCos() * xoffset, + oldTranslation.getY() + rot.getSin() * xoffset + ), oldPose.getRotation()); + } +}