diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 4dd1b15c..471cba73 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -371,32 +371,31 @@ public static final class Autoc { 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the // angular constraints have no effect. } - // #endregion } public static final class Limelightc { public static final String INTAKE_LL_NAME = "limelight-intake"; public static final String SHOOTER_LL_NAME = "limelight-shooter"; - public static final double ERROR_TOLERANCE_RAD = 0.1; - public static final double HORIZONTAL_FOV_DEG = 0; - public static final double RESOLUTION_WIDTH_PIX = 640; - public static final double MOUNT_ANGLE_DEG_SHOOTER = 42.5; - public static final double MOUNT_ANGLE_DEG_INTAKE = -22; // 23.228 - public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(11.5); // 16.6 - public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); // 16.6 - public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; - public static final double NOTE_HEIGHT = Units.inchesToMeters(0); + public static final double ERROR_TOLERANCE_RAD = 0.1; // unused + public static final double HORIZONTAL_FOV_DEG = 0; // unused + public static final double RESOLUTION_WIDTH_PIX = 640; // unused + public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446; + public static final double MOUNT_ANGLE_DEG_INTAKE = -29; + public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); + public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); + public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused + public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused public static final double MIN_MOVEMENT_METERSPSEC = 0.5; public static final double MIN_MOVEMENT_RADSPSEC = 0.5; public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675); - public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(19.5); + public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911); public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75); - public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); + public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused public static final class Apriltag { public static final int RED_SPEAKER_CENTER_TAG_ID = 4; public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; - public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125 + public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125); public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875); } diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index 06c2e603..a4efdcce 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java @@ -33,7 +33,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { rotationPID.enableContinuousInput(-180, 180); Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg())); + .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad())); rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); SendableRegistry.addChild(this, rotationPID); @@ -43,7 +43,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { @Override public void execute() { Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()) - .minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg())); + .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad())); rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); if (teleopDrive == null) drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index e172a7ce..f7124b35 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -34,8 +34,13 @@ public void periodic() { getDistanceToSpeakerMeters(); getCurrentPose(); getDistanceToNoteMeters(); - } + // intake limelight testing + SmartDashboard.putBoolean("see note", LimelightHelpers.getTV(INTAKE_LL_NAME)); + SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters()); + SmartDashboard.putNumber("intake tx", LimelightHelpers.getTX(INTAKE_LL_NAME)); + SmartDashboard.putNumber("rotation to align", getRotateAngleRad()); + } public void updateBotPose3d() { botPose = LimelightHelpers.getBotPose3d(SHOOTER_LL_NAME); @@ -49,7 +54,6 @@ public Pose2d getCurrentPose() { return estimatedPos; } - public double getTXDeg(String limelightName) { return (limelightName == INTAKE_LL_NAME) ? LimelightHelpers.getTX(INTAKE_LL_NAME) : -LimelightHelpers.getTY(SHOOTER_LL_NAME); } @@ -61,21 +65,18 @@ public double getTYDeg(String limelightName) { public double getDistanceToSpeakerMeters() { if (LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == RED_SPEAKER_CENTER_TAG_ID || LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == BLUE_SPEAKER_CENTER_TAG_ID) { - // TODO: change MOUNT_ANGLE_DEG_SHOOTER Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER) .plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); //because limelight is mounted horizontally double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER) / angleToGoal.getTan(); // SmartDashboard.putNumber("limelight distance", distance); return distance; } - else { // SmartDashboard.putNumber("limelight distance", -1); return -1; } } - public double getDistanceToNoteMeters() { Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_INTAKE) .plus(Rotation2d.fromDegrees(getTYDeg(INTAKE_LL_NAME))); @@ -89,16 +90,15 @@ public double getDistanceToNoteMeters() { } } - public double getArmAngleToShootSpeakerRad(){ + public double getArmAngleToShootSpeakerRad() { double armRestingHeightToSubwooferMeters = HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS; double horizontalDistanceMeters = getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH; return END_EFFECTOR_BASE_ANGLE_RADS - Math.atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters); } - public double getRotateAngleDeg() { + public double getRotateAngleRad() { double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters(); double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters()); return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters()); } - } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java index a9da99ef..2c03899d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java +++ b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.1 (March 1, 2023) +//LimelightHelpers v1.5.0 (March 27, 2024) package org.carlmontrobotics.subsystems; @@ -303,6 +303,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -363,8 +375,58 @@ public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + public String error; + public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -382,7 +444,7 @@ static final String sanitizeName(String name) { return name; } - static Pose3d toPose3D(double[] inData){ + private static Pose3d toPose3D(double[] inData) { if(inData.length < 6) { //System.err.println("Bad LL 3D Pose Data!"); @@ -394,10 +456,10 @@ static Pose3d toPose3D(double[] inData){ Units.degreesToRadians(inData[5]))); } - static Pose2d toPose2D(double[] inData){ + private static Pose2d toPose2D(double[] inData) { if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + // System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -405,6 +467,83 @@ static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray, 6); + int tagCount = (int) extractBotPoseEntry(poseArray, 7); + double tagSpan = extractBotPoseEntry(poseArray, 8); + double tagDist = extractBotPoseEntry(poseArray, 9); + double tagArea = extractBotPoseEntry(poseArray, 10); + // getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -544,8 +683,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -604,6 +743,30 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -618,6 +781,30 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -643,6 +830,10 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -696,6 +887,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -765,7 +978,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime();