From 968b04f839acacbbe532017db70e10b3aa2ee7d6 Mon Sep 17 00:00:00 2001 From: TimB-87 Date: Sat, 1 Feb 2025 11:23:29 -0500 Subject: [PATCH] Move things around / add vision steps --- src/main/java/frc/robot/FieldConstants.java | 54 +++++++++---------- src/main/java/frc/robot/subsystems/Drive.java | 29 +++++++--- 2 files changed, 47 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index a7ca370..6a79d43 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -25,35 +25,6 @@ public class FieldConstants { public static final double startingLineX = Units.inchesToMeters(299.438); // Measured from the inside of starting // line - public static class Processor { - public static final Pose2d centerFace = new Pose2d(Units.inchesToMeters(235.726), 0, - Rotation2d.fromDegrees(90)); - } - - public static class Barge { - public static final Translation2d farCage = new Translation2d(Units.inchesToMeters(345.428), - Units.inchesToMeters(286.779)); - public static final Translation2d middleCage = new Translation2d(Units.inchesToMeters(345.428), - Units.inchesToMeters(242.855)); - public static final Translation2d closeCage = new Translation2d(Units.inchesToMeters(345.428), - Units.inchesToMeters(199.947)); - - // Measured from floor to bottom of cage - public static final double deepHeight = Units.inchesToMeters(3.125); - public static final double shallowHeight = Units.inchesToMeters(30.125); - } - - public static class CoralStation { - public static final Pose2d leftCenterFace = new Pose2d( - Units.inchesToMeters(33.526), - Units.inchesToMeters(291.176), - Rotation2d.fromDegrees(90 - 144.011)); - public static final Pose2d rightCenterFace = new Pose2d( - Units.inchesToMeters(33.526), - Units.inchesToMeters(25.824), - Rotation2d.fromDegrees(144.011 - 90)); - } - public static class Reef { public static final Translation2d center = new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); @@ -94,6 +65,15 @@ public static class Reef { Units.inchesToMeters(130.144), Rotation2d.fromDegrees(-120)); + HashMap reefTags = new HashMap(); + // Add poses and IDs (pose, ID) + reefTags.put(centerFaces[1], 6); + reefTags.put(centerFaces[0], 7); + reefTags.put(centerFaces[5], 8); + reefTags.put(centerFaces[4], 9); + reefTags.put(centerFaces[3], 10); + reefTags.put(centerFaces[2], 11); + // Initialize branch positions for (int face = 0; face < 6; face++) { Map fillRight = new HashMap<>(); @@ -140,6 +120,22 @@ public static class Reef { } } + public static class Processor { + public static final Pose2d centerFace = new Pose2d(Units.inchesToMeters(235.726), 0, + Rotation2d.fromDegrees(90)); + } + + public static class CoralStation { + public static final Pose2d leftCenterFace = new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(291.176), + Rotation2d.fromDegrees(90 - 144.011)); + public static final Pose2d rightCenterFace = new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(25.824), + Rotation2d.fromDegrees(144.011 - 90)); + } + public static class StagingPositions { // Measured from the center of the ice cream (coral marks) public static final Pose2d leftIceCream = new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 9b92fa4..40ac5c9 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -96,7 +96,7 @@ public Drive(SwerveDriveMap map, DoubleSupplier xSpeed, DoubleSupplier ySpeed, D this.ySpeedSupplier = ySpeed; this.rotationSupplier = rotation; - for (int i = 6; i < 12; i++) { + for (int i = 6; i < 11; i++) { kTagLayout.getTagPose(i).ifPresent(pose -> { RED_APRIL_TAGS_REEF_POSITIONS.add(pose.toPose2d()); }); @@ -110,6 +110,11 @@ public Drive(SwerveDriveMap map, DoubleSupplier xSpeed, DoubleSupplier ySpeed, D } + public enum Branch { + leftBranch, + rightBranch + }; + public void setPose(Pose2d pose) { estimator.resetPosition(getMap().gyro.getRotation2d(), getData().getModulePositions(), pose); @@ -152,15 +157,25 @@ public Command aimAtReefCenter() { // Need rotation in here somewhere. Logic from rotateTo command, since ideally // we move and rotate at the same time - public enum Branch { - leftBranch, - rightbranch - - }; - // Function that takes current robot pose and finds the nearest reef apriltag to // it, returning the id /////////////////////// After --> and taking the average pose from both cameras? + /// + // align to reef branch command part two: + // 1. Filter list of apriltags and figure out which ones we see + // 2. filter down to reef apriltags and get poses + // 3. find best target (nearest tag or tag closest to center of sight? Another + // way?) + // 4. now we know that best target plane is a reef side. Apriltags are always in + // the middle at the same point in relation to the reef side, so then + // apply translation offset for left or right branch + // 5. With branch pose, use autoBuilder to maneuver to the branch (angle + // funky??) + + // + public Pose2d getVisibleTags() { + + } public Pose2d findNearestTag() { Pose2d robotPose = estimator.getEstimatedPosition();