Add code from photonvision

src/main/java/frc4388/robot/Constants.java

@@ -27,6 +27,15 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
 import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
@@ -267,6 +276,16 @@ public final class Constants {
       }
     
     public static final class VisionConstants {
+        public static final String CAMERA_NAME = "photonvision";
+
+        public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(), new Rotation3d());
+
+        public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+
+        // The standard deviations of our vision estimated poses, which affect correction rate
+        // (Fake values. Experiment and determine estimation noise on an actual robot.)
+        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+        public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
     }
 
     public static final class DriveConstants {

src/main/java/frc4388/robot/RobotContainer.java

@@ -33,6 +33,7 @@ import frc4388.robot.commands.Swerve.neoJoystickRecorder;
 
 // Subsystems
 // import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
@@ -53,8 +54,10 @@ public class RobotContainer {
     
     /* Subsystems */
     // private final LED m_robotLED = new LED();
+    public final Vision m_vision = new Vision(m_robotMap.camera);
+
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
 
-    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);

src/main/java/frc4388/robot/RobotMap.java

@@ -8,6 +8,9 @@
 package frc4388.robot;
 
 import com.ctre.phoenix6.hardware.TalonFX;
+
+import org.photonvision.PhotonCamera;
+
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
@@ -18,6 +21,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 // import edu.wpi.first.wpilibj.motorcontrol.Spark;
 // import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.Constants.VisionConstants;
 // import frc4388.robot.subsystems.SwerveModule;
 import frc4388.utility.RobotGyro;
 
@@ -28,6 +32,8 @@ import frc4388.utility.RobotGyro;
 public class RobotMap {
     // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
     // public RobotGyro gyro = new RobotGyro(m_pigeon2);
+
+    public PhotonCamera camera = new PhotonCamera(VisionConstants.CAMERA_NAME);
     
     public RobotMap() {
         configureDriveMotorControllers();

src/main/java/frc4388/robot/commands/GotoPositionCommand.java

@@ -0,0 +1,43 @@
+package frc4388.robot.commands;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.UtilityStructs.AutoRecordingFrame;
+import frc4388.utility.controller.VirtualController;
+
+public class GotoPositionCommand extends Command {
+    private boolean isFinished = false;
+
+    SwerveDrive swerveDrive;
+
+    /**
+     * Command to drive robot to position.
+     * @param SwerveDrive m_robotSwerveDrive
+     */
+    public GotoPositionCommand(SwerveDrive swerveDrive) {
+        this.swerveDrive = swerveDrive;
+    }
+
+    @Override
+    public void initialize() {
+        isFinished = false;
+    }
+
+    @Override
+    public void execute() {
+        
+
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+       
+    }
+    
+    @Override
+    public boolean isFinished() {
+        return isFinished;
+    }
+}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -33,6 +33,8 @@ import frc4388.utility.Status.ReportLevel;
 
 public class SwerveDrive extends Subsystem {
   private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+
+  private Vision vision;
   
   private int gear_index = SwerveDriveConstants.STARTING_GEAR;
   private boolean stopped = false;
@@ -45,16 +47,14 @@ public class SwerveDrive extends Subsystem {
   public Rotation2d orientRotTarget = new Rotation2d();
   public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
-  private Field2d field = new Field2d();
-
   /** Creates a new SwerveDrive. */
-  public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
+  public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
     super();
 
-    SmartDashboard.putData(field);
-
     this.swerveDriveTrain = swerveDriveTrain;
+    this.vision = vision;
   }
+
   // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
   //   // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
   //   // rightStick.getAngle()
@@ -172,10 +172,9 @@ public class SwerveDrive extends Subsystem {
     SmartDashboard.putNumber("Gyro", getGyroAngle());
     SmartDashboard.putNumber("RotTartget", rotTarget);
 
-    Optional<Pose2d> e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds());
+    vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(Vision.getTime()));
 
-    if(e.isPresent())
-      field.setRobotPose(e.get());
+    // if(e.isPresent())
   }
 
   private void reset_index() {

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

@@ -0,0 +1,242 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
+import java.util.List;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import com.ctre.phoenix6.Utils;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+
+public class Vision extends Subsystem {
+
+    private PhotonCamera camera;
+
+    private boolean isTag = false;
+    private Pose2d lastVisionPose = new Pose2d();
+    private Pose2d lastPhysOdomPose = new Pose2d();
+
+    private Matrix<N3, N1> curStdDevs;
+    private final PhotonPoseEstimator photonEstimator;
+
+    private Field2d field = new Field2d();
+    
+    public Vision(PhotonCamera camera){
+        this.camera = camera;
+        SmartDashboard.putData(field);
+
+        photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS);
+        photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+    }
+
+    @Override
+    public void periodic() {
+        var result = camera.getLatestResult();
+        isTag = result.hasTargets();
+
+        if(!isTag){
+            field.setRobotPose(lastPhysOdomPose);
+            return;
+        }
+
+        var EstimatedRobotPose = getEstimatedGlobalPose();
+
+        // In case the pose estimator fails to estimate the pose, fallback to physical odometry.
+        if(EstimatedRobotPose.isEmpty()){
+            field.setRobotPose(lastPhysOdomPose);
+            isTag = false;
+            return;
+        }
+
+        field.setRobotPose(lastVisionPose);
+    }
+
+
+
+
+    /**
+     * The latest estimated robot pose on the field from vision data. This may be empty. This should
+     * only be called once per loop.
+     *
+     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
+     * {@link getEstimationStdDevs}
+     *
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
+     *     used for estimation.
+     */
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
+        Optional<EstimatedRobotPose> visionEst = Optional.empty();
+        for (var change : camera.getAllUnreadResults()) {
+            visionEst = photonEstimator.update(change);
+            updateEstimationStdDevs(visionEst, change.getTargets());
+
+            // if (Robot.isSimulation()) {
+            //     visionEst.ifPresentOrElse(
+            //             est ->
+            //                     getSimDebugField()
+            //                             .getObject("VisionEstimation")
+            //                             .setPose(est.estimatedPose.toPose2d()),
+            //             () -> {
+            //                 getSimDebugField().getObject("VisionEstimation").setPoses();
+            //             });
+            // }
+        }
+        return visionEst;
+    }
+
+
+
+
+
+
+
+    /**
+     * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
+     * deviations based on number of tags, estimation strategy, and distance from the tags.
+     *
+     * @param estimatedPose The estimated pose to guess standard deviations for.
+     * @param targets All targets in this camera frame
+     */
+    private void updateEstimationStdDevs(
+            Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
+        if (estimatedPose.isEmpty()) {
+            // No pose input. Default to single-tag std devs
+            curStdDevs = VisionConstants.kSingleTagStdDevs;
+
+        } else {
+            // Pose present. Start running Heuristic
+            var estStdDevs = VisionConstants.kSingleTagStdDevs;
+            int numTags = 0;
+            double avgDist = 0;
+
+            // Precalculation - see how many tags we found, and calculate an average-distance metric
+            for (var tgt : targets) {
+                var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+                if (tagPose.isEmpty()) continue;
+                numTags++;
+                avgDist +=
+                        tagPose
+                                .get()
+                                .toPose2d()
+                                .getTranslation()
+                                .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+            }
+
+            if (numTags == 0) {
+                // No tags visible. Default to single-tag std devs
+                curStdDevs = VisionConstants.kSingleTagStdDevs;
+            } else {
+                // One or more tags visible, run the full heuristic.
+                avgDist /= numTags;
+                // Decrease std devs if multiple targets are visible
+                if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
+                // Increase std devs based on (average) distance
+                if (numTags == 1 && avgDist > 4)
+                    estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+                else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+                curStdDevs = estStdDevs;
+            }
+        }
+    }
+
+    /**
+     * Returns the latest standard deviations of the estimated pose from {@link
+     * #getEstimatedGlobalPose()}, for use with {@link
+     * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
+     * only be used when there are targets visible.
+     */
+    public Matrix<N3, N1> getEstimationStdDevs() {
+        return curStdDevs;
+    }
+
+
+
+
+
+
+
+
+
+    public void setLastOdomPose(Optional<Pose2d> pose){
+        if(pose.isPresent())
+            lastPhysOdomPose = pose.get();
+    }
+
+    public Pose2d getPose2d() {
+        if(isTag)
+            return lastVisionPose;
+        else
+            return lastPhysOdomPose;
+    }
+
+    public static double getTime() {
+        return Utils.getCurrentTimeSeconds();
+    }
+
+
+
+
+
+
+
+
+
+
+    @Override
+    public String getSubsystemName() {
+        return "Vision";
+    }
+
+
+  ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
+  .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+  .withSize(2, 2);
+
+  GenericEntry sbTag = subsystemLayout
+  .add("Tag Detected", false)
+  .withWidget(BuiltInWidgets.kBooleanBox)
+  .getEntry();
+
+//   GenericEntry sbShiftState = subsystemLayout
+//   .add("Shift State", 0)
+//   .withWidget(BuiltInWidgets.kNumberBar)
+//   .getEntry();
+
+
+    @Override
+    public void queryStatus() {
+        sbTag.setBoolean(isTag);
+        field.setRobotPose(getPose2d());
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status status = new Status();
+
+        return status;
+    }
+    
+}

src/main/java/frc4388/utility/CanDevice.java

@@ -35,13 +35,13 @@ public class CanDevice {
         System.out.println(getName() + " - " + str);
     }
 
-    public Status queryStatus() {
-         Status s = new Status();
+    // public Status queryStatus() {
+    //      Status s = new Status();
 
-        s.addReport(ReportLevel.INFO, "TODO");
+    //     s.addReport(ReportLevel.INFO, "TODO");
 
-        return s;
-    }
+    //     return s;
+    // }
 
     public Status diagnosticStatus() {
         Status s = new Status();