diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b7..b9abcb4b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,6 +1,7 @@ { "java.configuration.updateBuildConfiguration": "automatic", "java.server.launchMode": "Standard", + "terminal.integrated.fontFamily": "monospace", "files.exclude": { "**/.git": true, "**/.svn": true, diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java new file mode 100644 index 00000000..89d86ba9 --- /dev/null +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -0,0 +1,780 @@ +//LimelightHelpers v1.2.1 (March 1, 2023) + +package frc.robot; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +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.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +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; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("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("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + 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]; + + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public LimelightResults() { + targetingResults = new Results(); + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static double getNeuralClassID(String limelightName) { + return getLimelightNTDouble(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + 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 Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void 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"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + System.err.println("lljson error: " + e.getMessage()); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cfe8a955..7031f580 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,9 +11,12 @@ import frc.robot.commands.ArcadeDrive; import frc.robot.commands.CalibrateExtender; import frc.robot.commands.CalibratePivot; +import frc.robot.commands.DriveTo; import frc.robot.commands.BumpDrive; import frc.robot.commands.ForzaDrive; +import frc.robot.commands.Intake; import frc.robot.commands.SequentialAutoCommand; +import frc.robot.commands.Shooter; import frc.robot.commands.TrackTarget; import frc.robot.commands.RCFDrive; import frc.robot.commands.MoveArm; @@ -21,6 +24,7 @@ import frc.robot.commands.SequentialAutoCommand.StartPositions; import frc.robot.subsystems.Targeting; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Limelight; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Claw; @@ -51,6 +55,7 @@ public class RobotContainer { public final Drivetrain m_drivetrain = new Drivetrain(m_kinematics); public final Arm m_arm = new Arm(); public final Claw m_claw = new Claw(); + public final Limelight m_limelight = new Limelight(new LimelightHelpers(), m_kinematics); // Joysticks public final XboxController m_driverXboxController = new XboxController(0); @@ -113,15 +118,13 @@ private void configureButtonBindings() { XboxController.Button.kX.value); xboxXBtn.onTrue(new MoveArm(m_arm, Node.MID)); - // Set arm preset to hybrid location - final JoystickButton xboxABtn = new JoystickButton(m_operatorXboxController, + // Start intake motor + final JoystickButton xboxABtn = new JoystickButton(m_driverXboxController, XboxController.Button.kA.value); - xboxABtn.onTrue(new MoveArm(m_arm, Node.HYBRID)); + xboxABtn.whileTrue(new Intake(new Drivetrain(m_kinematics))); - // Set arm preset to substation location - final JoystickButton xboxBBtn = new JoystickButton(m_operatorXboxController, - XboxController.Button.kB.value); - xboxBBtn.onTrue(new MoveArm(m_arm, Node.SUBSTATION)); + // Start shooter motor + /******** Driver Controls ********/ @@ -138,7 +141,8 @@ private void configureButtonBindings() { // Set the arm preset to the stow location, inside the robot final JoystickButton xboxStowButton = new JoystickButton(m_driverXboxController, XboxController.Button.kX.value); - xboxStowButton.onTrue(new MoveArm(m_arm, Node.STOW)); + // xboxStowButton.onTrue(new MoveArm(m_arm, Node.STOW)); + xboxStowButton.whileTrue(new DriveTo(new Position2D(1, 0, 0), 0.2, m_kinematics, m_drivetrain)); // Trigger autobalance // final JoystickButton xboxAButton = new JoystickButton(m_driverXboxController, @@ -146,9 +150,9 @@ private void configureButtonBindings() { // xboxAButton.onTrue(new AutoBalance(m_drivetrain)); // Trigger autoalign - final JoystickButton xboxYButton = new JoystickButton(m_driverXboxController, - XboxController.Button.kY.value); - xboxYButton.onTrue(new TrackTarget(m_drivetrain, m_targeting)); + // final JoystickButton xboxYButton = new JoystickButton(m_driverXboxController, + // XboxController.Button.kY.value); + // xboxYButton.onTrue(new TrackTarget(m_drivetrain, m_targeting)); // Added options to the dropdown for driveChooser and putting it into // smartdashboard diff --git a/src/main/java/frc/robot/commands/AlignAndShoot.java b/src/main/java/frc/robot/commands/AlignAndShoot.java new file mode 100644 index 00000000..2d3bd947 --- /dev/null +++ b/src/main/java/frc/robot/commands/AlignAndShoot.java @@ -0,0 +1,38 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.classes.Kinematics; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Limelight; + +public class AlignAndShoot extends CommandBase { + private final Limelight m_limelight; + private final Drivetrain m_drivetrain; + private final Kinematics m_kinematics; + + public AlignAndShoot(Limelight limelight, Drivetrain drivetrain, Kinematics kinematics) { + m_limelight = limelight; + m_drivetrain = drivetrain; + m_kinematics = kinematics; + addRequirements(m_limelight, m_drivetrain); + + } + + @Override + public void execute() { + new AutoAlign(m_limelight, m_kinematics, m_drivetrain); + if (m_limelight.angleAligned ()) { + new Intake(m_drivetrain); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutoAlign.java b/src/main/java/frc/robot/commands/AutoAlign.java new file mode 100644 index 00000000..3d3b7dfb --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoAlign.java @@ -0,0 +1,114 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.LimelightHelpers; +import frc.robot.classes.Kinematics; +import frc.robot.classes.Position2D; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Limelight; + +public class AutoAlign extends CommandBase { + private static final double LINEAR_INTERGRAL_LIMIT = 5; + private static final double ANGULAR_INTEGRAL_LIMIT = 5; + // linear pid + private double m_linearError; + private double m_linearLastError; + private double m_linearP; + private double m_linearI; + private double m_linearD; + private double m_linearChange; + private double m_linearIntegralError; + private double m_linearClamp; + private double m_linearVelOutput; + + // angular pid + private double m_angularError; + private double m_angularLastError; + private double m_angularP; + private double m_angularI; + private double m_angularD; + private double m_angularChange; + private double m_angularIntegralError; + private double m_angularClamp; + private double m_angularVelOutput; + + private Limelight m_limelight; + private final Kinematics m_kinematics; + private final Drivetrain m_drivetrain; + + public AutoAlign(Limelight limelight, Kinematics kinematics, Drivetrain drivetrain) { + m_limelight = limelight; + m_kinematics = kinematics; + m_drivetrain = drivetrain; + m_linearI = 0.0; + m_linearP = 0.001; + m_linearD = 0.0; + + m_angularP = 2; + m_angularI = 0; + m_angularD = 0.0; + Drivetrain.getTab().setDouble("Angular P", m_angularP); + Drivetrain.getTab().setDouble("Angular I", m_angularI); + Drivetrain.getTab().setDouble("Angular D", m_angularD); + } + + @Override + public void execute() { + m_angularP = Drivetrain.getTab().getDouble("Angular P", m_angularP); + m_angularI = Drivetrain.getTab().getDouble("Angular I", m_angularI); + m_angularD = Drivetrain.getTab().getDouble("Angular D", m_angularD); + + m_linearClamp = 1d; + m_angularClamp = 1d; + // set last errors to the past error + m_linearLastError = m_linearError; + m_angularLastError = m_angularError; + + Position2D robotPosition = m_kinematics.getPose(); // Robot space + double tagHeading = LimelightHelpers.getTargetPose_RobotSpace(m_limelight.getLimelightName())[5]; + m_angularError = LimelightHelpers.getTX("limelight"); // (tagHeading - robotPosition.getHeadingDegrees()); + m_linearError = m_limelight.getDistance(); + m_linearChange = m_linearError - m_linearLastError; + m_angularChange = m_angularError - m_angularLastError; + + if (Math.abs(m_linearError) < LINEAR_INTERGRAL_LIMIT) { + m_linearIntegralError += m_linearError; + } + if (Math.abs(m_angularError) < ANGULAR_INTEGRAL_LIMIT) { + m_angularIntegralError += m_angularError; + } + + m_linearVelOutput = (m_linearP * m_linearError) + (m_linearI * m_linearIntegralError) + (m_linearD * m_linearChange); + m_linearVelOutput = MathUtil.clamp(m_linearVelOutput, -m_linearClamp, m_linearClamp); + + // Angular align + m_angularVelOutput = (m_angularP * m_angularError) + (m_angularI * m_angularIntegralError) + (m_angularD * m_angularChange); + m_angularVelOutput = MathUtil.clamp(m_angularVelOutput, -m_angularClamp, m_angularClamp); + + if(Math.abs(m_angularError) < 0.1) + { + /* Found the target */ + m_angularVelOutput = 0.0d; + m_angularIntegralError = 0.0d; + } + + /* Only turn for the time being */ + m_drivetrain.arcadeDrive(0, m_angularVelOutput); + + + Drivetrain.getTab().setDouble("m_angularError (DEG)", m_angularError); + Drivetrain.getTab().setDouble("m_angularVelOutput", m_angularVelOutput); + Drivetrain.getTab().setDouble("Robot heading (DEG)", robotPosition.getHeadingDegrees()); + } + + @Override + public void end(boolean interrupted) { + m_drivetrain.arcadeDrive(0, 0); + } + + @Override + public boolean isFinished() { + return false; // Math.abs(m_angularError) < 1; + } +} diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java new file mode 100644 index 00000000..e2bc0699 --- /dev/null +++ b/src/main/java/frc/robot/commands/Intake.java @@ -0,0 +1,68 @@ +package frc.robot.commands; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drivetrain; + +public class Intake extends CommandBase { + private static final int CAN_ID = 5; + private WPI_TalonFX m_intakeMotor; + private double m_leftCurrentSpeed; + private double m_rightCurrentSpeed; + private double m_targetRpms; + private Drivetrain drivetrain; + + public Intake(Drivetrain drivetrain) { + this.drivetrain = drivetrain; + m_intakeMotor = new WPI_TalonFX(CAN_ID); + m_intakeMotor.clearStickyFaults(); + m_intakeMotor.configFactoryDefault(); + m_intakeMotor.setInverted(false); + m_intakeMotor.setSensorPhase(false); + m_intakeMotor.config_kD(0, 0.021); + m_intakeMotor.config_kI(0, 0.0001); + m_intakeMotor.config_kP(0, 0.001); + m_intakeMotor.config_kF(0, 0.068); + Drivetrain.getTab().setBoolean("moving at same speed", false); + Drivetrain.getTab().setDouble("left current speed", m_leftCurrentSpeed); + Drivetrain.getTab().setDouble("right current speed", m_rightCurrentSpeed); + Drivetrain.getTab().setDouble("total counteracting speed", m_targetRpms); + m_intakeMotor.setNeutralMode(NeutralMode.Coast); + m_intakeMotor.configNeutralDeadband(0); + } + + @Override + public void execute() { + m_leftCurrentSpeed = drivetrain.getRawLeftEncoderVelocity(); + m_rightCurrentSpeed = drivetrain.getRawRightEncoderVelocity(); + Drivetrain.getTab().setDouble("left current speed", m_leftCurrentSpeed); + Drivetrain.getTab().setDouble("right current speed", m_rightCurrentSpeed); + double totalSpeed = (m_leftCurrentSpeed + m_rightCurrentSpeed) / 2; + m_targetRpms = -200d - totalSpeed; + MathUtil.clamp(totalSpeed, -10000000d, 0); + Drivetrain.getTab().setDouble("total counteracting speed", m_targetRpms); + m_intakeMotor.set(ControlMode.Velocity, -200d - totalSpeed); + if (m_leftCurrentSpeed == m_rightCurrentSpeed) { + Drivetrain.getTab().setBoolean("moving at same speed", true); + } else { + Drivetrain.getTab().setBoolean("moving at same speed", false); + } + } + + @Override + public void end(boolean interrupted) { + m_intakeMotor.set(0.0d); + } + + private double encoderToRpm(double encoderUnits) { + return encoderUnits*600/2048d; + } + + private double RpmToEncoder(double rpm) { + return rpm*2048d/600d; + } +} diff --git a/src/main/java/frc/robot/commands/SequentialAutoCommand.java b/src/main/java/frc/robot/commands/SequentialAutoCommand.java index bc416f8e..b9e571ea 100644 --- a/src/main/java/frc/robot/commands/SequentialAutoCommand.java +++ b/src/main/java/frc/robot/commands/SequentialAutoCommand.java @@ -38,6 +38,8 @@ public SequentialAutoCommand(Drivetrain drivetrain, Arm arm, Claw claw, Kinemati RobotContainer.getAutoBoard().setBoolean("AutoDone", false); + m_startPosition = StartPositions.SCORE_AND_ENGAGE; + switch (m_startPosition) { // Changes the robot path based on the starting position of the robot case SCORE_DONT_MOVE: // FACE SCORING GRID @@ -75,16 +77,16 @@ public SequentialAutoCommand(Drivetrain drivetrain, Arm arm, Claw claw, Kinemati case SCORE_AND_ENGAGE: // FACE SCORING GRID resetKinematics(); - score(); + // score(); addCommands( // Drive backwards - new DriveTo(new Position2D(-0.5, 0, Math.toRadians(0)), -2.0d, m_kinematics, m_drivetrain), + new DriveTo(new Position2D(-2, 0, 0), 1, m_kinematics, m_drivetrain) // Lower arm - new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES), - // Drive backwards (~3 feet) - new DriveTo(new Position2D(-6, 0, Math.toRadians(0)), -3.5d, m_kinematics, m_drivetrain), - // Autobalance - new AutoBalance(m_drivetrain) + // new SetArm(m_arm, Arm.STOW_ANGLE, Arm.STOW_R_INCHES), + // // Drive backwards (~3 feet) + // new DriveTo(new Position2D(-6, 0, Math.toRadians(0)), -3.5d, m_kinematics, m_drivetrain), + // // Autobalance + // new AutoBalance(m_drivetrain) ); break; @@ -117,6 +119,7 @@ public SequentialAutoCommand(Drivetrain drivetrain, Arm arm, Claw claw, Kinemati System.out.println("ERROR: Invalid autonomous starting position! [" + m_startPosition + "]"); break; } + new DriveTo(new Position2D(0, 1, 0), 0.2, kinematics, drivetrain); // Alert smart dashboard that autonomous is done RobotContainer.getAutoBoard().setBoolean("AutoDone", true); diff --git a/src/main/java/frc/robot/commands/Shooter.java b/src/main/java/frc/robot/commands/Shooter.java new file mode 100644 index 00000000..a17c6019 --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooter.java @@ -0,0 +1,52 @@ +package frc.robot.commands; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drivetrain; + +public class Shooter extends CommandBase { + private static final int CAN_ID = 5; + private WPI_TalonFX m_shooterMotor; + private double m_targetRpms = 0.0d; + private double m_currentRpms = 0.0d; + + public Shooter() { + m_shooterMotor = new WPI_TalonFX(CAN_ID); + m_shooterMotor.clearStickyFaults(); + m_shooterMotor.configFactoryDefault(); + m_shooterMotor.setInverted(false); + m_shooterMotor.setSensorPhase(false); + m_shooterMotor.config_kD(0, 0.021); + m_shooterMotor.config_kI(0, 0.0001); + m_shooterMotor.config_kP(0, 0.001); + m_shooterMotor.config_kF(0, 0.068); + Drivetrain.getTab().setDouble("shooter target rpm", m_targetRpms); + Drivetrain.getTab().setDouble("shooter current rpm", m_currentRpms); + + m_shooterMotor.setNeutralMode(NeutralMode.Coast); + m_shooterMotor.configNeutralDeadband(0); + } + + @Override + public void execute() { + m_targetRpms = Drivetrain.getTab().getDouble("shooter target rpm", 0.1d); + double encoderSpeed = RpmToEncoder(m_targetRpms); + m_shooterMotor.set(ControlMode.Velocity, encoderSpeed); + } + + @Override + public void end(boolean interrupted) { + m_shooterMotor.set(0.0d); + } + + private double encoderToRpm(double encoderUnits) { + return encoderUnits*600/2048d; + } + + private double RpmToEncoder(double rpm) { + return rpm*2048d/600d; + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index c0ca191b..2a89ca30 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -89,54 +89,54 @@ public class Arm extends SubsystemBase { // Gear ratios public Arm() { - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS + // // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS pivotTalonFX = new WPI_TalonFX(PIVOT_TALON_FX_CAN_ID); extenderTalonFX = new WPI_TalonFX(EXTENDER_TALON_FX_CAN_ID); - // Clears motor errors - pivotTalonFX.clearStickyFaults(); - extenderTalonFX.clearStickyFaults(); - - // Set factory defaults for onboard PID - pivotTalonFX.configFactoryDefault(); - extenderTalonFX.configFactoryDefault(); - - pivotTalonFX.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, - CONFIG_ARM_FEEDBACKSENSOR_TIMEOUT_MS); - extenderTalonFX.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, - CONFIG_ARM_FEEDBACKSENSOR_TIMEOUT_MS); - - pivotTalonFX.setInverted(true); - pivotTalonFX.setSensorPhase(true); - extenderTalonFX.setInverted(false); - extenderTalonFX.setSensorPhase(false); - - // Configure Position PID - pivotTalonFX.config_kF(0, PIVOT_KF, PID_CONFIG_TIMEOUT_MS); - pivotTalonFX.config_kP(0, PIVOT_KP, PID_CONFIG_TIMEOUT_MS); - pivotTalonFX.config_kI(0, PIVOT_KI, PID_CONFIG_TIMEOUT_MS); - pivotTalonFX.config_kD(0, PIVOT_KD, PID_CONFIG_TIMEOUT_MS); - pivotTalonFX.config_IntegralZone(0, 1000); - pivotTalonFX.configMotionCruiseVelocity(PIVOT_MAX_VELOCITY, PID_CONFIG_TIMEOUT_MS); - pivotTalonFX.configMotionAcceleration(PIVOT_MAX_ACCELERATION, PID_CONFIG_TIMEOUT_MS); - - extenderTalonFX.config_kF(0, EXTENDER_KF, PID_CONFIG_TIMEOUT_MS); - extenderTalonFX.config_kP(0, EXTENDER_KP, PID_CONFIG_TIMEOUT_MS); - extenderTalonFX.config_kI(0, EXTENDER_KI, PID_CONFIG_TIMEOUT_MS); - extenderTalonFX.config_kD(0, EXTENDER_KD, PID_CONFIG_TIMEOUT_MS); - extenderTalonFX.config_IntegralZone(0, 100); - extenderTalonFX.configMotionCruiseVelocity(EXTENDER_MAX_VELOCITY, PID_CONFIG_TIMEOUT_MS); - extenderTalonFX.configMotionAcceleration(EXTENDER_MAX_ACCELERATION, PID_CONFIG_TIMEOUT_MS); - - extenderTalonFX.setNeutralMode(NeutralMode.Brake); - pivotTalonFX.setNeutralMode(NeutralMode.Brake); - - extenderTalonFX.configNeutralDeadband(0); - pivotTalonFX.configNeutralDeadband(0); - - // Start the calibration process - isPivotCalibrated = false; - isExtenderCalibrated = false; + // // Clears motor errors + // pivotTalonFX.clearStickyFaults(); + // extenderTalonFX.clearStickyFaults(); + + // // Set factory defaults for onboard PID + // pivotTalonFX.configFactoryDefault(); + // extenderTalonFX.configFactoryDefault(); + + // pivotTalonFX.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, + // CONFIG_ARM_FEEDBACKSENSOR_TIMEOUT_MS); + // extenderTalonFX.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, + // CONFIG_ARM_FEEDBACKSENSOR_TIMEOUT_MS); + + // pivotTalonFX.setInverted(true); + // pivotTalonFX.setSensorPhase(true); + // extenderTalonFX.setInverted(false); + // extenderTalonFX.setSensorPhase(false); + + // // Configure Position PID + // pivotTalonFX.config_kF(0, PIVOT_KF, PID_CONFIG_TIMEOUT_MS); + // pivotTalonFX.config_kP(0, PIVOT_KP, PID_CONFIG_TIMEOUT_MS); + // pivotTalonFX.config_kI(0, PIVOT_KI, PID_CONFIG_TIMEOUT_MS); + // pivotTalonFX.config_kD(0, PIVOT_KD, PID_CONFIG_TIMEOUT_MS); + // pivotTalonFX.config_IntegralZone(0, 1000); + // pivotTalonFX.configMotionCruiseVelocity(PIVOT_MAX_VELOCITY, PID_CONFIG_TIMEOUT_MS); + // pivotTalonFX.configMotionAcceleration(PIVOT_MAX_ACCELERATION, PID_CONFIG_TIMEOUT_MS); + + // extenderTalonFX.config_kF(0, EXTENDER_KF, PID_CONFIG_TIMEOUT_MS); + // extenderTalonFX.config_kP(0, EXTENDER_KP, PID_CONFIG_TIMEOUT_MS); + // extenderTalonFX.config_kI(0, EXTENDER_KI, PID_CONFIG_TIMEOUT_MS); + // extenderTalonFX.config_kD(0, EXTENDER_KD, PID_CONFIG_TIMEOUT_MS); + // extenderTalonFX.config_IntegralZone(0, 100); + // extenderTalonFX.configMotionCruiseVelocity(EXTENDER_MAX_VELOCITY, PID_CONFIG_TIMEOUT_MS); + // extenderTalonFX.configMotionAcceleration(EXTENDER_MAX_ACCELERATION, PID_CONFIG_TIMEOUT_MS); + + // extenderTalonFX.setNeutralMode(NeutralMode.Brake); + // pivotTalonFX.setNeutralMode(NeutralMode.Brake); + + // extenderTalonFX.configNeutralDeadband(0); + // pivotTalonFX.configNeutralDeadband(0); + + // // Start the calibration process + // isPivotCalibrated = false; + // isExtenderCalibrated = false; } public static SpikeBoard getTab() { @@ -161,7 +161,7 @@ public void periodic() { checkPivotCalibration(); /* Are we calibrated? */ - if (false == isExtenderCalibrated) { + /*if (false == isExtenderCalibrated) { // The extender is not calibrated, start the extender calibration extenderTalonFX.set(-EXTENDER_CALIBRATION_MOTOR_SPEED); } else if (false == isPivotCalibrated) { @@ -176,7 +176,7 @@ public void periodic() { } else { // We are now calibrated and we know the position of the arm! moveToPosition(theta, rInches); - } + }*/ } /** diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 4158ada0..59b7a25e 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -336,6 +336,14 @@ public double getRightEncoderVelocity() { return SPIKE293Utils.controllerVelocityToFeetPerSec(rightTalonLead.getSelectedSensorVelocity()); } + public double getRawRightEncoderVelocity() { + return rightTalonLead.getSelectedSensorVelocity(); + } + + public double getRawLeftEncoderVelocity() { + return leftTalonLead.getSelectedSensorVelocity(); + } + /** * returns robot Velocity in ft/s * diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java new file mode 100644 index 00000000..57a6a0cc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -0,0 +1,81 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; +import frc.robot.classes.Kinematics; +import frc.robot.classes.SpikeBoard; + +public class Limelight extends SubsystemBase { + private static final String LIMELIGHT_NAME = "limelight"; + private static final double LIMELIGHT_HEIGHT = 29.8d; + private static final double TAG_HEIGHT = 57.75d; + private static final double MOUNTING_ANGLE = 21d; + + private double m_tx; + private double m_ty; + private double distanceToTag; + private double distanceToTagX; + private double distanceToTagY; + private boolean targetFound; + private double tagId; + + private final LimelightHelpers m_limelight; + private static SpikeBoard limelightTab; + private final Kinematics m_kinematics; + + public Limelight(LimelightHelpers limelightHelper, Kinematics kinematics) { + m_limelight = limelightHelper; + m_kinematics = kinematics; + } + + @Override + public void periodic() { + targetFound = LimelightHelpers.getTV(LIMELIGHT_NAME); + m_tx = LimelightHelpers.getTX(LIMELIGHT_NAME); + m_ty = LimelightHelpers.getTY(LIMELIGHT_NAME); + distanceToTag = getDistance(); + tagId = LimelightHelpers.getFiducialID(LIMELIGHT_NAME); + + getTab().setBoolean("April tag detected", targetFound); + getTab().setDouble("tx", m_tx); + getTab().setDouble("ty", m_ty); + getTab().setDouble("Distance to tag", distanceToTag); + getTab().setDouble("Distance to tag X", distanceToTagX); + getTab().setDouble("Distance to tag Y", distanceToTagY); + } + + public static SpikeBoard getTab() { + if (limelightTab == null) { + limelightTab = new SpikeBoard("Limelight"); + } + return limelightTab; + } + + public LimelightHelpers getHelper() { + return m_limelight; + } + + public String getLimelightName() { + return LIMELIGHT_NAME; + } + + public double getDistance() { + return (TAG_HEIGHT - LIMELIGHT_HEIGHT) / Math.tan(Math.toRadians(MOUNTING_ANGLE) + Math.toRadians(m_ty)); + } + + public double getAngle() { + return LimelightHelpers.getTX(LIMELIGHT_NAME); + } + + public boolean isAligned() { + return Math.abs(getAngle()) <= m_kinematics.getPose().getHeadingDegrees() + 3 && getDistance() < 0.5; + } + + public boolean angleAligned() { + return Math.abs(getAngle()) <= 3; + } + + public double getTagId() { + return tagId; + } +} diff --git a/src/main/java/frc/robot/subsystems/LimelightInterface.java b/src/main/java/frc/robot/subsystems/LimelightInterface.java new file mode 100644 index 00000000..8bf54609 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightInterface.java @@ -0,0 +1,91 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; +import frc.robot.classes.Kinematics; +import frc.robot.classes.Position2D; + +public class LimelightInterface extends SubsystemBase { + private static final String LIMELIGHT_NAME = "limelight"; + private static final double TARGET_HEIGHT = 53.5d; // average height of april tags (they are on varying heights) + private static final double LIMELIGHT_HEIGHT = 36d; // height of limelight mounted on robot + private static final double LIMELIGHT_INITIAL_ANGLE = 17d; // in degrees the angle of the limelight mounted on robot + private static final double VISION_ANGLE_TOLERANCE = 3.0d; // max deviation in actual angle measure + + private double tv; // target in sight + private double tx; // horizontal offset from crosshair to target + private double ty; // vertical offset from crosshair to target + + private double distanceToGoal; // in inches + private int targetCount; // ensure target being identified correctly x amount of times before complete + private Kinematics kinematics; + + public LimelightInterface(Kinematics kinematics) { + this.kinematics = kinematics; + + + tv = 0.0d; + tx = 0.0d; + ty = 0.0d; + distanceToGoal = 0.0d; + targetCount = 0; + } + + @Override + public void periodic() { + tv = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tv").getDouble(0); + tx = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tx").getDouble(0); + ty = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("ty").getDouble(0); + + + calcDist(); + + if (Math.abs(tx) <= VISION_ANGLE_TOLERANCE) { + targetCount++; + } else { + targetCount = 0; + } + } + + private void calcDist() { + distanceToGoal = ((TARGET_HEIGHT - LIMELIGHT_HEIGHT) / Math.cos(Math.toRadians(ty + LIMELIGHT_INITIAL_ANGLE))) * 12.0; + } + + public double calcDistance() { + return Math.sqrt(( + Math.pow((LimelightHelpers.getTargetPose_RobotSpace(LIMELIGHT_NAME)[0] - LimelightHelpers.getBotPose(LIMELIGHT_NAME)[0]), 2)) + + Math.pow(LimelightHelpers.getTargetPose_RobotSpace(LIMELIGHT_NAME)[1], 2) + ); + } + + public double getDistance() { + return distanceToGoal; + } + + public boolean tooClose() { + return distanceToGoal <= 24.0d; + } + + public boolean tooFar() { + return distanceToGoal >= 1.0; + } + + public double getId() { + return LimelightHelpers.getFiducialID(LIMELIGHT_NAME); + } + + public boolean hasTarget() { + return tv > 0; + } + + public boolean isAligned() { + return hasTarget() && Math.abs(tx) <= VISION_ANGLE_TOLERANCE && targetCount >= 7; + } + +} \ No newline at end of file