Vision with Auto Aim

src/main/deploy/swerve/comp/swervedrive.json

@@ -4,7 +4,7 @@
     "id": 0,
     "canbus": null
   },
-  "invertedIMU": true,
+  "invertedIMU": false,
   "modules": [
     "frontleft.json",
     "frontright.json",

src/main/java/frc3512/robot/Constants.java

@@ -1,5 +1,6 @@
 package frc3512.robot;
 
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 
 public final class Constants {
@@ -27,4 +28,9 @@ public static final class GeneralConstants {
 
     public static final RobotType robotType = RobotType.COMP;
   }
+
+  public static final class VisionConstants {
+    public static final String cameraName = "USB_GS_Camera";
+    public static final Transform3d robotToCam = new Transform3d();
+  }
 }

src/main/java/frc3512/robot/subsystems/Superstructure.java

@@ -16,6 +16,7 @@ public class Superstructure extends SubsystemBase {
 
   // Subsystems
   public final Swerve swerve = new Swerve();
+  public final Vision vision = new Vision();
 
   // Joysticks
   private final CommandXboxController driverXbox =
@@ -46,8 +47,9 @@ public void configureAxisActions() {
                     driverXbox.getRawAxis(strafeAxis), Constants.SwerveConstants.swerveDeadband),
             () ->
                 MathUtil.applyDeadband(
-                    driverXbox.getRawAxis(rotationAxis),
-                    Constants.SwerveConstants.swerveDeadband)));
+                    driverXbox.getRawAxis(rotationAxis), Constants.SwerveConstants.swerveDeadband),
+            () -> driverXbox.leftBumper().getAsBoolean(),
+            vision.returnCamera(vision)));
   }
 
   public void setMotorBrake(boolean brake) {

src/main/java/frc3512/robot/subsystems/Swerve.java

@@ -1,6 +1,7 @@
 package frc3512.robot.subsystems;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -13,7 +14,10 @@
 import frc3512.lib.logging.SpartanEntryManager;
 import frc3512.robot.Constants;
 import java.io.File;
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
 import swervelib.SwerveController;
 import swervelib.SwerveDrive;
 import swervelib.parser.SwerveDriveConfiguration;
@@ -64,22 +68,42 @@ public Swerve() {
    * @return Drive command.
    */
   public Command driveCommand(
-      DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) {
+      DoubleSupplier translationX,
+      DoubleSupplier translationY,
+      DoubleSupplier angularRotationX,
+      BooleanSupplier doAim,
+      PhotonCamera camera) {
     return run(
         () -> {
-          swerve.drive(
-              new Translation2d(
-                  MathUtil.applyDeadband(
-                      translationX.getAsDouble() * swerve.getMaximumVelocity(),
-                      Constants.SwerveConstants.swerveDeadband),
-                  MathUtil.applyDeadband(
-                      translationY.getAsDouble() * swerve.getMaximumVelocity(),
-                      Constants.SwerveConstants.swerveDeadband)),
-              MathUtil.applyDeadband(
-                  angularRotationX.getAsDouble() * swerve.getMaximumAngularVelocity(),
-                  Constants.SwerveConstants.swerveDeadband),
-              true,
-              false);
+          PhotonPipelineResult result = camera.getLatestResult();
+          final double ANGULAR_P = 0.1;
+          final double ANGULAR_D = 0.0;
+          PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
+          if (result.hasTargets() && doAim.getAsBoolean()) {
+            drive(
+                swerve.swerveController.getRawTargetSpeeds(
+                    MathUtil.applyDeadband(
+                        translationX.getAsDouble() * swerve.getMaximumVelocity(),
+                        Constants.SwerveConstants.swerveDeadband),
+                    MathUtil.applyDeadband(
+                        translationY.getAsDouble() * swerve.getMaximumVelocity(),
+                        Constants.SwerveConstants.swerveDeadband),
+                    -turnController.calculate(result.getBestTarget().getYaw(), 0)));
+          } else {
+            swerve.drive(
+                new Translation2d(
+                    MathUtil.applyDeadband(
+                        translationX.getAsDouble() * swerve.getMaximumVelocity(),
+                        Constants.SwerveConstants.swerveDeadband),
+                    MathUtil.applyDeadband(
+                        translationY.getAsDouble() * swerve.getMaximumVelocity(),
+                        Constants.SwerveConstants.swerveDeadband)),
+                MathUtil.applyDeadband(
+                    angularRotationX.getAsDouble() * swerve.getMaximumAngularVelocity(),
+                    Constants.SwerveConstants.swerveDeadband),
+                true,
+                false);
+          }
         });
   }
 
@@ -170,7 +194,7 @@ public void setMotorBrake(boolean brake) {
    * @return The yaw angle
    */
   public Rotation2d getHeading() {
-    return swerve.getOdometryHeading();
+    return swerve.getYaw();
   }
 
   /**

src/main/java/frc3512/robot/subsystems/Vision.java

@@ -0,0 +1,47 @@
+package frc3512.robot.subsystems;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc3512.robot.Constants;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+
+public class Vision extends SubsystemBase {
+  public PhotonCamera photonCamera = new PhotonCamera(Constants.VisionConstants.cameraName);
+  public PhotonPoseEstimator photonPoseEstimator;
+  public AprilTagFieldLayout atfl;
+
+  public Vision() {
+    PhotonCamera.setVersionCheckEnabled(false);
+    // photonCamera.setDriverMode(true);
+
+    atfl = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+
+    // Create pose estimator
+    photonPoseEstimator =
+        new PhotonPoseEstimator(
+            atfl,
+            PoseStrategy.LOWEST_AMBIGUITY,
+            photonCamera,
+            Constants.VisionConstants.robotToCam);
+  }
+
+  /**
+   * @param estimatedRobotPose The current best guess at robot pose
+   * @return A pair of the fused camera observations to a single Pose2d on the field, and the time
+   *     of the observation. Assumes a planar field and the robot is always firmly on the ground
+   */
+  public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
+    photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
+    return photonPoseEstimator.update();
+  }
+
+  public PhotonCamera returnCamera(Vision vision) {
+    return vision.photonCamera;
+  }
+}

vendordeps/photonlib.json

@@ -0,0 +1,57 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.2.4",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-cpp",
+            "version": "v2024.2.4",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2024.2.4",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2024.2.4"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2024.2.4"
+        }
+    ]
+}