From be15fbe301295fbc7240dbbcaa2e1ef35340c77a Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Tue, 23 Jan 2024 11:39:20 -0800 Subject: [PATCH] Introduce vision subsystem Still needed: handling updates --- .../cyberknights4911/vision/CameraConfig.java | 11 + .../vision/CameraConstants.java | 14 + .../com/cyberknights4911/vision/Vision.java | 340 ++++++++++++++++++ .../vision/VisionConstants.java | 15 + .../{wham => }/vision/VisionIO.java | 15 +- .../{wham => }/vision/VisionIOPhoton.java | 48 ++- .../cyberknights4911/vision/VisionUpdate.java | 15 + .../java/com/cyberknights4911/wham/Wham.java | 19 +- .../cyberknights4911/wham/WhamConstants.java | 48 ++- .../cyberknights4911/wham/vision/Vision.java | 26 -- 10 files changed, 509 insertions(+), 42 deletions(-) create mode 100644 src/main/java/com/cyberknights4911/vision/CameraConfig.java create mode 100644 src/main/java/com/cyberknights4911/vision/CameraConstants.java create mode 100644 src/main/java/com/cyberknights4911/vision/Vision.java create mode 100644 src/main/java/com/cyberknights4911/vision/VisionConstants.java rename src/main/java/com/cyberknights4911/{wham => }/vision/VisionIO.java (53%) rename src/main/java/com/cyberknights4911/{wham => }/vision/VisionIOPhoton.java (53%) create mode 100644 src/main/java/com/cyberknights4911/vision/VisionUpdate.java delete mode 100644 src/main/java/com/cyberknights4911/wham/vision/Vision.java diff --git a/src/main/java/com/cyberknights4911/vision/CameraConfig.java b/src/main/java/com/cyberknights4911/vision/CameraConfig.java new file mode 100644 index 0000000..6f9f466 --- /dev/null +++ b/src/main/java/com/cyberknights4911/vision/CameraConfig.java @@ -0,0 +1,11 @@ +// Copyright (c) 2023 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.vision; + +public record CameraConfig( + CameraConstants constants, VisionIO visionIO, VisionIOInputsAutoLogged inputs) {} diff --git a/src/main/java/com/cyberknights4911/vision/CameraConstants.java b/src/main/java/com/cyberknights4911/vision/CameraConstants.java new file mode 100644 index 0000000..f38d37e --- /dev/null +++ b/src/main/java/com/cyberknights4911/vision/CameraConstants.java @@ -0,0 +1,14 @@ +// Copyright (c) 2023 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import io.soabase.recordbuilder.core.RecordBuilder; + +@RecordBuilder +public record CameraConstants(String name, String photonCameraName, Transform3d robotToCamera) {} diff --git a/src/main/java/com/cyberknights4911/vision/Vision.java b/src/main/java/com/cyberknights4911/vision/Vision.java new file mode 100644 index 0000000..81d3f28 --- /dev/null +++ b/src/main/java/com/cyberknights4911/vision/Vision.java @@ -0,0 +1,340 @@ +// Copyright (c) 2023 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.vision; + +import com.cyberknights4911.logging.LoggedTunableNumber; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +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.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Comparator; +import java.util.HashMap; +import java.util.Optional; +import java.util.function.Consumer; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class Vision extends SubsystemBase { + private final Supplier poseSupplier; + private final Consumer visionConsumer; + private final CameraConfig[] cameraConfigs; + + private final HashMap lastTimestamp = new HashMap<>(); + + private boolean updatePoseWithVisionReadings = true; + private boolean useMaxValidDistanceAway = true; + + private final VisionConstants visionConstants; + + private static double MAX_ALLOWABLE_PITCH = 3; + private static double MAX_ALLOWABLE_ROLL = 3; + + private static final LoggedTunableNumber twoTargetDeviation = + new LoggedTunableNumber("Vision/deviations/twoTarget", 0.6); + private static final LoggedTunableNumber threeTargetDeviation = + new LoggedTunableNumber("Vision/deviations/threeTarget", 0.4); + private static final LoggedTunableNumber fourTargetDeviation = + new LoggedTunableNumber("Vision/deviations/fourTarget", 0.2); + + private static final Matrix defaultDeviation = VecBuilder.fill(0.9, 0.9, 0.9); + + public Vision( + VisionConstants visionConstants, + Supplier poseSupplier, + Consumer visionConsumer, + CameraConfig... cameraConfigs) { + super(); + this.visionConstants = visionConstants; + this.poseSupplier = poseSupplier; + this.visionConsumer = visionConsumer; + this.cameraConfigs = cameraConfigs; + + Logger.recordOutput("Vision/updatePoseWithVisionReadings", true); + Logger.recordOutput("Vision/useMaxValidDistanceAway", true); + + for (AprilTag tag : visionConstants.layout().getTags()) { + Logger.recordOutput("Vision/AprilTags/" + tag.ID, tag.pose); + } + + for (int i = 0; i < cameraConfigs.length; i++) { + Logger.recordOutput( + "Vision/" + cameraConfigs[i].constants().name(), + new Pose3d().transformBy(cameraConfigs[i].constants().robotToCamera())); + lastTimestamp.put(cameraConfigs[i].constants().name(), 0.0); + } + + // TODO(rbrewer): warn for no cameras + // NOTE: the camera object is not getting set in SIM or REPLAY + // if (R_VisionIO.getCamera() == null) { + // System.out.println("NO RIGHT CAMERA"); + // } + // if (L_VisionIO.getCamera() == null) { + // System.out.println("NO LEFT CAMERA"); + // } + } + + @Override + public void periodic() { + for (int i = 0; i < cameraConfigs.length; i++) { + cameraConfigs[i].visionIO().updateInputs(cameraConfigs[i].inputs()); + Logger.processInputs( + "Vision/" + cameraConfigs[i].constants().name(), cameraConfigs[i].inputs()); + } + + if (!updatePoseWithVisionReadings) { + return; + } + + // TODO(rbrewer): test whether necessary + // if (Math.abs(drivetrain.getGyroPitch()) >= MAX_ALLOWABLE_PITCH + // || Math.abs(drivetrain.getGyroRoll()) >= MAX_ALLOWABLE_ROLL) { + + // Logger.recordOutput("Vision/ValidGyroAngle", false); + // return; + // } + // Logger.recordOutput("Vision/ValidGyroAngle", true); + + for (int i = 0; i < cameraConfigs.length; i++) { + updatePose(cameraConfigs[i]); + Logger.recordOutput( + "Vision/" + cameraConfigs[i].constants().name() + "Connected", + cameraConfigs[i].inputs().lastTimestamp > 0.0); + } + } + + private void updatePose(CameraConfig cameraConfig) { + boolean updated = false; + boolean pnpFailed = false; + Pose2d prevEstimatedRobotPose = poseSupplier.get(); + double timestamp = 0; + double distance = 0.0; + PhotonPipelineResult cameraResult = null; + Pose3d robotPose = null; + + double standardDeviation = -1.0; + + // TODO(rbrewer): this is gross, find an alternative + synchronized (cameraConfig.constants()) { + cameraResult = cameraConfig.inputs().lastResult; + timestamp = cameraConfig.inputs().lastTimestamp; + } + + // is this a new camera result? + double prevTimestamp = lastTimestamp.get(cameraConfig.constants().name()); + if (prevTimestamp >= timestamp) { + return; + } + lastTimestamp.put(cameraConfig.constants().name(), timestamp); + + int targetsSeen = cameraResult.getTargets().size(); + Logger.recordOutput("Vision/" + cameraConfig.constants().name() + "/SeenTargets", targetsSeen); + + if (targetsSeen > 1) { + // more than one target seen, use PNP with PV estimator + + cameraConfig.visionIO().setReferencePose(prevEstimatedRobotPose); + Optional result = cameraConfig.visionIO().update(cameraResult); + + if (result.isPresent()) { + robotPose = result.get().estimatedPose; + + standardDeviation = getDeviationForNumTargets(targetsSeen); + + Logger.recordOutput( + "Vision/" + cameraConfig.constants().name() + "/CameraPose", + robotPose.transformBy(cameraConfig.constants().robotToCamera())); + } else { + pnpFailed = true; + } + + Logger.recordOutput("Vision/" + cameraConfig.constants().name() + "/Ambiguity", 0.0); + + } else { + // zero or 1 target, manually check if it is accurate enough + for (PhotonTrackedTarget target : cameraResult.getTargets()) { + if (isValidTarget(target)) { + Transform3d cameraToTarget = target.getBestCameraToTarget(); + Optional tagPoseOptional = + visionConstants.layout().getTagPose(target.getFiducialId()); + if (tagPoseOptional.isEmpty()) { + break; + } + + Pose3d cameraPose = tagPoseOptional.get().transformBy(cameraToTarget.inverse()); + + // TODO(rbrewer) verify this! naming was super sketch + robotPose = cameraPose.transformBy(cameraConfig.constants().robotToCamera().inverse()); + + standardDeviation = cameraToTarget.getTranslation().getNorm() / 2.0; + + Logger.recordOutput( + "Vision/" + cameraConfig.constants().name() + "/distanceNorm3D", + cameraToTarget.getTranslation().getNorm()); + + Logger.recordOutput( + "Vision/" + cameraConfig.constants().name() + "/Ambiguity", + target.getPoseAmbiguity()); + Logger.recordOutput( + "Vision/" + cameraConfig.constants().name() + "/CameraPose", cameraPose); + } + } + } + + if (robotPose != null) { + distance = + prevEstimatedRobotPose + .getTranslation() + .getDistance(new Translation2d(robotPose.getX(), robotPose.getY())); + + Logger.recordOutput( + "Vision/" + cameraConfig.constants().name() + "/DistanceFromRobot", distance); + Logger.recordOutput( + "Vision/" + cameraConfig.constants().name() + "/RobotPose", robotPose.toPose2d()); + Logger.recordOutput("Vision/" + cameraConfig.constants().name() + "/3DRobotPose", robotPose); + + // distance from vision estimate to last position estimate + if (!useMaxValidDistanceAway + || distance <= visionConstants.maxValidDistanceMeters() * targetsSeen) { + // we passed all the checks, update the pose + updated = true; + + Matrix updateDeviation; + // -1 = error when calculating deviation + if (standardDeviation == -1) { + updateDeviation = defaultDeviation; + } else { + updateDeviation = VecBuilder.fill(standardDeviation, standardDeviation, 0.9); + } + + synchronized (visionConsumer) { + visionConsumer.accept(new VisionUpdate(timestamp, robotPose.toPose2d(), updateDeviation)); + } + } + } + + Logger.recordOutput("Vision/" + cameraConfig.constants().name() + "/Updated", updated); + Logger.recordOutput("Vision/" + cameraConfig.constants().name() + "/pnpFailed", pnpFailed); + Logger.recordOutput( + "Vision/" + cameraConfig.constants().name() + "/standardDeviation", standardDeviation); + } + + public boolean tagVisible(int id, PhotonPipelineResult result) { + for (PhotonTrackedTarget target : result.getTargets()) { + if (target.getFiducialId() == id && isValidTarget(target)) { + return true; + } + } + return false; + } + + /** + * returns the best Rotation3d from the robot to the given target. + * + * @param id + * @return the Transform3d or null if there isn't + */ + public Transform3d getTransform3dToTag( + int id, PhotonPipelineResult result, Transform3d robotToCamera) { + Transform3d bestTransform3d = null; + + for (PhotonTrackedTarget target : result.getTargets()) { + // the target must be the same as the given target id, and the target must be available + if (target.getFiducialId() != id || !isValidTarget(target)) { + break; + } + + bestTransform3d = robotToCamera.plus(target.getBestCameraToTarget()); + } + return bestTransform3d; + } + + public Rotation2d getAngleToTag(int id, PhotonPipelineResult result, Transform3d robotToCamera) { + Transform3d transform = getTransform3dToTag(id, result, robotToCamera); + if (transform != null) { + return new Rotation2d(transform.getTranslation().getX(), transform.getTranslation().getY()); + } else { + return null; + } + } + + public double getDistanceToTag(int id, PhotonPipelineResult result, Transform3d robotToCamera) { + Transform3d transform = getTransform3dToTag(id, result, robotToCamera); + if (transform != null) { + return transform.getTranslation().toTranslation2d().getNorm(); + } else { + return -1; + } + } + + public boolean isValidTarget(PhotonTrackedTarget target) { + return target.getFiducialId() != -1 + && target.getPoseAmbiguity() != -1 + && target.getPoseAmbiguity() < visionConstants.maxAmbiguity() + && visionConstants.layout().getTagPose(target.getFiducialId()).isPresent(); + } + + public void enableMaxDistanceAwayForTags() { + Logger.recordOutput("Vision/useMaxValidDistanceAway", true); + useMaxValidDistanceAway = true; + } + + public void disableMaxDistanceAwayForTags() { + Logger.recordOutput("Vision/useMaxValidDistanceAway", false); + useMaxValidDistanceAway = false; + } + + public void enableUpdatePoseWithVisionReadings() { + Logger.recordOutput("Vision/updatePoseWithVisionReadings", true); + updatePoseWithVisionReadings = true; + } + + public void disableUpdatePoseWithVisionReadings() { + Logger.recordOutput("Vision/updatePoseWithVisionReadings", false); + updatePoseWithVisionReadings = false; + } + + private double getDeviationForNumTargets(int numTargets) { + if (numTargets == 2) { + return twoTargetDeviation.get(); + } + + if (numTargets == 3) { + return threeTargetDeviation.get(); + } + + if (numTargets > 3) { + return fourTargetDeviation.get(); + } + + return 1.2; + } +} + +final class TargetComparator implements Comparator { + @Override + public int compare(PhotonTrackedTarget a, PhotonTrackedTarget b) { + double ambiguityDiff = a.getPoseAmbiguity() - b.getPoseAmbiguity(); + if (ambiguityDiff < 0.0) { + return -1; + } + if (ambiguityDiff > 0.0) { + return 1; + } + return 0; + } +} diff --git a/src/main/java/com/cyberknights4911/vision/VisionConstants.java b/src/main/java/com/cyberknights4911/vision/VisionConstants.java new file mode 100644 index 0000000..a406583 --- /dev/null +++ b/src/main/java/com/cyberknights4911/vision/VisionConstants.java @@ -0,0 +1,15 @@ +// Copyright (c) 2023 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import io.soabase.recordbuilder.core.RecordBuilder; + +@RecordBuilder +public record VisionConstants( + AprilTagFieldLayout layout, double maxAmbiguity, double maxValidDistanceMeters) {} diff --git a/src/main/java/com/cyberknights4911/wham/vision/VisionIO.java b/src/main/java/com/cyberknights4911/vision/VisionIO.java similarity index 53% rename from src/main/java/com/cyberknights4911/wham/vision/VisionIO.java rename to src/main/java/com/cyberknights4911/vision/VisionIO.java index 4ed0c37..1b35987 100644 --- a/src/main/java/com/cyberknights4911/wham/vision/VisionIO.java +++ b/src/main/java/com/cyberknights4911/vision/VisionIO.java @@ -5,18 +5,27 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package com.cyberknights4911.wham.vision; +package com.cyberknights4911.vision; +import edu.wpi.first.math.geometry.Pose2d; +import java.util.Optional; import org.littletonrobotics.junction.AutoLog; +import org.photonvision.EstimatedRobotPose; import org.photonvision.targeting.PhotonPipelineResult; public interface VisionIO { @AutoLog public static class VisionIOInputs { public boolean isOnline = false; - public double lastTimeStamp = 0; + public double lastTimestamp = 0; public PhotonPipelineResult lastResult = new PhotonPipelineResult(); } - public default void updateInputs(VisionIOInputsAutoLogged inputs) {} + public default void updateInputs(VisionIOInputs inputs) {} + + public default void setReferencePose(Pose2d referencePose) {} + + public default Optional update(PhotonPipelineResult cameraResult) { + return Optional.empty(); + } } diff --git a/src/main/java/com/cyberknights4911/wham/vision/VisionIOPhoton.java b/src/main/java/com/cyberknights4911/vision/VisionIOPhoton.java similarity index 53% rename from src/main/java/com/cyberknights4911/wham/vision/VisionIOPhoton.java rename to src/main/java/com/cyberknights4911/vision/VisionIOPhoton.java index 4e6d5f9..bac1a3d 100644 --- a/src/main/java/com/cyberknights4911/wham/vision/VisionIOPhoton.java +++ b/src/main/java/com/cyberknights4911/vision/VisionIOPhoton.java @@ -5,42 +5,62 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package com.cyberknights4911.wham.vision; +package com.cyberknights4911.vision; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.DoubleArraySubscriber; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; import java.util.EnumSet; +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; public class VisionIOPhoton implements VisionIO { private final PhotonCamera camera; + private final PhotonPoseEstimator photonPoseEstimator; + private double lastTimestamp = 0; private PhotonPipelineResult lastResult = new PhotonPipelineResult(); - public VisionIOPhoton(String cameraName) { - camera = new PhotonCamera(cameraName); - NetworkTableInstance inst = NetworkTableInstance.getDefault(); - + public VisionIOPhoton(VisionConstants visionConstants, CameraConstants constants) { + camera = new PhotonCamera(constants.photonCameraName()); camera.setDriverMode(false); + // TODO(rbrewer) test roborio version of multi tag + photonPoseEstimator = + new PhotonPoseEstimator( + visionConstants.layout(), + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + constants.robotToCamera()); + // TODO(rbrewer) test this + // photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.CLOSEST_TO_REFERENCE_POSE); + + NetworkTableInstance tables = NetworkTableInstance.getDefault(); + /* * based on https://docs.wpilib.org/en/latest/docs/software/networktables/listening-for-change.html#listening-for-changes * and https://github.com/Mechanical-Advantage/RobotCode2022/blob/main/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java */ DoubleArraySubscriber targetPoseSub = - inst.getTable("/photonvision/" + cameraName) + tables + .getTable("/photonvision/" + constants.photonCameraName()) .getDoubleArrayTopic("targetPose") .subscribe(new double[0]); - inst.addListener( + tables.addListener( targetPoseSub, EnumSet.of(NetworkTableEvent.Kind.kValueAll), event -> { PhotonPipelineResult result = camera.getLatestResult(); double timestamp = Timer.getFPGATimestamp() - (result.getLatencyMillis() / 1000.0); + + // TODO(rbrewer): fix this synchronized (VisionIOPhoton.this) { lastTimestamp = timestamp; lastResult = result; @@ -49,9 +69,19 @@ public VisionIOPhoton(String cameraName) { } @Override - public synchronized void updateInputs(VisionIOInputsAutoLogged inputs) { + public synchronized void updateInputs(VisionIOInputs inputs) { inputs.isOnline = camera.isConnected(); inputs.lastResult = lastResult; - inputs.lastTimeStamp = lastTimestamp; + inputs.lastTimestamp = lastTimestamp; + } + + @Override + public void setReferencePose(Pose2d referencePose) { + photonPoseEstimator.setReferencePose(referencePose); + } + + @Override + public Optional update(PhotonPipelineResult cameraResult) { + return photonPoseEstimator.update(cameraResult); } } diff --git a/src/main/java/com/cyberknights4911/vision/VisionUpdate.java b/src/main/java/com/cyberknights4911/vision/VisionUpdate.java new file mode 100644 index 0000000..d24eeb4 --- /dev/null +++ b/src/main/java/com/cyberknights4911/vision/VisionUpdate.java @@ -0,0 +1,15 @@ +// Copyright (c) 2023 FRC 4911 +// https://github.com/frc4911 +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.cyberknights4911.vision; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +public record VisionUpdate(double timestamp, Pose2d pose, Matrix stdDevs) {} diff --git a/src/main/java/com/cyberknights4911/wham/Wham.java b/src/main/java/com/cyberknights4911/wham/Wham.java index 96db5d3..3f7118c 100644 --- a/src/main/java/com/cyberknights4911/wham/Wham.java +++ b/src/main/java/com/cyberknights4911/wham/Wham.java @@ -15,13 +15,16 @@ import com.cyberknights4911.drive.ModuleIO; import com.cyberknights4911.drive.ModuleIOSim; import com.cyberknights4911.entrypoint.RobotContainer; +import com.cyberknights4911.vision.CameraConfig; +import com.cyberknights4911.vision.Vision; +import com.cyberknights4911.vision.VisionIOInputsAutoLogged; +import com.cyberknights4911.vision.VisionIOPhoton; +import com.cyberknights4911.vision.VisionUpdate; import com.cyberknights4911.wham.drive.ModuleIOTalonFX; import com.cyberknights4911.wham.slurpp.Slurpp; import com.cyberknights4911.wham.slurpp.SlurppIO; import com.cyberknights4911.wham.slurpp.SlurppIOReal; import com.cyberknights4911.wham.slurpp.SlurppIOSim; -import com.cyberknights4911.wham.vision.Vision; -import com.cyberknights4911.wham.vision.VisionIOPhoton; import edu.wpi.first.wpilibj2.command.Commands; import org.littletonrobotics.junction.LoggedRobot; @@ -37,7 +40,17 @@ public Wham() { binding = new WhamControllerBinding(); drive = createDrive(); slurpp = createSlurpp(); - vision = new Vision(new VisionIOPhoton("Arducam_OV2311_USB_Camera")); + vision = + new Vision( + WhamConstants.VISION_CONSTANTS, + drive::getPose, + (VisionUpdate update) -> { + System.out.println("Vision update at time: " + update.timestamp()); + }, + new CameraConfig( + WhamConstants.CAMERA_CONSTANTS, + new VisionIOPhoton(WhamConstants.VISION_CONSTANTS, WhamConstants.CAMERA_CONSTANTS), + new VisionIOInputsAutoLogged())); configureControls(); } diff --git a/src/main/java/com/cyberknights4911/wham/WhamConstants.java b/src/main/java/com/cyberknights4911/wham/WhamConstants.java index db262e9..99af5c3 100644 --- a/src/main/java/com/cyberknights4911/wham/WhamConstants.java +++ b/src/main/java/com/cyberknights4911/wham/WhamConstants.java @@ -14,8 +14,21 @@ import com.cyberknights4911.constants.DriveConstants; import com.cyberknights4911.constants.DriveConstantsBuilder; import com.cyberknights4911.constants.DriveConstantsModuleConstantsBuilder; +import com.cyberknights4911.logging.Alert; +import com.cyberknights4911.logging.Alert.AlertType; import com.cyberknights4911.logging.Mode; +import com.cyberknights4911.vision.CameraConstants; +import com.cyberknights4911.vision.CameraConstantsBuilder; +import com.cyberknights4911.vision.VisionConstants; +import com.cyberknights4911.vision.VisionConstantsBuilder; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +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.util.Units; +import java.io.UncheckedIOException; +import java.util.ArrayList; public final class WhamConstants { private WhamConstants() {} @@ -25,7 +38,7 @@ private WhamConstants() {} .name("Wham") .loopPeriodSecs(0.02) .tuningMode(true) - .logPath("/media/sda1/") + .logPath(null) .mode(Mode.REAL) .supplier(Wham::new) .build(); @@ -79,4 +92,37 @@ private WhamConstants() {} .encoderOffset(1.804 - Math.PI) .build()) .build(); + + private static Alert noAprilTagLayoutAlert = + new Alert("No AprilTag layout file found. Update VisionConstants.", AlertType.WARNING); + + private static AprilTagFieldLayout getLayout() { + try { + noAprilTagLayoutAlert.set(false); + return AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); + } catch (UncheckedIOException e) { + noAprilTagLayoutAlert.set(true); + return new AprilTagFieldLayout( + new ArrayList<>(), Units.feetToMeters(12), Units.feetToMeters(12)); + } + } + + public static final VisionConstants VISION_CONSTANTS = + VisionConstantsBuilder.builder() + .layout(getLayout()) + .maxAmbiguity(0.03) + .maxValidDistanceMeters(3.0) + .build(); + + // 10 inches back of center, 36 inches from floor, 3 inches left of center + public static final CameraConstants CAMERA_CONSTANTS = + CameraConstantsBuilder.builder() + .name("whamcam") + .photonCameraName("Arducam_OV2311_USB_Camera") + .robotToCamera( + new Transform3d( + new Translation3d( + Units.inchesToMeters(-10), Units.inchesToMeters(3), Units.inchesToMeters(36)), + new Rotation3d(0, 0, 0))) + .build(); } diff --git a/src/main/java/com/cyberknights4911/wham/vision/Vision.java b/src/main/java/com/cyberknights4911/wham/vision/Vision.java deleted file mode 100644 index cf79fc6..0000000 --- a/src/main/java/com/cyberknights4911/wham/vision/Vision.java +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright (c) 2023 FRC 4911 -// https://github.com/frc4911 -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package com.cyberknights4911.wham.vision; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; - -public class Vision extends SubsystemBase { - private final VisionIOInputsAutoLogged inputs = new VisionIOInputsAutoLogged(); - private final VisionIO visionIO; - - public Vision(VisionIO visionIO) { - this.visionIO = visionIO; - } - - @Override - public void periodic() { - visionIO.updateInputs(inputs); - Logger.processInputs("Vision", inputs); - } -}