From 0150b5c53e92e1d93eef5120ea9181d554f17123 Mon Sep 17 00:00:00 2001 From: Nora Beda Date: Sat, 15 Mar 2025 12:44:59 -0700 Subject: [PATCH 1/6] integrate trig vision with 'global vision' --- .../java/frc/robot/commands/AlignBarge.java | 31 ++-- .../subsystems/GlobalVisionSubsystem.java | 71 +++++++- .../robot/subsystems/TrigVisionSubsystem.java | 152 +++++++++++------- src/main/java/frc/robot/vision/Camera.java | 8 +- .../frc/robot/vision/LimeLightAprilTag.java | 91 ----------- .../frc/robot/vision/LimelightCamera.java | 22 ++- .../frc/robot/vision/PhotonlibCamera.java | 21 ++- 7 files changed, 225 insertions(+), 171 deletions(-) delete mode 100644 src/main/java/frc/robot/vision/LimeLightAprilTag.java diff --git a/src/main/java/frc/robot/commands/AlignBarge.java b/src/main/java/frc/robot/commands/AlignBarge.java index 91f3db0..1cb479a 100644 --- a/src/main/java/frc/robot/commands/AlignBarge.java +++ b/src/main/java/frc/robot/commands/AlignBarge.java @@ -33,33 +33,41 @@ public class AlignBarge extends Command { private final CommandSwerveDrivetrain drivetrain; private final DoubleSupplier horizontalInputSupplier; - private final PIDController translationPIDController = new PIDController(AutoConstants.Turbo.kTranslationP, AutoConstants.Turbo.kTranslationI, AutoConstants.Turbo.kTranslationD); + private final PIDController translationPIDController = new PIDController(AutoConstants.Turbo.kTranslationP, + AutoConstants.Turbo.kTranslationI, AutoConstants.Turbo.kTranslationD); - private final SwerveRequest.FieldCentricFacingAngle driveRequest = new SwerveRequest.FieldCentricFacingAngle().withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); + private final SwerveRequest.FieldCentricFacingAngle driveRequest = new SwerveRequest.FieldCentricFacingAngle() + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - public AlignBarge(TrigVisionSubsystem vision, CommandSwerveDrivetrain drivetrain, DoubleSupplier horizontalInputSupplier) { + public AlignBarge(TrigVisionSubsystem vision, CommandSwerveDrivetrain drivetrain, + DoubleSupplier horizontalInputSupplier) { this.vision = vision; this.drivetrain = drivetrain; this.horizontalInputSupplier = horizontalInputSupplier; addRequirements(drivetrain); - driveRequest.HeadingController.setPID(AutoConstants.Turbo.kRotationP, AutoConstants.Turbo.kRotationI, AutoConstants.Turbo.kRotationD); + driveRequest.HeadingController.setPID(AutoConstants.Turbo.kRotationP, AutoConstants.Turbo.kRotationI, + AutoConstants.Turbo.kRotationD); } private double sign = 1; @Override public void execute() { - Optional distanceOptional = vision.getLateralDistanceToBarge(); + var offset = vision.getRobotToTag(); + double pidOutput = 0; - if (!distanceOptional.isEmpty()) { - pidOutput = -translationPIDController.calculate(distanceOptional.get().in(Meters), AutoConstants.targetDistanceFromBarge.in(Meters)); + if (offset.isPresent()) { + var parallelDistance = offset.get().getMeasureX(); + pidOutput = -translationPIDController.calculate(parallelDistance.in(Meters), + AutoConstants.targetDistanceFromBarge.in(Meters)); } - Optional tagID = vision.getTagID(); - if (tagID.isPresent()) { - if (tagID.get() == 4 || tagID.get() == 5) { + var tag = vision.getBestTag(); + if (tag != null) { + int id = tag.ID; + if (id == 4 || id == 5) { sign = -1; } else { sign = 1; @@ -72,7 +80,6 @@ public void execute() { if (translationPIDController.atSetpoint()) { vision.isAlignedTimestamp = RobotController.getFPGATime(); } - driveRequest.withVelocityX(Meters.of(sign * pidOutput).per(Second)); driveRequest.withVelocityY(horizontalInputSupplier.getAsDouble()); @@ -85,5 +92,5 @@ public void execute() { public boolean isFinished() { return super.isFinished(); } - + } diff --git a/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java b/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java index ef1e3d5..35cc44a 100644 --- a/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java @@ -1,12 +1,17 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Radians; + import java.io.IOException; import java.util.HashSet; 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 edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.Robot; @@ -21,6 +26,17 @@ public final class GlobalVisionSubsystem implements Subsystem { public static final double kMaxAmbiguity = 0.7; public static final double kMaxDistance = Units.feetToMeters(10); + public static final Translation3d kLimelightTranslation = new Translation3d(0, 0, 0); + public static final Rotation3d kLimelightRotation = new Rotation3d( + Radians.of(0), + Radians.of(0), + Radians.of(0)); + + public static final Transform3d kLimelightTransform = new Transform3d(kLimelightTranslation, kLimelightRotation); + public static final CameraDescription[] kCameras = new CameraDescription[] { + new CameraDescription("limelight", CameraType.LIMELIGHT, kLimelightTransform, null) + }; + public static enum CameraType { PHOTONVISION, LIMELIGHT @@ -31,9 +47,17 @@ public static class CameraDescription { CameraType type; Transform3d offset; Specification spec; + + public CameraDescription(String name, CameraType type, Transform3d offset, Specification spec) { + this.name = name; + this.type = type; + this.offset = offset; + this.spec = spec; + } }; private static class CameraData { + CameraType type; Camera camera; Result result; Simulator sim; @@ -44,10 +68,9 @@ private static class CameraData { private HashSet m_ViableResults; private int m_Frame; + private AprilTagFieldLayout m_Layout; public static GlobalVisionSubsystem configure(CommandSwerveDrivetrain swerve) { - var cameras = new CameraDescription[] { /* cameras */ }; - AprilTagFieldLayout layout; try { layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); @@ -56,7 +79,7 @@ public static GlobalVisionSubsystem configure(CommandSwerveDrivetrain swerve) { layout = null; } - return new GlobalVisionSubsystem(swerve, cameras, layout); + return new GlobalVisionSubsystem(swerve, layout); } private static Camera createCamera(CameraDescription desc, AprilTagFieldLayout layout) { @@ -70,16 +93,17 @@ private static Camera createCamera(CameraDescription desc, AprilTagFieldLayout l } } - public GlobalVisionSubsystem(CommandSwerveDrivetrain swerve, CameraDescription[] cameras, AprilTagFieldLayout layout) { + public GlobalVisionSubsystem(CommandSwerveDrivetrain swerve, AprilTagFieldLayout layout) { m_Swerve = swerve; m_ViableResults = new HashSet<>(); m_Frame = 0; - m_Cameras = new CameraData[cameras.length]; - for (int i = 0; i < cameras.length; i++) { - var desc = cameras[i]; + m_Cameras = new CameraData[kCameras.length]; + for (int i = 0; i < kCameras.length; i++) { + var desc = kCameras[i]; var data = new CameraData(); + data.type = desc.type; data.camera = createCamera(desc, layout); data.result = new Result(); data.sim = null; @@ -106,10 +130,43 @@ private static boolean isResultViable(Result result) { return true; } + public AprilTagFieldLayout getLayout() { + return m_Layout; + } + + public int getCameraCount() { + return m_Cameras.length; + } + + public Camera getCamera(int camera) { + return m_Cameras[camera].camera; + } + + public CameraType getCameraType(int camera) { + return m_Cameras[camera].type; + } + public Result getResult(int camera) { return m_Cameras[camera].result; } + /** + * Finds the first camera of the given type. Returns -1 on failure. + * + * @param type Camera type to search for. + * @return The index of the camera. + */ + public int getCameraByType(CameraType type) { + for (int i = 0; i < m_Cameras.length; i++) { + var data = m_Cameras[i]; + if (data.type == type) { + return i; + } + } + + return -1; + } + @Override public void periodic() { m_ViableResults.clear(); diff --git a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java index 146e962..b611317 100644 --- a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java @@ -1,20 +1,18 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Microseconds; import static edu.wpi.first.units.Units.Milliseconds; +import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Seconds; +import java.util.HashSet; import java.util.Optional; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.LEDPattern; @@ -24,20 +22,41 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.AutoConstants; -import frc.robot.vision.LimeLightAprilTag; -import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.GlobalVisionSubsystem.CameraType; +import frc.robot.vision.Camera.Tag; public final class TrigVisionSubsystem extends SubsystemBase { - private LimeLightAprilTag tag; - private StructPublisher tagRelativePosePublisher = NetworkTableInstance.getDefault() - .getStructTopic("Vision/Position to Barge", Pose2d.struct).publish(); - private Optional lastRecordedPose = Optional.empty(); + private StructPublisher tagRelativePosePublisher = NetworkTableInstance.getDefault() + .getStructTopic("Vision/Position to Barge", Translation3d.struct).publish(); + private Optional lastRecordedOffset = Optional.empty(); private Time timeSinceTagSeen = Seconds.of(0); - private LEDSubsystem m_LedSubsystem;; + private LEDSubsystem leds; + private GlobalVisionSubsystem globalVision; + private int cameraIndex; - public TrigVisionSubsystem(LEDSubsystem m_LedSubsystem) { - tag = new LimeLightAprilTag(); - this.m_LedSubsystem = m_LedSubsystem; + private static final HashSet kValidIDs; + + static { + kValidIDs = new HashSet<>(); + + var validIDs = new int[] { + 14, + 15, + 4, + 5 + }; + + for (int i = 0; i < validIDs.length; i++) { + kValidIDs.add(validIDs[i]); + } + } + + public TrigVisionSubsystem(LEDSubsystem leds, GlobalVisionSubsystem globalVision) { + this.leds = leds; + this.globalVision = globalVision; + + // just use the limelight + cameraIndex = globalVision.getCameraByType(CameraType.LIMELIGHT); } LEDPattern seeingColor = LEDPattern.solid(new Color(255, 0, 255)); @@ -52,67 +71,84 @@ public boolean isAligned() { @Override public void periodic() { - SmartDashboard.putNumber("Vision Aligned Timestamp", Math.abs(RobotController.getFPGATime() - isAlignedTimestamp) * 1e6); - if (tag.hasTarget()) { + SmartDashboard.putNumber("Vision Aligned Timestamp", + Math.abs(RobotController.getFPGATime() - isAlignedTimestamp) * 1e6); + + var tag = getBestTag(); + if (tag != null) { if (!isAligned()) { - this.m_LedSubsystem.applyPatternOnce(seeingColor); + leds.applyPatternOnce(seeingColor); } else { - this.m_LedSubsystem.applyPatternOnce(alignedColor); + leds.applyPatternOnce(alignedColor); } + var camera = globalVision.getCamera(cameraIndex); + var cameraOffset = camera.getOffset(); + + var tagRotation = cameraOffset.getRotation().plus(tag.rotationOffset); + var pitch = tagRotation.getMeasureY(); + var yaw = tagRotation.getMeasureY(); + var horizontalDistance = tag.cameraDistance.times(Math.cos(pitch.in(Radians))); + var verticalDistance = tag.cameraDistance.times(Math.sin(pitch.in(Radians))); + + var directDistance = horizontalDistance.times(Math.cos(yaw.in(Radians))); + var perpendicularDistance = horizontalDistance.times(Math.sin(yaw.in(Radians))); + var cameraToTag = new Translation3d(directDistance, perpendicularDistance, verticalDistance); + + var robotToCamera = cameraOffset.getTranslation(); + var robotToTag = robotToCamera.plus(cameraToTag); + timeSinceTagSeen = Seconds.of(0); - lastRecordedPose = Optional.of(new Pose2d(Meters.of(tag.getVerticalDistanceMeters()), Meters.of(tag.getLateralDistanceMeters()), new Rotation2d())); - tagRelativePosePublisher.set(lastRecordedPose.get()); + lastRecordedOffset = Optional.of(robotToTag); + tagRelativePosePublisher.set(robotToTag); } else { - if (timeSinceTagSeen.gt(AutoConstants.lastPoseTimeout) && lastRecordedPose.isPresent()) { - lastRecordedPose = Optional.empty(); + if (timeSinceTagSeen.gt(AutoConstants.lastPoseTimeout)) { + lastRecordedOffset = Optional.empty(); } + timeSinceTagSeen = timeSinceTagSeen.plus(Milliseconds.of(20)); - this.m_LedSubsystem.applyPatternOnce(this.m_LedSubsystem.allianceColorGetter()); + leds.applyPatternOnce(leds.allianceColorGetter()); } } - public Command resetLastPose() { - return run(() -> { lastRecordedPose = Optional.empty(); }); + public Command reset() { + return run(() -> { + lastRecordedOffset = Optional.empty(); + }); } - public Optional getTagID() { - long tid = tag.getTargetID(); - if (tid == -1) { - return Optional.empty(); + public Tag getBestTag() { + if (cameraIndex < 0) { + return null; } - return Optional.of(tid); - } - - public Optional getHorizontalRotation() { - if (tag.hasTarget()) { - return Optional.of(Degrees.of(tag.getHorizontalOffset())); - } else { - return Optional.empty(); + var result = globalVision.getResult(cameraIndex); + if (!result.isNew) { + return null; } - } - public Optional getLateralDistanceToBarge() { - if (lastRecordedPose.isPresent()) { - return Optional.of(lastRecordedPose.get().getMeasureY()); - } else { - return Optional.empty(); - } - } + int bestTag = -1; + Distance minDistance = null; - public Optional getVerticalDistanceToBarge() { - if (lastRecordedPose.isPresent()) { - return Optional.of(lastRecordedPose.get().getMeasureX()); - } else { - return Optional.empty(); + for (int i = 0; i < result.tags.length; i++) { + var tag = result.tags[i]; + if (!kValidIDs.contains(tag.ID)) { + continue; + } + + if (minDistance == null || tag.cameraDistance.lt(minDistance)) { + bestTag = i; + minDistance = tag.cameraDistance; + } } - } - public boolean canSeeTag() { - return tag.hasTarget(); + return bestTag < 0 ? null : result.tags[bestTag]; } - public boolean hasLastPosition() { - return lastRecordedPose.isPresent(); + + /** + * Returns the last calculated position relative to the barge. + */ + public Optional getRobotToTag() { + return lastRecordedOffset; } } diff --git a/src/main/java/frc/robot/vision/Camera.java b/src/main/java/frc/robot/vision/Camera.java index 9e92bc5..e1a234d 100644 --- a/src/main/java/frc/robot/vision/Camera.java +++ b/src/main/java/frc/robot/vision/Camera.java @@ -2,7 +2,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.units.measure.Distance; public interface Camera { public static interface Simulator { @@ -12,13 +14,17 @@ public static interface Simulator { public static class Tag { public int ID; - public double cameraDistance; + public Distance cameraDistance; + public double ambiguity; + public Rotation3d rotationOffset; + public double area; } public static class Result { public Pose2d pose; public double maxAmbiguity, maxDistance, minDistance; public Tag[] tags; + public int bestTagIndex; public boolean isNew; public double timestamp; diff --git a/src/main/java/frc/robot/vision/LimeLightAprilTag.java b/src/main/java/frc/robot/vision/LimeLightAprilTag.java deleted file mode 100644 index 5462e98..0000000 --- a/src/main/java/frc/robot/vision/LimeLightAprilTag.java +++ /dev/null @@ -1,91 +0,0 @@ -package frc.robot.vision; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Radians; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class LimeLightAprilTag { - // Get the LimeLight NetworkTable. Adjust the table name if necessary. - private final NetworkTable limeTable = NetworkTableInstance.getDefault().getTable("limelight"); - // Camera and target configuration (modify these to match your setup) - private final double cameraHeightMeters = Inches.of(10.5).in(Meters); // Height of the camera off the ground in meters - private final double targetHeightMeters = Inches.of(73).in(Meters); // Height of the AprilTag on the field in meters - private final double cameraAngleDegrees = 65.0; // Angle at which the camera is mounted - // 3, 4.75, 10.5 - - /** -Checks if a valid target is detected. -@return true if target is detected; false otherwise. - */ - public boolean hasTarget() { - return limeTable.getEntry("tv").getDouble(0) == 1.0; - } - /** -Gets the horizontal offset (tx) in degrees. -@return Horizontal offset from the crosshair. - */ - public double getHorizontalOffset() { - return limeTable.getEntry("tx").getDouble(0); - } - /** -Gets the vertical offset (ty) in degrees. -@return Vertical offset from the crosshair. - */ - public double getVerticalOffset() { - return limeTable.getEntry("ty").getDouble(0); - } - /** -Gets the target area (ta). -@return The area of the detected target. - */ - public double getTargetArea() { - return limeTable.getEntry("ta").getDouble(0); - } - /** -Gets the target id (tod). -@return The ID of the detected target. - */ - public long getTargetID() { - return limeTable.getEntry("tid").getInteger(-1); - } - /** -Computes an approximate distance to the target using the vertical offset. -Uses the formula: - distance = (targetHeight - cameraHeight) / tan(cameraAngle + ty) -Make sure to convert angles to radians. - * -@return Estimated distance in meters. - */ - public double getLateralDistanceMeters() { - double ty = getVerticalOffset(); - // Combine the mounting angle with the offset - double angleToTargetRadians = Math.toRadians(cameraAngleDegrees + ty); - return (targetHeightMeters - cameraHeightMeters) / Math.tan(angleToTargetRadians); - } - public double getVerticalDistanceMeters() { - double tx = getHorizontalOffset(); - double verticalDist = getLateralDistanceMeters(); - - return verticalDist * Math.tan(Degrees.of(tx).in(Radians)); - } - /** -Example method to update and print the current LimeLight data. - */ - public void update() { - if (hasTarget()) { - double tx = getHorizontalOffset(); - double ty = getVerticalOffset(); - double distance = getVerticalDistanceMeters(); - System.out.println("Target Detected!"); - System.out.println("Horizontal Offset (tx): " + tx + " degrees"); - System.out.println("Vertical Offset (ty): " + ty + " degrees"); - System.out.println("Estimated Distance: " + distance + " meters"); - } else { - System.out.println("No target detected."); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/LimelightCamera.java b/src/main/java/frc/robot/vision/LimelightCamera.java index 4e99e5e..df0e26c 100644 --- a/src/main/java/frc/robot/vision/LimelightCamera.java +++ b/src/main/java/frc/robot/vision/LimelightCamera.java @@ -1,6 +1,10 @@ package frc.robot.vision; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import frc.lib.LimelightHelpers; @@ -45,7 +49,9 @@ public void update(Result result) { result.pose = estimate.pose; result.timestamp = estimate.timestampSeconds; result.isNew = true; + result.bestTagIndex = -1; + double minAmbiguity = Double.MAX_VALUE; result.maxAmbiguity = Double.MIN_VALUE; result.maxDistance = Double.MIN_VALUE; result.minDistance = Double.MAX_VALUE; @@ -54,13 +60,27 @@ public void update(Result result) { for (int i = 0; i < estimate.tagCount; i++) { var fiducial = estimate.rawFiducials[i]; + if (fiducial.ambiguity < minAmbiguity) { + result.bestTagIndex = i; + minAmbiguity = fiducial.ambiguity; + } + result.maxAmbiguity = Math.max(result.maxAmbiguity, fiducial.ambiguity); result.maxDistance = Math.max(result.maxDistance, fiducial.distToCamera); result.minDistance = Math.min(result.minDistance, fiducial.distToCamera); var tag = new Tag(); tag.ID = fiducial.id; - tag.cameraDistance = fiducial.distToCamera; + tag.cameraDistance = Meters.of(fiducial.distToCamera); + tag.ambiguity = fiducial.ambiguity; + tag.area = fiducial.ta; + + // horizontal -> rotate around vertical axis -> yaw + // vertical -> rotate around horizontal axis -> pitch + var yaw = Degrees.of(fiducial.txnc); + var pitch = Degrees.of(fiducial.tync); + tag.rotationOffset = new Rotation3d(Degrees.of(0), pitch, yaw); + result.tags[i] = tag; } } diff --git a/src/main/java/frc/robot/vision/PhotonlibCamera.java b/src/main/java/frc/robot/vision/PhotonlibCamera.java index aec3f1e..aec3777 100644 --- a/src/main/java/frc/robot/vision/PhotonlibCamera.java +++ b/src/main/java/frc/robot/vision/PhotonlibCamera.java @@ -1,5 +1,8 @@ package frc.robot.vision; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; + import java.util.Optional; import org.photonvision.EstimatedRobotPose; @@ -13,6 +16,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; public final class PhotonlibCamera implements Camera { @@ -94,10 +98,12 @@ private void update(Result result, PhotonPipelineResult cameraResult) { result.isNew = true; result.pose = pose.estimatedPose.toPose2d(); result.timestamp = pose.timestampSeconds; + result.bestTagIndex = -1; result.minDistance = Double.MAX_VALUE; result.maxDistance = Double.MIN_VALUE; result.maxAmbiguity = Double.MIN_VALUE; + double minAmbiguity = Double.MAX_VALUE; var targets = cameraResult.getTargets(); result.tags = new Tag[targets.size()]; @@ -108,13 +114,26 @@ private void update(Result result, PhotonPipelineResult cameraResult) { double distance = transform.getTranslation().getNorm(); double ambiguity = target.getPoseAmbiguity(); + if (ambiguity < minAmbiguity) { + result.bestTagIndex = i; + minAmbiguity = ambiguity; + } + result.minDistance = Math.min(result.minDistance, distance); result.maxDistance = Math.max(result.maxDistance, distance); result.maxAmbiguity = Math.max(result.maxAmbiguity, ambiguity); var tag = new Tag(); tag.ID = target.getFiducialId(); - tag.cameraDistance = distance; + tag.cameraDistance = Meters.of(distance); + tag.ambiguity = ambiguity; + tag.area = target.area; + + var roll = Degrees.of(target.skew); + var pitch = Degrees.of(target.pitch); + var yaw = Degrees.of(target.yaw); + tag.rotationOffset = new Rotation3d(roll, pitch, yaw); + result.tags[i] = tag; } From d3b9199badf0a6114fe5dc039c1c3473726a79d0 Mon Sep 17 00:00:00 2001 From: Nora Beda Date: Sat, 15 Mar 2025 15:50:29 -0700 Subject: [PATCH 2/6] fix robot container --- src/main/java/frc/robot/RobotContainer.java | 178 +++++++++++--------- 1 file changed, 102 insertions(+), 76 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ed78c11..da16594 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -54,36 +54,39 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; public final class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(Units.MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = Units.RotationsPerSecond.of(0.75).in(Units.RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - /* Setting up bindings for necessary control of the swerve drive platform */ - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo); - private final SwerveRequest.RobotCentric autoDrive = new SwerveRequest.RobotCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo); - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - private final Telemetry logger = new Telemetry(); - - private final CommandXboxController m_Controller = new CommandXboxController(0); - private final CommandXboxController m_ScuffedController = new CommandXboxController(2); - private final CommandXboxController m_TuningController = new CommandXboxController(4); - private final CommandJoystick m_OperatorController = new CommandJoystick(1); - - public final CommandSwerveDrivetrain drivetrain; + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(Units.MetersPerSecond); // kSpeedAt12Volts desired top + // speed + private double MaxAngularRate = Units.RotationsPerSecond.of(0.75).in(Units.RadiansPerSecond); // 3/4 of a rotation per + // second max angular + // velocity + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.MotionMagicExpo); + private final SwerveRequest.RobotCentric autoDrive = new SwerveRequest.RobotCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.MotionMagicExpo); + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + private final Telemetry logger = new Telemetry(); + + private final CommandXboxController m_Controller = new CommandXboxController(0); + private final CommandXboxController m_ScuffedController = new CommandXboxController(2); + private final CommandXboxController m_TuningController = new CommandXboxController(4); + private final CommandJoystick m_OperatorController = new CommandJoystick(1); + + public final CommandSwerveDrivetrain drivetrain; public final BotType botType = RobotDiscoverer.getRobot(); -// private SwerveSubsystem m_Swerve; + // private SwerveSubsystem m_Swerve; private GlobalVisionSubsystem m_GlobalVision; + private TrigVisionSubsystem m_TrigVision; private LEDSubsystem m_LedSubsystem = new LEDSubsystem(); private ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); - private TrigVisionSubsystem m_TrigVision = new TrigVisionSubsystem(m_LedSubsystem); private ClawSubsystemTurbo m_ClawSubsystem = new ClawSubsystemTurbo(botType); private ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(botType); public PivotSubsystem m_PivotSubsystem = new PivotSubsystem(botType, m_ElevatorSubsystem.elevator); @@ -108,15 +111,17 @@ public RobotContainer(Rev2mDistanceSensor distance) { DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); } - + if (botType == BotType.ALPHA_BOT) { drivetrain = TunerConstantsAlpha.createDrivetrain(); } else { drivetrain = TunerConstants.createDrivetrain(); } + sysIdChooser = new SysIdChooser(drivetrain, m_ElevatorSubsystem, m_PivotSubsystem); - - initSubsystems(); + + m_GlobalVision = GlobalVisionSubsystem.configure(drivetrain); + m_TrigVision = new TrigVisionSubsystem(m_LedSubsystem, m_GlobalVision); switch (botType) { case MAIN_BOT: @@ -134,7 +139,7 @@ public RobotContainer(Rev2mDistanceSensor distance) { initManualAutos(); SmartDashboard.putData("Auto Chooser", autoChooser); - + // SignalLogger.setPath("/ctre-logs/"); SignalLogger.start(); if (RobotBase.isReal()) @@ -150,31 +155,31 @@ public double getTravelDir() { return -1; } - + private void initManualAutos() { autoChooser.setDefaultOption("None", Commands.none()); autoChooser.addOption("Travel", drivetrain.applyRequest(() -> drive.withVelocityY(getTravelDir())).withTimeout(2)); - autoChooser.addOption("Coral Yipee", - CoralAutoBuilder.build(distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); - - } + autoChooser.addOption("Coral Yipee", + CoralAutoBuilder.build(distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, + m_TrigVision)); - private void initSubsystems() { } private void changeLevel(boolean moveUp) { - if(moveUp) + if (moveUp) algaeLevel++; else algaeLevel--; - - if(algaeLevel > 4) + + if (algaeLevel > 4) algaeLevel = 4; - if(algaeLevel < 0) + if (algaeLevel < 0) algaeLevel = 0; - levelLog.update((double)algaeLevel); - // ParallelCommandGroup parallelLevelCommands = new ParallelCommandGroup(m_PivotSubsystem.goToAngle(algaeLevel), m_ElevatorSubsystem.goToHeight(algaeLevel)); + levelLog.update((double) algaeLevel); + // ParallelCommandGroup parallelLevelCommands = new + // ParallelCommandGroup(m_PivotSubsystem.goToAngle(algaeLevel), + // m_ElevatorSubsystem.goToHeight(algaeLevel)); // parallelLevelCommands.schedule(); } @@ -188,14 +193,16 @@ public SwerveRequest.FieldCentric getFieldCentricDriveReq() { if (m_ElevatorSubsystem.getElevatorHeight() > 15) { multiplier /= 2; } - + if (m_ElevatorSubsystem.getElevatorHeight() > 30) { multiplier /= 2; } - return drive.withVelocityX(-m_Controller.getLeftY() * MaxSpeed * multiplier) // Drive forward with negative Y (forward) - .withVelocityY(-m_Controller.getLeftX() * MaxSpeed * multiplier) // Drive left with negative X (left) - .withRotationalRate(-m_Controller.getRightX() * MaxAngularRate * multiplier); // Drive counterclockwise with negative X (left) + return drive.withVelocityX(-m_Controller.getLeftY() * MaxSpeed * multiplier) // Drive forward with negative Y + // (forward) + .withVelocityY(-m_Controller.getLeftX() * MaxSpeed * multiplier) // Drive left with negative X (left) + .withRotationalRate(-m_Controller.getRightX() * MaxAngularRate * multiplier); // Drive counterclockwise with + // negative X (left) } public SwerveRequest getDriveReq() { @@ -210,17 +217,19 @@ private void configureSwerveBindings() { // and Y is defined as to the left according to WPILib convention. drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(this::getDriveReq) - ); + drivetrain.applyRequest(this::getDriveReq)); // m_Controller.a().whileTrue(drivetrain.applyRequest(() -> brake)); // m_Controller.b().whileTrue(drivetrain.applyRequest(() -> - // point.withModuleDirection(new Rotation2d(-m_Controller.getLeftY(), -m_Controller.getLeftX())) + // point.withModuleDirection(new Rotation2d(-m_Controller.getLeftY(), + // -m_Controller.getLeftX())) // )); // // reset the field-centric heading on left bumper press m_Controller.x().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - m_Controller.rightBumper().whileTrue(new AlignBarge(m_TrigVision, drivetrain, () -> { return getFieldCentricDriveReq().VelocityY; })); + m_Controller.rightBumper().whileTrue(new AlignBarge(m_TrigVision, drivetrain, () -> { + return getFieldCentricDriveReq().VelocityY; + })); drivetrain.registerTelemetry(logger::telemeterize); } @@ -231,7 +240,7 @@ private void configureBindings() { configureTuningBindings(); configureScuffedBindings(); configureOperatorBindings(); - + new Trigger(m_ClawSubsystem.beambreak::get).negate().whileTrue(m_LedSubsystem.solidColor(new Color(0, 155, 255))); // m_Controller.rightBumper().whileTrue(m_ClawSubsystem.intakeWithBeambreak()); @@ -249,16 +258,18 @@ private void configureBindings() { m_Controller.leftBumper().whileTrue(m_ClimberSubsystem.runClimberup()); m_Controller.povDown().whileTrue(m_ElevatorSubsystem.autoHonePose().withName("Elevator Hone Command")); - + // m_Controller.rightBumper().onTrue( - // ( - // ( - // scuffedElevator(ElevatorConstantb s.stowHeight).alongWith(scuffedPivot(Rotations.of(0.055), false)) - // ).andThen(new WaitCommand(1)) - // ) - // .until(() -> m_PivotSubsystem.closeEnough()) - // .andThen(() -> { m_PivotSubsystem.setDefaultCommand(m_PivotSubsystem.stop());}) - // .andThen(m_PivotSubsystem.stop())); + // ( + // ( + // scuffedElevator(ElevatorConstantb + // s.stowHeight).alongWith(scuffedPivot(Rotations.of(0.055), false)) + // ).andThen(new WaitCommand(1)) + // ) + // .until(() -> m_PivotSubsystem.closeEnough()) + // .andThen(() -> { + // m_PivotSubsystem.setDefaultCommand(m_PivotSubsystem.stop());}) + // .andThen(m_PivotSubsystem.stop())); } public Command scuffedElevator(double rotations) { @@ -270,12 +281,18 @@ public Command scuffedPivot(Angle rotations) { } public void configureScuffedBindings() { - m_ScuffedController.y().onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight).andThen(scuffedPivot(Constants.PivotConstants.bargeAngle))); - m_ScuffedController.a().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(scuffedPivot(Constants.PivotConstants.floorAngle))); - m_ScuffedController.x().onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight).andThen(scuffedPivot(Constants.PivotConstants.stowAngle))); - m_ScuffedController.b().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(scuffedPivot(Constants.PivotConstants.processorAngle))); - m_ScuffedController.povDown().onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight).andThen(scuffedPivot(Constants.PivotConstants.reefOneAngle))); - m_ScuffedController.povUp().onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight).andThen(scuffedPivot(Constants.PivotConstants.reefTwoAngle))); + m_ScuffedController.y().onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight) + .andThen(scuffedPivot(Constants.PivotConstants.bargeAngle))); + m_ScuffedController.a().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight) + .andThen(scuffedPivot(Constants.PivotConstants.floorAngle))); + m_ScuffedController.x().onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight) + .andThen(scuffedPivot(Constants.PivotConstants.stowAngle))); + m_ScuffedController.b().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight) + .andThen(scuffedPivot(Constants.PivotConstants.processorAngle))); + m_ScuffedController.povDown().onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight) + .andThen(scuffedPivot(Constants.PivotConstants.reefOneAngle))); + m_ScuffedController.povUp().onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight) + .andThen(scuffedPivot(Constants.PivotConstants.reefTwoAngle))); // m_ScuffedController.povLeft().onTrue(scuffedElevator(Constants.ElevatorConstants.onCoralHeight).andThen(scuffedPivot(Constants.PivotConstants.onCoralAngle))); m_ScuffedController.leftTrigger().whileTrue(m_ClawSubsystem.intakeWithBeambreak()); @@ -283,7 +300,8 @@ public void configureScuffedBindings() { m_ScuffedController.rightTrigger().whileTrue(m_ClawSubsystem.shootWithBeambreak()); m_ScuffedController.rightBumper().whileTrue(m_ClawSubsystem.shoot()); - m_ScuffedController.povRight().whileTrue(scuffedElevator(Constants.ElevatorConstants.coralIntake).andThen(scuffedPivot(Constants.PivotConstants.coralIntake))); + m_ScuffedController.povRight().whileTrue(scuffedElevator(Constants.ElevatorConstants.coralIntake) + .andThen(scuffedPivot(Constants.PivotConstants.coralIntake))); m_ScuffedController.povLeft().whileTrue(m_ElevatorSubsystem.autoHonePose().withName("Elevator Hone Command")); } @@ -301,12 +319,18 @@ private void configureSimBindings() { } private void configureOperatorBindings() { - m_OperatorController.button(7).onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight).andThen(scuffedPivot(Constants.PivotConstants.bargeAngle))); - m_OperatorController.button(5).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(scuffedPivot(Constants.PivotConstants.floorAngle))); - m_OperatorController.button(8).onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight).andThen(scuffedPivot(Constants.PivotConstants.stowAngle))); - m_OperatorController.button(2).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(scuffedPivot(Constants.PivotConstants.processorAngle))); - m_OperatorController.button(4).onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight).andThen(scuffedPivot(Constants.PivotConstants.reefOneAngle))); - m_OperatorController.button(1).onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight).andThen(scuffedPivot(Constants.PivotConstants.reefTwoAngle))); + m_OperatorController.button(7).onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight) + .andThen(scuffedPivot(Constants.PivotConstants.bargeAngle))); + m_OperatorController.button(5).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight) + .andThen(scuffedPivot(Constants.PivotConstants.floorAngle))); + m_OperatorController.button(8).onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight) + .andThen(scuffedPivot(Constants.PivotConstants.stowAngle))); + m_OperatorController.button(2).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight) + .andThen(scuffedPivot(Constants.PivotConstants.processorAngle))); + m_OperatorController.button(4).onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight) + .andThen(scuffedPivot(Constants.PivotConstants.reefOneAngle))); + m_OperatorController.button(1).onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight) + .andThen(scuffedPivot(Constants.PivotConstants.reefTwoAngle))); m_OperatorController.button(6).whileTrue(m_ClawSubsystem.intakeWithBeambreak()); m_OperatorController.button(9).whileTrue(m_ClawSubsystem.intake()); // m_OperatorController.button().whileTrue(m_ClawSubsystem.shootWithBeambreak()); @@ -322,7 +346,7 @@ private void configureTuningBindings() { public Command getAutonomousCommand() { final double sign; - + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { sign = -1; } else { @@ -330,11 +354,13 @@ public Command getAutonomousCommand() { } Command base = scuffedPivot(Rotations.of(0.241)) - .andThen(Commands.runOnce(() -> { - drivetrain.resetRotation(Rotation2d.fromDegrees(sign * 90)); - }, drivetrain)); + .andThen(Commands.runOnce(() -> { + drivetrain.resetRotation(Rotation2d.fromDegrees(sign * 90)); + }, drivetrain)); // if (Robot.isReal()) { - // base = base.andThen(Commands.run(() -> {}).until(m_PivotSubsystem::closeEnough)); //.andThen(m_ElevatorSubsystem.autoHonePose().asProxy()); + // base = base.andThen(Commands.run(() -> + // {}).until(m_PivotSubsystem::closeEnough)); + // //.andThen(m_ElevatorSubsystem.autoHonePose().asProxy()); // } Command cmd = autoChooser.getSelected().asProxy(); cmd.addRequirements(drivetrain); From f542705a688f06a11f34d9f435930aa8c7c239e8 Mon Sep 17 00:00:00 2001 From: Nora Beda Date: Thu, 20 Mar 2025 15:41:21 -0700 Subject: [PATCH 3/6] full transform --- .../subsystems/GlobalVisionSubsystem.java | 1 - .../robot/subsystems/TrigVisionSubsystem.java | 29 +++++-------------- src/main/java/frc/robot/vision/Camera.java | 3 +- .../frc/robot/vision/LimelightCamera.java | 13 ++++----- .../frc/robot/vision/PhotonlibCamera.java | 6 +--- 5 files changed, 15 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java b/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java index 35cc44a..d3b8b13 100644 --- a/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.Robot; diff --git a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java index b611317..1f4340c 100644 --- a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java @@ -1,16 +1,12 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Seconds; import java.util.HashSet; import java.util.Optional; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.measure.Distance; @@ -26,9 +22,9 @@ import frc.robot.vision.Camera.Tag; public final class TrigVisionSubsystem extends SubsystemBase { - private StructPublisher tagRelativePosePublisher = NetworkTableInstance.getDefault() - .getStructTopic("Vision/Position to Barge", Translation3d.struct).publish(); - private Optional lastRecordedOffset = Optional.empty(); + private StructPublisher tagRelativePosePublisher = NetworkTableInstance.getDefault() + .getStructTopic("Vision/Position to Barge", Transform3d.struct).publish(); + private Optional lastRecordedOffset = Optional.empty(); private Time timeSinceTagSeen = Seconds.of(0); private LEDSubsystem leds; private GlobalVisionSubsystem globalVision; @@ -83,19 +79,8 @@ public void periodic() { } var camera = globalVision.getCamera(cameraIndex); - var cameraOffset = camera.getOffset(); - - var tagRotation = cameraOffset.getRotation().plus(tag.rotationOffset); - var pitch = tagRotation.getMeasureY(); - var yaw = tagRotation.getMeasureY(); - var horizontalDistance = tag.cameraDistance.times(Math.cos(pitch.in(Radians))); - var verticalDistance = tag.cameraDistance.times(Math.sin(pitch.in(Radians))); - - var directDistance = horizontalDistance.times(Math.cos(yaw.in(Radians))); - var perpendicularDistance = horizontalDistance.times(Math.sin(yaw.in(Radians))); - var cameraToTag = new Translation3d(directDistance, perpendicularDistance, verticalDistance); - - var robotToCamera = cameraOffset.getTranslation(); + var robotToCamera = camera.getOffset(); + var cameraToTag = tag.transform; var robotToTag = robotToCamera.plus(cameraToTag); timeSinceTagSeen = Seconds.of(0); @@ -148,7 +133,7 @@ public Tag getBestTag() { /** * Returns the last calculated position relative to the barge. */ - public Optional getRobotToTag() { + public Optional getRobotToTag() { return lastRecordedOffset; } } diff --git a/src/main/java/frc/robot/vision/Camera.java b/src/main/java/frc/robot/vision/Camera.java index e1a234d..7d4477c 100644 --- a/src/main/java/frc/robot/vision/Camera.java +++ b/src/main/java/frc/robot/vision/Camera.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.units.measure.Distance; @@ -16,7 +15,7 @@ public static class Tag { public int ID; public Distance cameraDistance; public double ambiguity; - public Rotation3d rotationOffset; + public Transform3d transform; public double area; } diff --git a/src/main/java/frc/robot/vision/LimelightCamera.java b/src/main/java/frc/robot/vision/LimelightCamera.java index df0e26c..9c8f2d1 100644 --- a/src/main/java/frc/robot/vision/LimelightCamera.java +++ b/src/main/java/frc/robot/vision/LimelightCamera.java @@ -4,6 +4,7 @@ import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -51,14 +52,17 @@ public void update(Result result) { result.isNew = true; result.bestTagIndex = -1; + var limelightResults = LimelightHelpers.getLatestResults(m_Name); + double minAmbiguity = Double.MAX_VALUE; result.maxAmbiguity = Double.MIN_VALUE; result.maxDistance = Double.MIN_VALUE; result.minDistance = Double.MAX_VALUE; result.tags = new Tag[estimate.tagCount]; - for (int i = 0; i < estimate.tagCount; i++) { + for (int i = 0; i < result.tags.length; i++) { var fiducial = estimate.rawFiducials[i]; + var processed = limelightResults.targets_Fiducials[i]; if (fiducial.ambiguity < minAmbiguity) { result.bestTagIndex = i; @@ -74,12 +78,7 @@ public void update(Result result) { tag.cameraDistance = Meters.of(fiducial.distToCamera); tag.ambiguity = fiducial.ambiguity; tag.area = fiducial.ta; - - // horizontal -> rotate around vertical axis -> yaw - // vertical -> rotate around horizontal axis -> pitch - var yaw = Degrees.of(fiducial.txnc); - var pitch = Degrees.of(fiducial.tync); - tag.rotationOffset = new Rotation3d(Degrees.of(0), pitch, yaw); + tag.transform = processed.getTargetPose_CameraSpace().minus(new Pose3d()); result.tags[i] = tag; } diff --git a/src/main/java/frc/robot/vision/PhotonlibCamera.java b/src/main/java/frc/robot/vision/PhotonlibCamera.java index aec3777..124b12e 100644 --- a/src/main/java/frc/robot/vision/PhotonlibCamera.java +++ b/src/main/java/frc/robot/vision/PhotonlibCamera.java @@ -128,11 +128,7 @@ private void update(Result result, PhotonPipelineResult cameraResult) { tag.cameraDistance = Meters.of(distance); tag.ambiguity = ambiguity; tag.area = target.area; - - var roll = Degrees.of(target.skew); - var pitch = Degrees.of(target.pitch); - var yaw = Degrees.of(target.yaw); - tag.rotationOffset = new Rotation3d(roll, pitch, yaw); + tag.transform = transform; result.tags[i] = tag; } From 00f244d30265c5ab93b102cd0a168dd69b590618 Mon Sep 17 00:00:00 2001 From: jbrastad <--global> Date: Thu, 20 Mar 2025 16:15:08 -0700 Subject: [PATCH 4/6] minor syntax errors --- src/main/java/frc/robot/RobotContainer.java | 3 - .../robot/subsystems/TrigVisionSubsystem.java | 4 ++ .../frc/robot/vision/LimeLightAprilTag.java | 69 +++++++++++++++++++ 3 files changed, 73 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/vision/LimeLightAprilTag.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc450a9..2d41889 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -166,9 +166,6 @@ private void initManualAutos() { m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); autoChooser.addOption("L1 + Shoot Algae", CoralAutoBuilder.build(AutoType.Two, distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); - - } - } private void changeLevel(boolean moveUp) { diff --git a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java index 1f4340c..0e3ebc2 100644 --- a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java @@ -136,4 +136,8 @@ public Tag getBestTag() { public Optional getRobotToTag() { return lastRecordedOffset; } + + public boolean hasLastPosition() { + return lastRecordedOffset.isPresent(); + } } diff --git a/src/main/java/frc/robot/vision/LimeLightAprilTag.java b/src/main/java/frc/robot/vision/LimeLightAprilTag.java new file mode 100644 index 0000000..7fbb550 --- /dev/null +++ b/src/main/java/frc/robot/vision/LimeLightAprilTag.java @@ -0,0 +1,69 @@ +package frc.robot.vision; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.Constants.AutoConstants; + +public class LimeLightAprilTag { + public class TagInfo { + public int tid; + public double tx; + public double ty; + + public TagInfo(int id, double tx, double ty) { + this.tid = id; + this.tx = tx; + this.ty = ty; + } + } + // Get the LimeLight NetworkTable. Adjust the table name if necessary. + private final NetworkTable limeTable; + // Camera and target configuration (modify these to match your setup) + public LimeLightAprilTag(String limelightName) { + limeTable = NetworkTableInstance.getDefault().getTable(limelightName); + } + /** +Checks if a valid target is detected. +@return true if target is detected; false otherwise. + */ + public boolean hasTarget() { + return limeTable.getEntry("tv").getDouble(0) == 1.0; + } + /** +Gets the horizontal offset (tx) in degrees. +@return Horizontal offset from the crosshair. + */ + public double getHorizontalOffset() { + return limeTable.getEntry("tx").getDouble(0); + } + /** +Gets the vertical offset (ty) in degrees. +@return Vertical offset from the crosshair. + */ + public double getVerticalOffset() { + return limeTable.getEntry("ty").getDouble(0); + } + /** +Gets the target area (ta). +@return The area of the detected target. + */ + public double getTargetArea() { + return limeTable.getEntry("ta").getDouble(0); + } + /** +Gets the target id (tod). +@return The ID of the detected target. + */ + public long getTargetID() { + return limeTable.getEntry("tid").getInteger(-1); + } + + public TagInfo getInfo() { + return new TagInfo((int)getTargetID(), getHorizontalOffset(), getVerticalOffset()); + } +} \ No newline at end of file From f386387aaf2aa907796044dd4286074544ef8120 Mon Sep 17 00:00:00 2001 From: Nora Beda Date: Thu, 20 Mar 2025 18:22:14 -0700 Subject: [PATCH 5/6] jonas restored a file we didnt need --- .../frc/robot/vision/LimeLightAprilTag.java | 69 ------------------- 1 file changed, 69 deletions(-) delete mode 100644 src/main/java/frc/robot/vision/LimeLightAprilTag.java diff --git a/src/main/java/frc/robot/vision/LimeLightAprilTag.java b/src/main/java/frc/robot/vision/LimeLightAprilTag.java deleted file mode 100644 index 7fbb550..0000000 --- a/src/main/java/frc/robot/vision/LimeLightAprilTag.java +++ /dev/null @@ -1,69 +0,0 @@ -package frc.robot.vision; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Radians; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import frc.robot.Constants.AutoConstants; - -public class LimeLightAprilTag { - public class TagInfo { - public int tid; - public double tx; - public double ty; - - public TagInfo(int id, double tx, double ty) { - this.tid = id; - this.tx = tx; - this.ty = ty; - } - } - // Get the LimeLight NetworkTable. Adjust the table name if necessary. - private final NetworkTable limeTable; - // Camera and target configuration (modify these to match your setup) - public LimeLightAprilTag(String limelightName) { - limeTable = NetworkTableInstance.getDefault().getTable(limelightName); - } - /** -Checks if a valid target is detected. -@return true if target is detected; false otherwise. - */ - public boolean hasTarget() { - return limeTable.getEntry("tv").getDouble(0) == 1.0; - } - /** -Gets the horizontal offset (tx) in degrees. -@return Horizontal offset from the crosshair. - */ - public double getHorizontalOffset() { - return limeTable.getEntry("tx").getDouble(0); - } - /** -Gets the vertical offset (ty) in degrees. -@return Vertical offset from the crosshair. - */ - public double getVerticalOffset() { - return limeTable.getEntry("ty").getDouble(0); - } - /** -Gets the target area (ta). -@return The area of the detected target. - */ - public double getTargetArea() { - return limeTable.getEntry("ta").getDouble(0); - } - /** -Gets the target id (tod). -@return The ID of the detected target. - */ - public long getTargetID() { - return limeTable.getEntry("tid").getInteger(-1); - } - - public TagInfo getInfo() { - return new TagInfo((int)getTargetID(), getHorizontalOffset(), getVerticalOffset()); - } -} \ No newline at end of file From f07f2ee40b7ed26d777b7aec09465632a8c615d4 Mon Sep 17 00:00:00 2001 From: jbrastad <--global> Date: Thu, 20 Mar 2025 19:03:00 -0700 Subject: [PATCH 6/6] initial testing on auto align --- src/main/java/frc/robot/Constants.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 2 +- .../subsystems/GlobalVisionSubsystem.java | 23 ++++++++++++------- .../robot/subsystems/TrigVisionSubsystem.java | 5 ++-- 4 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1ec3a8..01b3d32 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -37,7 +37,7 @@ public static class TelemetryConstants { // this will fully disable logging even when FMS is connected. public static final boolean killswitch = false; // If true, data won't be sent over network even when not connected to FMS - public static final boolean disableNetworkLogging = true; + public static final boolean disableNetworkLogging = false; // ONLY ENABLE IN DEV (this *should* be overwritten when connected to FMS, but that's untested) public static final boolean disableDatalog = false; // Prefix in NetworkTables, must end with a '/' diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 7e9c2b5..7c7f624 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -99,7 +99,7 @@ public class ElevatorSubsystem extends SubsystemBase { private final UltraSupplierLog rightPosePub = new UltraSupplierLog("Elevator/Right Pose", followerMotor.getPosition()); private final Alert estopAlert = new Alert("Elevator E-Stopped", AlertType.kError); - private boolean estop = false; + private boolean estop = true; // there will be at least one limit switch and an encoder to track the position of the elevator public ElevatorSubsystem(BotType bot) { diff --git a/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java b/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java index d3b8b13..4d132d8 100644 --- a/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; import java.io.IOException; import java.util.HashSet; @@ -25,15 +26,19 @@ public final class GlobalVisionSubsystem implements Subsystem { public static final double kMaxAmbiguity = 0.7; public static final double kMaxDistance = Units.feetToMeters(10); - public static final Translation3d kLimelightTranslation = new Translation3d(0, 0, 0); - public static final Rotation3d kLimelightRotation = new Rotation3d( - Radians.of(0), - Radians.of(0), - Radians.of(0)); + public static final Translation3d kBargeCameraTranslation = new Translation3d( + Inches.of(-8), + Inches.of(11), + Inches.of(10.25)); - public static final Transform3d kLimelightTransform = new Transform3d(kLimelightTranslation, kLimelightRotation); + public static final Rotation3d kBargeCameraRotation = new Rotation3d( + Degrees.of(0), + Degrees.of(75), + Degrees.of(90)); + + public static final Transform3d kBargeCameraTransform = new Transform3d(kBargeCameraTranslation, kBargeCameraRotation); public static final CameraDescription[] kCameras = new CameraDescription[] { - new CameraDescription("limelight", CameraType.LIMELIGHT, kLimelightTransform, null) + new CameraDescription("limelight-barge", CameraType.LIMELIGHT, kBargeCameraTransform, null) }; public static enum CameraType { @@ -110,6 +115,8 @@ public GlobalVisionSubsystem(CommandSwerveDrivetrain swerve, AprilTagFieldLayout if (Robot.isSimulation() && desc.spec != null) { data.sim = data.camera.createSimulator(desc.spec); } + + m_Cameras[i] = data; } } diff --git a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java index 0e3ebc2..07e5095 100644 --- a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java @@ -39,7 +39,8 @@ public final class TrigVisionSubsystem extends SubsystemBase { 14, 15, 4, - 5 + 5, + 2 }; for (int i = 0; i < validIDs.length; i++) { @@ -137,7 +138,7 @@ public Optional getRobotToTag() { return lastRecordedOffset; } - public boolean hasLastPosition() { + public boolean canSeeTag() { return lastRecordedOffset.isPresent(); } }