Photon vision

src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java

@@ -21,6 +21,7 @@
 import com.stuypulse.stuylib.util.AngleVelocity;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public abstract class SwerveDriveDriveAligned extends Command {
@@ -99,5 +100,7 @@ public void execute() {
                     )
                 )         
             );
+        SmartDashboard.putNumber("Alignment/Distance to Target", getDistanceToTarget());
+        SmartDashboard.putNumber("Alignment/Target Angle", getTargetAngle().getDegrees());
     }
 }

src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java

@@ -4,8 +4,11 @@
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.stuylib.input.Gamepad;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class SwerveDriveDriveAlignedSpeaker extends SwerveDriveDriveAligned{
 
@@ -17,7 +20,7 @@ public SwerveDriveDriveAlignedSpeaker(Gamepad driver) {
     protected Rotation2d getTargetAngle() {
         Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation();
         Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation();
-        return speakerPose.minus(currentPose).getAngle();
+        return currentPose.minus(speakerPose).getAngle();
     }
 
     @Override

src/main/java/com/stuypulse/robot/commands/vision/VisionChangeWhiteList.java

@@ -1,6 +1,6 @@
 package com.stuypulse.robot.commands.vision;
 
-import com.stuypulse.robot.subsystems.vision.TheiaTagVision;
+import com.stuypulse.robot.subsystems.vision.PhotonVision;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
@@ -19,6 +19,6 @@ public VisionChangeWhiteList(int... ids) {
 
     @Override
     public void initialize() {
-        TheiaTagVision.getInstance().setTagWhitelist(ids);
+        PhotonVision.getInstance().setTagWhitelist(ids);
     }
 }

src/main/java/com/stuypulse/robot/constants/Cameras.java

@@ -25,17 +25,17 @@ public interface Limelight {
     public CameraConfig[] APRILTAG_CAMERAS = new CameraConfig[] {
         // TO DO: find positions
         new CameraConfig(
-            "samera0", //tower camera
+            "tower-cam",
             new Pose3d(
                 new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(-3.333797), Units.inchesToMeters(23.929362)),
-                new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(0))
+                new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(180))
             ),
-            "29",
+            "11",
             3000
-        ), //10.6.94.29:5802
+        ), //10.6.94.11:5800/#/dashboard
 
         new CameraConfig(
-            "samera1", //electronic plate camera
+            "plate-cam",
             new Pose3d(
                 new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(4.863591), Units.inchesToMeters(19.216471)),
                 new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0))

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -178,7 +178,7 @@ public interface Swerve {
         double MAX_LINEAR_VELOCITY = SAFE_MODE_ENABLED ? 1.0 : 4.9;
         double MAX_LINEAR_ACCEL = SAFE_MODE_ENABLED ? 10 : 15;
         double MAX_ANGULAR_VELOCITY = SAFE_MODE_ENABLED ? 3.0 : 10.0;
-        double MAX_ANGULAR_ACCEL = SAFE_MODE_ENABLED ? 25.0 : 85.0;
+        double MAX_ANGULAR_ACCEL = SAFE_MODE_ENABLED ? 25.0 : 200.0;
 
         String CAN_BUS_NAME = "swerve";
 

src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java

@@ -212,11 +212,9 @@ private void updateEstimatorWithVisionData(ArrayList<VisionData> outputs) {
             timestampSum += data.getTimestamp() * data.getArea();
         }
 
-        // addVisionMeasurement(poseSum.div(areaSum), timestampSum / areaSum,
-            // DriverStation.isAutonomous() ? VecBuilder.fill(0.9, 0.9, 10) : VecBuilder.fill(0.7, 0.7, 10));
-
-        addVisionMeasurement(new Pose2d(3, 3, new Rotation2d()), timestampSum / areaSum,
+        addVisionMeasurement(poseSum.div(areaSum), timestampSum / areaSum,
             DriverStation.isAutonomous() ? VecBuilder.fill(0.9, 0.9, 10) : VecBuilder.fill(0.7, 0.7, 10));
+
     }
 
     public void setVisionEnabled(boolean enabled) {

src/main/java/com/stuypulse/robot/subsystems/vision/AprilTagVision.java

@@ -11,7 +11,7 @@ public abstract class AprilTagVision extends SubsystemBase {
     private static final AprilTagVision instance;
 
     static {
-        instance = new TheiaTagVision();
+        instance = new PhotonVision();
     }
 
     public static AprilTagVision getInstance() {

src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java

@@ -0,0 +1,132 @@
+package com.stuypulse.robot.subsystems.vision;
+
+import com.stuypulse.robot.constants.Cameras;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.util.vision.VisionData;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import java.util.ArrayList;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class PhotonVision extends AprilTagVision {
+
+    private final PhotonCamera[] cameras;
+    private final boolean[] enabled;
+    private final PhotonPoseEstimator[] poseEstimators;
+    private final ArrayList<VisionData> outputs;
+
+    private final FieldObject2d robot;
+
+    protected PhotonVision() {
+        this.cameras = new PhotonCamera[Cameras.APRILTAG_CAMERAS.length];
+        for (int i = 0; i < Cameras.APRILTAG_CAMERAS.length; i++) {
+            cameras[i] = new PhotonCamera(Cameras.APRILTAG_CAMERAS[i].getName());
+        }
+
+        enabled = new boolean[Cameras.APRILTAG_CAMERAS.length];
+
+        for (int i = 0; i < enabled.length; i++) {
+            enabled[i] = true;
+        }
+
+        poseEstimators = new PhotonPoseEstimator[Cameras.APRILTAG_CAMERAS.length];
+        for (int i = 0; i < Cameras.APRILTAG_CAMERAS.length; i++) {
+            poseEstimators[i] = new PhotonPoseEstimator(
+                AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), 
+                PoseStrategy.AVERAGE_BEST_TARGETS, 
+                Cameras.APRILTAG_CAMERAS[i].getLocation().minus(new Pose3d())
+                );
+        }
+
+        outputs = new ArrayList<VisionData>();
+
+        robot = SwerveDrive.getInstance().getField().getObject("Vision Pose");
+    }
+
+    /**
+     * Returns the outputs of the vision system.
+     *
+     * @return the outputs of the vision system
+     */
+    @Override
+    public ArrayList<VisionData> getOutputs() {
+        return outputs;
+    }
+
+    /**
+     * Sets the tag layout of the vision system.
+     *
+     * @param ids the tag IDs
+     */
+    @Override
+    public void setTagWhitelist(int... ids) {
+
+    }
+
+    @Override
+    public void setCameraEnabled(String name, boolean enabled) {
+        for (int i = 0; i < Cameras.APRILTAG_CAMERAS.length; i++) {
+            if (cameras[i].getName().equals(name)) {
+                this.enabled[i] = enabled;
+            }
+        }
+    }
+
+    private int[] getIDs(PhotonPipelineResult pipelineResult) {
+        ArrayList<Integer> ids = new ArrayList<Integer>();
+        for (PhotonTrackedTarget target : pipelineResult.getTargets()) {
+            ids.add(target.getFiducialId());
+        }
+        return ids.stream().mapToInt(i -> i).toArray();
+    }
+
+    @Override
+    public void periodic() {
+        super.periodic();
+
+        outputs.clear();
+
+        for (int i = 0; i < cameras.length; i++) {
+            final int index = i;
+            if (enabled[index]) {
+                PhotonPipelineResult latestResult = cameras[index].getLatestResult();
+                Optional<EstimatedRobotPose> estimatedRobotPose = poseEstimators[index].update(latestResult);
+                estimatedRobotPose.ifPresent(
+                    (EstimatedRobotPose robotPose) -> {
+                        VisionData data = new VisionData(robotPose.estimatedPose, getIDs(latestResult), robotPose.timestampSeconds, latestResult.getBestTarget().getArea());
+                        outputs.add(data);
+                        updateTelemetry("Vision/" + cameras[index].getName(), data);
+                    }
+                );
+            }
+        }
+
+        SmartDashboard.putBoolean("Vision/Has Any Data", outputs.size() > 0);
+    }
+
+    private void updateTelemetry(String prefix, VisionData data) {
+        SmartDashboard.putNumber(prefix + "/Pose X", data.getPose().getX());
+        SmartDashboard.putNumber(prefix + "/Pose Y", data.getPose().getY());
+        SmartDashboard.putNumber(prefix + "/Pose Z", data.getPose().getZ());
+
+        SmartDashboard.putNumber(prefix + "/Distance to Tag", data.getDistanceToPrimaryTag());
+
+        SmartDashboard.putNumber(prefix + "/Pose Rotation", Units.radiansToDegrees(data.getPose().getRotation().getAngle()));
+        SmartDashboard.putNumber(prefix + "/Timestamp", data.getTimestamp());
+
+        robot.setPose(data.getPose().toPose2d());
+    }
+}