REEF-161 - Set up vision subsystem and IO

.vscode/settings.json

@@ -56,5 +56,6 @@
     "edu.wpi.first.math.proto.*",
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
-  ]
+  ],
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

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.3.1"
 }
 
 java {

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

src/main/java/frc/robot/FieldConstants.java

@@ -0,0 +1,241 @@
+// Copyright (c) 2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.util.Units;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+
+/**
+ * Contains various field dimensions and useful reference points. All units are in meters and poses
+ * have a blue alliance origin.
+ */
+public class FieldConstants {
+    public static final double fieldLength = Units.inchesToMeters(690.876);
+    public static final double fieldWidth = Units.inchesToMeters(317);
+    public static final Translation2d fieldCenter =
+        new Translation2d(fieldLength / 2, fieldWidth / 2);
+    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 centerOfReef =
+            new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501));
+        public static final double faceToZoneLine =
+            Units.inchesToMeters(12); // Side of the reef to the inside of the
+        // reef zone line
+
+        public static final Pose2d[] centerFaces =
+            new Pose2d[12]; // Starting facing the driver station in clockwise
+        // order
+        public static final List<Map<ReefHeight, Pose3d>> branchPositions =
+            new ArrayList<>(); // Starting at the right
+        // branch facing the
+        // driver station in
+        // clockwise
+
+        static {
+            // Initialize faces
+            centerFaces[0] =
+                new Pose2d(
+                    Units.inchesToMeters(144.003),
+                    Units.inchesToMeters(158.500),
+                    Rotation2d.fromDegrees(180));
+            centerFaces[1] =
+                new Pose2d(
+                    Units.inchesToMeters(160.373),
+                    Units.inchesToMeters(186.857),
+                    Rotation2d.fromDegrees(120));
+            centerFaces[2] =
+                new Pose2d(
+                    Units.inchesToMeters(193.116),
+                    Units.inchesToMeters(186.858),
+                    Rotation2d.fromDegrees(60));
+            centerFaces[3] =
+                new Pose2d(
+                    Units.inchesToMeters(209.489),
+                    Units.inchesToMeters(158.502),
+                    Rotation2d.fromDegrees(0));
+            centerFaces[4] =
+                new Pose2d(
+                    Units.inchesToMeters(193.118),
+                    Units.inchesToMeters(130.145),
+                    Rotation2d.fromDegrees(-60));
+            centerFaces[5] =
+                new Pose2d(
+                    Units.inchesToMeters(160.375),
+                    Units.inchesToMeters(130.144),
+                    Rotation2d.fromDegrees(-120));
+
+            centerFaces[6] = centerFaces[0].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[7] = centerFaces[1].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[8] = centerFaces[2].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[9] = centerFaces[3].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[10] = centerFaces[4].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[11] = centerFaces[5].rotateAround(fieldCenter, Rotation2d.k180deg);
+
+            // Initialize branch positions
+            for (int face = 0; face < centerFaces.length; face++) {
+                Map<ReefHeight, Pose3d> fillRight = new HashMap<>();
+                Map<ReefHeight, Pose3d> fillLeft = new HashMap<>();
+                for (var level : ReefHeight.values()) {
+                    Pose2d poseDirection = new Pose2d();
+                    if (face < 6) {
+                        poseDirection =
+                            new Pose2d(centerOfReef, centerFaces[face].getRotation());
+                    } else {
+                        poseDirection =
+                            new Pose2d(centerOfReef.rotateAround(fieldCenter, Rotation2d.k180deg),
+                                centerFaces[face].getRotation());
+                    }
+
+                    double adjustX = Units.inchesToMeters(30.738); // Depth of branch from reef face
+                    double adjustY = Units.inchesToMeters(6.469); // Offset from reef face
+                                                                  // centerline to branch
+
+                    fillRight.put(
+                        level,
+                        new Pose3d(
+                            new Translation3d(
+                                poseDirection
+                                    .transformBy(
+                                        new Transform2d(adjustX, adjustY, new Rotation2d()))
+                                    .getX(),
+                                poseDirection
+                                    .transformBy(
+                                        new Transform2d(adjustX, adjustY, new Rotation2d()))
+                                    .getY(),
+                                level.height),
+                            new Rotation3d(
+                                0,
+                                Units.degreesToRadians(level.pitch),
+                                poseDirection.getRotation().getRadians())));
+                    fillLeft.put(
+                        level,
+                        new Pose3d(
+                            new Translation3d(
+                                poseDirection
+                                    .transformBy(
+                                        new Transform2d(adjustX, -adjustY, new Rotation2d()))
+                                    .getX(),
+                                poseDirection
+                                    .transformBy(
+                                        new Transform2d(adjustX, -adjustY, new Rotation2d()))
+                                    .getY(),
+                                level.height),
+                            new Rotation3d(
+                                0,
+                                Units.degreesToRadians(level.pitch),
+                                poseDirection.getRotation().getRadians())));
+                }
+                branchPositions.add(fillRight);
+                branchPositions.add(fillLeft);
+            }
+
+
+        }
+    }
+
+    public static class StagingPositions {
+        // Measured from the center of the ice cream
+        public static final Pose2d leftIceCream =
+            new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), new Rotation2d());
+        public static final Pose2d middleIceCream =
+            new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d());
+        public static final Pose2d rightIceCream =
+            new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d());
+    }
+
+    public enum ReefHeight {
+        L4(Units.inchesToMeters(72), -90),
+        L3(Units.inchesToMeters(47.625), -35),
+        L2(Units.inchesToMeters(31.875), -35),
+        L1(Units.inchesToMeters(18), 0);
+
+        ReefHeight(double height, double pitch)
+        {
+            this.height = height;
+            this.pitch = pitch; // in degrees
+        }
+
+        public final double height;
+        public final double pitch;
+    }
+
+    public static Pose2d getNearestReefFace(Pose2d currentPose)
+    {
+        return currentPose.nearest(List.of(FieldConstants.Reef.centerFaces));
+    }
+
+    public enum ReefSide {
+        LEFT,
+        RIGHT
+    }
+
+    public static Pose2d getNearestReefBranch(Pose2d currentPose, ReefSide side)
+    {
+        return FieldConstants.Reef.branchPositions
+            .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose))
+                * 2 + (side == ReefSide.LEFT ? 1 : 0))
+            .get(FieldConstants.ReefHeight.L1).toPose2d();
+    }
+
+    public static Pose2d getNearestCoralStation(Pose2d currentPose)
+    {
+        if (currentPose.getTranslation().getX() > FieldConstants.fieldLength / 2) {
+            if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) {
+                return FieldConstants.CoralStation.rightCenterFace
+                    .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg);
+            } else {
+                return FieldConstants.CoralStation.leftCenterFace
+                    .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg);
+            }
+        } else {
+            if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) {
+                return FieldConstants.CoralStation.leftCenterFace;
+            } else {
+                return FieldConstants.CoralStation.rightCenterFace;
+            }
+        }
+    }
+}

