Some additional PhotonVision/Drivetrain work, and a terrible auto path

src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path

@@ -0,0 +1,151 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.572076308784383,
+        "y": 3.3951907719609573
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1112866015971603,
+        "y": 2.6628305235137533
+      },
+      "prevControl": {
+        "x": 2.8612866015971603,
+        "y": 2.6628305235137533
+      },
+      "nextControl": {
+        "x": 3.3612866015971603,
+        "y": 2.6628305235137533
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.839529724933453,
+        "y": 2.357009760425909
+      },
+      "prevControl": {
+        "x": 5.055196983507387,
+        "y": 2.7986728575396165
+      },
+      "nextControl": {
+        "x": 6.668464951197871,
+        "y": 1.890230700976042
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.612129547471162,
+        "y": 0.8279059449866895
+      },
+      "prevControl": {
+        "x": 6.3623142538133335,
+        "y": 0.8182976644613882
+      },
+      "nextControl": {
+        "x": 7.030621118012423,
+        "y": 0.844001774622892
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.189520851818989,
+        "y": 0.8279059449866895
+      },
+      "prevControl": {
+        "x": 8.08578683847011,
+        "y": 0.4907704016028375
+      },
+      "nextControl": {
+        "x": 8.318287488908608,
+        "y": 1.246397515527949
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.189520851818989,
+        "y": 7.111241666666667
+      },
+      "prevControl": {
+        "x": 8.36001268105017,
+        "y": 7.294087328812116
+      },
+      "nextControl": {
+        "x": 6.244366666666667,
+        "y": 5.025141666666666
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.518108333333333,
+        "y": 5.592016666666666
+      },
+      "prevControl": {
+        "x": 3.205916666666667,
+        "y": 5.644925000000001
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 45.0
+    },
+    {
+      "waypointRelativePos": 2,
+      "rotationDegrees": 45.0
+    },
+    {
+      "waypointRelativePos": 3,
+      "rotationDegrees": -135.0
+    },
+    {
+      "waypointRelativePos": 5,
+      "rotationDegrees": -135.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -133.87669728592465
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -1,10 +1,17 @@
 package frc.robot.constants;
 
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.FileReader;
+import java.io.IOException;
+import java.nio.file.Path;
+
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.wpilibj.Filesystem;
 
 public class HoodConstants {
     // TODO Real Values
@@ -46,5 +53,28 @@ public class HoodConstants {
             .positionWrappingInputRange(0, Math.PI * 2)
             .feedForward
                 .sva(kS, kV, kA);
+
+
+        File interpolatorFile = Path.of(
+            Filesystem.getDeployDirectory().getAbsolutePath().toString(), 
+            "interpolatorData.csv"
+        ).toFile();
+
+        if(interpolatorFile.exists()) {
+            try (BufferedReader reader = new BufferedReader(new FileReader(interpolatorFile))) {
+                reader.lines().forEach((s) -> {
+                    if(s.trim() != "") { //Empty or whitespace line protection
+                        String[] lineSplit = s.split(",");
+
+                        kDistanceToAngle.put(
+                            Double.valueOf(lineSplit[0].replace("\"", "")), 
+                            Double.valueOf(lineSplit[1].replace("\"", ""))
+                        );
+                    }
+                });
+            } catch (IOException e) {
+                // This condition is never reached because of the if exists line above
+            }
+        }
     }
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -198,27 +198,7 @@ public class Drivetrain extends SubsystemBase {
      * @return A complete Command structure that performs the specified action
      */
     public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
-        return runOnce(yawRotationController::reset).andThen(
+        return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180)
-            drive(
-                () -> 0,
-                () -> 0, 
-                () -> {
-                    Rotation2d targetRotation = new Rotation2d(
-                        targetPose.getX() - getPose().getX(), 
-                        targetPose.getY() - getPose().getY()
-                    );
-
-                    if(rotate180) {
-                        targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
-                    }
-
-                    return yawRotationController.calculate(
-                        getHeading().getRadians(),
-                        targetRotation.getRadians()
-                    );
-                }, 
-                () -> true
-            ))
             .until(yawRotationController::atSetpoint);
     }
 
@@ -291,32 +271,27 @@ public class Drivetrain extends SubsystemBase {
         );
     }
 
