From 9dfb0d8e0c950fa1dcf361dd7577c9d5767398d5 Mon Sep 17 00:00:00 2001 From: Jonah Snider Date: Wed, 8 Nov 2023 12:33:49 -0800 Subject: [PATCH] Remove references to vision --- src/main/java/frc/robot/config/Config.java | 3 - .../robot/localization/JacksonLagUtil.java | 32 - .../robot/localization/LimelightHelpers.java | 729 ------------------ .../localization/LocalizationSubsystem.java | 112 --- .../frc/robot/localization/VisionMode.java | 11 - 5 files changed, 887 deletions(-) delete mode 100644 src/main/java/frc/robot/localization/JacksonLagUtil.java delete mode 100644 src/main/java/frc/robot/localization/LimelightHelpers.java delete mode 100644 src/main/java/frc/robot/localization/VisionMode.java diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index 6190917..f8361ce 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import frc.robot.localization.VisionMode; import frc.robot.swerve.SwerveCorner; import frc.robot.swerve.SwerveModuleConstants; @@ -25,8 +24,6 @@ public class Config { public static final int PDP_ID = 0; public static final ModuleType PDP_TYPE = ModuleType.kCTRE; - public static final VisionMode VISION_MODE = VisionMode.OFF; - public static final int PIGEON_ID = 1; public static final double WHEEL_DIAMETER = Units.inchesToMeters(4); diff --git a/src/main/java/frc/robot/localization/JacksonLagUtil.java b/src/main/java/frc/robot/localization/JacksonLagUtil.java deleted file mode 100644 index 72f7d47..0000000 --- a/src/main/java/frc/robot/localization/JacksonLagUtil.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.localization; - -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.localization.LimelightHelpers.LimelightResults; - -public class JacksonLagUtil { - public static void causeLag() { - ObjectMapper mapper = - new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - - System.out.println("JacksonLagUtil: Causing lag spike..."); - double time = Timer.getFPGATimestamp(); - try { - LimelightResults results = new LimelightResults(); - mapper.readValue(mapper.writeValueAsString(results), LimelightHelpers.LimelightResults.class); - } catch (JsonProcessingException e) { - System.err.println("JacksonLagUtil: Failed to cause lag spike, JSON parser error:"); - e.printStackTrace(); - } - System.out.println( - "JacksonLagUtil: Lag spike caused in " + (Timer.getFPGATimestamp() - time) + "s"); - } - - private JacksonLagUtil() {} -} diff --git a/src/main/java/frc/robot/localization/LimelightHelpers.java b/src/main/java/frc/robot/localization/LimelightHelpers.java deleted file mode 100644 index da60d6e..0000000 --- a/src/main/java/frc/robot/localization/LimelightHelpers.java +++ /dev/null @@ -1,729 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.localization; - -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import java.io.IOException; -import java.net.HttpURLConnection; -import java.net.MalformedURLException; -import java.net.URL; -import java.util.concurrent.CompletableFuture; - -public class LimelightHelpers { - - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } - - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } - - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } - - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } - - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } - - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } - - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } - - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } - - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - public static class LimelightTarget_Fiducial { - - @JsonProperty("fid") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } - - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } - - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } - - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } - - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } - - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } - - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } - - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } - - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - public static class LimelightTarget_Barcode {} - - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() {} - } - - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Detector() {} - } - - public static class Results { - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("tl_cap") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - public double valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public Results() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - } - } - - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public LimelightResults() { - targetingResults = new Results(); - } - } - - private static ObjectMapper mapper; - - /** Print JSON Parse time to the console in milliseconds */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if (name == "" || name == null) { - return "limelight"; - } - return name; - } - - private 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]))); - } - - private static Pose2d toPose2D(double[] inData) { - if (inData.length < 6) { - // System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - - ///// - ///// - - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "tl_cap"); - } - - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetSpace"); - } - - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - } - - public static double getTV(String limelightName) { - return getLimelightNTDouble(limelightName, "tv"); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - /** The LEDs will be controlled by Limelight pipeline settings, and not by robot code. */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - /** - * Sets the crop window. The crop window in the UI must be completely open for dynamic cropping to - * work. - */ - public static void setCropWindow( - String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** Asynchronously take snapshot. */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync( - () -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && snapshotName != "") { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** Parses Limelight's JSON results dump into a LimelightResults Object */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = - new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java index 3c1232a..35a5a05 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -4,20 +4,13 @@ package frc.robot.localization; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.InterpolatingTreeMap; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.config.Config; import frc.robot.fms.FmsSubsystem; import frc.robot.imu.ImuSubsystem; -import frc.robot.localization.LimelightHelpers.LimelightResults; import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; @@ -28,37 +21,17 @@ public class LocalizationSubsystem extends LifecycleSubsystem { private final SwerveSubsystem swerve; private final ImuSubsystem imu; - private final SwerveDrivePoseEstimator poseEstimator; private final SwerveDriveOdometry odometry; - private boolean visionWorking = false; - - private Pose2d previousPose = new Pose2d(); - - private final InterpolatingTreeMap visionStdLookup = - new InterpolatingTreeMap(); public LocalizationSubsystem(SwerveSubsystem swerve, ImuSubsystem imu) { super(SubsystemPriority.LOCALIZATION); this.swerve = swerve; this.imu = imu; - poseEstimator = - new SwerveDrivePoseEstimator( - SwerveSubsystem.KINEMATICS, - imu.getRobotHeading(), - swerve.getModulePositions(), - new Pose2d(), - VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), - VecBuilder.fill(0.0334, 0.1391, Units.degreesToRadians(30))); odometry = new SwerveDriveOdometry( SwerveSubsystem.KINEMATICS, imu.getRobotHeading(), swerve.getModulePositions()); - - visionStdLookup.put(0.1, 0.8); - visionStdLookup.put(0.5, 1.0); - visionStdLookup.put(1.0, 2.0); - visionStdLookup.put(2.0, 2.5); } @Override @@ -70,101 +43,20 @@ public void robotPeriodic() { } private void update() { - try { - poseEstimator.update(imu.getRobotHeading(), swerve.getModulePositions()); - } catch (Exception e) { - System.err.println("Pose estimator threw while adding odometry measurement:"); - e.printStackTrace(); - } try { odometry.update(imu.getRobotHeading(), swerve.getModulePositions()); } catch (Exception e) { System.err.println("Odometry threw while adding measurement:"); e.printStackTrace(); } - - boolean visionIsValid = false; // Indicates if vision is valid in this loop. - - if (Config.VISION_MODE == VisionMode.OFF) { - visionWorking = false; - } else if (Config.VISION_MODE == VisionMode.ENABLED_UNUSED) { - Pose2d ntCurrentVisionPose = LimelightHelpers.getBotPose2d_wpiBlue(""); - Pose2d angleAdjustedVisionPose = - new Pose2d(ntCurrentVisionPose.getTranslation(), imu.getRobotHeading()); - Logger.getInstance().recordOutput("Localization/VisionPose", angleAdjustedVisionPose); - visionWorking = false; - } else if (LimelightHelpers.getTV("") == 1) { - Pose2d ntCurrentVisionPose = LimelightHelpers.getBotPose2d_wpiBlue(""); - if (previousPose.getX() != ntCurrentVisionPose.getX() - && previousPose.getY() != ntCurrentVisionPose.getY()) { - previousPose = ntCurrentVisionPose; - - LimelightResults results = LimelightHelpers.getLatestResults(""); - Pose2d angleAdjustedVisionPose = - new Pose2d(ntCurrentVisionPose.getTranslation(), imu.getRobotHeading()); - - double visionTimestamp = - Timer.getFPGATimestamp() - - ((results.targetingResults.latency_capture - + results.targetingResults.latency_jsonParse - + results.targetingResults.latency_pipeline) - / 1000); - - double averageDistanceToIndividualFiducialTags = 0; - double fiducialTagCount = 0; - - // Calculate average distance of each tag seen. - if (results.targetingResults.valid == 1 - && ntCurrentVisionPose.getX() != 0.0 - && ntCurrentVisionPose.getY() != 0.0) { - for (int i = 0; i < results.targetingResults.targets_Fiducials.length; ++i) { - Pose2d fiducialPose = - results.targetingResults.targets_Fiducials[i].getTargetPose_RobotSpace2D(); - double fiducialDistanceAway = - Math.sqrt(Math.pow(fiducialPose.getX(), 2) + Math.pow(fiducialPose.getY(), 2)); - averageDistanceToIndividualFiducialTags += fiducialDistanceAway; - fiducialTagCount++; - } - visionIsValid = true; - } - - // Update pose estimator if vision is valid. - if (visionIsValid) { - // Adjust vision measurement standard deviation by average distance from tags. - double stdForVision = - visionStdLookup.get(averageDistanceToIndividualFiducialTags) / fiducialTagCount; - try { - poseEstimator.addVisionMeasurement( - angleAdjustedVisionPose, - visionTimestamp, - VecBuilder.fill(stdForVision, stdForVision, Units.degreesToRadians(360))); - - } catch (Exception e) { - System.err.println("Pose estimator threw while adding vision measurement:"); - e.printStackTrace(); - } - Logger.getInstance().recordOutput("Localization/VisionPose", angleAdjustedVisionPose); - visionWorking = true; - } - } - } - - if (!visionIsValid) { - Logger.getInstance().recordOutput("Localization/VisionPose", new Pose2d()); - } } public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - public Pose2d getOdometryPose() { return odometry.getPoseMeters(); } public void resetPose(Pose2d pose) { imu.setAngle(pose.getRotation()); - poseEstimator.resetPosition(pose.getRotation(), swerve.getModulePositions(), pose); odometry.resetPosition(pose.getRotation(), swerve.getModulePositions(), pose); } @@ -173,10 +65,6 @@ public void resetGyro(Rotation2d gyroAngle) { resetPose(pose); } - public boolean isVisionWorking() { - return visionWorking; - } - public Command getZeroCommand() { return Commands.runOnce( () -> resetGyro(Rotation2d.fromDegrees(FmsSubsystem.isRedAlliance() ? 180 : 0))); diff --git a/src/main/java/frc/robot/localization/VisionMode.java b/src/main/java/frc/robot/localization/VisionMode.java deleted file mode 100644 index 2b7f379..0000000 --- a/src/main/java/frc/robot/localization/VisionMode.java +++ /dev/null @@ -1,11 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.localization; - -public enum VisionMode { - OFF, - ENABLED_UNUSED, - FULLY_ENABLED; -}