Move things around / add vision steps

chopshop-166 · Feb 1, 2025 · 968b04f · 968b04f
1 parent c0ec475
commit 968b04f
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 36 deletions.
diff --git 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<Pose2d, Integer> reefTags = new HashMap<Pose2d, Integer>();
+            // 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<ReefHeight, Pose3d> 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
@@ -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();