Object detection

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

@@ -328,8 +328,15 @@ public void bindXboxDriver(int port) {
                     drive)
                 .ignoringDisable(true));
 
+    // Point at target while A button is held
+    xboxDriver
+        .a()
+        .whileTrue(
+            DriveCommands.pointAtTarget(
+                drive, () -> vision.getTargetX(0), allianceSelector::fieldRotated));
+
     // Drive 1m forward while A button is held
-    xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1)));
+    // xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1)));
 
     // Align with pose, approaching in correct orientation from 1 m away
     // xboxDriver

src/main/java/frc/robot/commands/DriveCommands.java

@@ -231,6 +231,46 @@ public static Command joystickDriveAtFixedOrientation(
         .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians()));
   }
 
+  /**
+   * Field relative drive command using joystick for linear control and PID for angular control.
+   * Possible use cases include snapping to an angle, aiming at a vision target, or controlling
+   * absolute rotation with a joystick.
+   */
+  public static Command pointAtTarget(
+      Drive drive, Supplier<Rotation2d> yawErrorSupplier, BooleanSupplier fieldRotatedSupplier) {
+
+    // Create PID controller
+    ProfiledPIDController angleController =
+        new ProfiledPIDController(
+            ANGLE_KP,
+            0.0,
+            ANGLE_KD,
+            new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
+    angleController.enableContinuousInput(-Math.PI, Math.PI);
+
+    // Construct command
+    return Commands.run(
+            () -> {
+
+              // Calculate angular speed
+              double omega = angleController.calculate(yawErrorSupplier.get().getRadians(), 0);
+
+              // Convert to field relative speeds & send command
+              ChassisSpeeds speeds = new ChassisSpeeds(0, 0, omega);
+              drive.runVelocity(
+                  ChassisSpeeds.fromFieldRelativeSpeeds(
+                      speeds,
+                      fieldRotatedSupplier.getAsBoolean()
+                          ? drive.getRotation().plus(Rotation2d.kPi)
+                          : drive.getRotation()));
+            },
+            drive)
+        .withName("Point At Target")
+
+        // Reset PID controller when command starts
+        .beforeStarting(() -> angleController.reset(yawErrorSupplier.get().getRadians()));
+  }
+
   /**
    * Measures the velocity feedforward constants for the drive motors.
    *

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -146,7 +146,7 @@ public class DriveConstants {
       new Slot0Configs()
           .withKP(300)
           .withKI(0)
-          .withKD(0.5)
+          .withKD(1.5)
           .withKS(0.1)
           .withKV(1.91)
           .withKA(0)

src/main/java/frc/robot/subsystems/vision/Vision.java

@@ -67,12 +67,12 @@ public Vision(VisionConsumer consumer, Supplier<Pose2d> poseSupplier, VisionIO..
   }
 
   /**
-   * Returns the X angle to the best target, which can be used for simple servoing with vision.
+   * Returns the yaw angle to the best target, which can be used for simple servoing with vision.
    *
    * @param cameraIndex The index of the camera to use.
    */
   public Rotation2d getTargetX(int cameraIndex) {
-    return inputs[cameraIndex].latestTargetObservation.tx();
+    return inputs[cameraIndex].latestTargetObservation.yaw();
   }
 
   @Override

src/main/java/frc/robot/subsystems/vision/VisionConstants.java

@@ -26,20 +26,20 @@ public class VisionConstants {
   public static AprilTagFields defaultAprilTagFieldLayout = AprilTagFields.k2025ReefscapeAndyMark;
 
   // Camera names, must match names configured on coprocessor
-  public static String cameraFrontRightName = "OV2311_TH_4";
-  public static String cameraFrontLeftName = "OV2311_TH_3";
-  public static String cameraBackRightName = "OV2311_TH_2";
-  public static String cameraBackLeftName = "OV2311_TH_1";
+  public static String cameraFrontRightName = "OV2311_TH_8";
+  public static String cameraFrontLeftName = "OV2311_TH_5";
+  public static String cameraBackRightName = "OV2311_TH_6";
+  public static String cameraBackLeftName = "OV2311_TH_7";
 
   // Robot to camera transforms
   public static Transform3d robotToFrontRightCamera =
-      new Transform3d(0.286, -0.332, 0.361, new Rotation3d(0, 0, 0.768));
+      new Transform3d(0.248, -0.318, 0.513, new Rotation3d(0.0, 0.0, 0.0));
   public static Transform3d robotToFrontLeftCamera =
-      new Transform3d(0.292, 0.316, 0.361, new Rotation3d(0, 0, 5.498));
+      new Transform3d(0.222, 0.331, 0.513, new Rotation3d(0.0, 0, Math.PI / 2.0));
   public static Transform3d robotToBackRightCamera =
-      new Transform3d(-0.259, -0.346, 0.628, new Rotation3d(0, 0, 4.311));
+      new Transform3d(-0.375, -0.331, 0.513, new Rotation3d(0.0, 0.0, 3.0 * Math.PI / 2.0));
   public static Transform3d robotToBackLeftCamera =
-      new Transform3d(-0.252, 0.341, 0.628, new Rotation3d(0, 0, 1.972));
+      new Transform3d(-0.401, 0.318, 0.513, new Rotation3d(0.0, 0.0, Math.PI));
 
   public static Distance minRobotWidth = Inches.of(36.875);
 

src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -16,13 +16,19 @@ public interface VisionIO {
   public static class VisionIOInputs {
     public boolean connected = false;
     public TargetObservation latestTargetObservation =
-        new TargetObservation(Rotation2d.kZero, Rotation2d.kZero);
+        new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, -1, -1);
     public PoseObservation[] poseObservations = new PoseObservation[0];
     public int[] tagIds = new int[0];
   }
 
   /** Represents the angle to a simple target, not used for pose estimation. */
-  public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
+  public static record TargetObservation(
+      Rotation2d yaw,
+      Rotation2d pitch,
+      Rotation2d skew,
+      double area,
+      float confidence,
+      int objectID) {}
 
   /** Represents a robot pose sample used for pose estimation. */
   public static record PoseObservation(

src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java

@@ -39,15 +39,22 @@ public void updateInputs(VisionIOInputs inputs) {
     // Read new camera observations
     Set<Short> tagIds = new HashSet<>();
     List<PoseObservation> poseObservations = new LinkedList<>();
+
     for (var result : camera.getAllUnreadResults()) {
       // Update latest target observation
       if (result.hasTargets()) {
+        var bestTarget = result.getBestTarget();
         inputs.latestTargetObservation =
             new TargetObservation(
-                Rotation2d.fromDegrees(result.getBestTarget().getYaw()),
-                Rotation2d.fromDegrees(result.getBestTarget().getPitch()));
+                Rotation2d.fromDegrees(bestTarget.getYaw()),
+                Rotation2d.fromDegrees(bestTarget.getPitch()),
+                Rotation2d.fromDegrees(bestTarget.getSkew()),
+                bestTarget.getArea(),
+                bestTarget.getDetectedObjectConfidence(),
+                bestTarget.getDetectedObjectClassID());
       } else {
-        inputs.latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero);
+        inputs.latestTargetObservation =
+            new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, -1, -1);
       }
 
       // Add pose observation