Merge pull request 'Bringing in basic drivetrain stuff' (#1) from brads-tinkering into main

Reviewed-on: #1

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

@@ -22,12 +22,11 @@ public class Robot extends LoggedRobot {
 
   private final RobotContainer m_robotContainer;
 
-  @SuppressWarnings("resource")
   public Robot() {
     Logger.recordMetadata("ProjectName", "2026_Robot_Code");
 
     if(isReal()) {
-      if(CompetitionConstants.kLogToNetworkTables) {
+      if(CompetitionConstants.logToNetworkTables) {
         Logger.addDataReceiver(new NT4Publisher());
       }
       
@@ -42,7 +41,7 @@ public class Robot extends LoggedRobot {
     }
 
     Logger.start();
-
+    
     m_robotContainer = new RobotContainer();
   }
 

src/main/java/frc/robot/constants/CompetitionConstants.java

@@ -1,13 +1,6 @@
 package frc.robot.constants;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-
 public class CompetitionConstants {
     // THIS SHOULD BE FALSE DURING COMPETITION PLAY
-    public static final boolean kLogToNetworkTables = true;
-
-    public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(
-        AprilTagFields.kDefaultField
-    );
+    public static final boolean logToNetworkTables = true;
 }

src/main/java/frc/robot/constants/OIConstants.java

@@ -1,10 +0,0 @@
-package frc.robot.constants;
-
-public class OIConstants {
-    public static final int kDriverControllerPort = 0;
-    public static final int kOperatorControllerPort = 1;
-    
-    public static final double kDriveDeadband = .01;
-
-    public static final double kJoystickExponential = 3;
-}

src/main/java/frc/robot/interfaces/IAprilTagProvider.java

@@ -17,9 +17,10 @@ public interface IAprilTagProvider {
      * A method to get the distance from <i>the camera</i> to the AprilTag specified
      * 
      * @param id The ID of the AprilTag to give a distance to
+     * @param targetHeightMeters The height of the AprilTag off the ground, in meters
      * @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view
      */
-    public OptionalDouble getTagDistanceFromCameraByID(int id);
+    public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
 
     /**
      * A method to get the pitch from the center of the image of a particular AprilTag

src/main/java/frc/robot/utilities/PhotonVision.java

@@ -13,9 +13,10 @@ import org.photonvision.PhotonUtils;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
-import frc.robot.constants.CompetitionConstants;
 import frc.robot.interfaces.IAprilTagProvider;
 import frc.robot.interfaces.IVisualPoseProvider;
 
@@ -34,7 +35,9 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
         camera = new PhotonCamera(cameraName);
 
         photonPoseEstimator = new PhotonPoseEstimator(
-            CompetitionConstants.kTagLayout, 
+            AprilTagFieldLayout.loadFromResource(
+                AprilTagFields.kDefaultField.m_resourceFile
+            ), 
             PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
             robotToCam
         );
@@ -75,7 +78,7 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
     }
 
     @Override
-    public OptionalDouble getTagDistanceFromCameraByID(int id) {
+    public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
         if (latestResult == null) {
             return OptionalDouble.empty();
         }
@@ -85,7 +88,7 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
         }
 
         Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-        
+
         if (desiredTarget.isEmpty()) {
             return OptionalDouble.empty();
         }
@@ -93,7 +96,7 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
         return OptionalDouble.of(
             PhotonUtils.calculateDistanceToTargetMeters(
                 cameraHeightMeters, 
-                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
+                targetHeightMeters, 
                 cameraPitchRadians, 
                 Units.degreesToRadians(desiredTarget.get().getPitch()))
         );