From 3e61df91774d185d9d73210f993f6678068396a0 Mon Sep 17 00:00:00 2001 From: Eden Date: Mon, 13 Jan 2025 20:52:44 -0600 Subject: [PATCH 1/6] Calibrated camera, started on vision subsystem --- src/main/java/frc/robot/Constants.java | 6 ++++ .../frc/robot/subsystems/vision/VisionIO.java | 28 +++++++++++++++++++ .../subsystems/vision/VisionIOPhotonLib.java | 11 ++++++++ 3 files changed, 45 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 925ff63..214f7dc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -99,6 +99,12 @@ General.ROBOT_MASS, General.ROBOT_MOI, new ModuleConfig(WHEEL_RADIUS, General.MA ); } + public static class Vision + { + public static final String PHOTON_CAMERA_NAME = "photonCam"; //TODO Name + + } + public static class General { public static final double MAX_LINEAR_SPEED = 4.8; // m/s diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..ecdee3e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface VisionIO +{ + @AutoLog + public static class VisionIOInputs + { + public double captureTimestamp = 0.0; + public double[] cornerX = new double[] {}; + public double[] cornerY = new double[] {}; + public double[] closeCornerX = new double[] {}; + public double[] closeCornerY = new double[] {}; + public Pose2d pose = new Pose2d(); + public boolean hasPose = false; + public int numProcessedTargets = 0; + public double[] targetDistances = new double[] {}; + public int[] targetIds = new int[] {}; + public double[] targetYaws = new double[] {}; + } + + public default void updateInputs(VisionIOInputs inputs) + { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java new file mode 100644 index 0000000..7be6832 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.vision; + +import org.photonvision.PhotonCamera; + +import frc.robot.Constants; + +public class VisionIOPhotonLib implements VisionIO +{ + private final PhotonCamera _camera = new PhotonCamera(Constants.Vision.PHOTON_CAMERA_NAME); + +} From 6fb11128984b2c702c230aaf9d78a513d2bf24ae Mon Sep 17 00:00:00 2001 From: Eden Date: Mon, 20 Jan 2025 20:41:01 -0600 Subject: [PATCH 2/6] Vision work --- .../frc/robot/subsystems/vision/Vision.java | 6 ++ .../frc/robot/subsystems/vision/VisionIO.java | 10 +-- .../subsystems/vision/VisionIOPhotonLib.java | 63 +++++++++++++++++++ 3 files changed, 74 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/vision/Vision.java diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..caef367 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,6 @@ +package frc.robot.subsystems.vision; + +public class Vision +{ + +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index ecdee3e..6a23679 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -10,14 +10,14 @@ public interface VisionIO public static class VisionIOInputs { public double captureTimestamp = 0.0; - public double[] cornerX = new double[] {}; - public double[] cornerY = new double[] {}; - public double[] closeCornerX = new double[] {}; - public double[] closeCornerY = new double[] {}; + public double[] tagX = new double[] {}; //represents the x distance of all tags + //public double[] tagY = new double[] {}; + public double[] closeTagX = new double[] {}; + //public double[] closeTagY = new double[] {}; public Pose2d pose = new Pose2d(); public boolean hasPose = false; public int numProcessedTargets = 0; - public double[] targetDistances = new double[] {}; + public double[] targetDistances = new double[] {}; //Target = specific april tag public int[] targetIds = new int[] {}; public double[] targetYaws = new double[] {}; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java index 7be6832..2273ce1 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java @@ -1,11 +1,74 @@ package frc.robot.subsystems.vision; +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 edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.math.estimator.PoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; import frc.robot.Constants; public class VisionIOPhotonLib implements VisionIO { private final PhotonCamera _camera = new PhotonCamera(Constants.Vision.PHOTON_CAMERA_NAME); + private double _captureTimesStamp = 0.0; + private double[] _tagX = new double[] {}; + private double[] _closeTagX = new double[] {}; + private Pose2d _pose = new Pose2d(); + private boolean _hasPose = false; + private int[] _targetIds = new int[] {}; + private double[] _targetYaws = new double[] {}; + private int _numProcessedTargets ; + private final AprilTagFieldLayout fieldLayout; + private final PhotonPoseEstimator _PoseEstimator; + private final Transform3d _robotToCamera; + private final Pose3d _lastEstimatedPose = new Pose3d(); + public class VisionIOPhotonlib(Drive drive) + { + try + { + fieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); //TODO: update layout + } + catch(Exception e) + { + System.out.println("Exception encountered: " + e.getMessage()); + } + _PoseEstimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, _robotToCamera); + _PoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + @Override + public synchronized void updateInputs(VisionIOInputs inputs) + { + PhotonPipelineResult _result = _camera.getLatestResult(); + inputs.captureTimestamp = _captureTimesStamp; + inputs.tagX = _tagX; + inputs.closeTagX = _closeTagX; + inputs.pose = _pose; + inputs.hasPose = _hasPose; + inputs.targetIds = _targetIds; + inputs.targetYaws = _targetYaws; + inputs.numProcessedTargets = _numProcessedTargets; + + if (_result.hasTargets()) + { + Optional _poseOptional = _PoseEstimator.update(_result); + if (_poseOptional.isPresent()) + { + EstimatedRobotPose _estimatedPose = _poseOptional.get(); + inputs.captureTimestamp = _result.getTimestampSeconds(); + inputs._estimatedPose + } + } + } } From 5fb332625f052bf7f8edbfdfd75e933506abac80 Mon Sep 17 00:00:00 2001 From: Eden Date: Tue, 21 Jan 2025 19:15:01 -0600 Subject: [PATCH 3/6] "finsihed" vision --- src/main/java/frc/robot/Constants.java | 4 +- .../frc/robot/subsystems/vision/Vision.java | 52 +++++- .../frc/robot/subsystems/vision/VisionIO.java | 14 +- .../subsystems/vision/VisionIOPhotonLib.java | 160 +++++++++++++----- 4 files changed, 178 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 214f7dc..b82e37f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -101,8 +101,8 @@ General.ROBOT_MASS, General.ROBOT_MOI, new ModuleConfig(WHEEL_RADIUS, General.MA public static class Vision { - public static final String PHOTON_CAMERA_NAME = "photonCam"; //TODO Name - + public static final String PHOTON_CAMERA_NAME = "photonCam"; // TODO Name + public static final double MAX_DETECTION_RANGE = Units.inchesToMeters(300); } public static class General diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index caef367..be751c5 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,6 +1,54 @@ package frc.robot.subsystems.vision; -public class Vision +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.drive.Drive; + +public class Vision extends SubsystemBase { - + private final VisionIO _io; + private final VisionIOInputsAutoLogged _inputs = new VisionIOInputsAutoLogged(); + private final Drive _drive; + private final PIDController _rotatePID; + private double _maxSpeed = 0.3; + private double _lastTimestamp = 0.0; + private boolean _targetingEnabled = false; + + public Vision(Drive drive, VisionIO io) + { + _drive = drive; + _io = io; + + _rotatePID = new PIDController(0.026, 0, 0); + _rotatePID.enableContinuousInput(-180, 180); + _rotatePID.setTolerance(2); + } + + @Override + public void periodic() + { + _io.updateInputs(_inputs); + Logger.processInputs("Vision", _inputs); + + // if (Constants.Vision.ENABLE_POSE_CORRECTION) + // { + // if (!DriverStation.isAutonomous() && _lastTimestamp != + // _inputs.captureTimestamp) + // { + // if (_inputs.hasPose != false) + // { + // _drive.addVisionMeasurement(_inputs.pose, _inputs.captureTimestamp); + // } + + // _lastTimestamp = _inputs.captureTimestamp; + // } + // } + } + + public void setTargetingEnabled(boolean enabled) + { + _targetingEnabled = enabled; + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 6a23679..4305e3d 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -9,15 +9,15 @@ public interface VisionIO @AutoLog public static class VisionIOInputs { - public double captureTimestamp = 0.0; - public double[] tagX = new double[] {}; //represents the x distance of all tags - //public double[] tagY = new double[] {}; - public double[] closeTagX = new double[] {}; - //public double[] closeTagY = new double[] {}; + public double captureTimestamp = 0.0; + public double[] tagX = new double[] {}; // represents the x distance of all tags + // public double[] tagY = new double[] {}; + public double[] closeTagX = new double[] {}; + // public double[] closeTagY = new double[] {}; public Pose2d pose = new Pose2d(); public boolean hasPose = false; public int numProcessedTargets = 0; - public double[] targetDistances = new double[] {}; //Target = specific april tag + public double[] targetDistances = new double[] {}; // Target = specific april tag public int[] targetIds = new int[] {}; public double[] targetYaws = new double[] {}; } @@ -25,4 +25,4 @@ public static class VisionIOInputs public default void updateInputs(VisionIOInputs inputs) { } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java index 2273ce1..b137495 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java @@ -1,5 +1,8 @@ package frc.robot.subsystems.vision; +import java.util.ArrayList; +import java.util.EnumSet; +import java.util.List; import java.util.Optional; import org.photonvision.EstimatedRobotPose; @@ -7,68 +10,143 @@ import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.epilogue.Logged.Strategy; -import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.Constants; +import frc.robot.subsystems.drive.Drive; public class VisionIOPhotonLib implements VisionIO { - private final PhotonCamera _camera = new PhotonCamera(Constants.Vision.PHOTON_CAMERA_NAME); - private double _captureTimesStamp = 0.0; - private double[] _tagX = new double[] {}; - private double[] _closeTagX = new double[] {}; - private Pose2d _pose = new Pose2d(); - private boolean _hasPose = false; - private int[] _targetIds = new int[] {}; - private double[] _targetYaws = new double[] {}; - private int _numProcessedTargets ; - private final AprilTagFieldLayout fieldLayout; - private final PhotonPoseEstimator _PoseEstimator; - private final Transform3d _robotToCamera; - private final Pose3d _lastEstimatedPose = new Pose3d(); - - public class VisionIOPhotonlib(Drive drive) + private final PhotonCamera _camera = new PhotonCamera(Constants.Vision.PHOTON_CAMERA_NAME); + private double _captureTimesStamp = 0.0; + private double[] _tagX = new double[] {}; + private double[] _closeTagX = new double[] {}; + private Pose2d _pose = new Pose2d(); + private boolean _hasPose = false; + private double[] _distances = new double[] {}; + private int[] _targetIds = new int[] {}; + private double[] _targetYaws = new double[] {}; + private int _numProcessedTargets; + private AprilTagFieldLayout fieldLayout; + private PhotonPoseEstimator _poseEstimator; + private Transform3d _robotToCamera; + private final Pose3d _lastEstimatedPose = new Pose3d(); + + public void VisionIOPhotonlib(Drive drive) { try { - fieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); //TODO: update layout + fieldLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); } - catch(Exception e) + catch (Exception e) { System.out.println("Exception encountered: " + e.getMessage()); } - _PoseEstimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, _robotToCamera); - _PoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + _poseEstimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, _robotToCamera); + // _poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + NetworkTableInstance.getDefault().addListener(NetworkTableInstance.getDefault().getEntry("/photonvision/" + Constants.Vision.PHOTON_CAMERA_NAME + "/latencyMillis"), EnumSet.of(NetworkTableEvent.Kind.kValueRemote), event -> + { + PhotonPipelineResult result = _camera.getLatestResult(); + double timestamp = result.getTimestampSeconds(); + List tagXList = new ArrayList<>(); + List closeTagXList = new ArrayList<>(); + List processedTargets = new ArrayList<>(); + List distances = new ArrayList<>(); + List ids = new ArrayList<>(); + List yaws = new ArrayList<>(); + + for (PhotonTrackedTarget target : result.getTargets()) + { + ids.add(target.getFiducialId()); + yaws.add(target.getYaw()); + + for (TargetCorner corner : target.getDetectedCorners()) + { + tagXList.add(corner.x); + } + + var transform = target.getBestCameraToTarget(); + var distance = Math.hypot(transform.getX(), transform.getY()); + distances.add(distance); + + if (distance <= Constants.Vision.MAX_DETECTION_RANGE) + { + processedTargets.add(target); + + for (TargetCorner corner : target.getDetectedCorners()) + { + closeTagXList.add(corner.x); + } + + } + } + + PhotonPipelineResult processedResult = new PhotonPipelineResult(); + processedResult.getTimestampSeconds(); // TODO: see if need change + + Optional estimatedPose = getEstimatedGlobalPose(drive.getPose(), processedResult); + Pose2d pose = new Pose2d(); + boolean hasPose = false; + + if (estimatedPose.isPresent()) + { + pose = estimatedPose.get().estimatedPose.toPose2d(); + hasPose = true; + } + + synchronized (VisionIOPhotonLib.this) + { + _captureTimesStamp = timestamp; + _tagX = tagXList.stream().mapToDouble(Double::doubleValue).toArray(); + _closeTagX = closeTagXList.stream().mapToDouble(Double::doubleValue).toArray(); + _distances = distances.stream().mapToDouble(Double::doubleValue).toArray(); + _pose = pose; + _numProcessedTargets = processedTargets.size(); + _hasPose = hasPose; + _targetIds = ids.stream().mapToInt(Integer::intValue).toArray(); + _targetYaws = yaws.stream().mapToDouble(Double::doubleValue).toArray(); + } + }); } + @Override public synchronized void updateInputs(VisionIOInputs inputs) { - PhotonPipelineResult _result = _camera.getLatestResult(); - inputs.captureTimestamp = _captureTimesStamp; - inputs.tagX = _tagX; - inputs.closeTagX = _closeTagX; - inputs.pose = _pose; - inputs.hasPose = _hasPose; - inputs.targetIds = _targetIds; - inputs.targetYaws = _targetYaws; + // PhotonPipelineResult _result = _camera.getLatestResult(); + inputs.captureTimestamp = _captureTimesStamp; + inputs.tagX = _tagX; + inputs.closeTagX = _closeTagX; + inputs.pose = _pose; + inputs.hasPose = _hasPose; + inputs.targetIds = _targetIds; + inputs.targetYaws = _targetYaws; inputs.numProcessedTargets = _numProcessedTargets; - - if (_result.hasTargets()) - { - Optional _poseOptional = _PoseEstimator.update(_result); - if (_poseOptional.isPresent()) - { - EstimatedRobotPose _estimatedPose = _poseOptional.get(); - inputs.captureTimestamp = _result.getTimestampSeconds(); - inputs._estimatedPose - } - } + + // if (_result.hasTargets()) + // { + // Optional _poseOptional = _PoseEstimator.update(_result); + // if (_poseOptional.isPresent()) + // { + // EstimatedRobotPose _estimatedPose = _poseOptional.get(); + // inputs.captureTimestamp = _result.getTimestampSeconds(); + // inputs._estimatedPose + // } + // } + } + + private Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose, PhotonPipelineResult result) + { + _poseEstimator.setLastPose(prevEstimatedRobotPose); + return _poseEstimator.update(result); } } From 54e6cdd5c01b4d98225593e5aeaa73f1c0287016 Mon Sep 17 00:00:00 2001 From: Saketh Date: Wed, 22 Jan 2025 18:22:01 -0600 Subject: [PATCH 4/6] updated input structure and add standard deviation calculations for pose estimation for the vision subsystem. --- src/main/java/frc/robot/Constants.java | 15 +- src/main/java/frc/robot/RobotContainer.java | 10 +- .../frc/robot/subsystems/drive/Drive.java | 12 +- .../frc/robot/subsystems/vision/Vision.java | 55 +++++-- .../frc/robot/subsystems/vision/VisionIO.java | 19 ++- .../subsystems/vision/VisionIOPhotonLib.java | 143 ++++++++++-------- 6 files changed, 164 insertions(+), 90 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b82e37f..675be00 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,7 +6,10 @@ import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.math.geometry.Rotation2d; +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.geometry.Rotation3d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Current; @@ -101,8 +104,16 @@ General.ROBOT_MASS, General.ROBOT_MOI, new ModuleConfig(WHEEL_RADIUS, General.MA public static class Vision { - public static final String PHOTON_CAMERA_NAME = "photonCam"; // TODO Name - public static final double MAX_DETECTION_RANGE = Units.inchesToMeters(300); + public static final String PHOTON_CAMERA_NAME = "photonCam"; // TODO: Name + public static final double MAX_DETECTION_RANGE = Units.inchesToMeters(300); + public static final Transform3d ROBOT_TO_CAMERA = new Transform3d( + new Translation3d(0.5, 0.0, 0.5), // X forward, Y left, Z up from robot center + new Rotation3d(0, Units.degreesToRadians(-30), 0)); // Camera tilted up 30 degrees + public static final double VISION_STD_DEV_BASE_XY = 0.5; // meters + public static final double VISION_STD_DEV_BASE_THETA = Units.degreesToRadians(30); + public static final double VISION_STD_DEV_MULTI_XY = 0.1; + public static final double VISION_STD_DEV_MULTI_THETA = Units.degreesToRadians(10); + public static final double VISION_DISTANCE_SCALE = 0.2; // How much to increase uncertainty per meter } public static class General diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70200b9..92e9720 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,13 +14,18 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOHardware; import frc.robot.subsystems.drive.ModuleIOSim; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIOPhotonLib; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { // Subsystems - private final Drive _drive; + private final Drive _drive; + @SuppressWarnings("unused") + private final Vision _vision; // Add this field // Controller private final CommandXboxController _controller = new CommandXboxController(0); @@ -38,16 +43,19 @@ public RobotContainer() case REAL: // Real robot, instantiate hardware IO implementations _drive = new Drive(new GyroIONavX(), new ModuleIOHardware(0), new ModuleIOHardware(1), new ModuleIOHardware(2), new ModuleIOHardware(3)); + _vision = new Vision(_drive, new VisionIOPhotonLib(_drive)); break; case SIM: // Sim robot, instantiate physics sim IO implementations _drive = new Drive(new GyroIO() {}, new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + _vision = new Vision(_drive, new VisionIO() {}); // Empty implementation for sim break; default: // Replayed robot, disable IO implementations _drive = new Drive(new GyroIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + _vision = new Vision(_drive, new VisionIO() {}); break; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e99c49a..d63041f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -22,6 +22,7 @@ import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -32,6 +33,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -103,7 +105,11 @@ public Drive(GyroIO gyroIO, ModuleIO flModuleIO, ModuleIO frModuleIO, ModuleIO b Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); }); - _poseEstimator = new SwerveDrivePoseEstimator(_kinematics, new Rotation2d(), getModulePositions(), new Pose2d()); + // Configure state estimation with standard deviations + var stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)); // x, y, heading + var visionStdDevs = VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)); // Higher uncertainty for vision + + _poseEstimator = new SwerveDrivePoseEstimator(_kinematics, new Rotation2d(), getModulePositions(), new Pose2d(), stateStdDevs, visionStdDevs); // Create a list of bezier points from poses. Each pose represents one waypoint. // The rotation component of the pose should be the direction of travel. Do not @@ -301,6 +307,10 @@ public void setPose(Pose2d pose) public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + // Scale vision measurement standard deviations based on distance from robot to + // tags + // and number of tags visible - this is handled by the Vision subsystem passing + // appropriate stdDevs _poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index be751c5..9f10c57 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,9 +2,16 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.subsystems.drive.Drive; +import edu.wpi.first.math.util.Units; +import java.util.Arrays; public class Vision extends SubsystemBase { @@ -18,9 +25,8 @@ public class Vision extends SubsystemBase public Vision(Drive drive, VisionIO io) { - _drive = drive; - _io = io; - + _drive = drive; + _io = io; _rotatePID = new PIDController(0.026, 0, 0); _rotatePID.enableContinuousInput(-180, 180); _rotatePID.setTolerance(2); @@ -32,19 +38,36 @@ public void periodic() _io.updateInputs(_inputs); Logger.processInputs("Vision", _inputs); - // if (Constants.Vision.ENABLE_POSE_CORRECTION) - // { - // if (!DriverStation.isAutonomous() && _lastTimestamp != - // _inputs.captureTimestamp) - // { - // if (_inputs.hasPose != false) - // { - // _drive.addVisionMeasurement(_inputs.pose, _inputs.captureTimestamp); - // } - - // _lastTimestamp = _inputs.captureTimestamp; - // } - // } + if (_inputs.hasPose) + { + double avgDistance = 0; + if (_inputs.targetDistances.length > 0) + { + avgDistance = Arrays.stream(_inputs.targetDistances).average().getAsDouble(); + } + + double xyStdDev; + double thetaStdDev; + + if (_inputs.numTargets > 1) + { + xyStdDev = Constants.Vision.VISION_STD_DEV_MULTI_XY + + (avgDistance * Constants.Vision.VISION_DISTANCE_SCALE); + thetaStdDev = Constants.Vision.VISION_STD_DEV_MULTI_THETA; + } + else + { + xyStdDev = Constants.Vision.VISION_STD_DEV_BASE_XY + + (avgDistance * Constants.Vision.VISION_DISTANCE_SCALE); + thetaStdDev = Constants.Vision.VISION_STD_DEV_BASE_THETA; + } + + var stdDevs = VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev); + + _drive.addVisionMeasurement(_inputs.pose, _inputs.captureTimestamp, stdDevs); + } + + _lastTimestamp = _inputs.captureTimestamp; } public void setTargetingEnabled(boolean enabled) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 4305e3d..e2d2722 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -10,16 +10,15 @@ public interface VisionIO public static class VisionIOInputs { public double captureTimestamp = 0.0; - public double[] tagX = new double[] {}; // represents the x distance of all tags - // public double[] tagY = new double[] {}; - public double[] closeTagX = new double[] {}; - // public double[] closeTagY = new double[] {}; - public Pose2d pose = new Pose2d(); - public boolean hasPose = false; - public int numProcessedTargets = 0; - public double[] targetDistances = new double[] {}; // Target = specific april tag - public int[] targetIds = new int[] {}; - public double[] targetYaws = new double[] {}; + public Pose2d pose = new Pose2d(); + public boolean hasTargets = false; + public boolean hasPose = false; + public int numTargets = 0; + public double[] targetDistances = new double[] {}; // Target = specific april tag + public int[] targetIds = new int[] {}; + public double[] targetYaws = new double[] {}; + public double[] targetPitches = new double[] {}; + public double[] targetAreas = new double[] {}; } public default void updateInputs(VisionIOInputs inputs) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java index b137495..02a6e8c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java @@ -11,12 +11,10 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; @@ -25,10 +23,8 @@ public class VisionIOPhotonLib implements VisionIO { - private final PhotonCamera _camera = new PhotonCamera(Constants.Vision.PHOTON_CAMERA_NAME); + private final PhotonCamera _camera; private double _captureTimesStamp = 0.0; - private double[] _tagX = new double[] {}; - private double[] _closeTagX = new double[] {}; private Pose2d _pose = new Pose2d(); private boolean _hasPose = false; private double[] _distances = new double[] {}; @@ -37,44 +33,42 @@ public class VisionIOPhotonLib implements VisionIO private int _numProcessedTargets; private AprilTagFieldLayout fieldLayout; private PhotonPoseEstimator _poseEstimator; - private Transform3d _robotToCamera; - private final Pose3d _lastEstimatedPose = new Pose3d(); - public void VisionIOPhotonlib(Drive drive) + public VisionIOPhotonLib(Drive drive) { + _camera = new PhotonCamera(Constants.Vision.PHOTON_CAMERA_NAME); + try { - fieldLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + // Load the 2024 field layout + fieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); } catch (Exception e) { - System.out.println("Exception encountered: " + e.getMessage()); + System.err.println("Failed to load AprilTag layout: " + e.getMessage()); + throw new RuntimeException(e); } - _poseEstimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, _robotToCamera); - // _poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + _poseEstimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, Constants.Vision.ROBOT_TO_CAMERA); + _poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); NetworkTableInstance.getDefault().addListener(NetworkTableInstance.getDefault().getEntry("/photonvision/" + Constants.Vision.PHOTON_CAMERA_NAME + "/latencyMillis"), EnumSet.of(NetworkTableEvent.Kind.kValueRemote), event -> { - PhotonPipelineResult result = _camera.getLatestResult(); - double timestamp = result.getTimestampSeconds(); - List tagXList = new ArrayList<>(); - List closeTagXList = new ArrayList<>(); - List processedTargets = new ArrayList<>(); + PhotonPipelineResult result = _camera.getLatestResult(); + double timestamp = result.getTimestampSeconds(); + List targets = result.getTargets(); + List distances = new ArrayList<>(); List ids = new ArrayList<>(); List yaws = new ArrayList<>(); + List processedTargets = new ArrayList<>(); - for (PhotonTrackedTarget target : result.getTargets()) + // Process all targets + for (PhotonTrackedTarget target : targets) { ids.add(target.getFiducialId()); yaws.add(target.getYaw()); - for (TargetCorner corner : target.getDetectedCorners()) - { - tagXList.add(corner.x); - } - var transform = target.getBestCameraToTarget(); var distance = Math.hypot(transform.getX(), transform.getY()); distances.add(distance); @@ -82,19 +76,12 @@ public void VisionIOPhotonlib(Drive drive) if (distance <= Constants.Vision.MAX_DETECTION_RANGE) { processedTargets.add(target); - - for (TargetCorner corner : target.getDetectedCorners()) - { - closeTagXList.add(corner.x); - } - } } - PhotonPipelineResult processedResult = new PhotonPipelineResult(); - processedResult.getTimestampSeconds(); // TODO: see if need change - - Optional estimatedPose = getEstimatedGlobalPose(drive.getPose(), processedResult); + // Get pose estimate from processed targets + _poseEstimator.setReferencePose(drive.getPose()); + Optional estimatedPose = _poseEstimator.update(result); Pose2d pose = new Pose2d(); boolean hasPose = false; @@ -107,8 +94,6 @@ public void VisionIOPhotonlib(Drive drive) synchronized (VisionIOPhotonLib.this) { _captureTimesStamp = timestamp; - _tagX = tagXList.stream().mapToDouble(Double::doubleValue).toArray(); - _closeTagX = closeTagXList.stream().mapToDouble(Double::doubleValue).toArray(); _distances = distances.stream().mapToDouble(Double::doubleValue).toArray(); _pose = pose; _numProcessedTargets = processedTargets.size(); @@ -119,34 +104,72 @@ public void VisionIOPhotonlib(Drive drive) }); } - @Override - public synchronized void updateInputs(VisionIOInputs inputs) + public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose, PhotonPipelineResult result) { - // PhotonPipelineResult _result = _camera.getLatestResult(); - inputs.captureTimestamp = _captureTimesStamp; - inputs.tagX = _tagX; - inputs.closeTagX = _closeTagX; - inputs.pose = _pose; - inputs.hasPose = _hasPose; - inputs.targetIds = _targetIds; - inputs.targetYaws = _targetYaws; - inputs.numProcessedTargets = _numProcessedTargets; - - // if (_result.hasTargets()) - // { - // Optional _poseOptional = _PoseEstimator.update(_result); - // if (_poseOptional.isPresent()) - // { - // EstimatedRobotPose _estimatedPose = _poseOptional.get(); - // inputs.captureTimestamp = _result.getTimestampSeconds(); - // inputs._estimatedPose - // } - // } + _poseEstimator.setReferencePose(prevEstimatedRobotPose); + return _poseEstimator.update(result); } - private Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose, PhotonPipelineResult result) + @Override + public synchronized void updateInputs(VisionIOInputs inputs) { - _poseEstimator.setLastPose(prevEstimatedRobotPose); - return _poseEstimator.update(result); + var result = _camera.getLatestResult(); + + inputs.captureTimestamp = result.getTimestampSeconds(); + inputs.hasTargets = result.hasTargets(); + + if (inputs.hasTargets) + { + List targets = result.getTargets(); + int numTargets = targets.size(); + + inputs.targetIds = new int[numTargets]; + inputs.targetYaws = new double[numTargets]; + inputs.targetPitches = new double[numTargets]; + inputs.targetAreas = new double[numTargets]; + inputs.targetDistances = new double[numTargets]; + + for (int i = 0; i < numTargets; i++) + { + PhotonTrackedTarget target = targets.get(i); + inputs.targetIds[i] = target.getFiducialId(); + inputs.targetYaws[i] = target.getYaw(); + inputs.targetPitches[i] = target.getPitch(); + inputs.targetAreas[i] = target.getArea(); + + Transform3d bestCameraToTarget = target.getBestCameraToTarget(); + inputs.targetDistances[i] = Math.hypot(bestCameraToTarget.getX(), bestCameraToTarget.getY()); + } + + inputs.numTargets = numTargets; + + // Update pose estimate if we have new data + if (_captureTimesStamp != inputs.captureTimestamp) + { + Optional poseEstimate = getEstimatedGlobalPose(_pose, result); + if (poseEstimate.isPresent()) + { + inputs.pose = poseEstimate.get().estimatedPose.toPose2d(); + inputs.hasPose = true; + _pose = inputs.pose; + } + else + { + inputs.hasPose = false; + } + _captureTimesStamp = inputs.captureTimestamp; + } + } + else + { + // No targets visible + inputs.numTargets = 0; + inputs.targetIds = new int[0]; + inputs.targetYaws = new double[0]; + inputs.targetPitches = new double[0]; + inputs.targetAreas = new double[0]; + inputs.targetDistances = new double[0]; + inputs.hasPose = false; + } } } From cc19856d86872b98235abe5d90e6228532c2fa1b Mon Sep 17 00:00:00 2001 From: Saketh Date: Wed, 22 Jan 2025 18:33:13 -0600 Subject: [PATCH 5/6] Removed deprecated method --- src/main/java/frc/robot/Constants.java | 21 ++++++++------- .../frc/robot/subsystems/vision/Vision.java | 6 ++--- .../subsystems/vision/VisionIOPhotonLib.java | 26 +++++++++++++++++-- 3 files changed, 37 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 675be00..5d6dd1c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -104,16 +104,17 @@ General.ROBOT_MASS, General.ROBOT_MOI, new ModuleConfig(WHEEL_RADIUS, General.MA public static class Vision { - public static final String PHOTON_CAMERA_NAME = "photonCam"; // TODO: Name - public static final double MAX_DETECTION_RANGE = Units.inchesToMeters(300); - public static final Transform3d ROBOT_TO_CAMERA = new Transform3d( - new Translation3d(0.5, 0.0, 0.5), // X forward, Y left, Z up from robot center - new Rotation3d(0, Units.degreesToRadians(-30), 0)); // Camera tilted up 30 degrees - public static final double VISION_STD_DEV_BASE_XY = 0.5; // meters - public static final double VISION_STD_DEV_BASE_THETA = Units.degreesToRadians(30); - public static final double VISION_STD_DEV_MULTI_XY = 0.1; - public static final double VISION_STD_DEV_MULTI_THETA = Units.degreesToRadians(10); - public static final double VISION_DISTANCE_SCALE = 0.2; // How much to increase uncertainty per meter + public static final String PHOTON_CAMERA_NAME = "photonCam"; // TODO: Name + public static final double MAX_DETECTION_RANGE = Units.inchesToMeters(300); + public static final Transform3d ROBOT_TO_CAMERA = new Transform3d( + new Translation3d(0.5, 0.0, 0.5), // X forward, Y left, Z up from robot center + new Rotation3d(0, Units.degreesToRadians(-30), 0) + ); // Camera tilted up 30 degrees + public static final double VISION_STD_DEV_BASE_XY = 0.5; // meters + public static final double VISION_STD_DEV_BASE_THETA = Units.degreesToRadians(30); + public static final double VISION_STD_DEV_MULTI_XY = 0.1; + public static final double VISION_STD_DEV_MULTI_THETA = Units.degreesToRadians(10); + public static final double VISION_DISTANCE_SCALE = 0.2; // How much to increase uncertainty per meter } public static class General diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 9f10c57..ec82de1 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -51,14 +51,12 @@ public void periodic() if (_inputs.numTargets > 1) { - xyStdDev = Constants.Vision.VISION_STD_DEV_MULTI_XY - + (avgDistance * Constants.Vision.VISION_DISTANCE_SCALE); + xyStdDev = Constants.Vision.VISION_STD_DEV_MULTI_XY + (avgDistance * Constants.Vision.VISION_DISTANCE_SCALE); thetaStdDev = Constants.Vision.VISION_STD_DEV_MULTI_THETA; } else { - xyStdDev = Constants.Vision.VISION_STD_DEV_BASE_XY - + (avgDistance * Constants.Vision.VISION_DISTANCE_SCALE); + xyStdDev = Constants.Vision.VISION_STD_DEV_BASE_XY + (avgDistance * Constants.Vision.VISION_DISTANCE_SCALE); thetaStdDev = Constants.Vision.VISION_STD_DEV_BASE_THETA; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java index 02a6e8c..afdb0b4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java @@ -54,7 +54,14 @@ public VisionIOPhotonLib(Drive drive) NetworkTableInstance.getDefault().addListener(NetworkTableInstance.getDefault().getEntry("/photonvision/" + Constants.Vision.PHOTON_CAMERA_NAME + "/latencyMillis"), EnumSet.of(NetworkTableEvent.Kind.kValueRemote), event -> { - PhotonPipelineResult result = _camera.getLatestResult(); + var results = _camera.getAllUnreadResults(); + if (results.isEmpty()) + { + return; + } + // get the latest result (last in the list) + var result = results.get(results.size() - 1); + double timestamp = result.getTimestampSeconds(); List targets = result.getTargets(); @@ -113,7 +120,22 @@ public Optional getEstimatedGlobalPose(Pose2d prevEstimatedR @Override public synchronized void updateInputs(VisionIOInputs inputs) { - var result = _camera.getLatestResult(); + var results = _camera.getAllUnreadResults(); + if (results.isEmpty()) + { + // No results available + inputs.hasTargets = false; + inputs.numTargets = 0; + inputs.targetIds = new int[0]; + inputs.targetYaws = new double[0]; + inputs.targetPitches = new double[0]; + inputs.targetAreas = new double[0]; + inputs.targetDistances = new double[0]; + inputs.hasPose = false; + return; + } + + var result = results.get(results.size() - 1); inputs.captureTimestamp = result.getTimestampSeconds(); inputs.hasTargets = result.hasTargets(); From b2076f7a2c239ac92560b74666cf6885c2b15e82 Mon Sep 17 00:00:00 2001 From: Saketh Date: Mon, 3 Feb 2025 18:34:04 -0600 Subject: [PATCH 6/6] removed unused variables and methods --- src/main/java/frc/robot/RobotContainer.java | 4 +-- .../frc/robot/subsystems/drive/Drive.java | 4 --- .../frc/robot/subsystems/vision/Vision.java | 21 +-------------- .../subsystems/vision/VisionIOPhotonLib.java | 26 +++++++++---------- 4 files changed, 16 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eb2423e..0fd4d28 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,7 +50,7 @@ public class RobotContainer // Subsystems private final Drive _drive; @SuppressWarnings("unused") - private final Vision _vision; // Add this field + private final Vision _vision; private final Elevator _elevator; private final Manipulator _manipulator; private final Funnel _funnel; @@ -82,7 +82,7 @@ public RobotContainer() case SIM: // Sim robot, instantiate physics sim IO implementations _drive = new Drive(new GyroIO() {}, new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); - _vision = new Vision(_drive, new VisionIO() {}); // Empty implementation for sim + _vision = new Vision(_drive, new VisionIOPhotonLib(_drive)); _elevator = new Elevator(new ElevatorIOSim()); _manipulator = new Manipulator(new ManipulatorIOSim(() -> _controller.leftTrigger().getAsBoolean())); _funnel = new Funnel(new FunnelIOSim()); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5e3b25b..748b3e6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -307,10 +307,6 @@ public void setPose(Pose2d pose) public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - // Scale vision measurement standard deviations based on distance from robot to - // tags - // and number of tags visible - this is handled by the Vision subsystem passing - // appropriate stdDevs _poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index ec82de1..99d3929 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,15 +2,10 @@ import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.subsystems.drive.Drive; -import edu.wpi.first.math.util.Units; import java.util.Arrays; public class Vision extends SubsystemBase @@ -18,18 +13,11 @@ public class Vision extends SubsystemBase private final VisionIO _io; private final VisionIOInputsAutoLogged _inputs = new VisionIOInputsAutoLogged(); private final Drive _drive; - private final PIDController _rotatePID; - private double _maxSpeed = 0.3; - private double _lastTimestamp = 0.0; - private boolean _targetingEnabled = false; public Vision(Drive drive, VisionIO io) { _drive = drive; _io = io; - _rotatePID = new PIDController(0.026, 0, 0); - _rotatePID.enableContinuousInput(-180, 180); - _rotatePID.setTolerance(2); } @Override @@ -64,12 +52,5 @@ public void periodic() _drive.addVisionMeasurement(_inputs.pose, _inputs.captureTimestamp, stdDevs); } - - _lastTimestamp = _inputs.captureTimestamp; - } - - public void setTargetingEnabled(boolean enabled) - { - _targetingEnabled = enabled; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java index afdb0b4..74fa73b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonLib.java @@ -23,16 +23,16 @@ public class VisionIOPhotonLib implements VisionIO { - private final PhotonCamera _camera; - private double _captureTimesStamp = 0.0; - private Pose2d _pose = new Pose2d(); - private boolean _hasPose = false; - private double[] _distances = new double[] {}; - private int[] _targetIds = new int[] {}; - private double[] _targetYaws = new double[] {}; - private int _numProcessedTargets; - private AprilTagFieldLayout fieldLayout; - private PhotonPoseEstimator _poseEstimator; + private final PhotonCamera _camera; + private final AprilTagFieldLayout _fieldLayout; + private final PhotonPoseEstimator _poseEstimator; + private double _captureTimesStamp = 0.0; + private Pose2d _pose = new Pose2d(); + private boolean _hasPose = false; + private double[] _distances = new double[] {}; + private int[] _targetIds = new int[] {}; + private double[] _targetYaws = new double[] {}; + private int _numProcessedTargets; public VisionIOPhotonLib(Drive drive) { @@ -41,7 +41,7 @@ public VisionIOPhotonLib(Drive drive) try { // Load the 2024 field layout - fieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + _fieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.kDefaultField.m_resourceFile); } catch (Exception e) { @@ -49,7 +49,7 @@ public VisionIOPhotonLib(Drive drive) throw new RuntimeException(e); } - _poseEstimator = new PhotonPoseEstimator(fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, Constants.Vision.ROBOT_TO_CAMERA); + _poseEstimator = new PhotonPoseEstimator(_fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, Constants.Vision.ROBOT_TO_CAMERA); _poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); NetworkTableInstance.getDefault().addListener(NetworkTableInstance.getDefault().getEntry("/photonvision/" + Constants.Vision.PHOTON_CAMERA_NAME + "/latencyMillis"), EnumSet.of(NetworkTableEvent.Kind.kValueRemote), event -> @@ -111,7 +111,7 @@ public VisionIOPhotonLib(Drive drive) }); } - public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose, PhotonPipelineResult result) + private Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose, PhotonPipelineResult result) { _poseEstimator.setReferencePose(prevEstimatedRobotPose); return _poseEstimator.update(result);