From 40f8c4e08eecec2ca35c43de174400202818cdb2 Mon Sep 17 00:00:00 2001 From: Podcakero Date: Wed, 25 Feb 2026 10:28:58 -0500 Subject: [PATCH] Delete unnecessary files. Clean up code. Change over to BangBangController + Feedforward for Velocity Control. Change over to PIDController + Feedforward for Position Control. --- gradlew | 0 src/main/java/frc/robot/Constants.java | 120 +- src/main/java/frc/robot/LimelightHelpers.java | 1947 ----------------- src/main/java/frc/robot/Robot.java | 23 - src/main/java/frc/robot/RobotContainer.java | 9 - .../robot/subsystems/ClimberSubsystem.java | 16 +- .../frc/robot/subsystems/IntakeSubsystem.java | 79 +- .../robot/subsystems/ShooterSubsystem.java | 107 +- .../robot/subsystems/TargetingSubsystems.java | 207 -- .../swervedrive/SwerveSubsystem.java | 1279 +++++------ .../robot/subsystems/swervedrive/Vision.java | 444 ++-- 11 files changed, 1050 insertions(+), 3181 deletions(-) mode change 100644 => 100755 gradlew delete mode 100644 src/main/java/frc/robot/LimelightHelpers.java delete mode 100644 src/main/java/frc/robot/subsystems/TargetingSubsystems.java diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 60c1364..310de74 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,17 +4,13 @@ package frc.robot; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; 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.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -41,7 +37,7 @@ public final class Constants { public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag - public static final double MAX_SPEED = Units.feetToMeters(14.5); + public static final double MAX_SPEED_MPS = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration. // public static final class AutonConstants @@ -81,14 +77,25 @@ public final class Constants { public static final int LEFT_SHOOTER_MOTOR_ID = 41; public static final int RIGHT_SHOOTER_MOTOR_ID = 40; public static final int INDEXER_MOTOR_ID = 43; + + public static final int SHOOTER_MOTOR_CURRENT_LIMIT = 80; - public static double SHOOTER_MOTOR_P = 0.0018; - public static double SHOOTER_MOTOR_I = 0; - public static double SHOOTER_MOTOR_D = 0; + public static final double SHOOTER_MOTOR_P = 0.0018; + public static final double SHOOTER_MOTOR_I = 0; + public static final double SHOOTER_MOTOR_D = 0; - public static double INDEXER_MOTOR_P = 0.0001; - public static double INDEXER_MOTOR_I = 0; - public static double INDEXER_MOTOR_D = 0; + public static final double INDEXER_MOTOR_P = 0.0001; + public static final double INDEXER_MOTOR_I = 0; + public static final double INDEXER_MOTOR_D = 0; + + public static final double CENTER_MOTOR_S = 0.0; + public static final double CENTER_MOTOR_V = 0.0; + + public static final double LEFT_MOTOR_S = 0.0; + public static final double LEFT_MOTOR_V = 0.0; + + public static final double RIGHT_MOTOR_S = 0.0; + public static final double RIGHT_MOTOR_V = 0.0; /*private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer RPM", 2000) @@ -115,20 +122,37 @@ public final class Constants { }*/ public static final int INTAKE_WHEELS_MOTOR_ID = 50; + public static final int INTAKE_WHEELS_CURRENT_LIMIT = 60; + public static final double INTAKE_WHEELS_POSITION_CONVERSION_FACTOR = 2 * Math.PI; // Encoder Unit * Conversion Factor = Radians. 1 Rotation = 2PI Radians + public static final double INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR = INTAKE_WHEELS_POSITION_CONVERSION_FACTOR * 60.0; // Encoder Units per Minute * Conversion Factor = Radians per Second + public static final int INTAKE_ROTATOR_MOTOR_ID = 51; + public static final int INTAKE_ROTATOR_CURRENT_LIMIT = 40; + public static final double INTAKE_ROTATOR_INITIAL_ENCODER_VALUE = Units.degreesToRadians(-90); + public static final double INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR = (2 * Math.PI) / 12.0; // Encoder Unit * Conversion Factor = Radians. 1 Rotation = 2PI Radians. Gear ratio is 12:1, so 12 Rotations = 1 full intake Rotation. + public static final double INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR = INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR * 60.0; // Encoder Units per Minute * Conversion Factor = Radians per Second - public static class IntakeRotatorPID { - public static final double INTAKE_ROTATOR_P = 0.03; - public static final double INTAKE_ROTATOR_I = 0; - public static final double INTAKE_ROTATOR_D = 0; - } - public static final double INTAKE_MOTOR_P = 0.0001; - public static final double INTAKE_MOTOR_I = 0; - public static final double INTAKE_MOTOR_D = 0; + public static final double INTAKE_WHEELS_MOTOR_P = 0.0001; // Radians -> Radians Per Second + public static final double INTAKE_WHEELS_MOTOR_I = 0.0; // Radians -> Radians Per Second + public static final double INTAKE_WHEELS_MOTOR_D = 0.0; // Radians -> Radians Per Second + public static final double INTAKE_WHEELS_MOTOR_S = 0.0; // Voltage to overcome static friction/inertia + public static final double INTAKE_WHEELS_MOTOR_V = 0.0; // Voltage per Rads/Second gain - public static final double INTAKE_COLLECT_ENCODER_VALUE = 5; - public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5; - public static final double INTAKE_RETRACT_ENCODER_VALUE = 0; + public static final double INTAKE_ROTATOR_DOWN_P = 0.03; // Radians -> Radians Per Second + public static final double INTAKE_ROTATOR_DOWN_I = 0.0; // Radians -> Radians Per Second + public static final double INTAKE_ROTATOR_DOWN_D = 0.0; // Radians -> Radians Per Second + + public static final double INTAKE_ROTATOR_UP_P = 0.06; // Radians -> Radians Per Second + public static final double INTAKE_ROTATOR_UP_I = 0.0; // Radians -> Radians Per Second + public static final double INTAKE_ROTATOR_UP_D = 0.0; // Radians -> Radians Per Second + + public static final double INTAKE_ROTATOR_MOTOR_S = 0.0; // Voltage to overcome static friction/inertia. V + public static final double INTAKE_ROTATOR_MOTOR_G = 0.0; // Voltage to overcome Gravity. V + public static final double INTAKE_ROTATOR_MOTOR_V = 0.0; // Voltage per Radians/Second. V/(Rad/s) + + public static final double INTAKE_COLLECT_POSITION_RADS = 0.0; + public static final double INTAKE_MIDDLE_POSITION_RADS = -Units.degreesToRadians(45); + public static final double INTAKE_RETRACT_POSITION_RADS = -Units.degreesToRadians(90); } @@ -138,45 +162,31 @@ public final class Constants { public static class TargetingConstants { - public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90)); - public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90)); + public static final Pose2d RIGHT_CLIMB_POSE_METERS = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90)); + public static final Pose2d LEFT_CLIMB_POSE_METERS = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90)); - public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Rear Left Camera"); - public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Rear Right Camera"); - public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera"); - public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera"); + public static final Vector SINGLE_TAG_STD_DEVS = VecBuilder.fill(4, 4, 8); + public static final Vector MULTI_TAG_STD_DEVS = VecBuilder.fill(0.5, 0.5, 1); - public static final Pose2d HUB_POSE = new Pose2d(4.625, 4.03, new Rotation2d()); + public static final Pose2d HUB_POSE_METERS = new Pose2d(4.625, 4.03, new Rotation2d()); - public static final Transform3d ORANGE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), - new Rotation3d(0, 0, 0)); - public static final Transform3d BLACK_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), - new Rotation3d(0, 0, 0)); - public static final Transform3d RED_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), - new Rotation3d(0, 0, 0)); - public static final Transform3d PURPLE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), - new Rotation3d(0, 0, 0)); + public static final Translation3d FRONT_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0); + public static final Translation3d FRONT_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0); + public static final Translation3d REAR_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0); + public static final Translation3d REAR_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0); - public static final PhotonPoseEstimator ORANGE_PHOTON_ESTIMATOR = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - ORANGE_ROBOT_TO_CAM); - public static final PhotonPoseEstimator BLACK_PHOTON_ESTIMATOR = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - BLACK_ROBOT_TO_CAM); - public static final PhotonPoseEstimator RED_PHOTON_ESTIMATOR = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - RED_ROBOT_TO_CAM); - public static final PhotonPoseEstimator PURPLE_PHOTON_ESTIMATOR = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - PURPLE_ROBOT_TO_CAM); + public static final Rotation3d FRONT_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0); + public static final Rotation3d FRONT_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0); + public static final Rotation3d REAR_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0); + public static final Rotation3d REAR_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0); } public static class ClimberConstants { public static final int CLIMB_MOTOR_ID = 60; public static final int RATCHET_PWM_PORT = 9; - public static final double RATCHET_UNLOCK_ANGLE = 0; - public static final double RATCHET_LOCK_ANGLE = 180; - public static final double CLIMBER_SPEED = .5; + public static final double RATCHET_UNLOCK_ANGLE_DEGREES = 0; + public static final double RATCHET_LOCK_ANGLE_DEGREES = 180; + public static final double CLIMBER_SPEED_DUTY_CYCLE = .5; } } diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java deleted file mode 100644 index ded212c..0000000 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ /dev/null @@ -1,1947 +0,0 @@ -//LimelightHelpers v1.14 (REQUIRES LLOS 2026.0 OR LATER) - -package frc.robot; - -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 frc.robot.LimelightHelpers.LimelightResults; -import frc.robot.LimelightHelpers.PoseEstimate; -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.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; - -import java.net.MalformedURLException; -import java.net.URL; -import java.util.Arrays; -import java.util.Map; - -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -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 java.util.concurrent.ConcurrentHashMap; -import edu.wpi.first.net.PortForwarder; - -/** - * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. - * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. - */ -public class LimelightHelpers { - - private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - - /** - * Represents a Color/Retroreflective Target Result extracted from JSON Output - */ - 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("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @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]; - } - - } - - /** - * Represents an AprilTag/Fiducial Target Result extracted from JSON Output - */ - 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("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @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]; - } - } - - /** - * Represents a Barcode Target Result extracted from JSON Output - */ - public static class LimelightTarget_Barcode { - - /** - * Barcode family type (e.g. "QR", "DataMatrix", etc.) - */ - @JsonProperty("fam") - public String family; - - /** - * Gets the decoded data content of the barcode - */ - @JsonProperty("data") - public String data; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("pts") - public double[][] corners; - - public LimelightTarget_Barcode() { - } - - public String getFamily() { - return family; - } - } - - /** - * Represents a Neural Classifier Pipeline Result extracted from JSON Output - */ - 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() { - } - } - - /** - * Represents a Neural Detector Pipeline Result extracted from JSON Output - */ - 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("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - public LimelightTarget_Detector() { - } - } - - /** - * Represents hardware statistics from the Limelight. - */ - public static class HardwareReport { - @JsonProperty("cid") - public String cameraId; - - @JsonProperty("cpu") - public double cpuUsage; - - @JsonProperty("dfree") - public double diskFree; - - @JsonProperty("dtot") - public double diskTotal; - - @JsonProperty("ram") - public double ramUsage; - - @JsonProperty("temp") - public double temperature; - - public HardwareReport() { - } - } - - /** - * Represents IMU data from the JSON results. - */ - public static class IMUResults { - @JsonProperty("data") - public double[] data; - - @JsonProperty("quat") - public double[] quaternion; - - @JsonProperty("yaw") - public double yaw; - - // Parsed from data array - public double robotYaw; - public double roll; - public double pitch; - public double rawYaw; - public double gyroZ; - public double gyroX; - public double gyroY; - public double accelZ; - public double accelX; - public double accelY; - - public IMUResults() { - data = new double[0]; - quaternion = new double[4]; - } - - public void parseDataArray() { - if (data != null && data.length >= 10) { - robotYaw = data[0]; - roll = data[1]; - pitch = data[2]; - rawYaw = data[3]; - gyroZ = data[4]; - gyroX = data[5]; - gyroY = data[6]; - accelZ = data[7]; - accelX = data[8]; - accelY = data[9]; - } - } - } - - /** - * Represents capture rewind buffer statistics. - */ - public static class RewindStats { - @JsonProperty("bufferUsage") - public double bufferUsage; - - @JsonProperty("enabled") - public int enabled; - - @JsonProperty("flushing") - public int flushing; - - @JsonProperty("frameCount") - public int frameCount; - - @JsonProperty("latpen") - public int latencyPenalty; - - @JsonProperty("storedSeconds") - public double storedSeconds; - - public RewindStats() { - } - } - - /** - * Limelight Results object, parsed from a Limelight's JSON results output. - */ - public static class LimelightResults { - - public String error; - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("ts_nt") - public long timestamp_nt; - - @JsonProperty("ts_sys") - public long timestamp_sys; - - @JsonProperty("ts_us") - public long timestamp_us; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("pTYPE") - public String pipelineType; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txnc") - public double tx_nocrosshair; - - @JsonProperty("tync") - public double ty_nocrosshair; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @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("botpose_orb") - public double[] botpose_orb; - - @JsonProperty("botpose_orb_wpiblue") - public double[] botpose_orb_wpiblue; - - @JsonProperty("botpose_orb_wpired") - public double[] botpose_orb_wpired; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - @JsonProperty("hw") - public HardwareReport hardware; - - @JsonProperty("imu") - public IMUResults imuResults; - - @JsonProperty("rewind") - public RewindStats rewindStats; - - @JsonProperty("PythonOut") - public double[] pythonOutput; - - 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 LimelightResults() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - botpose_orb = new double[6]; - botpose_orb_wpiblue = new double[6]; - botpose_orb_wpired = new double[6]; - camerapose_robotspace = 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]; - pythonOutput = new double[0]; - pipelineType = ""; - } - - - } - - /** - * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. - */ - public static class RawFiducial { - 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) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null || getClass() != obj.getClass()) return false; - RawFiducial other = (RawFiducial) obj; - return id == other.id && - Double.compare(txnc, other.txnc) == 0 && - Double.compare(tync, other.tync) == 0 && - Double.compare(ta, other.ta) == 0 && - Double.compare(distToCamera, other.distToCamera) == 0 && - Double.compare(distToRobot, other.distToRobot) == 0 && - Double.compare(ambiguity, other.ambiguity) == 0; - } - - } - - /** - * Represents a Limelight Raw Target/Contour result from Limelight's NetworkTables output. - */ - public static class RawTarget { - public double txnc = 0; - public double tync = 0; - public double ta = 0; - - public RawTarget(double txnc, double tync, double ta) { - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null || getClass() != obj.getClass()) return false; - RawTarget other = (RawTarget) obj; - return Double.compare(txnc, other.txnc) == 0 && - Double.compare(tync, other.tync) == 0 && - Double.compare(ta, other.ta) == 0; - } - } - - /** - * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. - */ - 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; - } - } - - /** - * Represents a 3D Pose Estimate. - */ - 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 boolean isMegaTag2; - - /** - * Instantiates 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[]{}; - this.isMegaTag2 = false; - } - - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - this.isMegaTag2 = isMegaTag2; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null || getClass() != obj.getClass()) return false; - PoseEstimate that = (PoseEstimate) obj; - // We don't compare the timestampSeconds as it isn't relevant for equality and makes - // unit testing harder - return Double.compare(that.latency, latency) == 0 - && tagCount == that.tagCount - && Double.compare(that.tagSpan, tagSpan) == 0 - && Double.compare(that.avgTagDist, avgTagDist) == 0 - && Double.compare(that.avgTagArea, avgTagArea) == 0 - && pose.equals(that.pose) - && Arrays.equals(rawFiducials, that.rawFiducials); - } - - } - - /** - * Encapsulates the state of an internal Limelight IMU. - */ - public static class IMUData { - public double robotYaw = 0.0; - public double Roll = 0.0; - public double Pitch = 0.0; - public double Yaw = 0.0; - public double gyroX = 0.0; - public double gyroY = 0.0; - public double gyroZ = 0.0; - public double accelX = 0.0; - public double accelY = 0.0; - public double accelZ = 0.0; - - public IMUData() {} - - public IMUData(double[] imuData) { - if (imuData != null && imuData.length >= 10) { - this.robotYaw = imuData[0]; - this.Roll = imuData[1]; - this.Pitch = imuData[2]; - this.Yaw = imuData[3]; - this.gyroX = imuData[4]; - this.gyroY = imuData[5]; - this.gyroZ = imuData[6]; - this.accelX = imuData[7]; - this.accelY = imuData[8]; - this.accelZ = imuData[9]; - } - } - } - - - private static ObjectMapper mapper; - - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if ("".equals(name) || name == null) { - return "limelight"; - } - return name; - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose3d object. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose3d object representing the pose, or empty Pose3d if invalid data - */ - 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]))); - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose2d object. - * Uses only x, y, and yaw components, ignoring z, roll, and pitch. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose2d object representing the pose, or empty Pose2d if invalid data - */ - public 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); - } - - /** - * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * - * @param pose The Pose3d object to convert - * @return A 6-element array containing [x, y, z, roll, pitch, yaw] - */ - 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 in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. - * - * @param pose The Pose2d object to convert - * @return A 6-element array containing [x, y, 0, 0, 0, yaw] - */ - 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; - } - return inData[position]; - } - - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { - 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 new PoseEstimate(); - } - - var pose = toPose2D(poseArray); - 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); - - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - RawFiducial[] rawFiducials; - - if (poseArray.length != expectedTotalVals) { - // Array size mismatch - return empty array instead of null-filled array - rawFiducials = new RawFiducial[0]; - } else { - rawFiducials = new RawFiducial[tagCount]; - 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, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); - } - - /** - * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawFiducial objects containing detection details - */ - public 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; - } - - /** - * Gets the latest raw neural detector results from NetworkTables - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawDetection objects containing detection details - */ - public static RawDetection[] getRawDetections(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); - var rawDetectionArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 12; - 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; - } - - /** - * Gets the raw target contours from NetworkTables. - * Returns ungrouped contours in normalized screen space (-1 to 1). - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawTarget objects containing up to 3 contours - */ - public static RawTarget[] getRawTargets(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawtargets"); - var rawTargetArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 3; - if (rawTargetArray.length % valsPerEntry != 0) { - return new RawTarget[0]; - } - - int numTargets = rawTargetArray.length / valsPerEntry; - RawTarget[] rawTargets = new RawTarget[numTargets]; - - for (int i = 0; i < numTargets; i++) { - int baseIndex = i * valsPerEntry; - double txnc = extractArrayEntry(rawTargetArray, baseIndex); - double tync = extractArrayEntry(rawTargetArray, baseIndex + 1); - double ta = extractArrayEntry(rawTargetArray, baseIndex + 2); - - rawTargets[i] = new RawTarget(txnc, tync, ta); - } - - return rawTargets; - } - - /** - * Gets the corner coordinates of detected targets from NetworkTables. - * Requires "send contours" to be enabled in the Limelight Output tab. - * - * @param limelightName Name/identifier of the Limelight - * @return Array of doubles containing corner coordinates [x0, y0, x1, y1, ...] - */ - public static double[] getCornerCoordinates(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tcornxy"); - } - - /** - * Prints detailed information about a PoseEstimate to standard output. - * Includes timestamp, latency, tag count, tag span, average tag distance, - * average tag area, and detailed information about each detected fiducial. - * - * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." - */ - 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); - 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.printf("Is MegaTag2: %b%n", pose.isMegaTag2); - 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 Boolean validPoseEstimate(PoseEstimate pose) { - return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; - } - - 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); - } - - 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 String[] getLimelightNTStringArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); - } - - - ///// - - /** - * Does the Limelight have a valid target? - * @param limelightName Name of the Limelight camera ("" for default) - * @return True if a valid target is present, false otherwise - */ - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } - - /** - * Gets the horizontal offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - /** - * Gets the vertical offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - /** - * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTXNC(String limelightName) { - return getLimelightNTDouble(limelightName, "txnc"); - } - - /** - * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTYNC(String limelightName) { - return getLimelightNTDouble(limelightName, "tync"); - } - - /** - * Gets the target area as a percentage of the image (0-100%). - * @param limelightName Name of the Limelight camera ("" for default) - * @return Target area percentage (0-100) - */ - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - /** - * T2D is an array that contains several targeting metrcis - * @param limelightName Name of the Limelight camera - * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, - * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] - */ - public static double[] getT2DArray(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "t2d"); - } - - /** - * Gets the number of targets currently detected. - * @param limelightName Name of the Limelight camera - * @return Number of detected targets - */ - public static int getTargetCount(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[1]; - } - return 0; - } - - /** - * Gets the classifier class index from the currently running neural classifier pipeline - * @param limelightName Name of the Limelight camera - * @return Class index from classifier pipeline - */ - public static int getClassifierClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[11]; - } - return 0; - } - - /** - * Gets the detector class index from the primary result of the currently running neural detector pipeline. - * @param limelightName Name of the Limelight camera - * @return Class index from detector pipeline - */ - public static int getDetectorClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[10]; - } - return 0; - } - - /** - * Gets the current neural classifier result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from classifier pipeline - */ - public static String getClassifierClass (String limelightName) { - return getLimelightNTString(limelightName, "tcclass"); - } - - /** - * Gets the primary neural detector result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from detector pipeline - */ - public static String getDetectorClass (String limelightName) { - return getLimelightNTString(limelightName, "tdclass"); - } - - /** - * Gets the pipeline's processing latency contribution. - * @param limelightName Name of the Limelight camera - * @return Pipeline latency in milliseconds - */ - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - /** - * Gets the capture latency. - * @param limelightName Name of the Limelight camera - * @return Capture latency in milliseconds - */ - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - /** - * Gets the active pipeline index. - * @param limelightName Name of the Limelight camera - * @return Current pipeline index (0-9) - */ - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - /** - * Gets the current pipeline type. - * @param limelightName Name of the Limelight camera - * @return Pipeline type string (e.g. "retro", "apriltag", etc) - */ - public static String getCurrentPipelineType(String limelightName) { - return getLimelightNTString(limelightName, "getpipetype"); - } - - /** - * Gets the full JSON results dump. - * @param limelightName Name of the Limelight camera - * @return JSON string containing all current results - */ - 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"); - } - - /** - * Gets the average color under the crosshair region as a 3-element array. - * @param limelightName Name of the Limelight camera - * @return Array containing [Blue, Green, Red] color values (BGR order) - */ - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - /** - * Gets the Limelight heartbeat value. Increments once per frame, allowing you to detect if the Limelight is connected and alive. - * @param limelightName Name of the Limelight camera - * @return Heartbeat value that increments each frame - */ - public static double getHeartbeat(String limelightName) { - return getLimelightNTDouble(limelightName, "hb"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - public static String[] getRawBarcodeData(String limelightName) { - return getLimelightNTStringArray(limelightName, "rawbarcodes"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - /** - * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Red Alliance field space - */ - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - /** - * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space - */ - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - /** - * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation relative to the target - */ - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the target - */ - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the camera's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the camera - */ - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the robot - */ - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the robot - */ - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_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 MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); - } - - /** - * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * Make sure you are calling setRobotOrientation() before calling this method. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); - } - - /** - * 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 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", false); - } - - /** - * 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", true); - } - - /** - * 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); - - } - - /** - * Gets the current IMU data from NetworkTables. - * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. - * Returns all zeros if data is invalid or unavailable. - * - * @param limelightName Name/identifier of the Limelight - * @return IMUData object containing all current IMU data - */ - public static IMUData getIMUData(String limelightName) { - double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); - if (imuData == null || imuData.length < 10) { - return new IMUData(); // Returns object with all zeros - } - return new IMUData(imuData); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * Sets LED mode to be controlled by the current pipeline. - * @param limelightName Name of the Limelight camera - */ - 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); - } - - /** - * Enables standard side-by-side stream mode. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - /** - * Enables Picture-in-Picture mode with secondary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - /** - * Enables Picture-in-Picture mode with primary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - - /** - * Sets the crop window for the camera. The crop window in the UI must be completely open. - * @param limelightName Name of the Limelight camera - * @param cropXMin Minimum X value (-1 to 1) - * @param cropXMax Maximum X value (-1 to 1) - * @param cropYMin Minimum Y value (-1 to 1) - * @param cropYMax Maximum Y value (-1 to 1) - */ - 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); - } - - /** - * Sets the keystone modification for the crop window. - * @param limelightName Name of the Limelight camera - * @param horizontal Horizontal keystone value (-0.95 to 0.95) - * @param vertical Vertical keystone value (-0.95 to 0.95) - */ - public static void setKeystone(String limelightName, double horizontal, double vertical) { - double[] entries = new double[2]; - entries[0] = horizontal; - entries[1] = vertical; - setLimelightNTDoubleArray(limelightName, "keystone_set", entries); - } - - /** - * 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); - } - - /** - * Sets robot orientation values used by MegaTag2 localization algorithm. - * - * @param limelightName Name/identifier of the Limelight - * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC - * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second - * @param pitch (Unnecessary) Robot pitch in degrees - * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second - * @param roll (Unnecessary) Robot roll in degrees - * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second - */ - 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; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - if(flush) - { - Flush(); - } - } - - /** - * Configures the IMU mode for MegaTag2 Localization - * - * @param limelightName Name/identifier of the Limelight - * @param mode IMU mode. - */ - public static void SetIMUMode(String limelightName, int mode) { - setLimelightNTDouble(limelightName, "imumode_set", mode); - } - - /** - * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4) - * - * @param limelightName Name/identifier of the Limelight - * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the assist source more rapidly. - */ - public static void SetIMUAssistAlpha(String limelightName, double alpha) { - setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha); - } - - - /** - * Configures the throttle value. Set to 100-200 while disabled to reduce thermal output/temperature. - * - * @param limelightName Name/identifier of the Limelight - * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping frames. - */ - public static void SetThrottle(String limelightName, int throttle) { - setLimelightNTDouble(limelightName, "throttle_set", throttle); - } - - /** - * Overrides the valid AprilTag IDs that will be used for localization. - * Tags not in this list will be ignored for robot pose estimation. - * - * @param limelightName Name/identifier of the Limelight - * @param validIDs Array of valid AprilTag IDs to track - */ - 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); - } - - /** - * Sets the downscaling factor for AprilTag detection. - * Increasing downscale can improve performance at the cost of potentially reduced detection range. - * - * @param limelightName Name/identifier of the Limelight - * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. - */ - 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); - } - - /** - * Sets the camera pose relative to the robot. - * @param limelightName Name of the Limelight camera - * @param forward Forward offset in meters - * @param side Side offset in meters - * @param up Up offset in meters - * @param roll Roll angle in degrees - * @param pitch Pitch angle in degrees - * @param yaw Yaw angle in degrees - */ - 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; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** - * Triggers a snapshot capture via NetworkTables by incrementing the snapshot counter. - * Rate-limited to once per 10 frames on the Limelight. - * @param limelightName Name of the Limelight camera - */ - public static void triggerSnapshot(String limelightName) { - double current = getLimelightNTDouble(limelightName, "snapshot"); - setLimelightNTDouble(limelightName, "snapshot", current + 1); - } - - /** - * Enables or pauses the rewind buffer recording. - * @param limelightName Name of the Limelight camera - * @param enabled True to enable recording, false to pause - */ - public static void setRewindEnabled(String limelightName, boolean enabled) { - setLimelightNTDouble(limelightName, "rewind_enable_set", enabled ? 1 : 0); - } - - /** - * Triggers a rewind capture with the specified duration. - * Maximum duration is 165 seconds. Rate-limited on the Limelight. - * @param limelightName Name of the Limelight camera - * @param durationSeconds Duration of rewind capture in seconds (max 165) - */ - public static void triggerRewindCapture(String limelightName, double durationSeconds) { - double[] currentArray = getLimelightNTDoubleArray(limelightName, "capture_rewind"); - double counter = (currentArray.length > 0) ? currentArray[0] : 0; - double[] entries = new double[2]; - entries[0] = counter + 1; - entries[1] = Math.min(durationSeconds, 165); - setLimelightNTDoubleArray(limelightName, "capture_rewind", entries); - } - - /** - * Gets the latest JSON results output and returns a LimelightResults object. - * @param limelightName Name of the Limelight camera - * @return LimelightResults object containing all current target data - */ - 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 { - String jsonString = getJSONDump(limelightName); - if (jsonString == null || jsonString.isEmpty() || jsonString.isBlank()) { - results.error = "lljson error: empty json"; - } else { - results = mapper.readValue(jsonString, LimelightResults.class); - if (results.imuResults != null) { - results.imuResults.parseDataArray(); - } - } - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } - - /** - * Sets up port forwarding for a Limelight 3A/3G connected via USB. - * This allows access to the Limelight web interface and video stream - * when connected to the robot over USB. - * - * For usbIndex 0: ports 5800-5809 forward to 172.29.0.1 - * For usbIndex 1: ports 5810-5819 forward to 172.29.1.1 - * etc. - * - * Call this method once during robot initialization. - * To access the interface of the camera with usbIndex0, you would go to roboRIO-(teamnum)-FRC.local:5801. Port 5811 for usb index 1 - * - * @param usbIndex The USB index of the Limelight (0, 1, 2, etc.) - */ - public static void setupPortForwardingUSB(int usbIndex) { - String ip = "172.29." + usbIndex + ".1"; - int basePort = 5800 + (usbIndex * 10); - - for (int i = 0; i < 10; i++) { - PortForwarder.add(basePort + i, ip, 5800 + i); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 834b5d5..559bd6a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,22 +4,11 @@ package frc.robot; -import org.photonvision.targeting.PhotonPipelineResult; - -import com.pathplanner.lib.auto.AutoBuilder; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.TargetingSubsystems; /** * The VM is configured to automatically run this class, and to call the @@ -35,8 +24,6 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; private Timer disabledTimer; - private static NetworkTable table; - private static GenericEntry distanceFromLimelight; public Robot() { @@ -95,16 +82,6 @@ public class Robot extends TimedRobot { //Constants.IntakeConstants.updateIntakeWheelsRPM(); Constants.ShooterConstants.updateShooterRPM(); //Constants.ShooterConstants.updateIndexerAndRampMotorRPM(); - - TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM, - Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); - TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.BLACK_PHOTON_CAM, - Constants.TargetingConstants.BLACK_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); - TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.RED_PHOTON_CAM, - Constants.TargetingConstants.RED_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); - TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.PURPLE_PHOTON_CAM, - Constants.TargetingConstants.PURPLE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); - } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a139d09..dfe813e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,8 +7,6 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,7 +16,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -30,13 +27,7 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; -import java.lang.annotation.Target; -import java.util.concurrent.atomic.AtomicBoolean; -import java.util.function.DoubleSupplier; -import javax.lang.model.util.ElementScanner14; - -import frc.robot.subsystems.TargetingSubsystems; import swervelib.SwerveInputStream; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.subsystems.ClimberSubsystem; diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 2192787..3d66ac9 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -8,16 +8,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -public class ClimberSubsystem extends SubsystemBase{ +public class ClimberSubsystem extends SubsystemBase { private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID); private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT); public void liftRobot() { - climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED); + climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE); } public void lowerRobot() { - climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1); + climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE * -1); } public void stopClimber() { @@ -37,11 +37,11 @@ public class ClimberSubsystem extends SubsystemBase{ } public static void toggleRatchet(boolean toggle) { - if (toggle == true) { - climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE); - } else - climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE); - } + if (toggle == true) { + climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE_DEGREES); + } else + climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE_DEGREES); + } public Command toggleRatchetCommand(boolean toggle) { return runOnce(() -> toggleRatchet(toggle)); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index e57676d..2077bcd 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,66 +1,65 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.BangBangController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.ControlType; public class IntakeSubsystem extends SubsystemBase { - private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID, - MotorType.kBrushless); + private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID, MotorType.kBrushless); - private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, - MotorType.kBrushless); - private static SparkClosedLoopController intakeRotatorPIDController; - public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig(); + private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, MotorType.kBrushless); - private static SparkClosedLoopController intakeWheelsMotorPIDController; public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig(); + private static BangBangController intakeWheelsMotorBBController = new BangBangController(); + private static SimpleMotorFeedforward intakeWheelsMotorFeedforward; + + public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig(); + private static PIDController intakeRotatorMotorPIDController; + private static ArmFeedforward intakeRotatorMotorFeedforward; public IntakeSubsystem() { - intakeRotatorConfig.closedLoop - .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P) - .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I) - .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D) + intakeWheelsMotorConfig + .smartCurrentLimit(Constants.IntakeConstants.INTAKE_WHEELS_CURRENT_LIMIT) + .encoder + .positionConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_POSITION_CONVERSION_FACTOR) + .velocityConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR); + intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); - .p(.06, ClosedLoopSlot.kSlot1) - .i(0, ClosedLoopSlot.kSlot1) - .d(0, ClosedLoopSlot.kSlot1); - intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, - com.revrobotics.PersistMode.kNoPersistParameters); - intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); + intakeWheelsMotorFeedforward = new SimpleMotorFeedforward(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_S, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V); + + intakeRotatorConfig + .smartCurrentLimit(Constants.IntakeConstants.INTAKE_ROTATOR_CURRENT_LIMIT) + .encoder + .positionConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR) + .velocityConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR); + intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); + intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_ROTATOR_INITIAL_ENCODER_VALUE); - intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P, - Constants.IntakeConstants.INTAKE_MOTOR_I, - Constants.IntakeConstants.INTAKE_MOTOR_D); - intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, - com.revrobotics.PersistMode.kNoPersistParameters); - intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController(); + intakeRotatorMotorPIDController = new PIDController(Constants.IntakeConstants.INTAKE_ROTATOR_UP_P, Constants.IntakeConstants.INTAKE_ROTATOR_UP_I, Constants.IntakeConstants.INTAKE_ROTATOR_UP_D); + intakeRotatorMotorFeedforward = new ArmFeedforward(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_S, Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_G, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V); } public void startIntakeMotor() { - intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity); + intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM); } public void reverseIntakeMotor() { - intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity); + intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1); } public void stopIntakeMotor() { - intakeWheelsMotor.set(0); + intakeWheelsMotorBBController.setSetpoint(0.0); } public Command startIntakeMotorCommand() { @@ -76,7 +75,7 @@ public class IntakeSubsystem extends SubsystemBase { } public void deployIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0); + intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_POSITION_RADS); } public Command deployintakeCommand() { @@ -84,7 +83,7 @@ public class IntakeSubsystem extends SubsystemBase { } public void retractIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1); + intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_POSITION_RADS); } public Command retractIntakeCommand() { @@ -92,8 +91,7 @@ public class IntakeSubsystem extends SubsystemBase { } public void assistFuelIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE, - ControlType.kPosition); + intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_POSITION_RADS); } public Command assistFuelIntakeCommand() { @@ -103,6 +101,13 @@ public class IntakeSubsystem extends SubsystemBase { @Override public void periodic() { + intakeWheelsMotor.setVoltage( + 12.0 * intakeWheelsMotorBBController.calculate(intakeWheelsMotor.getEncoder().getVelocity()) + + 0.9 * intakeWheelsMotorFeedforward.calculate(intakeWheelsMotorBBController.getSetpoint())); + intakeRotatorMotor.setVoltage( + 12.0 * intakeRotatorMotorPIDController.calculate(intakeRotatorMotor.getEncoder().getPosition()) + + intakeRotatorMotorFeedforward.calculate(intakeRotatorMotorPIDController.getSetpoint(), 0.0)); + SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2e68527..fe835aa 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,30 +1,17 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.controls.Follower; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkBaseConfig; import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.LimelightHelpers; -import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.math.controller.BangBangController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class ShooterSubsystem extends SubsystemBase { private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID, @@ -39,46 +26,41 @@ public class ShooterSubsystem extends SubsystemBase { private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID, MotorType.kBrushless); - private static SparkClosedLoopController centerShooterMotorPIDController; public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig(); + private static BangBangController centerShooterMotorBBController = new BangBangController(); + private static SimpleMotorFeedforward centerShooterMotorFeedforward; - private static SparkClosedLoopController leftShooterMotorPIDController; public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig(); + private static BangBangController leftShooterMotorBBController = new BangBangController(); + private static SimpleMotorFeedforward leftShooterMotorFeedforward; - private static SparkClosedLoopController rightShooterMotorPIDController; public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig(); + private static BangBangController rightShooterMotorBBController = new BangBangController(); + private static SimpleMotorFeedforward rightShooterMotorFeedforward; - private static SparkClosedLoopController indexerAndRampMotorPIDController; public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig(); + private static PIDController indexerAndRampMotorPIDController; public ShooterSubsystem() { - centerShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, - Constants.ShooterConstants.SHOOTER_MOTOR_I, - Constants.ShooterConstants.SHOOTER_MOTOR_D); + centerShooterMotorConfig.smartCurrentLimit(60); centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, - com.revrobotics.PersistMode.kNoPersistParameters); - centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController(); + com.revrobotics.PersistMode.kNoPersistParameters); + centerShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.CENTER_MOTOR_S, Constants.ShooterConstants.CENTER_MOTOR_V); - leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, - Constants.ShooterConstants.SHOOTER_MOTOR_I, - Constants.ShooterConstants.SHOOTER_MOTOR_D); + leftShooterMotorConfig.smartCurrentLimit(60); leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, - com.revrobotics.PersistMode.kNoPersistParameters); - leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController(); + com.revrobotics.PersistMode.kNoPersistParameters); + leftShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.LEFT_MOTOR_S, Constants.ShooterConstants.LEFT_MOTOR_V); - rightShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, - Constants.ShooterConstants.SHOOTER_MOTOR_I, - Constants.ShooterConstants.SHOOTER_MOTOR_D); + rightShooterMotorConfig.smartCurrentLimit(60); rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, - com.revrobotics.PersistMode.kNoPersistParameters); - rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController(); + com.revrobotics.PersistMode.kNoPersistParameters); + rightShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.RIGHT_MOTOR_S, Constants.ShooterConstants.RIGHT_MOTOR_V); - indexerAndRampMotorConfig.closedLoop.pid(0.0001, - 0, - 0); + indexerAndRampMotorConfig.smartCurrentLimit(60); indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, - com.revrobotics.PersistMode.kNoPersistParameters); - indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController(); + com.revrobotics.PersistMode.kNoPersistParameters); + indexerAndRampMotorPIDController = new PIDController(Constants.ShooterConstants.INDEXER_MOTOR_P, Constants.ShooterConstants.INDEXER_MOTOR_I, Constants.ShooterConstants.INDEXER_MOTOR_D); } @@ -94,49 +76,49 @@ public class ShooterSubsystem extends SubsystemBase { private static double currentPotentiometerPosition; // might need second value for the right potentiometer public void setShooterMotorsRPM() { - centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); - leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); - rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM); + leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM); + rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM); } //test individual motor code public void setLeftShooterMotorRPM() { - leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM); } public Command testLeftShooterCommand() { return runOnce(() -> setLeftShooterMotorRPM()); } public void setRightShooterMotorRPM() { - rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM); } public Command testRightShooterCommand() { return runOnce(() -> setRightShooterMotorRPM()); } public void setCenterShooterMotorRPM() { - centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM); } public Command testCenterShooterCommand() { return runOnce(() -> setCenterShooterMotorRPM()); } public void stopLeftShooterMotorRPM() { - leftShooterMotor.set(0); + leftShooterMotorBBController.setSetpoint(0.0); } public Command stopLeftShooterCommand() { return runOnce(() -> stopLeftShooterMotorRPM()); } public void stopCenterShooterMotorRPM() { - centerShooterMotor.set(0); + centerShooterMotorBBController.setSetpoint(0.0); } public Command stopCenterShooterCommand() { return runOnce(() -> stopCenterShooterMotorRPM()); } public void stopRightShooterMotorRPM() { - rightShooterMotor.set(0); + rightShooterMotorBBController.setSetpoint(0.0); } public Command stopRightShooterCommand() { return runOnce(() -> stopRightShooterMotorRPM()); @@ -148,11 +130,11 @@ public class ShooterSubsystem extends SubsystemBase { } public void setIndexerAndRampMotorRPM() { - indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity); + indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM); } public void reverseIndexerAndRampMotorRPM() { - indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, ControlType.kVelocity); + indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1); } public Command reverseIndexerAndRampMotorRPMCommand() { @@ -164,7 +146,7 @@ public class ShooterSubsystem extends SubsystemBase { } public Command stopIndexerAndRampMotorCommand() { - return runOnce(()-> indexerAndRampMotor.set(0)); + return runOnce(()-> indexerAndRampMotorPIDController.setSetpoint(0.0)); } /*public Command shootFuelCommand() { @@ -181,12 +163,10 @@ public class ShooterSubsystem extends SubsystemBase { public void stopShooters() { - - centerShooterMotor.set(0); - leftShooterMotor.set(0); - rightShooterMotor.set(0); - indexerAndRampMotor.set(0); - + centerShooterMotorBBController.setSetpoint(0.0); + leftShooterMotorBBController.setSetpoint(0.0); + rightShooterMotorBBController.setSetpoint(0.0); + indexerAndRampMotorPIDController.setSetpoint(0.0); } public Command stopShooterCommand() { @@ -217,6 +197,17 @@ public class ShooterSubsystem extends SubsystemBase { @Override public void periodic() { + centerShooterMotor.setVoltage( + 12.0 * centerShooterMotorBBController.calculate(centerShooterMotor.getEncoder().getVelocity()) + + 0.9 * centerShooterMotorFeedforward.calculate(centerShooterMotorBBController.getSetpoint())); + leftShooterMotor.setVoltage( + 12.0 * leftShooterMotorBBController.calculate(leftShooterMotor.getEncoder().getVelocity()) + + 0.9 * leftShooterMotorFeedforward.calculate(leftShooterMotorBBController.getSetpoint())); + rightShooterMotor.setVoltage( + 12.0 * rightShooterMotorBBController.calculate(rightShooterMotor.getEncoder().getVelocity()) + + 0.9 * rightShooterMotorFeedforward.calculate(rightShooterMotorBBController.getSetpoint())); + indexerAndRampMotor.setVoltage( + 12.0 * indexerAndRampMotorPIDController.calculate(indexerAndRampMotor.getEncoder().getVelocity())); SmartDashboard.putNumber("Shooter Velocity", leftShooterMotor.getEncoder().getVelocity()); /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get()); diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java deleted file mode 100644 index 330a444..0000000 --- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java +++ /dev/null @@ -1,207 +0,0 @@ -package frc.robot.subsystems; - -import java.util.List; -import java.util.Optional; -import java.util.function.DoubleSupplier; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonUtils; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonTrackedTarget; - -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.PathPoint; -import com.pathplanner.lib.path.RotationTarget; -import com.pathplanner.lib.path.Waypoint; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -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.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import frc.robot.Constants; -import frc.robot.LimelightHelpers; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.RobotContainer; -import frc.robot.Constants; - -public class TargetingSubsystems extends SubsystemBase { - - PIDController photonAimPIDController = new PIDController(3, 0, 0.001); - - public TargetingSubsystems() { - photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI); - } - - Pose2d currentRobotPose; - - public List pathWaypoints; - - public Command pathPlanToPoseCommand(Pose2d desiredPose, SwerveSubsystem swerveDrive) { - GoalEndState goalEndState = new GoalEndState(0, desiredPose.getRotation()); - PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0); - currentRobotPose = swerveDrive.getPose(); - pathWaypoints = PathPlannerPath.waypointsFromPoses( - currentRobotPose, desiredPose); - - PathPlannerPath goToDesiredPose = new PathPlannerPath(pathWaypoints, pathConstraints, null, - goalEndState); - goToDesiredPose.preventFlipping = true; - - return swerveDrive.getAutonomousCommand("goToDesiredPose"); - } - - public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) { - return new RunCommand(() -> { - currentRobotPose = swerveDrive.getPose(); - - Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose); - - double xError = errorFromDesiredPose.getX(); - double yError = errorFromDesiredPose.getY(); - double angleError = errorFromDesiredPose.getRotation().getRadians(); - - PIDController xController = new PIDController(1.5, 0, 0); - PIDController yController = new PIDController(1.5, 0, 0); - - double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX()); - double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY()); - double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), - desiredPose.getRotation().getRadians()); - - swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, false); - }, swerveDrive); - } - - public Command aimAtHub(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { - return new RunCommand(() -> { - currentRobotPose = swerveDrive.getPose(); - - // Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose); - - Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose, - Constants.TargetingConstants.HUB_POSE); - - double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), - angleDifference.getRadians()); - - angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0); - - swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1, -driverXbox.getLeftY() * -1), angleSpeed, - false); - }, swerveDrive); - } - - Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { - return new RunCommand(() -> { - double rot = 0.0; - var result = Constants.TargetingConstants.RED_PHOTON_CAM.getLatestResult(); - if (result.hasTargets()) { - double yawError = result.getBestTarget().getYaw(); - rot = photonAimPIDController.calculate(yawError, 0); - } - - rot = MathUtil.clamp(rot, -3.0, 3.0); - - swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1, - driverXbox.getLeftY() * -1), rot, false); - }, swerveDrive); - } - - public PhotonPoseEstimator getPhotonPoseEstimator(PhotonPoseEstimator poseEstimator) { - return poseEstimator; - } - - /* - * // static public NetworkTable table = - * // - * NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME - * ); - * // static public NetworkTableEntry ty = table.getEntry("ty"); - * // static double targetOffsetAngle_Vertical = ty.getDouble(0.0); - * - * // how many degrees back is your limelight rotated from perfectly vertical? - * static double limelightMountAngleDegrees = 25.0; - * - * // distance from the center of the Limelight lens to the floor - * static double limelightLensHeightInches = 27.5; - * - * // distance from the target to the floor - * static double goalHeightInches = 44; - * - * static double angleToGoalDegrees = limelightMountAngleDegrees + - * Constants.LimeLight.LIMELIGHT_TY; - * static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); - * - * // calculate distance - * static double distanceFromLimelightToGoalInches = (goalHeightInches - - * limelightLensHeightInches) - * / Math.tan(angleToGoalRadians); - * - * public static double getDistanceFromAprilTag() { - * angleToGoalDegrees = limelightMountAngleDegrees + - * Constants.LimeLight.LIMELIGHT_TY; - * angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); - * distanceFromLimelightToGoalInches = (goalHeightInches - - * limelightLensHeightInches) - * / Math.tan(angleToGoalRadians); - * return distanceFromLimelightToGoalInches; - * } - */ - - public static Optional visionEst = Optional.empty(); - - public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) { - /* for (var result : camera.getAllUnreadResults()) { - visionEst = poseEstimator.estimateCoprocMultiTagPose(result); - - if (visionEst.isPresent()) { - EstimatedRobotPose estimatedPose = visionEst.get(); - swerveDrive.getSwerveDrive() - .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); - } - - - }*/ -} - - @Override - public void periodic() { - /* - * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value", - * photonVision.getLatestResult().getBestTarget().getYaw()); - * Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value", - * photonVision.getLatestResult().getBestTarget().getPitch()); - * Shuffleboard.getTab("Vision").add("Limelight TX Value", - * LimelightHelpers.getTX("limelight")); - * Shuffleboard.getTab("Vision").add("Limelight April Tag ID", - * LimelightHelpers.getFiducialID("limelight")); - * Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null); - * Shuffleboard.getTab("Vision").addCamera("Photon", - * "Arducam_OV9281_USB_Camera", - * "http://photonvision.local:5800"); - */ - } -} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index bb15541..244e823 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -26,6 +26,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -41,7 +42,6 @@ import java.util.concurrent.atomic.AtomicReference; import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.json.simple.parser.ParseException; -import org.photonvision.EstimatedRobotPose; import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; @@ -53,668 +53,679 @@ import swervelib.parser.SwerveParser; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; -public class SwerveSubsystem extends SubsystemBase { - /** - * Swerve drive object. - */ - private final SwerveDrive swerveDrive; +public class SwerveSubsystem extends SubsystemBase +{ + /** + * Swerve drive object. + */ + private final SwerveDrive swerveDrive; + + /** + * Enable vision odometry updates while driving. + */ + private final boolean visionDriveTest = false; + + /** + * PhotonVision class to keep an accurate odometry. + */ + private Vision vision; - /** - * Enable vision odometry updates while driving. - */ - private final boolean visionDriveTest = true; - - /** - * PhotonVision class to keep an accurate odometry. - */ - private Vision vision; - - /** - * Initialize {@link SwerveDrive} with the directory provided. - * - * @param directory Directory of swerve drive config files. - */ - public SwerveSubsystem(File directory) { - boolean blueAlliance = false; - Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1), - Meter.of(4)), - Rotation2d.fromDegrees(0)) - : new Pose2d(new Translation2d(Meter.of(16), - Meter.of(4)), - Rotation2d.fromDegrees(180)); - // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary - // objects being created. - SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; - try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose); - // Alternative method if you don't want to supply the conversion factor via JSON - // files. - // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, - // angleConversionFactor, driveConversionFactor); - } catch (Exception e) { - throw new RuntimeException(e); - } - swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot - // via - // angle. - swerveDrive.setCosineCompensator(false);// !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation - // for - // simulations since it causes discrepancies not seen in real life. - swerveDrive.setAngularVelocityCompensation(true, - true, - 0.1); // Correct for skew that gets worse as angular velocity increases. Start with a - // coefficient of 0.1. - swerveDrive.setModuleEncoderAutoSynchronize(false, - 1); // Enable if you want to resynchronize your absolute encoders and motor encoders - // periodically when they are not moving. - // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used - // over the internal encoder and push the offsets onto it. Throws warning if not - // possible - if (visionDriveTest) { - setupPhotonVision(); - // Stop the odometry thread if we are using vision that way we can synchronize - // updates better. - swerveDrive.stopOdometryThread(); - } - setupPathPlanner(); + /** + * Initialize {@link SwerveDrive} with the directory provided. + * + * @param directory Directory of swerve drive config files. + */ + public SwerveSubsystem(File directory) + { + boolean blueAlliance = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Blue; + Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1), + Meter.of(4)), + Rotation2d.fromDegrees(0)) + : new Pose2d(new Translation2d(Meter.of(16), + Meter.of(4)), + Rotation2d.fromDegrees(180)); + // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being created. + SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; + try + { + swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED_MPS, startingPose); + // Alternative method if you don't want to supply the conversion factor via JSON files. + // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor); + } catch (Exception e) + { + throw new RuntimeException(e); } - - /** - * Construct the swerve drive. - * - * @param driveCfg SwerveDriveConfiguration for the swerve. - * @param controllerCfg Swerve Controller. - */ - public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, - controllerCfg, - Constants.MAX_SPEED, - new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), - Rotation2d.fromDegrees(0))); + swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. + swerveDrive.setAngularVelocityCompensation(true, + true, + 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. + swerveDrive.setModuleEncoderAutoSynchronize(false, + 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. + // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible + if (visionDriveTest) + { + setupPhotonVision(); + // Stop the odometry thread if we are using vision that way we can synchronize updates better. + swerveDrive.stopOdometryThread(); } + setupPathPlanner(); + } - /** - * Setup the photon vision class. - */ - public void setupPhotonVision() { - vision = new Vision(swerveDrive::getPose, swerveDrive.field); + /** + * Construct the swerve drive. + * + * @param driveCfg SwerveDriveConfiguration for the swerve. + * @param controllerCfg Swerve Controller. + */ + public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) + { + swerveDrive = new SwerveDrive(driveCfg, + controllerCfg, + Constants.MAX_SPEED_MPS, + new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), + Rotation2d.fromDegrees(0))); + } + + /** + * Setup the photon vision class. + */ + public void setupPhotonVision() + { + vision = new Vision(swerveDrive::getPose, swerveDrive.field); + } + + @Override + public void periodic() + { + // When vision is enabled we must manually update odometry in SwerveDrive + if (visionDriveTest) + { + swerveDrive.updateOdometry(); + vision.updatePoseEstimation(swerveDrive); } + } - @Override - public void periodic() { + @Override + public void simulationPeriodic() + { + } - // When vision is enabled we must manually update odometry in SwerveDrive - if (visionDriveTest) { - swerveDrive.updateOdometry(); - //vision.updatePoseEstimation(swerveDrive); - } - } + /** + * Setup AutoBuilder for PathPlanner. + */ + public void setupPathPlanner() + { + // Load the RobotConfig from the GUI settings. You should probably + // store this in your Constants file + RobotConfig config; + try + { + config = RobotConfig.fromGUISettings(); - @Override - public void simulationPeriodic() { - } - - /** - * Setup AutoBuilder for PathPlanner. - */ - public void setupPathPlanner() { - // Load the RobotConfig from the GUI settings. You should probably - // store this in your Constants file - RobotConfig config; - try { - config = RobotConfig.fromGUISettings(); - - final boolean enableFeedforward = true; - // Configure AutoBuilder last - AutoBuilder.configure( - this::getPose, - // Robot pose supplier - this::resetOdometry, - // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotVelocity, - // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speedsRobotRelative, moduleFeedForwards) -> { - if (enableFeedforward) { - swerveDrive.drive( - speedsRobotRelative, - swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative), - moduleFeedForwards.linearForces()); - } else { - swerveDrive.setChassisSpeeds(speedsRobotRelative); - } - }, - // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also - // optionally outputs individual module feedforwards - new PPHolonomicDriveController( - // PPHolonomicController is the built in path following controller for holonomic - // drive trains - new PIDConstants(5.0, 0.0, 0.0), - // Translation PID constants - new PIDConstants(3.8, 0.0, 0.0) - // Rotation PID constants - ), - config, - // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red - // alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this - // Reference to this subsystem to set requirements - ); - - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } - - // Preload PathPlanner Path finding - // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE - PathfindingCommand.warmupCommand().schedule(); - } - - /** - * Aim the robot at the target returned by PhotonVision. - * - * @return A {@link Command} which will run the alignment. - */ - public Command aimAtTarget(Cameras camera) { - - return run(() -> { - Optional resultO = camera.getBestResult(); - if (resultO.isPresent()) { - var result = resultO.get(); - if (result.hasTargets()) { - drive(getTargetSpeeds(0, - 0, - Rotation2d.fromDegrees(result.getBestTarget() - .getYaw()))); // Not sure if this will work, more math may be required. - } + final boolean enableFeedforward = true; + // Configure AutoBuilder last + AutoBuilder.configure( + this::getPose, + // Robot pose supplier + this::resetOdometry, + // Method to reset odometry (will be called if your auto has a starting pose) + this::getRobotVelocity, + // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speedsRobotRelative, moduleFeedForwards) -> { + if (enableFeedforward) + { + swerveDrive.drive( + speedsRobotRelative, + swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative), + moduleFeedForwards.linearForces() + ); + } else + { + swerveDrive.setChassisSpeeds(speedsRobotRelative); } - }); + }, + // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( + // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), + // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) + // Rotation PID constants + ), + config, + // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) + { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this + // Reference to this subsystem to set requirements + ); + + } catch (Exception e) + { + // Handle exception as needed + e.printStackTrace(); } - /** - * Get the path follower with events. - * - * @param pathName PathPlanner path name. - * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. - */ - public Command getAutonomousCommand(String pathName) { - // Create a path following command using AutoBuilder. This will also trigger - // event markers. - return new PathPlannerAuto(pathName); - } + //Preload PathPlanner Path finding + // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE + PathfindingCommand.warmupCommand().schedule(); + } - /** - * Use PathPlanner Path finding to go to a point on the field. - * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command - */ - public Command driveToPose(Pose2d pose) { - // Create the constraints to use while pathfinding - PathConstraints constraints = new PathConstraints( - swerveDrive.getMaximumChassisVelocity(), 4.0, - swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720)); + /** + * Aim the robot at the target returned by PhotonVision. + * + * @return A {@link Command} which will run the alignment. + */ + public Command aimAtTarget(Cameras camera) + { - // Since AutoBuilder is configured, we can use it to build pathfinding commands - return AutoBuilder.pathfindToPose( - pose, - constraints, - edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec - ); - } - - /** - * Drive with {@link SwerveSetpointGenerator} from 254, implemented by - * PathPlanner. - * - * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to - * achieve. - * @return {@link Command} to run. - * @throws IOException If the PathPlanner GUI settings is invalid - * @throws ParseException If PathPlanner GUI settings is nonexistent. - */ - private Command driveWithSetpointGenerator(Supplier robotRelativeChassisSpeed) - throws IOException, ParseException { - SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(), - swerveDrive.getMaximumChassisAngularVelocity()); - AtomicReference prevSetpoint = new AtomicReference<>( - new SwerveSetpoint(swerveDrive.getRobotVelocity(), - swerveDrive.getStates(), - DriveFeedforwards.zeros(swerveDrive.getModules().length))); - AtomicReference previousTime = new AtomicReference<>(); - - return startRun(() -> previousTime.set(Timer.getFPGATimestamp()), - () -> { - double newTime = Timer.getFPGATimestamp(); - SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint(prevSetpoint.get(), - robotRelativeChassisSpeed.get(), - newTime - previousTime.get()); - swerveDrive.drive(newSetpoint.robotRelativeSpeeds(), - newSetpoint.moduleStates(), - newSetpoint.feedforwards().linearForces()); - prevSetpoint.set(newSetpoint); - previousTime.set(newTime); - - }); - } - - /** - * Drive with 254's Setpoint generator; port written by PathPlanner. - * - * @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds} - * @return Command to drive the robot using the setpoint generator. - */ - public Command driveWithSetpointGeneratorFieldRelative(Supplier fieldRelativeSpeeds) { - try { - return driveWithSetpointGenerator(() -> { - return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading()); - - }); - } catch (Exception e) { - DriverStation.reportError(e.toString(), true); + return run(() -> { + Optional resultO = camera.getBestResult(); + if (resultO.isPresent()) + { + var result = resultO.get(); + if (result.hasTargets()) + { + drive(getTargetSpeeds(0, + 0, + Rotation2d.fromDegrees(result.getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. } - return Commands.none(); + } + }); + } + /** + * Get the path follower with events. + * + * @param pathName PathPlanner path name. + * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. + */ + public Command getAutonomousCommand(String pathName) + { + // Create a path following command using AutoBuilder. This will also trigger event markers. + return new PathPlannerAuto(pathName); + } + + /** + * Use PathPlanner Path finding to go to a point on the field. + * + * @param pose Target {@link Pose2d} to go to. + * @return PathFinding command + */ + public Command driveToPose(Pose2d pose) + { +// Create the constraints to use while pathfinding + PathConstraints constraints = new PathConstraints( + swerveDrive.getMaximumChassisVelocity(), 4.0, + swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720)); + +// Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + pose, + constraints, + edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec + ); + } + + /** + * Drive with {@link SwerveSetpointGenerator} from 254, implemented by PathPlanner. + * + * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to achieve. + * @return {@link Command} to run. + * @throws IOException If the PathPlanner GUI settings is invalid + * @throws ParseException If PathPlanner GUI settings is nonexistent. + */ + private Command driveWithSetpointGenerator(Supplier robotRelativeChassisSpeed) + throws IOException, ParseException + { + SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(), + swerveDrive.getMaximumChassisAngularVelocity()); + AtomicReference prevSetpoint + = new AtomicReference<>(new SwerveSetpoint(swerveDrive.getRobotVelocity(), + swerveDrive.getStates(), + DriveFeedforwards.zeros(swerveDrive.getModules().length))); + AtomicReference previousTime = new AtomicReference<>(); + + return startRun(() -> previousTime.set(Timer.getFPGATimestamp()), + () -> { + double newTime = Timer.getFPGATimestamp(); + SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint(prevSetpoint.get(), + robotRelativeChassisSpeed.get(), + newTime - previousTime.get()); + swerveDrive.drive(newSetpoint.robotRelativeSpeeds(), + newSetpoint.moduleStates(), + newSetpoint.feedforwards().linearForces()); + prevSetpoint.set(newSetpoint); + previousTime.set(newTime); + + }); + } + + /** + * Drive with 254's Setpoint generator; port written by PathPlanner. + * + * @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds} + * @return Command to drive the robot using the setpoint generator. + */ + public Command driveWithSetpointGeneratorFieldRelative(Supplier fieldRelativeSpeeds) + { + try + { + return driveWithSetpointGenerator(() -> { + return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading()); + + }); + } catch (Exception e) + { + DriverStation.reportError(e.toString(), true); } + return Commands.none(); - /** - * Command to characterize the robot drive motors using SysId - * - * @return SysId Drive Command - */ - public Command sysIdDriveMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine( - new Config(), - this, swerveDrive, 12, true), - 3.0, 5.0, 3.0); + } + + + /** + * Command to characterize the robot drive motors using SysId + * + * @return SysId Drive Command + */ + public Command sysIdDriveMotorCommand() + { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setDriveSysIdRoutine( + new Config(), + this, swerveDrive, 12, true), + 3.0, 5.0, 3.0); + } + + /** + * Command to characterize the robot angle motors using SysId + * + * @return SysId Angle Command + */ + public Command sysIdAngleMotorCommand() + { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setAngleSysIdRoutine( + new Config(), + this, swerveDrive), + 3.0, 5.0, 3.0); + } + + /** + * Returns a Command that centers the modules of the SwerveDrive subsystem. + * + * @return a Command that centers the modules of the SwerveDrive subsystem + */ + public Command centerModulesCommand() + { + return run(() -> Arrays.asList(swerveDrive.getModules()) + .forEach(it -> it.setAngle(0.0))); + } + + /** + * Returns a Command that tells the robot to drive forward until the command ends. + * + * @return a Command that tells the robot to drive forward until the command ends + */ + public Command driveForward() + { + return run(() -> { + swerveDrive.drive(new Translation2d(1, 0), 0, false, false); + }).finallyDo(() -> swerveDrive.drive(new Translation2d(0, 0), 0, false, false)); + } + + + /** + * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. + * + * @param kS the static gain of the feedforward + * @param kV the velocity gain of the feedforward + * @param kA the acceleration gain of the feedforward + */ + public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) + { + swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); + } + + /** + * Command to drive the robot using translative values and heading as angular velocity. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) + { + return run(() -> { + // Make the robot move + swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d( + translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), + translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), 0.8), + Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(), + true, + false); + }); + } + + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param headingX Heading X to calculate angle of the joystick. + * @param headingY Heading Y to calculate angle of the joystick. + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, + DoubleSupplier headingY) + { + // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. + return run(() -> { + + Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(), + translationY.getAsDouble()), 0.8); + + // Make the robot move + driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), + headingX.getAsDouble(), + headingY.getAsDouble(), + swerveDrive.getOdometryHeading().getRadians(), + swerveDrive.getMaximumChassisVelocity())); + }); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and + * calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for + * the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per + * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is + * torwards port (left). In field-relative mode, positive x is away from the alliance wall + * (field North) and positive y is torwards the left wall when looking through the driver station + * glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot + * relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + */ + public void drive(Translation2d translation, double rotation, boolean fieldRelative) + { + swerveDrive.drive(translation, + rotation, + fieldRelative, + false); // Open loop is disabled since it shouldn't be used most of the time. + } + + /** + * Drive the robot given a chassis field oriented velocity. + * + * @param velocity Velocity according to the field. + */ + public void driveFieldOriented(ChassisSpeeds velocity) + { + swerveDrive.driveFieldOriented(velocity); + } + + /** + * Drive the robot given a chassis field oriented velocity. + * + * @param velocity Velocity according to the field. + */ + public Command driveFieldOriented(Supplier velocity) + { + return run(() -> { + swerveDrive.driveFieldOriented(velocity.get()); + }); + } + + /** + * Drive according to the chassis robot oriented velocity. + * + * @param velocity Robot oriented {@link ChassisSpeeds} + */ + public void drive(ChassisSpeeds velocity) + { + swerveDrive.drive(velocity); + } + + /** + * Get the swerve drive kinematics object. + * + * @return {@link SwerveDriveKinematics} of the swerve drive. + */ + public SwerveDriveKinematics getKinematics() + { + return swerveDrive.kinematics; + } + + /** + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this + * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to + * keep working. + * + * @param initialHolonomicPose The pose to set the odometry to + */ + public void resetOdometry(Pose2d initialHolonomicPose) + { + swerveDrive.resetOdometry(initialHolonomicPose); + } + + /** + * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * + * @return The robot's pose + */ + public Pose2d getPose() + { + return swerveDrive.getPose(); + } + + /** + * Set chassis speeds with closed-loop velocity control. + * + * @param chassisSpeeds Chassis Speeds to set. + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) + { + swerveDrive.setChassisSpeeds(chassisSpeeds); + } + + /** + * Post the trajectory to the field. + * + * @param trajectory The trajectory to post. + */ + public void postTrajectory(Trajectory trajectory) + { + swerveDrive.postTrajectory(trajectory); + } + + /** + * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. + */ + public void zeroGyro() + { + swerveDrive.zeroGyro(); + } + + /** + * Checks if the alliance is red, defaults to false if alliance isn't available. + * + * @return true if the red alliance, false if blue. Defaults to false if none is available. + */ + private boolean isRedAlliance() + { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + } + + /** + * This will zero (calibrate) the robot to assume the current position is facing forward + *

+ * If red alliance rotate the robot 180 after the drviebase zero command + */ + public void zeroGyroWithAlliance() + { + if (isRedAlliance()) + { + zeroGyro(); + //Set the pose 180 degrees + resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); + } else + { + zeroGyro(); } + } - /** - * Command to characterize the robot angle motors using SysId - * - * @return SysId Angle Command - */ - public Command sysIdAngleMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setAngleSysIdRoutine( - new Config(), - this, swerveDrive), - 3.0, 5.0, 3.0); - } + /** + * Sets the drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorBrake(boolean brake) + { + swerveDrive.setMotorIdleMode(brake); + } - /** - * Returns a Command that centers the modules of the SwerveDrive subsystem. - * - * @return a Command that centers the modules of the SwerveDrive subsystem - */ - public Command centerModulesCommand() { - return run(() -> Arrays.asList(swerveDrive.getModules()) - .forEach(it -> it.setAngle(0.0))); - } + /** + * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the underlying drivebase. + * Note, this is not the raw gyro reading, this may be corrected from calls to resetOdometry(). + * + * @return The yaw angle + */ + public Rotation2d getHeading() + { + return getPose().getRotation(); + } - /** - * Returns a Command that tells the robot to drive forward until the command - * ends. - * - * @return a Command that tells the robot to drive forward until the command - * ends - */ - public Command driveForward() { - return run(() -> { - swerveDrive.drive(new Translation2d(1, 0), 0, false, false); - }).finallyDo(() -> swerveDrive.drive(new Translation2d(0, 0), 0, false, false)); - } + /** + * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for + * the angle of the robot. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param headingX X joystick which controls the angle of the robot. + * @param headingY Y joystick which controls the angle of the robot. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) + { + Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); + return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), + scaledInputs.getY(), + headingX, + headingY, + getHeading().getRadians(), + Constants.MAX_SPEED_MPS); + } - /** - * Replaces the swerve module feedforward with a new SimpleMotorFeedforward - * object. - * - * @param kS the static gain of the feedforward - * @param kV the velocity gain of the feedforward - * @param kA the acceleration gain of the feedforward - */ - public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { - swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); - } + /** + * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of + * 90deg. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param angle The angle in as a {@link Rotation2d}. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) + { + Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - /** - * Command to drive the robot using translative values and heading as angular - * velocity. - * - * @param translationX Translation in the X direction. Cubed for smoother - * controls. - * @param translationY Translation in the Y direction. Cubed for smoother - * controls. - * @param angularRotationX Angular velocity of the robot to set. Cubed for - * smoother controls. - * @return Drive command. - */ - public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, - DoubleSupplier angularRotationX) { - return run(() -> { - // Make the robot move - swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d( - translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), - translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), 0.8), - Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(), - true, - false); - }); - } + return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), + scaledInputs.getY(), + angle.getRadians(), + getHeading().getRadians(), + Constants.MAX_SPEED_MPS); + } - /** - * Command to drive the robot using translative values and heading as a - * setpoint. - * - * @param translationX Translation in the X direction. Cubed for smoother - * controls. - * @param translationY Translation in the Y direction. Cubed for smoother - * controls. - * @param headingX Heading X to calculate angle of the joystick. - * @param headingY Heading Y to calculate angle of the joystick. - * @return Drive command. - */ - public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, - DoubleSupplier headingY) { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading - // correction for this kind of control. - return run(() -> { + /** + * Gets the current field-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current field-relative velocity + */ + public ChassisSpeeds getFieldVelocity() + { + return swerveDrive.getFieldVelocity(); + } - Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(), - translationY.getAsDouble()), 0.8); + /** + * Gets the current velocity (x, y and omega) of the robot + * + * @return A {@link ChassisSpeeds} object of the current velocity + */ + public ChassisSpeeds getRobotVelocity() + { + return swerveDrive.getRobotVelocity(); + } - // Make the robot move - driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), - headingX.getAsDouble(), - headingY.getAsDouble(), - swerveDrive.getOdometryHeading().getRadians(), - swerveDrive.getMaximumChassisVelocity())); - }); - } + /** + * Get the {@link SwerveController} in the swerve drive. + * + * @return {@link SwerveController} from the {@link SwerveDrive}. + */ + public SwerveController getSwerveController() + { + return swerveDrive.swerveController; + } - /** - * The primary method for controlling the drivebase. Takes a - * {@link Translation2d} and a rotation rate, and - * calculates and commands module states accordingly. Can use either open-loop - * or closed-loop velocity control for - * the wheel velocities. Also has field- and robot-relative modes, which affect - * how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear - * velocity of the robot, in meters per - * second. In robot-relative mode, positive x is torwards - * the bow (front) and positive y is - * torwards port (left). In field-relative mode, positive x - * is away from the alliance wall - * (field North) and positive y is torwards the left wall - * when looking through the driver station - * glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. - * Unaffected by field/robot - * relativity. - * @param fieldRelative Drive mode. True for field-relative, false for - * robot-relative. - */ - public void drive(Translation2d translation, double rotation, boolean fieldRelative) { - swerveDrive.drive(translation, - rotation, - fieldRelative, - false); // Open loop is disabled since it shouldn't be used most of the time. - } + /** + * Get the {@link SwerveDriveConfiguration} object. + * + * @return The {@link SwerveDriveConfiguration} fpr the current drive. + */ + public SwerveDriveConfiguration getSwerveDriveConfiguration() + { + return swerveDrive.swerveDriveConfiguration; + } - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public void driveFieldOriented(ChassisSpeeds velocity) { - swerveDrive.driveFieldOriented(velocity); - } + /** + * Lock the swerve drive to prevent it from moving. + */ + public void lock() + { + swerveDrive.lockPose(); + } - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public Command driveFieldOriented(Supplier velocity) { - return run(() -> { - swerveDrive.driveFieldOriented(velocity.get()); - }); - } + /** + * Gets the current pitch angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getPitch() + { + return swerveDrive.getPitch(); + } - /** - * Drive according to the chassis robot oriented velocity. - * - * @param velocity Robot oriented {@link ChassisSpeeds} - */ - public void drive(ChassisSpeeds velocity) { - swerveDrive.drive(velocity); - } + /** + * Add a fake vision reading for testing purposes. + */ + public void addFakeVisionReading() + { + swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); + } - /** - * Get the swerve drive kinematics object. - * - * @return {@link SwerveDriveKinematics} of the swerve drive. - */ - public SwerveDriveKinematics getKinematics() { - return swerveDrive.kinematics; - } - - /** - * Resets odometry to the given pose. Gyro angle and module positions do not - * need to be reset when calling this - * method. However, if either gyro angle or module position is reset, this must - * be called in order for odometry to - * keep working. - * - * @param initialHolonomicPose The pose to set the odometry to - */ - public void resetOdometry(Pose2d initialHolonomicPose) { - swerveDrive.resetOdometry(initialHolonomicPose); - } - - /** - * Gets the current pose (position and rotation) of the robot, as reported by - * odometry. - * - * @return The robot's pose - */ - public Pose2d getPose() { - return swerveDrive.getPose(); - } - - /** - * Set chassis speeds with closed-loop velocity control. - * - * @param chassisSpeeds Chassis Speeds to set. - */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { - swerveDrive.setChassisSpeeds(chassisSpeeds); - } - - /** - * Post the trajectory to the field. - * - * @param trajectory The trajectory to post. - */ - public void postTrajectory(Trajectory trajectory) { - swerveDrive.postTrajectory(trajectory); - } - - /** - * Resets the gyro angle to zero and resets odometry to the same position, but - * facing toward 0. - */ - public void zeroGyro() { - swerveDrive.zeroGyro(); - } - - /** - * Checks if the alliance is red, defaults to false if alliance isn't available. - * - * @return true if the red alliance, false if blue. Defaults to false if none is - * available. - */ - private boolean isRedAlliance() { - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - } - - /** - * This will zero (calibrate) the robot to assume the current position is facing - * forward - *

- * If red alliance rotate the robot 180 after the drviebase zero command - */ - public void zeroGyroWithAlliance() { - if (isRedAlliance()) { - zeroGyro(); - // Set the pose 180 degrees - resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); - } else { - zeroGyro(); - } - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorBrake(boolean brake) { - swerveDrive.setMotorIdleMode(brake); - } - - /** - * Gets the current yaw angle of the robot, as reported by the swerve pose - * estimator in the underlying drivebase. - * Note, this is not the raw gyro reading, this may be corrected from calls to - * resetOdometry(). - * - * @return The yaw angle - */ - public Rotation2d getHeading() { - return getPose().getRotation(); - } - - /** - * Get the chassis speeds based on controller input of 2 joysticks. One for - * speeds in which direction. The other for - * the angle of the robot. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), - scaledInputs.getY(), - headingX, - headingY, - getHeading().getRadians(), - Constants.MAX_SPEED); - } - - /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. - * Control the robot at an offset of - * 90deg. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param angle The angle in as a {@link Rotation2d}. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - - return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), - scaledInputs.getY(), - angle.getRadians(), - getHeading().getRadians(), - Constants.MAX_SPEED); - } - - /** - * Gets the current field-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current field-relative velocity - */ - public ChassisSpeeds getFieldVelocity() { - return swerveDrive.getFieldVelocity(); - } - - /** - * Gets the current velocity (x, y and omega) of the robot - * - * @return A {@link ChassisSpeeds} object of the current velocity - */ - public ChassisSpeeds getRobotVelocity() { - return swerveDrive.getRobotVelocity(); - } - - /** - * Get the {@link SwerveController} in the swerve drive. - * - * @return {@link SwerveController} from the {@link SwerveDrive}. - */ - public SwerveController getSwerveController() { - return swerveDrive.swerveController; - } - - /** - * Get the {@link SwerveDriveConfiguration} object. - * - * @return The {@link SwerveDriveConfiguration} fpr the current drive. - */ - public SwerveDriveConfiguration getSwerveDriveConfiguration() { - return swerveDrive.swerveDriveConfiguration; - } - - /** - * Lock the swerve drive to prevent it from moving. - */ - public void lock() { - swerveDrive.lockPose(); - } - - /** - * Gets the current pitch angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - public Rotation2d getPitch() { - return swerveDrive.getPitch(); - } - - /** - * Add a fake vision reading for testing purposes. - */ - public void addFakeVisionReading() { - swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); - } - - /** - * Gets the swerve drive object. - * - * @return {@link SwerveDrive} - */ - public SwerveDrive getSwerveDrive() { - return swerveDrive; - } + /** + * Gets the swerve drive object. + * + * @return {@link SwerveDrive} + */ + public SwerveDrive getSwerveDrive() + { + return swerveDrive; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index f8bc468..b00b9e1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -16,12 +16,12 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTablesJNI; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.robot.Robot; +import frc.robot.Constants; import java.awt.Desktop; import java.util.ArrayList; import java.util.List; @@ -40,57 +40,59 @@ import org.photonvision.targeting.PhotonTrackedTarget; import swervelib.SwerveDrive; import swervelib.telemetry.SwerveDriveTelemetry; + /** - * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken - * from + * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads */ -public class Vision { +public class Vision +{ /** * April Tag Field Layout of the year. */ - public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( - AprilTagFields.k2026RebuiltAndymark); + public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( + AprilTagFields.k2025ReefscapeAndyMark); /** - * Ambiguity defined as a value between (0,1). Used in - * {@link Vision#filterPose}. + * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. */ - private final double maximumAmbiguity = 0.25; + private final double maximumAmbiguity = 0.25; /** * Photon Vision Simulation */ - public VisionSystemSim visionSim; + public VisionSystemSim visionSim; /** - * Count of times that the odom thinks we're more than 10meters away from the - * april tag. + * Count of times that the odom thinks we're more than 10meters away from the april tag. */ - private double longDistangePoseEstimationCount = 0; + private double longDistangePoseEstimationCount = 0; /** * Current pose from the pose estimator using wheel odometry. */ - private Supplier currentPose; + private Supplier currentPose; /** * Field from {@link swervelib.SwerveDrive#field} */ - private Field2d field2d; + private Field2d field2d; + /** * Constructor for the Vision class. * - * @param currentPose Current pose supplier, should reference - * {@link SwerveDrive#getPose()} + * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} * @param field Current field, should be {@link SwerveDrive#field} */ - public Vision(Supplier currentPose, Field2d field) { + public Vision(Supplier currentPose, Field2d field) + { this.currentPose = currentPose; this.field2d = field; - if (Robot.isSimulation()) { + if (Robot.isSimulation()) + { visionSim = new VisionSystemSim("Vision"); visionSim.addAprilTags(fieldLayout); - for (Cameras c : Cameras.values()) { + for (Cameras c : Cameras.values()) + { c.addToVisionSim(visionSim); } @@ -102,48 +104,50 @@ public class Vision { * Calculates a target pose relative to an AprilTag on the field. * * @param aprilTag The ID of the AprilTag. - * @param robotOffset The offset {@link Transform2d} of the robot to apply to - * the pose for the robot to position + * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position * itself correctly. * @return The target pose of the AprilTag. */ - public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { + public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) + { Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); - if (aprilTagPose3d.isPresent()) { + if (aprilTagPose3d.isPresent()) + { return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); - } else { + } else + { throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); } } /** - * Update the pose estimation inside of {@link SwerveDrive} with all of the - * given poses. + * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. * * @param swerveDrive {@link SwerveDrive} instance. */ - public void updatePoseEstimation(SwerveDrive swerveDrive) { - if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) { + public void updatePoseEstimation(SwerveDrive swerveDrive) + { + if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) + { /* - * In the maple-sim, odometry is simulated using encoder values, accounting for - * factors like skidding and drifting. + * In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting. * As a result, the odometry may not always be 100% accurate. - * However, the vision system should be able to provide a reasonably accurate - * pose estimation, even when odometry is incorrect. + * However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect. * (This is why teams implement vision system to correct odometry.) - * Therefore, we must ensure that the actual robot pose is provided in the - * simulator when updating the vision simulation during the simulation. + * Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation. */ visionSim.update(swerveDrive.getSimulationDriveTrainPose().get()); } - for (Cameras camera : Cameras.values()) { + for (Cameras camera : Cameras.values()) + { Optional poseEst = getEstimatedGlobalPose(camera); - if (poseEst.isPresent()) { + if (poseEst.isPresent()) + { var pose = poseEst.get(); swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), - pose.timestampSeconds, - camera.curStdDevs); + pose.timestampSeconds, + camera.curStdDevs); } } @@ -152,22 +156,24 @@ public class Vision { /** * Generates the estimated robot pose. Returns empty if: *

    - *
  • No Pose Estimates could be generated
    • - *
  • The generated pose estimate was considered not accurate
    • + *
  • No Pose Estimates could be generated
    • + *
  • The generated pose estimate was considered not accurate
    • *
* - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and - * targets used to create the estimate + * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate */ - public Optional getEstimatedGlobalPose(Cameras camera) { + public Optional getEstimatedGlobalPose(Cameras camera) + { Optional poseEst = camera.getEstimatedGlobalPose(); - if (Robot.isSimulation()) { + if (Robot.isSimulation()) + { Field2d debugField = visionSim.getDebugField(); // Uncomment to enable outputting of vision targets in sim. poseEst.ifPresentOrElse( - est -> debugField - .getObject("VisionEstimation") - .setPose(est.estimatedPose.toPose2d()), + est -> + debugField + .getObject("VisionEstimation") + .setPose(est.estimatedPose.toPose2d()), () -> { debugField.getObject("VisionEstimation").setPoses(); }); @@ -175,39 +181,46 @@ public class Vision { return poseEst; } + /** - * Filter pose via the ambiguity and find best estimate between all of the - * camera's throwing out distances more than + * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than * 10m for a short amount of time. * * @param pose Estimated robot pose. * @return Could be empty if there isn't a good reading. */ @Deprecated(since = "2024", forRemoval = true) - private Optional filterPose(Optional pose) { - if (pose.isPresent()) { + private Optional filterPose(Optional pose) + { + if (pose.isPresent()) + { double bestTargetAmbiguity = 1; // 1 is max ambiguity - for (PhotonTrackedTarget target : pose.get().targetsUsed) { + for (PhotonTrackedTarget target : pose.get().targetsUsed) + { double ambiguity = target.getPoseAmbiguity(); - if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) { + if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) + { bestTargetAmbiguity = ambiguity; } } - // ambiguity to high dont use estimate - if (bestTargetAmbiguity > maximumAmbiguity) { + //ambiguity to high dont use estimate + if (bestTargetAmbiguity > maximumAmbiguity) + { return Optional.empty(); } - // est pose is very far from recorded robot pose - if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) { + //est pose is very far from recorded robot pose + if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) + { longDistangePoseEstimationCount++; - // if it calculates that were 10 meter away for more than 10 times in a row its - // probably right - if (longDistangePoseEstimationCount < 10) { + //if it calculates that were 10 meter away for more than 10 times in a row its probably right + if (longDistangePoseEstimationCount < 10) + { return Optional.empty(); } - } else { + } else + { longDistangePoseEstimationCount = 0; } return pose; @@ -215,13 +228,15 @@ public class Vision { return Optional.empty(); } + /** * Get distance of the robot from the AprilTag pose. * * @param id AprilTag ID * @return Distance */ - public double getDistanceFromAprilTag(int id) { + public double getDistanceFromAprilTag(int id) + { Optional tag = fieldLayout.getTagPose(id); return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0); } @@ -233,12 +248,17 @@ public class Vision { * @param camera Camera to check. * @return Tracked target. */ - public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) { + public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) + { PhotonTrackedTarget target = null; - for (PhotonPipelineResult result : camera.resultsList) { - if (result.hasTargets()) { - for (PhotonTrackedTarget i : result.getTargets()) { - if (i.getFiducialId() == id) { + for (PhotonPipelineResult result : camera.resultsList) + { + if (result.hasTargets()) + { + for (PhotonTrackedTarget i : result.getTargets()) + { + if (i.getFiducialId() == id) + { return i; } } @@ -253,46 +273,54 @@ public class Vision { * * @return Vision Simulation */ - public VisionSystemSim getVisionSim() { + public VisionSystemSim getVisionSim() + { return visionSim; } /** - * Open up the photon vision camera streams on the localhost, assumes running - * photon vision on localhost. + * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost. */ - private void openSimCameraViews() { - if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) { - // try - // { - // Desktop.getDesktop().browse(new URI("http://localhost:1182/")); - // Desktop.getDesktop().browse(new URI("http://localhost:1184/")); - // Desktop.getDesktop().browse(new URI("http://localhost:1186/")); - // } catch (IOException | URISyntaxException e) - // { - // e.printStackTrace(); - // } + private void openSimCameraViews() + { + if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) + { +// try +// { +// Desktop.getDesktop().browse(new URI("http://localhost:1182/")); +// Desktop.getDesktop().browse(new URI("http://localhost:1184/")); +// Desktop.getDesktop().browse(new URI("http://localhost:1186/")); +// } catch (IOException | URISyntaxException e) +// { +// e.printStackTrace(); +// } } } /** * Update the {@link Field2d} to include tracked targets/ */ - public void updateVisionField() { + public void updateVisionField() + { List targets = new ArrayList(); - for (Cameras c : Cameras.values()) { - if (!c.resultsList.isEmpty()) { + for (Cameras c : Cameras.values()) + { + if (!c.resultsList.isEmpty()) + { PhotonPipelineResult latest = c.resultsList.get(0); - if (latest.hasTargets()) { + if (latest.hasTargets()) + { targets.addAll(latest.targets); } } } List poses = new ArrayList<>(); - for (PhotonTrackedTarget target : targets) { - if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) { + for (PhotonTrackedTarget target : targets) + { + if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) + { Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); poses.add(targetPose); } @@ -304,100 +332,98 @@ public class Vision { /** * Camera Enum to select each camera */ - enum Cameras { + enum Cameras + { /** * Left Camera */ - LEFT_CAM("left", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), + FRONT_LEFT("Front Left Camera", + Constants.TargetingConstants.FRONT_LEFT_CAMERA_ANGLE_RADIANS, + Constants.TargetingConstants.FRONT_LEFT_CAMERA_LOCATION_METERS, + Constants.TargetingConstants.SINGLE_TAG_STD_DEVS, + Constants.TargetingConstants.MULTI_TAG_STD_DEVS), /** * Right Camera */ - RIGHT_CAM("right", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(-10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), + FRONT_RIGHT("Front Right Camera", + Constants.TargetingConstants.FRONT_RIGHT_CAMERA_ANGLE_RADIANS, + Constants.TargetingConstants.FRONT_RIGHT_CAMERA_LOCATION_METERS, + Constants.TargetingConstants.SINGLE_TAG_STD_DEVS, + Constants.TargetingConstants.MULTI_TAG_STD_DEVS), /** * Center Camera */ - CENTER_CAM("center", - new Rotation3d(0, Units.degreesToRadians(18), 0), - new Translation3d(Units.inchesToMeters(-4.628), - Units.inchesToMeters(-10.687), - Units.inchesToMeters(16.129)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); + REAR_LEFT("Rear Left Camera", + Constants.TargetingConstants.REAR_LEFT_CAMERA_ANGLE_RADIANS, + Constants.TargetingConstants.REAR_LEFT_CAMERA_LOCATION_METERS, + Constants.TargetingConstants.SINGLE_TAG_STD_DEVS, + Constants.TargetingConstants.MULTI_TAG_STD_DEVS), + + REAR_RIGHT("Rear Right Camera", + Constants.TargetingConstants.REAR_RIGHT_CAMERA_ANGLE_RADIANS, + Constants.TargetingConstants.REAR_RIGHT_CAMERA_LOCATION_METERS, + Constants.TargetingConstants.SINGLE_TAG_STD_DEVS, + Constants.TargetingConstants.MULTI_TAG_STD_DEVS); /** * Latency alert to use when high latency is detected. */ - public final Alert latencyAlert; + public final Alert latencyAlert; /** * Camera instance for comms. */ - public final PhotonCamera camera; + public final PhotonCamera camera; /** * Pose estimator for camera. */ - public final PhotonPoseEstimator poseEstimator; + public final PhotonPoseEstimator poseEstimator; /** * Standard Deviation for single tag readings for pose estimation. */ - private final Matrix singleTagStdDevs; + private final Matrix singleTagStdDevs; /** * Standard deviation for multi-tag readings for pose estimation. */ - private final Matrix multiTagStdDevs; + private final Matrix multiTagStdDevs; /** - * Transform of the camera rotation and translation relative to the center of - * the robot + * Transform of the camera rotation and translation relative to the center of the robot */ - private final Transform3d robotToCamTransform; + private final Transform3d robotToCamTransform; /** * Current standard deviations used. */ - public Matrix curStdDevs; + public Matrix curStdDevs; /** * Estimated robot pose. */ - public Optional estimatedRobotPose = Optional.empty(); + public Optional estimatedRobotPose = Optional.empty(); /** * Simulated camera instance which only exists during simulations. */ - public PhotonCameraSim cameraSim; + public PhotonCameraSim cameraSim; /** - * Results list to be updated periodically and cached to avoid unnecessary - * queries. + * Results list to be updated periodically and cached to avoid unnecessary queries. */ - public List resultsList = new ArrayList<>(); + public List resultsList = new ArrayList<>(); /** * Last read from the camera timestamp to prevent lag due to slow data fetches. */ - private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); + private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); /** - * Construct a Photon Camera class with help. Standard deviations are fake - * values, experiment and determine + * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine * estimation noise on an actual robot. * - * @param name Name of the PhotonVision camera found in the PV - * UI. + * @param name Name of the PhotonVision camera found in the PV UI. * @param robotToCamRotation {@link Rotation3d} of the camera. - * @param robotToCamTranslation {@link Translation3d} relative to the center of - * the robot. - * @param singleTagStdDevs Single AprilTag standard deviations of estimated - * poses from the camera. - * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated - * poses from the camera. + * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. + * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera. + * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera. */ Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, - Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) { + Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) + { latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning); camera = new PhotonCamera(name); @@ -406,22 +432,21 @@ public class Vision { robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCamTransform); + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + robotToCamTransform); poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); this.singleTagStdDevs = singleTagStdDevs; this.multiTagStdDevs = multiTagStdDevsMatrix; - if (Robot.isSimulation()) { + if (Robot.isSimulation()) + { SimCameraProperties cameraProp = new SimCameraProperties(); // A 640 x 480 camera with a 100 degree diagonal FOV. cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); - // Approximate detection noise with average and standard deviation error in - // pixels. + // Approximate detection noise with average and standard deviation error in pixels. cameraProp.setCalibError(0.25, 0.08); - // Set the camera image capture framerate (Note: this is limited by robot loop - // rate). + // Set the camera image capture framerate (Note: this is limited by robot loop rate). cameraProp.setFPS(30); // The average and standard deviation in milliseconds of image data latency. cameraProp.setAvgLatencyMs(35); @@ -437,31 +462,35 @@ public class Vision { * * @param systemSim {@link VisionSystemSim} to use. */ - public void addToVisionSim(VisionSystemSim systemSim) { - if (Robot.isSimulation()) { + public void addToVisionSim(VisionSystemSim systemSim) + { + if (Robot.isSimulation()) + { systemSim.addCamera(cameraSim, robotToCamTransform); } } /** - * Get the result with the least ambiguity from the best tracked target within - * the Cache. This may not be the most + * Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most * recent result! * - * @return The result in the cache with the least ambiguous best tracked target. - * This is not the most recent result! + * @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result! */ - public Optional getBestResult() { - if (resultsList.isEmpty()) { + public Optional getBestResult() + { + if (resultsList.isEmpty()) + { return Optional.empty(); } - PhotonPipelineResult bestResult = resultsList.get(0); - double amiguity = bestResult.getBestTarget().getPoseAmbiguity(); - double currentAmbiguity = 0; - for (PhotonPipelineResult result : resultsList) { + PhotonPipelineResult bestResult = resultsList.get(0); + double amiguity = bestResult.getBestTarget().getPoseAmbiguity(); + double currentAmbiguity = 0; + for (PhotonPipelineResult result : resultsList) + { currentAmbiguity = result.getBestTarget().getPoseAmbiguity(); - if (currentAmbiguity < amiguity && currentAmbiguity > 0) { + if (currentAmbiguity < amiguity && currentAmbiguity > 0) + { bestResult = result; amiguity = currentAmbiguity; } @@ -472,63 +501,63 @@ public class Vision { /** * Get the latest result from the current cache. * - * @return Empty optional if nothing is found. Latest result if something is - * there. + * @return Empty optional if nothing is found. Latest result if something is there. */ - public Optional getLatestResult() { + public Optional getLatestResult() + { return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0)); } /** - * Get the estimated robot pose. Updates the current robot pose estimation, - * standard deviations, and flushes the + * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the * cache of results. * * @return Estimated pose. */ - public Optional getEstimatedGlobalPose() { + public Optional getEstimatedGlobalPose() + { updateUnreadResults(); return estimatedRobotPose; } /** - * Update the latest results, cached with a maximum refresh rate of 1req/15ms. - * Sorts the list by timestamp. + * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp. */ - private void updateUnreadResults() { + private void updateUnreadResults() + { double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds(); - - for (PhotonPipelineResult result : resultsList) { + + for (PhotonPipelineResult result : resultsList) + { mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds()); } - resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults(); - resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> { - return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1; - }); - if (!resultsList.isEmpty()) { - updateEstimatedGlobalPose(); - } + resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults(); + resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> { + return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1; + }); + if (!resultsList.isEmpty()) + { + updateEstimatedGlobalPose(); + } } /** - * The latest estimated robot pose on the field from vision data. This may be - * empty. This should only be called once + * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once * per loop. * - *

- * Also includes updates for the standard deviations, which can (optionally) be - * retrieved with + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with * {@link Cameras#updateEstimationStdDevs} * - * @return An {@link EstimatedRobotPose} with an estimated pose, estimate - * timestamp, and targets used for - * estimation. + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for + * estimation. */ - private void updateEstimatedGlobalPose() { + private void updateEstimatedGlobalPose() + { Optional visionEst = Optional.empty(); - for (var change : resultsList) { + for (var change : resultsList) + { visionEst = poseEstimator.update(change); updateEstimationStdDevs(visionEst, change.getTargets()); } @@ -536,54 +565,63 @@ public class Vision { } /** - * Calculates new standard deviations This algorithm is a heuristic that creates - * dynamic standard deviations based + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based * on number of tags, estimation strategy, and distance from the tags. * * @param estimatedPose The estimated pose to guess standard deviations for. * @param targets All targets in this camera frame */ private void updateEstimationStdDevs( - Optional estimatedPose, List targets) { - if (estimatedPose.isEmpty()) { + Optional estimatedPose, List targets) + { + if (estimatedPose.isEmpty()) + { // No pose input. Default to single-tag std devs curStdDevs = singleTagStdDevs; - } else { + } else + { // Pose present. Start running Heuristic - var estStdDevs = singleTagStdDevs; - int numTags = 0; - double avgDist = 0; + var estStdDevs = singleTagStdDevs; + int numTags = 0; + double avgDist = 0; - // Precalculation - see how many tags we found, and calculate an - // average-distance metric - for (var tgt : targets) { + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) + { var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) { + if (tagPose.isEmpty()) + { continue; } numTags++; - avgDist += tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); } - if (numTags == 0) { + if (numTags == 0) + { // No tags visible. Default to single-tag std devs curStdDevs = singleTagStdDevs; - } else { + } else + { // One or more tags visible, run the full heuristic. avgDist /= numTags; // Decrease std devs if multiple targets are visible - if (numTags > 1) { + if (numTags > 1) + { estStdDevs = multiTagStdDevs; } // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) { + if (numTags == 1 && avgDist > 4) + { estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - } else { + } else + { estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); } curStdDevs = estStdDevs;