-    // TODO check both cameras
+    /**
-    /*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
+     * A method to lock to a particular source of an external "yaw". The intent is for this yaw to be sourced from
-        if (camera1 == null) {
+     * {@link frc.robot.subsystems.PhotonVision#getBestYawForTag(int)} which generates a "yaw" for a particular tag as referenced
-            return new PrintCommand("Camera 1 not available");
+     * from the center point of the cameras image frame. The objective being to "0 the source" using a PID Controller, or in
-        }
+     * other terms, to center the provided tag in the camera's image frame.
-
+     * 
-        // TODO The process variable is different here than what these constants are used for, may need to use something different
+     * @param yaw The "yaw" of the tag source relative to the center of the image frame
-        PIDController controller = new PIDController(
+     * @param xSpeed The X (forward/backward) translational speed of the robot
-            AutoConstants.kPThetaController, 
+     * @param ySpeed The Y (left/right) translational speed of the robot
-            0, 
+     * @return A complete Command structure that performs the specified action
-            0
+     */
-        );
+    public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
-
+        return runOnce(yawRotationController::reset).andThen(
-        return runOnce(controller::reset).andThen(
             drive(
                 xSpeed,
                 ySpeed, 
-                () -> {
+                () -> yawRotationController.calculate(yaw.getAsDouble(), 0),
-                    OptionalDouble tagYaw = camera1.getTagYawByID(tagID);
+                () -> true
-
-                    return (tagYaw.isEmpty() ? 0 : controller.calculate(tagYaw.getAsDouble(), 0));
-                },
-                () -> false
             )
         );
-    }*/
+    }
 
     public Command drivePathPlannerPath(PathPlannerPath path) {
         if(AutoConstants.kAutoConfigOk) {

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

@@ -3,6 +3,7 @@ package frc.robot.subsystems;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
+import java.util.OptionalDouble;
 import java.util.function.Consumer;
 
 import org.photonvision.EstimatedRobotPose;
@@ -138,8 +139,51 @@ public class PhotonVision extends SubsystemBase {
         return Optional.of(Pose3d.kZero
             .plus(bestCameraToTarget.inverse())
             .relativeTo(CompetitionConstants.kTagLayout.getOrigin())
-            .plus(config.robotToCamera().inverse())
+            .plus(config.robotToCamera().inverse()));
-            .plus(CompetitionConstants.kRobotToShooter));
+    }
+
+    /**
+     * Returns the best yaw for a given AprilTag ID as seen by the cameras on the robot.
+     * 
+     * All cameras fields of view are observed, if no camera can see the given tag ID, this
+     * method will return OptionalDouble.empty()
+     * 
+     * Note that this method has no minimum confidence threshold for a tag. This means that
+     * if one camera thinks it sees the tag, even with very low confidence, it'll still
+     * return some sort of yaw value.
+     * 
+     * Note that the yaw value here is the yaw of the observed tag relative to the center
+     * of the cameras image frame. 
+     * 
+     * @param tagID The ID of the tag to look for in the latest results from all cameras
+     * @return An OptionalDouble object containing a Double representing the described yaw
+     *   of the AprilTag specified, or OptionalDouble.empty() if the tag is not present
+     *   anywhere in the robots field of view
+     */
+    public OptionalDouble getBestYawForTag(int tagID) {
+        double bestTagYaw = -1;
+        float bestConfidence = -1;
+
+        for(PhotonPipelineResult result: latestResults) {
+            if(result == null) {
+                continue;
+            }
+
+            for(PhotonTrackedTarget target: result.getTargets()) {
+                if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectClassID()) {
+                    continue;
+                }
+
+                bestTagYaw = target.getYaw();
+                bestConfidence = target.getDetectedObjectConfidence();
+            }
+        }
+
+        if(bestConfidence == -1) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(bestTagYaw);
     }
 
     /**
@@ -158,103 +202,4 @@ public class PhotonVision extends SubsystemBase {
     public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
         poseEstimateConsumers.add(consumer);
     }
-
-    /*public Trigger tagPrescenseTrigger(int targetTag) {
-        return new Trigger(() -> {
-            return List.of(latestResults).stream()
-                .filter((p) -> p != null)
-                .anyMatch((p) -> {
-                    return p.getTargets().stream().map(PhotonTrackedTarget::getFiducialId).anyMatch((i) -> {
-                        return i == targetTag;
-                    });
-                });
-            });
-    }*/
-/* 
-    @Override
-    public OptionalDouble getTagDistanceFromCameraByID(int id) {
-        if (latestResult == null) {
-            return OptionalDouble.empty();
-        }
-
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-        
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            PhotonUtils.calculateDistanceToTargetMeters(
-                cameraHeightMeters, 
-                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
-                cameraPitchRadians, 
-                Units.degreesToRadians(desiredTarget.get().getPitch()))
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagPitchByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getPitch()
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagYawByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getYaw()
-        );
-    }
-
-    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
-        for (PhotonTrackedTarget target : targets) {
-            if (target.getFiducialId() == id) {
-                return Optional.of(target);
-            }
-        }
-
-        return Optional.empty();
-    }
-
-    @Override
-    public int[] getVisibleTagIDs() {
-        if(latestResult == null) {
-            return new int[] {};
-        }
-
-        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
-    }
-    */
 }