diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index b5a3d16..dbb9178 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -22,11 +22,12 @@ public class Robot extends LoggedRobot {
private final RobotContainer m_robotContainer;
+ @SuppressWarnings("resource")
public Robot() {
Logger.recordMetadata("ProjectName", "2026_Robot_Code");
if(isReal()) {
- if(CompetitionConstants.logToNetworkTables) {
+ if(CompetitionConstants.kLogToNetworkTables) {
Logger.addDataReceiver(new NT4Publisher());
}
@@ -41,7 +42,7 @@ public class Robot extends LoggedRobot {
}
Logger.start();
-
+
m_robotContainer = new RobotContainer();
}
diff --git a/src/main/java/frc/robot/constants/CompetitionConstants.java b/src/main/java/frc/robot/constants/CompetitionConstants.java
index 6ff076a..98e8de6 100644
--- a/src/main/java/frc/robot/constants/CompetitionConstants.java
+++ b/src/main/java/frc/robot/constants/CompetitionConstants.java
@@ -1,6 +1,13 @@
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 logToNetworkTables = true;
+ public static final boolean kLogToNetworkTables = true;
+
+ public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(
+ AprilTagFields.kDefaultField
+ );
}
diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java
index e69de29..cb8392f 100644
--- a/src/main/java/frc/robot/constants/OIConstants.java
+++ b/src/main/java/frc/robot/constants/OIConstants.java
@@ -0,0 +1,10 @@
+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;
+}
diff --git a/src/main/java/frc/robot/constants/PhotonConstants.java b/src/main/java/frc/robot/constants/PhotonConstants.java
new file mode 100644
index 0000000..e69de29
diff --git a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java
index 632ea73..86ea60b 100644
--- a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java
+++ b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java
@@ -17,10 +17,9 @@ public interface IAprilTagProvider {
* A method to get the distance from the camera 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, double targetHeightMeters);
+ public OptionalDouble getTagDistanceFromCameraByID(int id);
/**
* A method to get the pitch from the center of the image of a particular AprilTag
diff --git a/src/main/java/frc/robot/utilities/PhotonVision.java b/src/main/java/frc/robot/utilities/PhotonVision.java
index 0563c31..46e4ae4 100644
--- a/src/main/java/frc/robot/utilities/PhotonVision.java
+++ b/src/main/java/frc/robot/utilities/PhotonVision.java
@@ -13,10 +13,9 @@ 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;
@@ -35,9 +34,7 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
camera = new PhotonCamera(cameraName);
photonPoseEstimator = new PhotonPoseEstimator(
- AprilTagFieldLayout.loadFromResource(
- AprilTagFields.kDefaultField.m_resourceFile
- ),
+ CompetitionConstants.kTagLayout,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
robotToCam
);
@@ -78,7 +75,7 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
}
@Override
- public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
+ public OptionalDouble getTagDistanceFromCameraByID(int id) {
if (latestResult == null) {
return OptionalDouble.empty();
}
@@ -88,7 +85,7 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
}
Optional desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
+
if (desiredTarget.isEmpty()) {
return OptionalDouble.empty();
}
@@ -96,7 +93,7 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
return OptionalDouble.of(
PhotonUtils.calculateDistanceToTargetMeters(
cameraHeightMeters,
- targetHeightMeters,
+ CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
cameraPitchRadians,
Units.degreesToRadians(desiredTarget.get().getPitch()))
);