Vision challenge

src/main/java/frc/robot/Constants.java

@@ -1,7 +1,12 @@
 package frc.robot;
 
 import com.revrobotics.CANSparkBase.IdleMode;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
@@ -134,4 +139,18 @@ public static final class AutoConstants {
     public static final class NeoMotorConstants {
         public static final double kFreeSpeedRpm = 5676;
     }
+    // visionconstants
+     public static final class VisionConstants {
+        public static final String kCameraName = "Arducam_OV2311_USB_Camera";
+        public static final Transform3d kCameraOffset = new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(16.0),
+                Units.inchesToMeters(-1.5),
+                Units.inchesToMeters(16.0)),
+            new Rotation3d(
+                0.0,
+                Rotation2d.fromDegrees(-15.0).getRadians(),
+                0.0
+            ));
+    }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -4,11 +4,15 @@
 
 package frc.robot;
 
+import java.io.IOException;
+import java.sql.DriverPropertyInfo;
+
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -20,19 +24,27 @@
 import frc.robot.subsystems.DownBeat;
 import frc.robot.subsystems.Glissando;
 import frc.robot.subsystems.UpBeat;
+import frc.robot.subsystems.Vision;
 
 public class RobotContainer {
     private final Bass m_robotDrive = new Bass();
     private final DownBeat m_intake = new DownBeat();
     private final UpBeat m_shooter = new UpBeat();
-    private final Glissando m_climb = new Glissando();
+    //private final Glissando m_climb = new Glissando();
+    private Vision vision;
     private final CommandJoystick one = new CommandJoystick(0);
     private final CommandJoystick two = new CommandJoystick(1);
     private final CommandXboxController xboxController = new CommandXboxController(2);
     private final LoggedDashboardChooser<Command> autoChooser = new LoggedDashboardChooser<>("AutoChooser",
             AutoBuilder.buildAutoChooser());
 
     public RobotContainer() {
+        try {
+            vision = new Vision(m_robotDrive::visionPose);
+        }
+        catch(IOException e){
+            DriverStation.reportWarning("Unable to initialize vision", e.getStackTrace());
+        }
         configureBindings();
 
         m_robotDrive.setDefaultCommand(

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

@@ -10,12 +10,12 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
@@ -57,16 +57,17 @@ public class Bass extends SubsystemBase {
     private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DriveConstants.kRotationalSlewRate);
     private double m_prevTime = WPIUtilJNI.now() * 1e-6;
 
-    // Odometry class for tracking robot pose
-    SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
+    // Pose estimation class for tracking robot pose
+    SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(
             DriveConstants.kDriveKinematics,
             getHeading(),
             new SwerveModulePosition[] {
                     m_frontLeft.getPosition(),
                     m_frontRight.getPosition(),
                     m_rearLeft.getPosition(),
                     m_rearRight.getPosition()
-            });
+            }, 
+            new Pose2d());
 
     /** Creates a new DriveSubsystem. */
     public Bass() {
@@ -102,7 +103,7 @@ public Bass() {
     @Override
     public void periodic() {
         // Update the odometry in the periodic block
-        m_odometry.update(
+        m_poseEstimator.update(
                 getHeading(),
                 new SwerveModulePosition[] {
                         m_frontLeft.getPosition(),
@@ -126,7 +127,7 @@ public void periodic() {
      * @return The pose.
      */
     public Pose2d getPose() {
-        return m_odometry.getPoseMeters();
+        return m_poseEstimator.getEstimatedPosition();
     }
 
     /**
@@ -135,7 +136,7 @@ public Pose2d getPose() {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
-        m_odometry.resetPosition(
+        m_poseEstimator.resetPosition(
                 getHeading(),
                 new SwerveModulePosition[] {
                         m_frontLeft.getPosition(),
@@ -146,6 +147,9 @@ public void resetOdometry(Pose2d pose) {
                 pose);
     }
 
+    public void visionPose(Pose2d pose, double timestamp){
+        m_poseEstimator.addVisionMeasurement(pose, timestamp);
+    }
     /**
      * Method to drive the robot using joystick info.
      *

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

@@ -0,0 +1,60 @@
+package frc.robot.subsystems;
+
+import java.io.IOException;
+import java.util.Optional;
+import java.util.function.BiConsumer;
+
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.targeting.PhotonPipelineResult;
+
+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 frc.robot.Constants;
+
+public class Vision extends SubsystemBase {
+    private final PhotonCamera photonCamera;    
+    private final PhotonPoseEstimator poseEstimator;
+    private final BiConsumer<Pose2d, Double> consumer;
+
+    public Vision(BiConsumer<Pose2d, Double> consumer) throws IOException{
+        photonCamera = new PhotonCamera(Constants.VisionConstants.kCameraName);
+        poseEstimator = new PhotonPoseEstimator(
+            AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile),
+            PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_RIO,
+            Constants.VisionConstants.kCameraOffset);
+        this.consumer = consumer;
+    }
+
+    @Override
+    public void periodic(){
+        boolean connected = photonCamera.isConnected();
+        Logger.recordOutput("Vision/Connected", connected);
+        if (!connected)
+            return;
+
+        PhotonPipelineResult pipelineResult = photonCamera.getLatestResult();
+        boolean hasTargets = pipelineResult.hasTargets();
+        Logger.recordOutput("Vision/HasTargets", hasTargets);
+        if (!hasTargets)
+            return;
+
+        Optional<EstimatedRobotPose> poseResult = poseEstimator.update(pipelineResult);
+        boolean posePresent = poseResult.isPresent();
+        Logger.recordOutput("Vision/HasPose", posePresent);
+        if (!posePresent)
+            return;
+
+        EstimatedRobotPose estimatedPose = poseResult.get();
+        Logger.recordOutput("Vision/Pose", estimatedPose.estimatedPose);
+        Logger.recordOutput("Vision/Timestamp", estimatedPose.timestampSeconds);
+        Logger.recordOutput("Vision/Targets", estimatedPose.targetsUsed.size());
+        Logger.recordOutput("Vision/Strategy", estimatedPose.strategy);
+
+        consumer.accept(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
+    }
+}

vendordeps/photonlib.json

@@ -0,0 +1,57 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.2.2",
+    "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.2",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2024.2.2",
+            "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.2"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2024.2.2"
+        }
+    ]
+}