From 78f1e1afe9a98439247e50ea3107d18b5125a7d5 Mon Sep 17 00:00:00 2001 From: Zach Rutman Date: Thu, 1 Aug 2024 17:05:31 -0700 Subject: [PATCH 1/5] feat: update swerve config for krakens and adjusted inversion (#53) --- .../org/team1540/robot2024/Constants.java | 12 ++++----- .../subsystems/drive/ModuleIOTalonFX.java | 5 ++-- .../robot2024/util/swerve/SwerveFactory.java | 26 ++++++++++++++++--- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 71de1fa9..14b70cd7 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -52,10 +52,10 @@ public static class SwerveConfig { public static final String CAN_BUS = "swerve"; public static final double CAN_UPDATE_FREQUENCY_HZ = 200.0; - public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 1 : 9; - public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 7 : 2; - public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 4 : 6; - public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 3 : 5; + public static final int FRONT_LEFT = 4; + public static final int FRONT_RIGHT = 3; + public static final int BACK_LEFT = 1; + public static final int BACK_RIGHT = 7; public static final int PIGEON_ID = 9; } @@ -208,8 +208,8 @@ public static class Elevator { public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + MAX_HEIGHT - MINIMUM_HEIGHT; public static final double GEAR_RATIO = 11.571; - public static final int LEADER_ID = 7; - public static final int FOLLOWER_ID = 8; + public static final int LEADER_ID = 7; // RIGHT + public static final int FOLLOWER_ID = 8; // LEFT public static final int LEFT_FLIPPER_ID = 1; diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java index 9d055d52..c261d034 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -130,7 +129,7 @@ public void setTurnVoltage(double volts) { @Override public void setDriveBrakeMode(boolean enable) { MotorOutputConfigs config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; + driveTalon.getConfigurator().refresh(config); config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; driveTalon.getConfigurator().apply(config); } @@ -138,7 +137,7 @@ public void setDriveBrakeMode(boolean enable) { @Override public void setTurnBrakeMode(boolean enable) { MotorOutputConfigs config = new MotorOutputConfigs(); - config.Inverted = IS_TURN_MOTOR_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + turnTalon.getConfigurator().refresh(config); config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; turnTalon.getConfigurator().apply(config); } diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java index 2df557a4..857cd610 100644 --- a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java +++ b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java @@ -47,7 +47,6 @@ public static class SwerveModuleHW { public final TalonFX driveMotor; public final TalonFX turnMotor; public final CANcoder cancoder; - private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { if (id < 1 || id > 9) { throw new IllegalArgumentException("Swerve module id must be between 1 and 9"); @@ -56,6 +55,27 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { canbus = ""; } + boolean invertTurn = true; + boolean invertDrive = true; + switch (corner) { + case FRONT_LEFT: + invertTurn = false; + invertDrive = false; + break; + case FRONT_RIGHT: + invertTurn = true; + invertDrive = false; + break; + case BACK_LEFT: + invertTurn = true; + invertDrive = false; + break; + case BACK_RIGHT: + invertTurn = false; + invertDrive = true; + break; + } + int driveID = 30 + id; int turnID = 20 + id; int canCoderID = 10 + id; @@ -72,11 +92,11 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) { driveConfig.CurrentLimits.SupplyCurrentThreshold = 60.0; driveConfig.CurrentLimits.SupplyTimeThreshold = 0.1; driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + driveConfig.MotorOutput.Inverted = invertDrive ? InvertedValue.CounterClockwise_Positive: InvertedValue.Clockwise_Positive; turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - turnConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + turnConfig.MotorOutput.Inverted = invertTurn ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; turnConfig.Feedback.FeedbackRemoteSensorID = canCoderID; turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; turnConfig.Feedback.SensorToMechanismRatio = 1.0; From 3f2f86b137df9706688556b2da4677b4e222e582 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 16 Sep 2024 14:31:57 -0700 Subject: [PATCH 2/5] feat: bump to new LL lib version, send gyro velocity to MT2 --- .../java/org/team1540/robot2024/Robot.java | 3 - .../team1540/robot2024/RobotContainer.java | 3 +- .../subsystems/drive/Drivetrain.java | 6 +- .../subsystems/vision/AprilTagVision.java | 7 +- .../vision/AprilTagVisionIOLimelight.java | 1 - .../vision/AprilTagVisionIOMegaTag2.java | 15 +- .../util/vision/LimelightHelpers.java | 431 ++++++++++++++---- 7 files changed, 374 insertions(+), 92 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 8b510894..afe657e1 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -105,9 +105,6 @@ public void robotInit() { PathfindingCommand.warmupCommand().schedule(); AprilTagsCrescendo.getInstance().getTag(1); - - // Init driver cam - LimelightHelpers.setCameraMode_Driver("limelight-driver"); } /** diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index a6b00b22..a848b583 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -75,7 +75,8 @@ public RobotContainer() { aprilTagVision = AprilTagVision.createReal( drivetrain::addVisionMeasurement, elevator::getPosition, - drivetrain::getRotation); + drivetrain::getRotation, + () -> drivetrain.getChassisSpeeds().omegaRadiansPerSecond); } else { elevator = Elevator.createDummy(); drivetrain = Drivetrain.createReal(odometrySignalRefresher, () -> 0.0); diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 7e9a7abe..c925f4ce 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -192,7 +192,6 @@ public void periodic() { // Update odometry poseEstimator.update(rawGyroRotation, getModulePositions()); visionPoseEstimator.update(rawGyroRotation, getModulePositions()); - } /** @@ -285,9 +284,12 @@ public double getCharacterizationVelocity() { } return driveVelocityAverage / 4.0; } + @AutoLogOutput(key = "Odometry/ChassisSpeeds") public ChassisSpeeds getChassisSpeeds() { - return kinematics.toChassisSpeeds(getModuleStates()); + ChassisSpeeds speeds = kinematics.toChassisSpeeds(getModuleStates()); + if (gyroInputs.connected) speeds.omegaRadiansPerSecond = gyroInputs.yawVelocityRadPerSec; + return speeds; } @AutoLogOutput(key = "Odometry/ChassisSpeedMagnitude") diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java index 02ec3829..3cac01b2 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java @@ -12,6 +12,7 @@ import org.team1540.robot2024.util.vision.EstimatedVisionPose; import java.util.function.Consumer; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import static org.team1540.robot2024.Constants.Vision.*; @@ -47,13 +48,15 @@ private AprilTagVision( public static AprilTagVision createReal(Consumer visionPoseConsumer, Supplier elevatorHeightSupplierMeters, - Supplier headingSupplier) { + Supplier headingSupplier, + DoubleSupplier headingVelocitySupplierRadPerSec) { if (Constants.currentMode != Constants.Mode.REAL) { DriverStation.reportWarning("Using real vision on simulated robot", false); } return new AprilTagVision( new AprilTagVisionIOPhoton(FRONT_CAMERA_NAME, FRONT_CAMERA_POSE), - new AprilTagVisionIOMegaTag2(REAR_CAMERA_NAME, REAR_CAMERA_POSE, headingSupplier), + new AprilTagVisionIOMegaTag2( + REAR_CAMERA_NAME, REAR_CAMERA_POSE, headingSupplier, headingVelocitySupplierRadPerSec), visionPoseConsumer, elevatorHeightSupplierMeters); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java index 064d67e5..b67028bf 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -10,7 +10,6 @@ public class AprilTagVisionIOLimelight implements AprilTagVisionIO { public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) { this.name = name; - LimelightHelpers.setCameraMode_Processor(name); LimelightHelpers.setLEDMode_PipelineControl(name); setPoseOffset(cameraOffsetMeters); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java index d3d84f44..d5d7bbea 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java @@ -6,23 +6,32 @@ import org.team1540.robot2024.util.vision.LimelightHelpers; import java.util.Arrays; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; public class AprilTagVisionIOMegaTag2 implements AprilTagVisionIO { private final String name; private final Supplier heading; + private final DoubleSupplier headingVelocityRadPerSec; - public AprilTagVisionIOMegaTag2(String name, Pose3d cameraOffsetMeters, Supplier heading) { + public AprilTagVisionIOMegaTag2(String name, + Pose3d cameraOffsetMeters, + Supplier heading, + DoubleSupplier headingVelocityRadPerSec) { this.name = name; this.heading = heading; - LimelightHelpers.setCameraMode_Processor(name); + this.headingVelocityRadPerSec = headingVelocityRadPerSec; LimelightHelpers.setLEDMode_PipelineControl(name); setPoseOffset(cameraOffsetMeters); } @Override public void updateInputs(AprilTagVisionIOInputs inputs) { - LimelightHelpers.SetRobotOrientation(name, heading.get().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation( + name, + heading.get().getDegrees(), + headingVelocityRadPerSec.getAsDouble(), + 0, 0, 0, 0); LimelightHelpers.PoseEstimate measurement = DriverStation.isDisabled() ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name) diff --git a/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java index f57f50cc..92fd2002 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java +++ b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java @@ -1,10 +1,12 @@ -//LimelightHelpers v1.5.0 (March 27, 2024) +//LimelightHelpers v1.9 (REQUIRES 2024.9.1) package org.team1540.robot2024.util.vision; +import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,6 +19,7 @@ import java.net.HttpURLConnection; import java.net.MalformedURLException; import java.net.URL; +import java.util.Map; import java.util.concurrent.CompletableFuture; import com.fasterxml.jackson.annotation.JsonFormat; @@ -25,9 +28,13 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; public class LimelightHelpers { + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + public static class LimelightTarget_Retro { @JsonProperty("t6c_ts") @@ -179,7 +186,7 @@ public Pose2d getTargetPose_RobotSpace2D() { return toPose2D(targetPose_RobotSpace); } - + @JsonProperty("ta") public double ta; @@ -197,7 +204,7 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("ts") public double ts; - + public LimelightTarget_Fiducial() { cameraPose_TargetSpace = new double[6]; robotPose_FieldSpace = new double[6]; @@ -271,7 +278,9 @@ public LimelightTarget_Detector() { } } - public static class Results { + public static class LimelightResults { + + public String error; @JsonProperty("pID") public double pipelineID; @@ -305,13 +314,13 @@ public static class Results { @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; @@ -321,11 +330,11 @@ public static class Results { public Pose3d getBotPose3d() { return toPose3D(botpose); } - + public Pose3d getBotPose3d_wpiRed() { return toPose3D(botpose_wpired); } - + public Pose3d getBotPose3d_wpiBlue() { return toPose3D(botpose_wpiblue); } @@ -333,11 +342,11 @@ public Pose3d getBotPose3d_wpiBlue() { public Pose2d getBotPose2d() { return toPose2D(botpose); } - + public Pose2d getBotPose2d_wpiRed() { return toPose2D(botpose_wpired); } - + public Pose2d getBotPose2d_wpiBlue() { return toPose2D(botpose_wpiblue); } @@ -357,7 +366,7 @@ public Pose2d getBotPose2d_wpiBlue() { @JsonProperty("Barcode") public LimelightTarget_Barcode[] targets_Barcode; - public Results() { + public LimelightResults() { botpose = new double[6]; botpose_wpired = new double[6]; botpose_wpiblue = new double[6]; @@ -369,30 +378,18 @@ public Results() { targets_Barcode = new LimelightTarget_Barcode[0]; } - } - - 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 int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { @@ -406,6 +403,41 @@ public RawFiducial(int id, double txnc, double tync, double ta, double distToCam } } + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; @@ -414,11 +446,25 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; - public RawFiducial[] rawFiducials; + public RawFiducial[] rawFiducials; + + /** + * Makes a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + } - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, 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; @@ -429,6 +475,7 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, this.avgTagArea = avgTagArea; this.rawFiducials = rawFiducials; } + } private static ObjectMapper mapper; @@ -445,19 +492,19 @@ static final String sanitizeName(String name) { return name; } - private static Pose3d toPose3D(double[] inData){ + public static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); } - private static Pose2d toPose2D(double[] inData){ + public static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 2D Pose Data!"); @@ -468,7 +515,41 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } - private static double extractBotPoseEntry(double[] inData, int position){ + /** + * Converts a Pose3d object to an array of doubles. + * + * @param pose The Pose3d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles. + * + * @param pose The Pose2d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ if(inData.length < position+1) { return 0; @@ -477,26 +558,34 @@ private static double extractBotPoseEntry(double[] inData, int position){ } private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); - var poseArray = poseEntry.getDoubleArray(new double[0]); + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + 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); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); RawFiducial[] rawFiducials = new RawFiducial[tagCount]; int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial*tagCount; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; if (poseArray.length != expectedTotalVals) { // Don't populate fiducials - } - else{ + } else { for(int i = 0; i < tagCount; i++) { int baseIndex = 11 + (i * valsPerFiducial); int id = (int)poseArray[baseIndex]; @@ -510,15 +599,74 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr } } - return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 11; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; } - private static void printPoseEstimate(PoseEstimate pose) { + public 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); @@ -527,12 +675,12 @@ private static void printPoseEstimate(PoseEstimate pose) { 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]; @@ -552,10 +700,22 @@ public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { return getLimelightNTTable(tableName).getEntry(entryName); } + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + public static double getLimelightNTDouble(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); } @@ -572,10 +732,17 @@ public static double[] getLimelightNTDoubleArray(String tableName, String entryN return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); } + + public static String getLimelightNTString(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getString(""); } + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + public static URL getLimelightURLString(String tableName, String request) { String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; URL url; @@ -602,6 +769,45 @@ public static double getTA(String limelightName) { return getLimelightNTDouble(limelightName, "ta"); } + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + public static double getLatency_Pipeline(String limelightName) { return getLimelightNTDouble(limelightName, "tl"); } @@ -614,13 +820,17 @@ public static double getCurrentPipelineIndex(String limelightName) { return getLimelightNTDouble(limelightName, "getpipe"); } + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + public static String getJSONDump(String limelightName) { return getLimelightNTString(limelightName, "json"); } /** * Switch to getBotPose - * + * * @param limelightName * @return */ @@ -631,7 +841,7 @@ public static double[] getBotpose(String limelightName) { /** * Switch to getBotPose_wpiRed - * + * * @param limelightName * @return */ @@ -642,7 +852,7 @@ public static double[] getBotpose_wpiRed(String limelightName) { /** * Switch to getBotPose_wpiBlue - * + * * @param limelightName * @return */ @@ -691,6 +901,10 @@ public static String getNeuralClassID(String limelightName) { return getLimelightNTString(limelightName, "tclass"); } + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + ///// ///// @@ -737,7 +951,7 @@ public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) - * + * * @param limelightName * @return */ @@ -750,7 +964,7 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { /** * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE * alliance - * + * * @param limelightName * @return */ @@ -761,7 +975,7 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { /** * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE * alliance - * + * * @param limelightName * @return */ @@ -772,7 +986,7 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightN /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) - * + * * @param limelightName * @return */ @@ -806,7 +1020,7 @@ public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightNa /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) - * + * * @param limelightName * @return */ @@ -828,7 +1042,7 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } - + public static void setPriorityTagID(String limelightName, int ID) { setLimelightNTDouble(limelightName, "priorityid", ID); } @@ -865,13 +1079,6 @@ public static void setStreamMode_PiPSecondary(String limelightName) { setLimelightNTDouble(limelightName, "stream", 2); } - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } - /** * Sets the crop window. The crop window in the UI must be completely open for @@ -886,9 +1093,32 @@ 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) { + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { double[] entries = new double[6]; entries[0] = yaw; @@ -898,16 +1128,57 @@ public static void SetRobotOrientation(String limelightName, double yaw, double entries[4] = roll; entries[5] = rollRate; setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_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 SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + 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; @@ -982,7 +1253,7 @@ public static LimelightResults getLatestResults(String limelightName) { long end = System.nanoTime(); double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; + results.latency_jsonParse = millis; if (profileJSON) { System.out.printf("lljson: %.2f\r\n", millis); } From a06c1dc213fe831e7fec25d40f3aa94ed5a7a588 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 16 Sep 2024 16:05:09 -0700 Subject: [PATCH 3/5] chore: add comment to flag potentially sus behavior --- .../java/org/team1540/robot2024/subsystems/drive/Drivetrain.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index c925f4ce..2427bd45 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -288,6 +288,7 @@ public double getCharacterizationVelocity() { @AutoLogOutput(key = "Odometry/ChassisSpeeds") public ChassisSpeeds getChassisSpeeds() { ChassisSpeeds speeds = kinematics.toChassisSpeeds(getModuleStates()); + //FIXME: check to see if this does anything sus with autos if (gyroInputs.connected) speeds.omegaRadiansPerSecond = gyroInputs.yawVelocityRadPerSec; return speeds; } From 738b0fd2159931945c2e0363de8411441c7c5ad6 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Thu, 26 Sep 2024 10:03:39 -0700 Subject: [PATCH 4/5] fix: directly get angular velocity, don't use scuffed chassisspeed computation --- .../java/org/team1540/robot2024/RobotContainer.java | 3 +-- .../robot2024/subsystems/drive/Drivetrain.java | 10 +++++----- .../vision/gamepiece/NoteVisionIOLimelight.java | 1 - 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 3aa4e57e..2a0a123e 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -42,7 +42,6 @@ import java.util.function.BooleanSupplier; -import static org.team1540.robot2024.Constants.SwerveConfig; import static org.team1540.robot2024.Constants.isTuningMode; public class RobotContainer { @@ -83,7 +82,7 @@ public RobotContainer() { drivetrain::addVisionMeasurement, elevator::getPosition, drivetrain::getRotation, - () -> drivetrain.getChassisSpeeds().omegaRadiansPerSecond); + drivetrain::getAngularVelocityRadPerSec); noteVision = NoteVision.createReal(); } else { elevator = Elevator.createDummy(); diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 2427bd45..6a8bb179 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -1,7 +1,6 @@ package org.team1540.robot2024.subsystems.drive; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.FollowPathHolonomic; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; @@ -287,10 +286,7 @@ public double getCharacterizationVelocity() { @AutoLogOutput(key = "Odometry/ChassisSpeeds") public ChassisSpeeds getChassisSpeeds() { - ChassisSpeeds speeds = kinematics.toChassisSpeeds(getModuleStates()); - //FIXME: check to see if this does anything sus with autos - if (gyroInputs.connected) speeds.omegaRadiansPerSecond = gyroInputs.yawVelocityRadPerSec; - return speeds; + return kinematics.toChassisSpeeds(getModuleStates()); } @AutoLogOutput(key = "Odometry/ChassisSpeedMagnitude") @@ -334,6 +330,10 @@ public Rotation2d getRawGyroRotation() { return rawGyroRotation; } + public double getAngularVelocityRadPerSec() { + return gyroInputs.yawVelocityRadPerSec; + } + public void zeroFieldOrientationManual() { fieldOrientationOffset = rawGyroRotation; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java index 25dfac7b..0d9c536e 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java @@ -10,7 +10,6 @@ public class NoteVisionIOLimelight implements NoteVisionIO { public NoteVisionIOLimelight(String name) { this.name = name; - LimelightHelpers.setCameraMode_Processor(name); LimelightHelpers.setLEDMode_PipelineControl(name); LimelightHelpers.setPipelineIndex(name, PIPELINE_INDEX); } From ae7ba1afe419aa1cb378adaf4380eef190b3c1c6 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Thu, 26 Sep 2024 14:13:04 -0700 Subject: [PATCH 5/5] fix: convert angular velocity to degrees --- .../subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java index 5d97ec09..d69d010e 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java @@ -30,7 +30,7 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { LimelightHelpers.SetRobotOrientation( name, heading.get().getDegrees(), - headingVelocityRadPerSec.getAsDouble(), + Math.toDegrees(headingVelocityRadPerSec.getAsDouble()), 0, 0, 0, 0); LimelightHelpers.PoseEstimate measurement = DriverStation.isDisabled()