src/main/java/frc/robot/HardwareConstants.java

@@ -24,18 +24,18 @@ public class CAN {
 
         public static final int ARM_MTR_ID = 14;
         public static final int INTAKE_MTR_ID = 15;
-        public static final int CLIMBER_MTR_ID = 1;
 
         public static final int PRIMARY_ELEVATOR_ID = 16;
         public static final int SECONDARY_ELEVATOR_ID = 17;
 
         public static final int CANDLE_ID = 50;
-
-        public static final int FEEDER_MTR_ID = 20;
     }
 
     public class DIO {
         // Add digitial I/O ports used here
         public static final int LIGHT_SENSOR_CHANNEL = 0;
     }
+
+    // Use LoggedTunableNumbers
+    public static final boolean tuningMode = true;
 }

src/main/java/frc/robot/Robot.java

@@ -23,22 +23,58 @@ public class Robot extends LoggedRobot {
   private final RobotContainer m_robotContainer;
 
   public Robot() {
-    Logger.recordMetadata("ProjectName", "ReefCode"); // Set a metadata value
 
-    if (isReal()) {
-      Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
-      Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
-      new PowerDistribution(CAN.PDH_ID, ModuleType.kRev); // Enables power distribution logging
-    } else {
-      setUseTiming(false); // Run as fast as possible
-      String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
-      Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
-      Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+    switch (HardwareConstants.currentMode) {
+      case REAL:
+      /*
+       * If you are really tight on ram you can comment out the logwriter
+       * for the robot with the V1 rio. Keeo the publisher so you can view
+       * the live data on advantage scope.
+       */
+        Logger.addDataReceiver(new WPILOGWriter());
+        Logger.addDataReceiver(new NT4Publisher());
+        // new PowerDistribution(CAN.PDH_ID, ModuleType.kRev);
+        break;
+
+      case SIM:
+        System.out.println("SIM!!!");
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case REPLAY:
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog();
+        Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+        break;
     }
 
-    Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
+    Logger.recordMetadata("ProjectName", "ReefCode"); // Set a metadata value
+
+
+    /*********************************************************
+     * 
+     * README README README README README README
+     * 
+     * Commenting out the start logger function will save you enough
+     * ram for the robot code to run. If you add leds, cimbers, etc, 
+     * then you may need to save additional ram. The proposed solution
+     * is to add a boolen similar to the isreal and isSim flags to check
+     * if this is the v1 or v2 bot. If it is v1 with the roborio one, 
+     * then dont start the logger or start leds, etc.
+     * 
+     * 
+     * NOTE: adding the climber and elevator and intake back to the code was enough
+     * to put it over the ram limit. so either more boolean checks will be needed
+     * to enable and disable features or we need to trim down our code to use less
+     * ram.
+     * 
+     * Garrett
+     * 
+     ***********************************************************/
+    // Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
                     // be added.
-
+                      
     m_robotContainer = new RobotContainer();
   }
 
@@ -49,7 +85,6 @@ public void robotPeriodic() {
 
   @Override
   public void disabledInit() {
-    m_robotContainer.startIdleAnimations();
   }
 
   @Override
@@ -67,9 +102,6 @@ public void autonomousInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
-
-    m_robotContainer.calibrateAndStartPIDs();
-    m_robotContainer.startEnabledLEDs();
   }
 
   @Override
@@ -85,8 +117,6 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    m_robotContainer.calibrateAndStartPIDs();
-    m_robotContainer.startEnabledLEDs();
   }
 
   @Override