From 20b464f500be0611dcd4cdb8d918e7ba4cc3efd3 Mon Sep 17 00:00:00 2001 From: Seqi Date: Mon, 26 Jan 2026 16:12:44 -0600 Subject: [PATCH 01/80] Update LimelightHelpers.java --- src/main/java/frc/lib/LimelightHelpers.java | 3442 ++++++++++--------- 1 file changed, 1798 insertions(+), 1644 deletions(-) diff --git a/src/main/java/frc/lib/LimelightHelpers.java b/src/main/java/frc/lib/LimelightHelpers.java index e761213..af932c3 100644 --- a/src/main/java/frc/lib/LimelightHelpers.java +++ b/src/main/java/frc/lib/LimelightHelpers.java @@ -1,1645 +1,1799 @@ -//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) - -package frc.lib; - -import edu.wpi.first.networktables.DoubleArrayEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -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.Map; -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; -import java.util.concurrent.ConcurrentHashMap; - -/** - * 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() { - } - } - - /** - * 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("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("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - 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]; - 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]; - - } - - - } - - /** - * 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; - } - } - - /** - * 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; - } - - } - - /** - * 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 (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 null; // or some default 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); - - RawFiducial[] rawFiducials = new RawFiducial[tagCount]; - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } else { - for(int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int)poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate(pose, 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; - } - - /** - * 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]); - } - - - 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; - } - ///// - ///// - - /** - * 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[10]; - } - 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[11]; - } - 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"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - 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 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); - } - - /** - * Sets the 3D point-of-interest offset for the current fiducial pipeline. - * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking - * - * @param limelightName Name/identifier of the Limelight - * @param x X offset in meters - * @param y Y offset in meters - * @param z Z offset in meters - */ - public static void SetFidcuial3DOffset(String limelightName, double x, double y, - double z) { - - double[] entries = new double[3]; - entries[0] = x; - entries[1] = y; - entries[2] = z; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * 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"); - } - - ///// - ///// - - /** - * 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; - } - - /** - * 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 { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } 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; - } +//LimelightHelpers v1.13 (REQUIRES LLOS 2026.0 OR LATER) + +package frc.lib; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +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() { + } + } + + /** + * 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("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("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + 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]; + 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]; + + } + + + } + + /** + * 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 null; // or some default 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]); + } + + + 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; + } + ///// + ///// + + /** + * 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 { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } 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 From 0270239907adae08ed5b713edf737bf98e2cf494 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 26 Jan 2026 18:51:39 -0600 Subject: [PATCH 02/80] start to code auto align --- .../java/frc/robot/Command/AutoAlign.java | 56 +++++++++---------- 1 file changed, 25 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Command/AutoAlign.java b/src/main/java/frc/robot/Command/AutoAlign.java index e781ae3..90b39a4 100644 --- a/src/main/java/frc/robot/Command/AutoAlign.java +++ b/src/main/java/frc/robot/Command/AutoAlign.java @@ -10,43 +10,37 @@ import frc.robot.Subsystems.SwerveSubsystem; public class AutoAlign extends Command { + + private SwerveSubsystem m_SwerveSubsystem; + public AutoAlign(SwerveSubsystem SwerveSubsystem){ + this.m_SwerveSubsystem = SwerveSubsystem; + addRequirements(m_SwerveSubsystem); + } - private PIDController xController = new PIDController(0.8, 0, 0); - private PIDController yController = new PIDController(0.8, 0, 0); - private PIDController rotationController = new PIDController(0.005, 0, 0); - private boolean isLeft; - - private ChassisSpeeds getAutoAlignSpeed (Pose2d CurrentPosition, Translation2d ReefCenter, double DistanceFromReef, double BranchDistancefromMiddle){ - if (isLeft){ - BranchDistancefromMiddle *= -1; - } - Translation2d OffSet = CurrentPosition.getTranslation().minus(ReefCenter); - double goalAngle = Math.round((OffSet.getAngle().getDegrees())/60) * 60; - Rotation2d goalRotation = Rotation2d.fromDegrees(goalAngle); - Translation2d scoringLocation = new Translation2d(DistanceFromReef, BranchDistancefromMiddle); - scoringLocation = scoringLocation.rotateBy(goalRotation); - scoringLocation = scoringLocation.plus(ReefCenter); - - double xOutput = xController.calculate(CurrentPosition.getX(), scoringLocation.getX()); - double yOutput = yController.calculate(CurrentPosition.getY(), scoringLocation.getY()); - double rotationOutput = rotationController.calculate(CurrentPosition.getRotation().getDegrees(), goalAngle + 180); - return new ChassisSpeeds(xOutput, yOutput, rotationOutput); - } - public void execute(){ - ChassisSpeeds ssppeeeedd = getAutoAlignSpeed(m_SwerveSubsystem.getPose(), FieldConstants.flipForAlliance(FieldConstants.BLUE_REEF_CENTER), - 1.55, 0.2); - m_SwerveSubsystem.driveFromChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds(ssppeeeedd, m_SwerveSubsystem.getPose().getRotation()), true); + + @Override + public void initialize(){ + } - public AutoAlign(SwerveSubsystem drive, boolean Left){ - m_SwerveSubsystem = drive; - isLeft = Left; - rotationController.enableContinuousInput (-180, 180); - addRequirements(m_SwerveSubsystem); + + @Override + public void execute(){ + Pose2d FieldPosition = m_SwerveSubsystem.getPose(); //Get robot position on field, in variable + Translation2d HubLocation = new Translation2d(12, 13); //Hub location -> CHANGE + } -} + @Override + public void end(boolean interrupted){ + } + @Override + public boolean isFinished(){ + return false; + } + +} From 3ab3a8c32fb79cf1b2684a526ca6a33fea7bb2df Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 28 Jan 2026 19:58:52 -0600 Subject: [PATCH 03/80] Coded auto align, also commented for most of them. Need correction with orbit tuning constans later. --- .../java/frc/robot/Command/AutoAlign.java | 101 +++++++++++++++++- 1 file changed, 97 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Command/AutoAlign.java b/src/main/java/frc/robot/Command/AutoAlign.java index 90b39a4..34e126c 100644 --- a/src/main/java/frc/robot/Command/AutoAlign.java +++ b/src/main/java/frc/robot/Command/AutoAlign.java @@ -1,5 +1,6 @@ package frc.robot.Command; +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; @@ -7,40 +8,132 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; public class AutoAlign extends Command { - private SwerveSubsystem m_SwerveSubsystem; + private boolean m_orbitLeft = true; + //true = counterclockwise arc, false = clockwise arc + + + //Orbit tuning constants (NEED CHANGE - kDesiredOrbitRadiusMeters, kTangentialSpeedMetersPerSecond) + private static final double kDesiredOrbitRadiusMeters = 3.5; //How far from the hub we want the robot to be + private static final double kTangentialSpeedMetersPerSecond = 1.25; // Constant speed for sliding around the hub + private static final double kMaxRadialSpeedMetersPerSecond = 1.0; // Max speed for correcting radius errors + private static final double kRadialKp = 1.6; //P-gain for radial distance correction + private static final double kHeadingKp = 4.5; //P-gain for yaw control that faces the hub + + //PID that holds the robot's yaw pointed at the hub while driving the arc + private final PIDController m_headingController = new PIDController(kHeadingKp,0,0); + public AutoAlign(SwerveSubsystem SwerveSubsystem){ this.m_SwerveSubsystem = SwerveSubsystem; addRequirements(m_SwerveSubsystem); + m_headingController.enableContinuousInput(-Math.PI, Math.PI); + // Continuous input so heading errors are around ±π + } + + public AutoAlign(SwerveSubsystem SwerveSubsystem, boolean orbitLeft){ + this(SwerveSubsystem); //reuse the constructor for setup + this.m_orbitLeft = orbitLeft; } @Override public void initialize(){ - + m_headingController.reset(); //Reset yaw PID state every time the command starts } @Override public void execute(){ - Pose2d FieldPosition = m_SwerveSubsystem.getPose(); //Get robot position on field, in variable - Translation2d HubLocation = new Translation2d(12, 13); //Hub location -> CHANGE + Pose2d FieldPosition = m_SwerveSubsystem.getPose(); //Get robot position on field + + Translation2d HubLocation = new Translation2d(4.61,4.03); //Hub location + HubLocation = FieldConstants.flipForAlliance(HubLocation); //Mirror the hub point when we are Red + + Translation2d robotToHub = HubLocation.minus(FieldPosition.getTranslation()); //Vector pointing at hub ??? + double radialDistance = robotToHub.getNorm(); + /*translation2d that points from the robot to the hub + * getNorm() returns the vector's magnitude (length) + * this line computes how far the robot currently is from the hub + */ + + // Stop driving if odometry is incorrect + if (radialDistance < 0.05){ + m_SwerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(), true); + return; + } + + Translation2d radialDirection = robotToHub.div(radialDistance); //Unit vector that always points toward the hub + Translation2d tangentialDirection = new Translation2d(-radialDirection.getY(), radialDirection.getX()); + //Radial vector rotated 90 degrees counterclockwise + + if(!m_orbitLeft){ + tangentialDirection = tangentialDirection.times(-1); + //flip the tangent so we can orbit clockwise when needed + } + + double radiusError = radialDistance - kDesiredOrbitRadiusMeters; //Positive -> slid too far away + double radialSpeed = MathUtil.clamp( + radiusError * kRadialKp, + -kMaxRadialSpeedMetersPerSecond, + kMaxRadialSpeedMetersPerSecond); + //P loop to correct the radius + + + Translation2d tangentialVelocity = tangentialDirection.times(kTangentialSpeedMetersPerSecond); + // Constant arc speed + Translation2d radialVelocity = radialDirection.times(radialSpeed); + // Radius correction + Translation2d fieldRelativeVelocity = tangentialVelocity.plus(radialVelocity); + //Motion wanted in field coordinates + + + double speedMagnitude = fieldRelativeVelocity.getNorm(); // Total requested speed + if(speedMagnitude > SwerveConstants.maxSpeed){ + fieldRelativeVelocity = + fieldRelativeVelocity.times(SwerveConstants.maxSpeed / speedMagnitude); + // respect drivetrain max velocity + } + + + double desiredHeadingRadians = radialDirection.getAngle().getRadians(); + //Face stright at the hub while moving + double headingRate = MathUtil.clamp( + m_headingController.calculate((FieldPosition.getRotation().getRadians()), desiredHeadingRadians), + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + // Yaw PID output limited to drivetrain capabilities + + + ChassisSpeeds requestedSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeVelocity.getX(), + fieldRelativeVelocity.getY(), + headingRate, + FieldPosition.getRotation()); + // Convert into chassis-relative speeds + + + m_SwerveSubsystem.driveFromChassisSpeeds(requestedSpeeds, false); + // Command the swerve in closed loop } @Override public void end(boolean interrupted){ + m_SwerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(), true); + // Stop the drivetrain } @Override public boolean isFinished(){ return false; + // Driver holds the trigger to stay in auto align } } From fd480be0f77d0c42956c0ea38c4dbec0269ecefc Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 30 Jan 2026 13:17:39 -0600 Subject: [PATCH 04/80] Add PID controller for correct radius & feedforward with angular velocity --- src/main/java/frc/robot/Command/AutoAlign.java | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Command/AutoAlign.java b/src/main/java/frc/robot/Command/AutoAlign.java index 34e126c..864cef6 100644 --- a/src/main/java/frc/robot/Command/AutoAlign.java +++ b/src/main/java/frc/robot/Command/AutoAlign.java @@ -3,7 +3,6 @@ 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.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; @@ -23,15 +22,19 @@ public class AutoAlign extends Command { private static final double kTangentialSpeedMetersPerSecond = 1.25; // Constant speed for sliding around the hub private static final double kMaxRadialSpeedMetersPerSecond = 1.0; // Max speed for correcting radius errors private static final double kRadialKp = 1.6; //P-gain for radial distance correction + private static final double kRadialKi = 0.0; + private static final double kRadialKd = 0.0; private static final double kHeadingKp = 4.5; //P-gain for yaw control that faces the hub //PID that holds the robot's yaw pointed at the hub while driving the arc private final PIDController m_headingController = new PIDController(kHeadingKp,0,0); + private final PIDController m_radiusController = new PIDController(kRadialKp, kRadialKi, kRadialKd); public AutoAlign(SwerveSubsystem SwerveSubsystem){ this.m_SwerveSubsystem = SwerveSubsystem; addRequirements(m_SwerveSubsystem); m_headingController.enableContinuousInput(-Math.PI, Math.PI); + m_radiusController.setSetpoint(kDesiredOrbitRadiusMeters); // Continuous input so heading errors are around ±π } @@ -45,6 +48,7 @@ public AutoAlign(SwerveSubsystem SwerveSubsystem, boolean orbitLeft){ @Override public void initialize(){ m_headingController.reset(); //Reset yaw PID state every time the command starts + m_radiusController.reset(); } @@ -77,12 +81,12 @@ public void execute(){ //flip the tangent so we can orbit clockwise when needed } - double radiusError = radialDistance - kDesiredOrbitRadiusMeters; //Positive -> slid too far away + double radialPidOutput = m_radiusController.calculate(radialDistance); double radialSpeed = MathUtil.clamp( - radiusError * kRadialKp, + -radialPidOutput, -kMaxRadialSpeedMetersPerSecond, kMaxRadialSpeedMetersPerSecond); - //P loop to correct the radius + //PID loop to correct the radius (negative because radialDirection points toward the hub) Translation2d tangentialVelocity = tangentialDirection.times(kTangentialSpeedMetersPerSecond); @@ -103,7 +107,13 @@ public void execute(){ double desiredHeadingRadians = radialDirection.getAngle().getRadians(); //Face stright at the hub while moving + double headingFeedforward = 0.0; + if (radialDistance > 1e-3){ + headingFeedforward = (radialDirection.getY()*fieldRelativeVelocity.getX() + - radialDirection.getX() * fieldRelativeVelocity.getY()) / radialDistance; + } double headingRate = MathUtil.clamp( + headingFeedforward + m_headingController.calculate((FieldPosition.getRotation().getRadians()), desiredHeadingRadians), -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); From 6da7d15acb06d4c717a2b5f13bad7237a08c7cbb Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 30 Jan 2026 21:56:00 -0600 Subject: [PATCH 05/80] Fixed the swervemodules --- src/main/java/frc/robot/Constants.java | 8 ++-- .../frc/robot/Subsystems/SwerveSubsystem.java | 25 ++++++++++- src/main/java/frc/robot/SwerveModule.java | 42 ++++++++++++++++++- 3 files changed, 68 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4a5f2f2..ef9a7f1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,10 +111,10 @@ public record ModuleData( ){} public static ModuleData[] moduleData = { - new ModuleData(11, 14, 19, 162.07, BACK_RIGHT), //Mod 0 Back right - new ModuleData(17, 18, 22, 231.86, FRONT_RIGHT), //Mod 1 Front right - new ModuleData(15, 16, 21, 310.52, FRONT_LEFT), //Mod 2 Front left - new ModuleData(13, 12, 20, 298.48, BACK_LEFT) //Mod 3 Back left + new ModuleData(11, 52, 19, 340.32, BACK_RIGHT), //Mod 0 Back right + new ModuleData(17, 53, 22, 51.59, FRONT_RIGHT), //Mod 1 Front right + new ModuleData(15, 16, 21, 130.16, FRONT_LEFT), //Mod 2 Front left + new ModuleData(13, 12, 20, 118.47, BACK_LEFT) //Mod 3 Back left }; } diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 9fa5ddf..53582ff 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -180,6 +181,28 @@ public Rotation2d getYaw() { : Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); } + public void resyncModuleEncoders(){ + for (SwerveModule mod : mSwerveMods){ + mod.resyncToAbsolute(); + } + } + + public void saveModuleOffsets(){ + saveModuleOffsets(new Rotation2d()); + } + + public void saveModuleOffsets(Rotation2d desiredAngle){ + if(!DriverStation.isDisabled()){ + DriverStation.reportWarning( + "Attempted to save swerve module offsets while robot is enabled. Disable before calibrating.", + false); + return; + } + for (SwerveModule mod : mSwerveMods){ + mod.saveCanCoderOffset(desiredAngle); + } + } + @Override @@ -202,4 +225,4 @@ public void periodic() { swerveDataPublisher.set(getStates()); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index 4513565..e848cca 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -25,6 +25,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Preferences; import frc.lib.CANSparkUtil; import frc.lib.CANSparkUtil.Usage; import frc.robot.Constants.SwerveConstants; @@ -40,6 +41,7 @@ public class SwerveModule { private Rotation2d lastAngle; private Rotation2d angleOffset; + private final String angleOffsetPreferenceKey; private SparkMax angleMotor; private SparkFlex driveMotor; @@ -73,12 +75,15 @@ public class SwerveModule { */ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ this.moduleNumber = moduleNumber; + this.angleOffsetPreferenceKey = "Swerve/Module" + moduleNumber + "/AngleOffsetDegrees"; this.m_angleKP = SwerveConstants.angleKP; this.m_angleKI = SwerveConstants.angleKI; this.m_angleKD = SwerveConstants.angleKD; // Calibration offset to align absolute encoder zero with module zero position. - angleOffset = Rotation2d.fromDegrees(moduleConstants.angleOffset()); + double storedOffset = + Preferences.getDouble(angleOffsetPreferenceKey, moduleConstants.angleOffset()); + angleOffset = Rotation2d.fromDegrees(normalizeDegrees(storedOffset)); /* Angle Encoder Configuration * The CANcoder is an absolute encoder that provides the module's angle even after power loss. @@ -402,6 +407,39 @@ private void resetToAbsolute() { } + //Re-synchronizes the integrated encoder with the CANcoder using the stored offset. + public void resyncToAbsolute(){ + resetToAbsolute(); + } + + /*Saves the current CANcoder reading as the zero reference for this module. + Run this while the wheels are physically pointing straight to capture the + correct offset and persist it in WPILib Preferences. + */ + public void saveCanCoderZero(){ + saveCanCoderOffset(Rotation2d.fromDegrees(0.0)); + } + + /** + * Saves the current CANcoder reading as a reference for a desired heading. + * @param desiredAngle The field-relative angle that the wheel is currently aiming at. + */ + public void saveCanCoderOffset(Rotation2d desiredAngle){ + double absolute = getCanCoder().getDegrees(); + double newOffset = normalizeDegrees(absolute - desiredAngle.getDegrees()); + Preferences.setDouble(angleOffsetPreferenceKey, newOffset); + angleOffset = Rotation2d.fromDegrees(newOffset); + resetToAbsolute(); + } + + private double normalizeDegrees(double degrees){ + double normalized = degrees % 360.0; + if (normalized < 0){ + normalized += 360.0; + } + return normalized; + } + /** * Configures the drive motor (SparkFlex) with all necessary settings for velocity control. * Called once during module initialization in the constructor. Configures current limits, @@ -438,4 +476,4 @@ private void configDriveMotor(){ // Reset encoder position to zero (sets starting position for odometry) driveEncoder.setPosition(0.0); } -} \ No newline at end of file +} From 0b3e9c1a14d645eddc6d589e458022ae98918d1a Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 31 Jan 2026 17:17:38 -0600 Subject: [PATCH 06/80] rezeroed --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ef9a7f1..d514ac9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,10 +111,10 @@ public record ModuleData( ){} public static ModuleData[] moduleData = { - new ModuleData(11, 52, 19, 340.32, BACK_RIGHT), //Mod 0 Back right - new ModuleData(17, 53, 22, 51.59, FRONT_RIGHT), //Mod 1 Front right - new ModuleData(15, 16, 21, 130.16, FRONT_LEFT), //Mod 2 Front left - new ModuleData(13, 12, 20, 118.47, BACK_LEFT) //Mod 3 Back left + new ModuleData(11, 52, 19, 159.25, BACK_RIGHT), //Mod 0 Back right + new ModuleData(17, 53, 22, 230.89, FRONT_RIGHT), //Mod 1 Front right + new ModuleData(15, 16, 21, 311.67, FRONT_LEFT), //Mod 2 Front left + new ModuleData(13, 12, 20, 297.69, BACK_LEFT) //Mod 3 Back left }; } From 7b57335b67b482d5a7199ef8cdbaa601b70f924d Mon Sep 17 00:00:00 2001 From: Seqi Date: Mon, 2 Feb 2026 15:37:35 -0600 Subject: [PATCH 07/80] Added SmartDashboard "New Offset" --- src/main/java/frc/robot/Subsystems/SwerveSubsystem.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 53582ff..a73d76d 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -215,12 +215,18 @@ public void periodic() { SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble()); for (SwerveModule mod : mSwerveMods) { + + double canCoderDegrees = mod.getCanCoder().getDegrees(); + SmartDashboard.putNumber( "Mod " + mod.moduleNumber + " Cancoder", mod.getCanCoder().getDegrees()); SmartDashboard.putNumber( "Mod " + mod.moduleNumber + " Integrated", mod.getState().angle.getDegrees()); SmartDashboard.putNumber( "Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); + SmartDashboard.putNumber( + "Mod " + mod.moduleNumber + " New Offset", + canCoderDegrees < 0 ? 360 + canCoderDegrees : canCoderDegrees); } swerveDataPublisher.set(getStates()); } From 1871628f27394d05031fd877d02d39ed1cf3599d Mon Sep 17 00:00:00 2001 From: Seqi Date: Mon, 2 Feb 2026 16:40:34 -0600 Subject: [PATCH 08/80] rename --- src/main/java/frc/robot/Subsystems/SwerveSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index a73d76d..0599c90 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -225,7 +225,7 @@ public void periodic() { SmartDashboard.putNumber( "Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); SmartDashboard.putNumber( - "Mod " + mod.moduleNumber + " New Offset", + "Mod " + mod.moduleNumber + " New Cancoder Offset", canCoderDegrees < 0 ? 360 + canCoderDegrees : canCoderDegrees); } swerveDataPublisher.set(getStates()); From 8b5fc7cad233c25ae09bc6462af09e13cb0e0365 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 2 Feb 2026 16:46:38 -0600 Subject: [PATCH 09/80] Adjust distance away from hub --- src/main/java/frc/robot/Command/AutoAlign.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Command/AutoAlign.java b/src/main/java/frc/robot/Command/AutoAlign.java index 864cef6..7905290 100644 --- a/src/main/java/frc/robot/Command/AutoAlign.java +++ b/src/main/java/frc/robot/Command/AutoAlign.java @@ -18,7 +18,7 @@ public class AutoAlign extends Command { //Orbit tuning constants (NEED CHANGE - kDesiredOrbitRadiusMeters, kTangentialSpeedMetersPerSecond) - private static final double kDesiredOrbitRadiusMeters = 3.5; //How far from the hub we want the robot to be + private static final double kDesiredOrbitRadiusMeters = 2.22; //How far from the hub we want the robot to be private static final double kTangentialSpeedMetersPerSecond = 1.25; // Constant speed for sliding around the hub private static final double kMaxRadialSpeedMetersPerSecond = 1.0; // Max speed for correcting radius errors private static final double kRadialKp = 1.6; //P-gain for radial distance correction From a05ec4276ba0ceb63dc20ce610b4a7940837626c Mon Sep 17 00:00:00 2001 From: Seqi Date: Wed, 4 Feb 2026 16:50:32 -0600 Subject: [PATCH 10/80] Re-Zero --- ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Pigeon 2 - 023 - 0 - ext.dat | Bin 0 -> 2048 bytes src/main/java/frc/robot/Constants.java | 8 ++++---- 6 files changed, 4 insertions(+), 4 deletions(-) create mode 100644 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat create mode 100644 ctre_sim/Pigeon 2 - 023 - 0 - ext.dat diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..2bbdfc967067bac8f73d741a6b04f5ec05ebf026 GIT binary patch literal 2048 zcmdnU;Qyat6pV(zXb6mkz-S1JhQMeD49XB-Mt9YlWhIP2v5Lg_E)b0l6wu^rfeMh# IaDd7K0G)meegFUf literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..28822ad2c78934e69bef571755c5c2e6aa5868cc GIT binary patch literal 2048 zcmdnU;Qyat2!TxuenZN=LnvrRojMuwCpz;8k C& Date: Fri, 6 Feb 2026 17:05:01 -0600 Subject: [PATCH 11/80] Update translation2d. --- src/main/java/frc/robot/Constants.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f129b38..b9d708c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,10 +46,10 @@ public static final class SwerveConstants{ public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( - new Translation2d(-halfTrackWidth, halfWheelBase), //Back Right - new Translation2d(halfTrackWidth,halfWheelBase), // Front Right - new Translation2d(halfTrackWidth,-halfWheelBase), // Front Left - new Translation2d(-halfTrackWidth,-halfWheelBase)); // Back Left + new Translation2d(-halfTrackWidth, -halfWheelBase), //Back Right + new Translation2d(halfTrackWidth,-halfWheelBase), // Front Right + new Translation2d(halfTrackWidth, halfWheelBase), // Front Left + new Translation2d(-halfTrackWidth, halfWheelBase)); // Back Left //translation 2d locates the swerve module in cords //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html //SwerveDrive Kinematics converts between a ChassisSpeeds object and several SwerveModuleState objects, @@ -67,7 +67,7 @@ public static final class SwerveConstants{ /* Drive Motor PID Values */ public static final double driveKP = 0.1; //to tune public static final double driveKI = 0.0; //to tune - public static final double driveKD = 0.0; //to tune + public static final double driveKD = 0.0; //to tune /* Drive Motor Characterization Values */ //values to calculate the drive feedforward (KFF) @@ -100,10 +100,10 @@ public static final class SwerveConstants{ public static final boolean angleInvert = true; //Location of modules - public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, halfTrackWidth); - public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, halfTrackWidth); - public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, -halfTrackWidth); - public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, -halfTrackWidth); + public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); + public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); + public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); + public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); /* Module Specific Constants */ public record ModuleData( From fca86046a305ebcff75851403f60715b3963a750 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Feb 2026 14:17:51 -0600 Subject: [PATCH 12/80] fixed some comments --- .../java/frc/robot/Command/AutoAlign.java | 6 ++++-- .../java/frc/robot/Command/TeleopSwerve.java | 21 ++++++++++++------- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Main.java | 3 +++ .../frc/robot/Subsystems/SwerveSubsystem.java | 2 ++ 5 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Command/AutoAlign.java b/src/main/java/frc/robot/Command/AutoAlign.java index 7905290..664eb12 100644 --- a/src/main/java/frc/robot/Command/AutoAlign.java +++ b/src/main/java/frc/robot/Command/AutoAlign.java @@ -10,6 +10,8 @@ import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; +/* Drives the robot in an orbit around the hub while continuously facing the hub center */ + public class AutoAlign extends Command { private SwerveSubsystem m_SwerveSubsystem; @@ -59,7 +61,7 @@ public void execute(){ Translation2d HubLocation = new Translation2d(4.61,4.03); //Hub location HubLocation = FieldConstants.flipForAlliance(HubLocation); //Mirror the hub point when we are Red - Translation2d robotToHub = HubLocation.minus(FieldPosition.getTranslation()); //Vector pointing at hub ??? + Translation2d robotToHub = HubLocation.minus(FieldPosition.getTranslation()); //Vector from robot to hub. double radialDistance = robotToHub.getNorm(); /*translation2d that points from the robot to the hub * getNorm() returns the vector's magnitude (length) @@ -106,7 +108,7 @@ public void execute(){ double desiredHeadingRadians = radialDirection.getAngle().getRadians(); - //Face stright at the hub while moving + //Face straight at the hub while moving double headingFeedforward = 0.0; if (radialDistance > 1e-3){ headingFeedforward = (radialDirection.getY()*fieldRelativeVelocity.getX() diff --git a/src/main/java/frc/robot/Command/TeleopSwerve.java b/src/main/java/frc/robot/Command/TeleopSwerve.java index d45c504..c614e99 100644 --- a/src/main/java/frc/robot/Command/TeleopSwerve.java +++ b/src/main/java/frc/robot/Command/TeleopSwerve.java @@ -14,6 +14,8 @@ import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; +/* Default drive command for field-centric manual swerve control */ + public class TeleopSwerve extends Command { private SwerveSubsystem m_SwerveSubsystem; private DoubleSupplier m_translationSupplier; @@ -21,17 +23,20 @@ public class TeleopSwerve extends Command { private DoubleSupplier m_rotationSupplier; private BooleanSupplier m_robotCentricSupplier; - private SlewRateLimiter translationLimiter = new SlewRateLimiter(3.0); //can only change by 3 m/s in the span of 1 s + + //Limit acceleration to smooth driver inputs and reduce wheel slip + private SlewRateLimiter translationLimiter = new SlewRateLimiter(3.0); private SlewRateLimiter strafeLimiter = new SlewRateLimiter(3.0); private SlewRateLimiter rotationLimiter = new SlewRateLimiter(3.0); - /** Creates a new TeleopSwerve. */ + /** Creates a new TeleopSwerve command */ public TeleopSwerve(SwerveSubsystem SwerveSubsystem, DoubleSupplier translationSupplier, DoubleSupplier strafeSupplier, DoubleSupplier rotationSupplier, BooleanSupplier robotCentricSupplier, BooleanSupplier isAutoAlignSupplier) { - // Use addRequirements() here to declare subsystem dependencies. + + // Declare the swerve subsystem requirement so this is the active default drive command. this.m_SwerveSubsystem = SwerveSubsystem; addRequirements(m_SwerveSubsystem); this.m_translationSupplier = translationSupplier; @@ -48,7 +53,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - /* Get Values, applies Deadband, (doesnt do anything if stick is less than a value)*/ + /* Read joystick values, apply deadband, and slew-limit for smooth control*/ double xVal = translationLimiter.calculate( MathUtil.applyDeadband(m_translationSupplier.getAsDouble(), SwerveConstants.inputDeadband)); @@ -63,13 +68,13 @@ public void execute() { invert = -1; } - /* Drive */ + /* Command closed-loop swerve drive */ m_SwerveSubsystem.drive( - //the joystick values (-1 to 1) multiplied by the max speed of the drivetrain + // Scale joystick tranlation (-1 to 1) to real drivetrain speed. xVal * SwerveConstants.maxSpeed * invert, yVal * SwerveConstants.maxSpeed * invert, - //rotation value times max spin speed + //Scale joystick rotation (-1 to 1) to max angular velocity rotationVal * SwerveConstants.maxAngularVelocity, - //whether or not in field centric mode + //Drive field-relative unless robot-centric mode is requested. !m_robotCentricSupplier.getAsBoolean()); } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b9d708c..2c09395 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -/** Add your docs here. */ +/** Central location for robot-wide constants grouped by subsystem and feature */ public final class Constants { diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fe215d7..5d2bf51 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -6,9 +6,12 @@ import edu.wpi.first.wpilibj.RobotBase; +//Program entry point that launches the WPILib robot framework public final class Main { + //Prevents instantiation of this utility class private Main() {} + // Starts the robot by creating a {@link Robot} instance. public static void main(String... args) { RobotBase.startRobot(Robot::new); } diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 0599c90..59ea94d 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -28,6 +28,8 @@ import frc.robot.Constants.SwerveConstants.ModuleData; import frc.robot.SwerveModule; + +// Manges swerve drivetrain hardware, odometry, and vision-assisted pose up dates. public class SwerveSubsystem extends SubsystemBase { private final Pigeon2 pigeon; From 43c5d240ff2f58e63dc532346196b5e18a0fb581 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 14 Feb 2026 13:52:34 -0600 Subject: [PATCH 13/80] update ids to the Rebuilt drivebase values. --- src/main/java/frc/robot/Constants.java | 34 +++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 8 +++++ .../frc/robot/Subsystems/SwerveSubsystem.java | 8 ++++- 3 files changed, 34 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2c09395..2f71373 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,30 +26,34 @@ public final class Constants { // Swerve Constants public static final class SwerveConstants{ public static final double inputDeadband = .1; // Deadzone for joystick inputs to prevent drift - public static final int PIGEON_ID = 23; //CAN ID for Pigeon gyro sensor + public static final int PIGEON_ID = 17; //CAN ID for Pigeon gyro sensor public static final boolean invertPigeon = false; // Whether to invert gyro readings /* Drivetrain Constants */ - public static final double halfTrackWidth = Units.inchesToMeters(28/2.0);//to find - public static final double halfWheelBase = Units.inchesToMeters(28/2.0);//to find + public static final double halfTrackWidth = Units.inchesToMeters(27/2.0);//to find + public static final double halfWheelBase = Units.inchesToMeters(27/2.0);//to find public static final double wheelDiameter = Units.inchesToMeters(4.0); public static final double wheelCircumference = wheelDiameter * Math.PI; - public static final double driveBaseRadius = Math.hypot(halfTrackWidth/2, halfWheelBase/2); + //halfTrackWidth/halfwheelBase are already "half" distances, so don't divide again. + //public static final double driveBaseRadius = Math.hypot(halfTrackWidth/2, halfWheelBase/2); + public static final double driveBaseRadius = Math.hypot(halfWheelBase, halfTrackWidth); + public static final double openLoopRamp = 0.25; public static final double closedLoopRamp = 0.0; - public static final double driveGearRatio = (8.14 / 1.0); // 6.75:1 L2 Mk4 Modules + public static final double driveGearRatio = (6.75 / 1.0); // 6.75:1 L2 Mk4 Modules //L1 is 8.14:1, L2 is 6.75:1, L3 is 6.12:1, L4 is 5.14:1 public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1 MK4 SDS Modules //SDS Mk4 is 12.8:1, Mk4i is 21.4:1 public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( - new Translation2d(-halfTrackWidth, -halfWheelBase), //Back Right - new Translation2d(halfTrackWidth,-halfWheelBase), // Front Right - new Translation2d(halfTrackWidth, halfWheelBase), // Front Left - new Translation2d(-halfTrackWidth, halfWheelBase)); // Back Left + //WPILib coordinate system: +X = forward, +Y = left + new Translation2d(halfTrackWidth, halfWheelBase), //Front left + new Translation2d(halfTrackWidth, -halfWheelBase), //Front right + new Translation2d(-halfTrackWidth, -halfWheelBase), //Back right + new Translation2d(-halfTrackWidth, halfWheelBase)); //Back Left //translation 2d locates the swerve module in cords //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html //SwerveDrive Kinematics converts between a ChassisSpeeds object and several SwerveModuleState objects, @@ -100,9 +104,9 @@ public static final class SwerveConstants{ public static final boolean angleInvert = true; //Location of modules - public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); - public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); + public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); + public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); /* Module Specific Constants */ @@ -111,10 +115,10 @@ public record ModuleData( ){} public static ModuleData[] moduleData = { - new ModuleData(11, 52, 19, 159.34, BACK_RIGHT), //Mod 0 Back right - new ModuleData(17, 53, 22, 233.71, FRONT_RIGHT), //Mod 1 Front right - new ModuleData(15, 16, 21, 312.63, FRONT_LEFT), //Mod 2 Front left - new ModuleData(13, 12, 20, 298.75, BACK_LEFT) //Mod 3 Back left + new ModuleData(6, 5, 7, 31.46, FRONT_LEFT), //Mod 0 Front left + new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT), //Mod 1 Front right + new ModuleData(12, 11, 13, 33.13, BACK_RIGHT), //Mod 2 Back right + new ModuleData(15, 14, 16, 8.52, BACK_LEFT) //Mod 3 Back left }; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 06fa2f6..b71c890 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,6 +49,14 @@ private void configureBindings() { // Y Button = Zero gyro (reset heading to 0° or 180° based on alliance) driveController.button(Button.kY.value).onTrue(new InstantCommand(() -> m_drive.zeroGyro(), m_drive)); + + //Back button (view) = resync integrated angle encoders to CANcoders (DISABLED ONLY) + driveController.button(Button.kBack.value).onTrue(new InstantCommand(()->m_drive.resyncModuleEncoders(), m_drive)); + //Start Button (menu) = save current module offsets (DISABLED ONLY, wheels must be straight) + driveController.button(Button.kStart.value).onTrue(new InstantCommand(()->m_drive.saveModuleOffsets(), m_drive)); + + + // Left Trigger = Auto-align to left scoring position driveController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, true)); // Right Trigger = Auto-align to right scoring position diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 59ea94d..7aab9b4 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -126,7 +126,7 @@ public void driveFromChassisSpeeds(ChassisSpeeds driveSpeeds, boolean isOpenLoop desiredSwerveDataPublisher.set(desiredStates); for (SwerveModule mod : mSwerveMods) { - mod.setDesiredState(desiredStates[mod.moduleNumber], false); + mod.setDesiredState(desiredStates[mod.moduleNumber], isOpenLoop); //NEED CONFIRM } } @@ -184,6 +184,12 @@ public Rotation2d getYaw() { } public void resyncModuleEncoders(){ + if(!DriverStation.isDisabled()){ + DriverStation.reportWarning + ("Attempted to resync swerve module encoders while robot is enabled. Disable before resyncing", + false); //NEED CONFIRM + return; + } for (SwerveModule mod : mSwerveMods){ mod.resyncToAbsolute(); } From 6cc9bded1ba6e1aee6e172ee855446e26a906f5b Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 14 Feb 2026 14:07:09 -0600 Subject: [PATCH 14/80] Update Constants.java --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2f71373..d6bc260 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -44,7 +44,7 @@ public static final class SwerveConstants{ public static final double driveGearRatio = (6.75 / 1.0); // 6.75:1 L2 Mk4 Modules //L1 is 8.14:1, L2 is 6.75:1, L3 is 6.12:1, L4 is 5.14:1 - public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1 MK4 SDS Modules + public static final double angleGearRatio = (21.4 / 1.0); // 21.4:1 MK4i Modules //SDS Mk4 is 12.8:1, Mk4i is 21.4:1 public static final SwerveDriveKinematics swerveKinematics = From a6b3ecbfaae5a51960d683711add3548fe2a1bd6 Mon Sep 17 00:00:00 2001 From: Nonochen0104 <157759781+Nonochen0104@users.noreply.github.com> Date: Sat, 14 Feb 2026 14:42:30 -0600 Subject: [PATCH 15/80] Autonomous (#16) * Test Reject on Compile Error (#7) * test reject merge to main * test reject merge with compile error * fix compile error - pull request test * Shooter Subsystem * Shooter Subsystem * Kinda buggy light subsystem + Blinking lights uhh nothing really else * Constant motor speed * Slight organization * X config + More optimized code * Lights When Shooting + Shooting Lights + Light Control * ID Switch * Controll Changes and Feeder Addition * Minor Feeder Bot Changes * Moving IDS to constant * !!!(NEEDS TESTING)!!! Robot motor code updated maybe * Attempted fixes (not tested) * Configured the Motors * removed line 21 * made a boolean public * working shooter and feeder code shooter is a toggle feeder works when held * Remove lightSubsystem.java - moved to light-subsystem branch --------- Co-authored-by: AlexHuie Co-authored-by: Zachray <145231265+ZachrayL@users.noreply.github.com> Co-authored-by: jmmaloney3 * Auto align (#9) * Update LimelightHelpers.java * AutoAlign coded with placeholder distance * Coded auto align, also commented for most of them. Need correction with orbit tuning constans later. * Add PID controller for correct radius & feedforward with angular velocity * Fixed the swervemodules --------- Co-authored-by: Seqi * Pathplanner lib and basic auto structure (no commands) * errors but commit so other people can get code * Intake Toggle Code (#8) * intake code does not work yet * updated intake code changed control to toggle w/ x button on hems controller roborio connection problem can build but can't deploy * updated pid value for test board * toggle intake code * started intake arm code doesn't work yet * added intake arm code needs PID tuning and testing * arm code with setpoints moves to the setpoints but oscillates around them nees tuning * updated PID values (in progress) * Changed intake button to two separate ones, A for lowering the intake arm, Y for raising the intake arm on helms controller * adjust intake * updated controls * fixed intake arm (working), the motor is really strong but it's fine --------- Co-authored-by: Nonochen0104 * shooter subsystem (#10) * Shooter Subsystem * Kinda buggy light subsystem + Blinking lights uhh nothing really else * Constant motor speed * Slight organization * X config + More optimized code * Lights When Shooting + Shooting Lights + Light Control * ID Switch * Controll Changes and Feeder Addition * Minor Feeder Bot Changes * Moving IDS to constant * !!!(NEEDS TESTING)!!! Robot motor code updated maybe * Attempted fixes (not tested) * Configured the Motors * removed line 21 * made a boolean public * working shooter and feeder code shooter is a toggle feeder works when held * Remove lightSubsystem.java - moved to light-subsystem branch * Added hood and controllers for all shooter related parts * Fixed all the shooter parts, everything is working (shooter, feeder, hood) * Deleted repeated helms controller definition --------- Co-authored-by: FireheadReal Co-authored-by: AlexHuie Co-authored-by: jmmaloney3 Co-authored-by: Nonochen0104 Co-authored-by: Gavin * Fix compile error in startAutoAt() method - Complete the incomplete if statement - Implement proper odometry reset with alliance-aware coordinate flipping - Add missing Translation2d import - Method now properly resets robot pose to starting position for autonomous * fix compile error by completing startAutoAt method * Theoretically working auto (probably not) * partial autobuilder configuration code * finished autoBuilder config needs testing and review * changed build.gradle * Added pathplaner folder with eight lemon path * added depot auto(s) * add leave auto --------- Co-authored-by: jmmaloney3 Co-authored-by: FireheadReal Co-authored-by: AlexHuie Co-authored-by: Zachray <145231265+ZachrayL@users.noreply.github.com> Co-authored-by: jmmaloney3 Co-authored-by: Seqi --- .vscode/settings.json | 3 + build.gradle | 2 +- src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Eight Lemon Path.path | 54 ++++++ .../paths/depot-score (close).path | 54 ++++++ .../pathplanner/paths/depot-score (far).path | 54 ++++++ .../pathplanner/paths/intake from depot.path | 68 ++++++++ .../java/frc/robot/Auto/EightLemonAuto.java | 21 +++ src/main/java/frc/robot/Auto/LeaveAuto.java | 19 +++ src/main/java/frc/robot/Constants.java | 53 +++++- src/main/java/frc/robot/RobotContainer.java | 65 +++++-- .../frc/robot/Subsystems/IntakeSubsystem.java | 109 ++++++++++++ .../robot/Subsystems/ShooterSubsystem.java | 161 +++++++++++++++++- .../frc/robot/Subsystems/SwerveSubsystem.java | 106 +++++++++++- vendordeps/PathplannerLib.json | 38 +++++ 15 files changed, 783 insertions(+), 25 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Eight Lemon Path.path create mode 100644 src/main/deploy/pathplanner/paths/depot-score (close).path create mode 100644 src/main/deploy/pathplanner/paths/depot-score (far).path create mode 100644 src/main/deploy/pathplanner/paths/intake from depot.path create mode 100644 src/main/java/frc/robot/Auto/EightLemonAuto.java create mode 100644 src/main/java/frc/robot/Auto/LeaveAuto.java create mode 100644 src/main/java/frc/robot/Subsystems/IntakeSubsystem.java create mode 100644 vendordeps/PathplannerLib.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..c5f3f6b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "interactive" +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index e9b0020..c4acfc3 100644 --- a/build.gradle +++ b/build.gradle @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..ac5f521 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Eight Lemon Path.path b/src/main/deploy/pathplanner/paths/Eight Lemon Path.path new file mode 100644 index 0000000..6b319f7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Eight Lemon Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3157941905816637, + "y": 7.009728970050077 + }, + "prevControl": null, + "nextControl": { + "x": 4.315794190581666, + "y": 7.009728970050077 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8625764999036978, + "y": 6.373905286979969 + }, + "prevControl": { + "x": 0.8625764999036978, + "y": 6.373905286979969 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot-score (close).path b/src/main/deploy/pathplanner/paths/depot-score (close).path new file mode 100644 index 0000000..fa78ec5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot-score (close).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.2106347656250005, + "y": 5.95 + }, + "prevControl": null, + "nextControl": { + "x": 2.0998173828125, + "y": 6.015255283453525 + }, + "isLocked": false, + "linkedName": "intaked from depot" + }, + { + "anchor": { + "x": 3.2500775615985575, + "y": 4.8839239658453515 + }, + "prevControl": { + "x": 2.2500775615985575, + "y": 4.8839239658453515 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -32.11102661984112 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 1.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot-score (far).path b/src/main/deploy/pathplanner/paths/depot-score (far).path new file mode 100644 index 0000000..c07b4ae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot-score (far).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.2106347656250005, + "y": 5.95 + }, + "prevControl": null, + "nextControl": { + "x": 2.099817382812501, + "y": 6.015255283453525 + }, + "isLocked": true, + "linkedName": "intaked from depot" + }, + { + "anchor": { + "x": 2.9268750723379635, + "y": 6.679432508680556 + }, + "prevControl": { + "x": 1.9268750723379635, + "y": 6.679432508680556 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -57.304779755348484 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 1.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/intake from depot.path b/src/main/deploy/pathplanner/paths/intake from depot.path new file mode 100644 index 0000000..7995501 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/intake from depot.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4474162516126834, + "y": 5.95 + }, + "prevControl": null, + "nextControl": { + "x": 0.7506240448181309, + "y": 5.95 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2106347656250005, + "y": 5.95 + }, + "prevControl": { + "x": -0.2074593080750322, + "y": 5.933539163129617 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "intaked from depot" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/EightLemonAuto.java b/src/main/java/frc/robot/Auto/EightLemonAuto.java new file mode 100644 index 0000000..7625b76 --- /dev/null +++ b/src/main/java/frc/robot/Auto/EightLemonAuto.java @@ -0,0 +1,21 @@ +package frc.robot.Auto; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class EightLemonAuto extends SequentialCommandGroup{ + public EightLemonAuto (SwerveSubsystem drive, ShooterSubsystem shoot) { + addCommands( + drive.startAutoAt(3, 7.276, 180), //placeholder numbers from wherever we start auto + drive.autoDrive("Eight Lemon Path"), + shoot.autoShoot(), + new WaitCommand(3.0), + shoot.autoFeed(), + new WaitCommand(3.0), + shoot.autoStopFeed() + ); + } + +} diff --git a/src/main/java/frc/robot/Auto/LeaveAuto.java b/src/main/java/frc/robot/Auto/LeaveAuto.java new file mode 100644 index 0000000..d491026 --- /dev/null +++ b/src/main/java/frc/robot/Auto/LeaveAuto.java @@ -0,0 +1,19 @@ +package frc.robot.Auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import frc.robot.Subsystems.SwerveSubsystem; + +public class LeaveAuto extends SequentialCommandGroup{ + public LeaveAuto (SwerveSubsystem drive) { + addCommands( + new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive) + ); + } + + +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d6bc260..f24dbeb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,6 +15,8 @@ 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.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** Central location for robot-wide constants grouped by subsystem and feature */ public final class Constants { @@ -123,6 +125,53 @@ public record ModuleData( } + public class AutoConstants { + public enum AutoMode{ + EightLemonAuto, //go to band and shoot 8 lemon + LeaveAuto //leaves + } + + //create elastic chooser + // private static SendableChooser positionChooser = new SendableChooser(); + // needs to be reworked when there is not a set starting position + private static SendableChooser autoModeChooser = new SendableChooser(); + + static { + //send chooser to elastic + SmartDashboard.putData("Auto Chooser", autoModeChooser); + } + } + + /* Shooter Constants */ + public class ShooterConstants { + public static final int SHOOTER_ID = 60; //Placeholder ID + public static final int FEEDER_ID = 61; //Feeder ID + public static final int HOOD_ID = 62; //Hood ID (NEED CHANGE) + + public static final double SHOOTER_SPEED = 0.5; //Placeholder speed + public static final double FEEDER_SPEED = 0.5; + + public static final double HOOD_ANGLE_LOW = 0.0; + public static final double HOOD_ANGLE_HIGH = 0.5; + public static final double HOOD_KP = 1.2; + public static final double HOOD_MAX_OUTPUT = 0.4; + public static final double HOOD_TOLERANCE = 0.02; + } + public class IntakeConstants { + public static int INTAKE_ID = 60; // placeholder + public static double INTAKE_SPEED = 50; //placeholder for percent power for intake + + public static int INTAKE_ARM_ID = 62; //placeholder + public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later + public static double INTAKE_ARM_LOWERED_POSITION = 0; + public static double INTAKE_ARM_MINIMUM = 0; // placeholders + public static double INTAKE_ARM_MAXIMUM = 90; + public static int GEAR_RATIO = 3; + + public static double INTAKE_ARM_kP = 0.01; + public static double INTAKE_ARM_kI = 0; + public static double INTAKE_ARM_kD = 0; + } public class FieldConstants { public static final double FIELD_LENGTH = 17.54824934; @@ -154,6 +203,6 @@ public static Translation2d flipForAlliance(Translation2d pos){ public static Pose2d flipForAlliance(Pose2d pose){ return new Pose2d(flipForAlliance(pose.getTranslation()), flipForAlliance(pose.getRotation())); } + } - -} \ 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 b71c890..c735e87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,19 +8,28 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.ShooterConstants; import frc.robot.Command.AutoAlign; import frc.robot.Command.TeleopSwerve; +import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; public class RobotContainer { - + + // Xbox controller configuration for drive controls private final CommandXboxController driveController = new CommandXboxController(0); + // Xbox controller configuration for helms controls + private final CommandXboxController helmsController = new CommandXboxController(1); + // Left Stick Y = Forward/backward motion private final int translationAxis = XboxController.Axis.kLeftY.value; // Left Stick X = Side-to-side motion @@ -33,6 +42,11 @@ public class RobotContainer { // SwerveSubsystem instance for the drive subsystem private final SwerveSubsystem m_drive = new SwerveSubsystem(); + // IntakeSubsystem for intake + private final IntakeSubsystem m_intake = new IntakeSubsystem(); + + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + /** * Constructs the RobotContainer. Creates subsystems (which configure themselves) * and sets up command bindings to map controller inputs to commands. @@ -66,23 +80,41 @@ private void configureBindings() { // It automatically pauses when commands like AutoAlign take control, then resumes // when they finish. m_drive.setDefaultCommand( - new TeleopSwerve( - // SwerveSubsystem - The drive subsystem to control - m_drive, - // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.5, - // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.5, - // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.5, - // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) - () -> robotCentric.getAsBoolean(), - // isAutoAlignSupplier - Auto-align active flag - () -> driveController.getRightTriggerAxis() > 0.1 - )); - + new TeleopSwerve( + // SwerveSubsystem - The drive subsystem to control + m_drive, + // translationSupplier - Forward/backward speed + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.5, + // strafeSupplier - Side-to-side speed + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.5, + // rotationSupplier - Rotation speed + () -> -driveController.getRawAxis(rotationAxis) * 0.5, + // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) + () -> robotCentric.getAsBoolean(), + // isAutoAlignSupplier - Auto-align active flag + () -> driveController.getRightTriggerAxis() > 0.1 + )); + + //INTAKE + // raises the intake using the A button on the helms controller + m_intake.setDefaultCommand( + new RunCommand( + () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), + m_intake)); + + + //lowers the intake using the A button on the helms controller + helmsController.button(Button.kA.value).onTrue( + new InstantCommand(() -> m_intake.raiseIntake(), m_intake) + ); + + // lowers the intake using the X button on the helms controller + helmsController.button(Button.kX.value).onTrue( + new InstantCommand(() -> m_intake.lowerIntake(), m_intake) + ); } + /** * Determines if the driver has requested speed reduction for precise positioning * or delicate tasks. @@ -104,4 +136,5 @@ public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..57533e6 --- /dev/null +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -0,0 +1,109 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Subsystems; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.TunableControllers.TunableArmFeedforward; +import frc.robot.Constants.IntakeConstants; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +public class IntakeSubsystem extends SubsystemBase { + + private final SparkMax intakeMotor = new SparkMax(IntakeConstants.INTAKE_ID, MotorType.kBrushless); + private final SparkMax intakeArmMotor = new SparkMax(IntakeConstants.INTAKE_ARM_ID, MotorType.kBrushless); + + private RelativeEncoder intakeArmEncoder = intakeArmMotor.getEncoder(); + + private PIDController intakeArmPID = new PIDController(IntakeConstants.INTAKE_ARM_kP, IntakeConstants.INTAKE_ARM_kI, IntakeConstants.INTAKE_ARM_kD); + + private ArmFeedforward intakeArmFeedForward = new ArmFeedforward(0,0,0); + + public double targetPosition; + + private boolean intakeOn = false; + private boolean intakeUp = true; + + /** Creates a new IntakeSubsystem. */ + public IntakeSubsystem() { + SparkMaxConfig intakeConfig = new SparkMaxConfig(); + intakeConfig.inverted(false); + intakeConfig.idleMode(IdleMode.kCoast); + + intakeMotor.configure(intakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + SparkMaxConfig intakeArmConfig = new SparkMaxConfig(); + intakeArmConfig.inverted(false); + intakeArmConfig.idleMode(IdleMode.kBrake); + intakeArmConfig.encoder.positionConversionFactor(360/IntakeConstants.GEAR_RATIO); + + intakeArmMotor.configure(intakeArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + intakeArmEncoder.setPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); + targetPosition = IntakeConstants.INTAKE_ARM_RAISED_POSITION; // start with arm raised + } + + public void toggleIntake() { + if (!intakeOn) { + intakeOn = true; + intakeMotor.set(IntakeConstants.INTAKE_SPEED); + } + else { + intakeOn = false; + intakeMotor.set(0); + } + } + + public void setIntakePower(double power) { + double clampedPower = Math.max(-1.0, Math.min(1.0, power)); + intakeOn = Math.abs(clampedPower) > 0.0; + intakeMotor.set(clampedPower * IntakeConstants.INTAKE_SPEED); + } + + + public void setTargetPosition(double position) { + targetPosition = Math.max(IntakeConstants.INTAKE_ARM_MINIMUM, Math.min(IntakeConstants.INTAKE_ARM_MAXIMUM, position)); + } + + public void raiseIntake() { + setTargetPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); + intakeUp = true; + } + + public void lowerIntake() { + setTargetPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); + intakeUp = false; + } + + public void moveIntake() { + if (intakeUp){ + lowerIntake(); + } + else { + raiseIntake(); + } + } + + public double getArmPosition() { + return intakeArmEncoder.getPosition() * 360; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + double PIDOutput = intakeArmFeedForward.calculate( + Units.degreesToRadians(intakeArmEncoder.getPosition()),0) + + intakeArmPID.calculate(getArmPosition(), targetPosition); + intakeArmMotor.set(PIDOutput); + } +} diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index c761f83..5c7c37f 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -3,15 +3,168 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.Subsystems; - +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import frc.robot.Constants.ShooterConstants; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import frc.robot.Constants.ShooterConstants; public class ShooterSubsystem extends SubsystemBase { - /** Creates a new ShooterSubsystem. */ - public ShooterSubsystem() {} + + public boolean isShooterActive = false; //Shooter True + + SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); + SparkMax feederMotor = new SparkMax(ShooterConstants.FEEDER_ID, MotorType.kBrushless); + SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); + + private final PIDController hoodController = new PIDController( + ShooterConstants.HOOD_KP, + 0.0, + 0.0 + ); + private double hoodTargetPosition = ShooterConstants.HOOD_ANGLE_LOW; + private boolean hoodActive = false; + + public enum HoodAngle { + LOW, + HIGH + } + + //LightSubsystem m_lightSubsystem = new LightSubsystem(); + + public ShooterSubsystem() { + SparkMaxConfig shootConfig = new SparkMaxConfig(); + shootConfig.inverted(false); + shootConfig.idleMode(IdleMode.kCoast); + + SparkMaxConfig feedConfig = new SparkMaxConfig(); + feedConfig.inverted(false); + feedConfig.idleMode(IdleMode.kBrake); + + SparkMaxConfig hoodConfig = new SparkMaxConfig(); + hoodConfig.inverted(false); + hoodConfig.idleMode(IdleMode.kBrake); + + shooterMotor.configure(shootConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + feederMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + hoodMotor.configure(hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + hoodController.setTolerance(ShooterConstants.HOOD_TOLERANCE); + } + + public void toggleShooter() { + if (!isShooterActive) { + isShooterActive = true; + shooterMotor.set(ShooterConstants.SHOOTER_SPEED); + } + else { + isShooterActive = false; + shooterMotor.set(0); + } + } + + + public void runShooter(boolean shooterOn) { + if (shooterOn) { + isShooterActive = true; + shooterMotor.set(ShooterConstants.SHOOTER_SPEED); + } else { + isShooterActive = false; + shooterMotor.set(0); + } + } + + + public void runFeeder(boolean feederOn){ + runFeederSpeed(feederOn ? ShooterConstants.FEEDER_SPEED : 0); + } + + public void runFeederSpeed(double speed) { + feederMotor.set(speed); + } + + public void setHoodAngle(HoodAngle angle) { + switch (angle) { + case LOW: + hoodTargetPosition = ShooterConstants.HOOD_ANGLE_LOW; + break; + case HIGH: + hoodTargetPosition = ShooterConstants.HOOD_ANGLE_HIGH; + break; + default: + hoodTargetPosition = ShooterConstants.HOOD_ANGLE_HIGH; + } + hoodController.reset(); + hoodActive = true; + } + + public double getHoodPosition() { + return hoodMotor.getEncoder().getPosition(); + } + + public Command autoShoot() { + return new InstantCommand(() -> { + if (!isShooterActive) { + isShooterActive = true; + shooterMotor.set(ShooterConstants.SHOOTER_SPEED); + } + else { + isShooterActive = false; + shooterMotor.set(0); + } + }, this); + } + + public Command autoFeed() { + return new InstantCommand(() -> runFeeder(true), this); + } + + public Command autoStopFeed() { + return new InstantCommand(() -> runFeeder(false), this); + } + @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putBoolean("Is Shooter Active", isShooterActive); + SmartDashboard.putNumber("Hood Target Position", hoodTargetPosition); + SmartDashboard.putNumber("Hood Position", getHoodPosition()); + + + if (hoodActive) { + double output = hoodController.calculate(getHoodPosition(), hoodTargetPosition); + output = Math.max(-ShooterConstants.HOOD_MAX_OUTPUT, Math.min(ShooterConstants.HOOD_MAX_OUTPUT, output)); + + if (hoodController.atSetpoint()) { + hoodMotor.set(0); + hoodActive = false; + } else { + hoodMotor.set(output); + } + } else { + hoodMotor.set(0); + } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 7aab9b4..f4f9bf7 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -3,14 +3,18 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.Subsystems; - import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; - +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -20,9 +24,11 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.LimelightHelpers; import frc.robot.Constants; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.SwerveConstants.ModuleData; @@ -78,9 +84,94 @@ public SwerveSubsystem() { //puts out the field field = new Field2d(); SmartDashboard.putData("Field", field); + + RobotConfig autoConfig; + try { + autoConfig = RobotConfig.fromGUISettings(); + } + catch (Exception e) { + e.printStackTrace(); + autoConfig = null; + } + + AutoBuilder.configure( + this::getPose, + this::resetOdometry, + this::getChassisSpeeds, + (speeds, feedforwards) -> driveFromChassisSpeeds(speeds, false), + new PPHolonomicDriveController( + new PIDConstants(SwerveConstants.driveKP, SwerveConstants.driveKI, SwerveConstants.driveKD), + new PIDConstants(SwerveConstants.angleKP, SwerveConstants.angleKI, SwerveConstants.angleKD) + ), + autoConfig, + () -> { + return FieldConstants.isRedAlliance(); + }, + this + ); } + + public Command autoDrive(String filename){ + try{ + PathPlannerPath path = PathPlannerPath.fromPathFile(filename); + return AutoBuilder.followPath(path); + } + catch (Exception e) { //exception e: see what the error was + DriverStation.reportError("Pathplanner Error: "+ e.getMessage(), e.getStackTrace()); + return null; + } + } + /** + * Creates a command that resets the robot's odometry to a specified starting position and orientation. + * + *

This method is used at the beginning of autonomous routines to tell the robot where it is + * physically located on the field. It does NOT move the robot - it only updates the software's + * position estimate (odometry). + * + *

IMPORTANT: The robot must be physically placed at the specified position before + * this command is executed. If the physical position doesn't match the coordinates passed to + * this method, autonomous paths will be incorrect and the robot may drive to wrong locations. + * + *

The method automatically handles alliance-aware coordinate flipping. If the robot is on the + * red alliance, the coordinates and rotation are automatically mirrored to account for field + * symmetry. + * + *

This command should typically be the first command in an autonomous sequence, before any + * path-following commands. + * + * @param x The X coordinate of the starting position in meters (field coordinates) + * @param y The Y coordinate of the starting position in meters (field coordinates) + * @param direction The starting heading in degrees (0° = east/right, 90° = north/up, 180° = west/left, 270° = south/down) + * @return A command that resets odometry to the specified pose when executed + * + *

Example usage: + * 

{@code
+   * // Robot is physically placed at (7.13, 7.276) facing 180° (south)
+   * // Then in autonomous command sequence:
+   * addCommands(
+   *     drive.startAutoAt(7.13, 7.276, 180),  // Reset odometry to match physical position
+   *     drive.autoDrive("MyPath")             // Follow path from this starting position
+   * );
+   * }
+ */ + public Command startAutoAt(double x, double y, double direction) { + return runOnce(() -> { + // Create starting position and rotation + Translation2d startPos = new Translation2d(x, y); + Rotation2d startRotation = Rotation2d.fromDegrees(direction); + + // Apply alliance flip if on red side (field symmetry) + Pose2d startPose = new Pose2d( + FieldConstants.flipForAlliance(startPos), + FieldConstants.flipForAlliance(startRotation) + ); + + // Reset odometry to the starting position + resetOdometry(startPose); + }); + } private void updateOdometryWithVision (String limelightName){ boolean doRejectUpdate = false; @@ -212,6 +303,17 @@ public void saveModuleOffsets(Rotation2d desiredAngle){ } + public void saveModuleOffsets(Rotation2d desiredAngle){ + if(!DriverStation.isDisabled()){ + DriverStation.reportWarning( + "Attempted to save swerve module offsets while robot is enabled. Disable before calibrating.", + false); + return; + } + for (SwerveModule mod : mSwerveMods){ + mod.saveCanCoderOffset(desiredAngle); + } + } @Override public void periodic() { diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..4414707 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2026.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2026", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2026.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2026.1.2", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file From 24fa677671c69cec359b91763238fe708a9264ee Mon Sep 17 00:00:00 2001 From: Seqi Date: Sat, 14 Feb 2026 14:49:33 -0600 Subject: [PATCH 16/80] fixed mistake in merge conflict --- .../java/frc/robot/Subsystems/SwerveSubsystem.java | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index f4f9bf7..7f53aa2 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -290,19 +290,6 @@ public void saveModuleOffsets(){ saveModuleOffsets(new Rotation2d()); } - public void saveModuleOffsets(Rotation2d desiredAngle){ - if(!DriverStation.isDisabled()){ - DriverStation.reportWarning( - "Attempted to save swerve module offsets while robot is enabled. Disable before calibrating.", - false); - return; - } - for (SwerveModule mod : mSwerveMods){ - mod.saveCanCoderOffset(desiredAngle); - } - } - - public void saveModuleOffsets(Rotation2d desiredAngle){ if(!DriverStation.isDisabled()){ DriverStation.reportWarning( From a2810d9362bf2037b7a71ae179a6e9b00cc88677 Mon Sep 17 00:00:00 2001 From: Seqi Date: Sat, 14 Feb 2026 15:40:22 -0600 Subject: [PATCH 17/80] Revert "fixed mistake in merge conflict" This reverts commit 24fa677671c69cec359b91763238fe708a9264ee. --- .../java/frc/robot/Subsystems/SwerveSubsystem.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 7f53aa2..f4f9bf7 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -290,6 +290,19 @@ public void saveModuleOffsets(){ saveModuleOffsets(new Rotation2d()); } + public void saveModuleOffsets(Rotation2d desiredAngle){ + if(!DriverStation.isDisabled()){ + DriverStation.reportWarning( + "Attempted to save swerve module offsets while robot is enabled. Disable before calibrating.", + false); + return; + } + for (SwerveModule mod : mSwerveMods){ + mod.saveCanCoderOffset(desiredAngle); + } + } + + public void saveModuleOffsets(Rotation2d desiredAngle){ if(!DriverStation.isDisabled()){ DriverStation.reportWarning( From 1e40a3eb601ff379ef23bf5e9a32165fbf2e0ca2 Mon Sep 17 00:00:00 2001 From: Seqi Date: Sat, 14 Feb 2026 15:40:35 -0600 Subject: [PATCH 18/80] Revert "Autonomous (#16)" This reverts commit a6b3ecbfaae5a51960d683711add3548fe2a1bd6. --- .vscode/settings.json | 3 - build.gradle | 2 +- src/main/deploy/pathplanner/navgrid.json | 1 - .../pathplanner/paths/Eight Lemon Path.path | 54 ------ .../paths/depot-score (close).path | 54 ------ .../pathplanner/paths/depot-score (far).path | 54 ------ .../pathplanner/paths/intake from depot.path | 68 -------- .../java/frc/robot/Auto/EightLemonAuto.java | 21 --- src/main/java/frc/robot/Auto/LeaveAuto.java | 19 --- src/main/java/frc/robot/Constants.java | 53 +----- src/main/java/frc/robot/RobotContainer.java | 65 ++----- .../frc/robot/Subsystems/IntakeSubsystem.java | 109 ------------ .../robot/Subsystems/ShooterSubsystem.java | 161 +----------------- .../frc/robot/Subsystems/SwerveSubsystem.java | 106 +----------- vendordeps/PathplannerLib.json | 38 ----- 15 files changed, 25 insertions(+), 783 deletions(-) delete mode 100644 .vscode/settings.json delete mode 100644 src/main/deploy/pathplanner/navgrid.json delete mode 100644 src/main/deploy/pathplanner/paths/Eight Lemon Path.path delete mode 100644 src/main/deploy/pathplanner/paths/depot-score (close).path delete mode 100644 src/main/deploy/pathplanner/paths/depot-score (far).path delete mode 100644 src/main/deploy/pathplanner/paths/intake from depot.path delete mode 100644 src/main/java/frc/robot/Auto/EightLemonAuto.java delete mode 100644 src/main/java/frc/robot/Auto/LeaveAuto.java delete mode 100644 src/main/java/frc/robot/Subsystems/IntakeSubsystem.java delete mode 100644 vendordeps/PathplannerLib.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index c5f3f6b..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "java.configuration.updateBuildConfiguration": "interactive" -} \ No newline at end of file diff --git a/build.gradle b/build.gradle index c4acfc3..e9b0020 100644 --- a/build.gradle +++ b/build.gradle @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = true // Change to true to delete files on roboRIO that no + deleteOldFiles = false // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json deleted file mode 100644 index ac5f521..0000000 --- a/src/main/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Eight Lemon Path.path b/src/main/deploy/pathplanner/paths/Eight Lemon Path.path deleted file mode 100644 index 6b319f7..0000000 --- a/src/main/deploy/pathplanner/paths/Eight Lemon Path.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.3157941905816637, - "y": 7.009728970050077 - }, - "prevControl": null, - "nextControl": { - "x": 4.315794190581666, - "y": 7.009728970050077 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8625764999036978, - "y": 6.373905286979969 - }, - "prevControl": { - "x": 0.8625764999036978, - "y": 6.373905286979969 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot-score (close).path b/src/main/deploy/pathplanner/paths/depot-score (close).path deleted file mode 100644 index fa78ec5..0000000 --- a/src/main/deploy/pathplanner/paths/depot-score (close).path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2106347656250005, - "y": 5.95 - }, - "prevControl": null, - "nextControl": { - "x": 2.0998173828125, - "y": 6.015255283453525 - }, - "isLocked": false, - "linkedName": "intaked from depot" - }, - { - "anchor": { - "x": 3.2500775615985575, - "y": 4.8839239658453515 - }, - "prevControl": { - "x": 2.2500775615985575, - "y": 4.8839239658453515 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -32.11102661984112 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 1.0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot-score (far).path b/src/main/deploy/pathplanner/paths/depot-score (far).path deleted file mode 100644 index c07b4ae..0000000 --- a/src/main/deploy/pathplanner/paths/depot-score (far).path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2106347656250005, - "y": 5.95 - }, - "prevControl": null, - "nextControl": { - "x": 2.099817382812501, - "y": 6.015255283453525 - }, - "isLocked": true, - "linkedName": "intaked from depot" - }, - { - "anchor": { - "x": 2.9268750723379635, - "y": 6.679432508680556 - }, - "prevControl": { - "x": 1.9268750723379635, - "y": 6.679432508680556 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -57.304779755348484 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 1.0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/intake from depot.path b/src/main/deploy/pathplanner/paths/intake from depot.path deleted file mode 100644 index 7995501..0000000 --- a/src/main/deploy/pathplanner/paths/intake from depot.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4474162516126834, - "y": 5.95 - }, - "prevControl": null, - "nextControl": { - "x": 0.7506240448181309, - "y": 5.95 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.2106347656250005, - "y": 5.95 - }, - "prevControl": { - "x": -0.2074593080750322, - "y": 5.933539163129617 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "intaked from depot" - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 1.0, - "rotation": 90.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/EightLemonAuto.java b/src/main/java/frc/robot/Auto/EightLemonAuto.java deleted file mode 100644 index 7625b76..0000000 --- a/src/main/java/frc/robot/Auto/EightLemonAuto.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.Auto; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Subsystems.ShooterSubsystem; -import frc.robot.Subsystems.SwerveSubsystem; - -public class EightLemonAuto extends SequentialCommandGroup{ - public EightLemonAuto (SwerveSubsystem drive, ShooterSubsystem shoot) { - addCommands( - drive.startAutoAt(3, 7.276, 180), //placeholder numbers from wherever we start auto - drive.autoDrive("Eight Lemon Path"), - shoot.autoShoot(), - new WaitCommand(3.0), - shoot.autoFeed(), - new WaitCommand(3.0), - shoot.autoStopFeed() - ); - } - -} diff --git a/src/main/java/frc/robot/Auto/LeaveAuto.java b/src/main/java/frc/robot/Auto/LeaveAuto.java deleted file mode 100644 index d491026..0000000 --- a/src/main/java/frc/robot/Auto/LeaveAuto.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot.Auto; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -import frc.robot.Subsystems.SwerveSubsystem; - -public class LeaveAuto extends SequentialCommandGroup{ - public LeaveAuto (SwerveSubsystem drive) { - addCommands( - new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive) - ); - } - - -} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f24dbeb..d6bc260 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,8 +15,6 @@ 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.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** Central location for robot-wide constants grouped by subsystem and feature */ public final class Constants { @@ -125,53 +123,6 @@ public record ModuleData( } - public class AutoConstants { - public enum AutoMode{ - EightLemonAuto, //go to band and shoot 8 lemon - LeaveAuto //leaves - } - - //create elastic chooser - // private static SendableChooser positionChooser = new SendableChooser(); - // needs to be reworked when there is not a set starting position - private static SendableChooser autoModeChooser = new SendableChooser(); - - static { - //send chooser to elastic - SmartDashboard.putData("Auto Chooser", autoModeChooser); - } - } - - /* Shooter Constants */ - public class ShooterConstants { - public static final int SHOOTER_ID = 60; //Placeholder ID - public static final int FEEDER_ID = 61; //Feeder ID - public static final int HOOD_ID = 62; //Hood ID (NEED CHANGE) - - public static final double SHOOTER_SPEED = 0.5; //Placeholder speed - public static final double FEEDER_SPEED = 0.5; - - public static final double HOOD_ANGLE_LOW = 0.0; - public static final double HOOD_ANGLE_HIGH = 0.5; - public static final double HOOD_KP = 1.2; - public static final double HOOD_MAX_OUTPUT = 0.4; - public static final double HOOD_TOLERANCE = 0.02; - } - public class IntakeConstants { - public static int INTAKE_ID = 60; // placeholder - public static double INTAKE_SPEED = 50; //placeholder for percent power for intake - - public static int INTAKE_ARM_ID = 62; //placeholder - public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later - public static double INTAKE_ARM_LOWERED_POSITION = 0; - public static double INTAKE_ARM_MINIMUM = 0; // placeholders - public static double INTAKE_ARM_MAXIMUM = 90; - public static int GEAR_RATIO = 3; - - public static double INTAKE_ARM_kP = 0.01; - public static double INTAKE_ARM_kI = 0; - public static double INTAKE_ARM_kD = 0; - } public class FieldConstants { public static final double FIELD_LENGTH = 17.54824934; @@ -203,6 +154,6 @@ public static Translation2d flipForAlliance(Translation2d pos){ public static Pose2d flipForAlliance(Pose2d pose){ return new Pose2d(flipForAlliance(pose.getTranslation()), flipForAlliance(pose.getRotation())); } - } -} + +} \ 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 c735e87..b71c890 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,28 +8,19 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.ShooterConstants; import frc.robot.Command.AutoAlign; import frc.robot.Command.TeleopSwerve; -import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -import frc.robot.Subsystems.ShooterSubsystem; public class RobotContainer { - - + // Xbox controller configuration for drive controls private final CommandXboxController driveController = new CommandXboxController(0); - // Xbox controller configuration for helms controls - private final CommandXboxController helmsController = new CommandXboxController(1); - // Left Stick Y = Forward/backward motion private final int translationAxis = XboxController.Axis.kLeftY.value; // Left Stick X = Side-to-side motion @@ -42,11 +33,6 @@ public class RobotContainer { // SwerveSubsystem instance for the drive subsystem private final SwerveSubsystem m_drive = new SwerveSubsystem(); - // IntakeSubsystem for intake - private final IntakeSubsystem m_intake = new IntakeSubsystem(); - - private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - /** * Constructs the RobotContainer. Creates subsystems (which configure themselves) * and sets up command bindings to map controller inputs to commands. @@ -80,40 +66,22 @@ private void configureBindings() { // It automatically pauses when commands like AutoAlign take control, then resumes // when they finish. m_drive.setDefaultCommand( - new TeleopSwerve( - // SwerveSubsystem - The drive subsystem to control - m_drive, - // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.5, - // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.5, - // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.5, - // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) - () -> robotCentric.getAsBoolean(), - // isAutoAlignSupplier - Auto-align active flag - () -> driveController.getRightTriggerAxis() > 0.1 - )); - - //INTAKE - // raises the intake using the A button on the helms controller - m_intake.setDefaultCommand( - new RunCommand( - () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), - m_intake)); - - - //lowers the intake using the A button on the helms controller - helmsController.button(Button.kA.value).onTrue( - new InstantCommand(() -> m_intake.raiseIntake(), m_intake) - ); - - // lowers the intake using the X button on the helms controller - helmsController.button(Button.kX.value).onTrue( - new InstantCommand(() -> m_intake.lowerIntake(), m_intake) - ); - } + new TeleopSwerve( + // SwerveSubsystem - The drive subsystem to control + m_drive, + // translationSupplier - Forward/backward speed + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.5, + // strafeSupplier - Side-to-side speed + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.5, + // rotationSupplier - Rotation speed + () -> -driveController.getRawAxis(rotationAxis) * 0.5, + // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) + () -> robotCentric.getAsBoolean(), + // isAutoAlignSupplier - Auto-align active flag + () -> driveController.getRightTriggerAxis() > 0.1 + )); + } /** * Determines if the driver has requested speed reduction for precise positioning @@ -136,5 +104,4 @@ public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java deleted file mode 100644 index 57533e6..0000000 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.Subsystems; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.TunableControllers.TunableArmFeedforward; -import frc.robot.Constants.IntakeConstants; - -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; - -public class IntakeSubsystem extends SubsystemBase { - - private final SparkMax intakeMotor = new SparkMax(IntakeConstants.INTAKE_ID, MotorType.kBrushless); - private final SparkMax intakeArmMotor = new SparkMax(IntakeConstants.INTAKE_ARM_ID, MotorType.kBrushless); - - private RelativeEncoder intakeArmEncoder = intakeArmMotor.getEncoder(); - - private PIDController intakeArmPID = new PIDController(IntakeConstants.INTAKE_ARM_kP, IntakeConstants.INTAKE_ARM_kI, IntakeConstants.INTAKE_ARM_kD); - - private ArmFeedforward intakeArmFeedForward = new ArmFeedforward(0,0,0); - - public double targetPosition; - - private boolean intakeOn = false; - private boolean intakeUp = true; - - /** Creates a new IntakeSubsystem. */ - public IntakeSubsystem() { - SparkMaxConfig intakeConfig = new SparkMaxConfig(); - intakeConfig.inverted(false); - intakeConfig.idleMode(IdleMode.kCoast); - - intakeMotor.configure(intakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - - SparkMaxConfig intakeArmConfig = new SparkMaxConfig(); - intakeArmConfig.inverted(false); - intakeArmConfig.idleMode(IdleMode.kBrake); - intakeArmConfig.encoder.positionConversionFactor(360/IntakeConstants.GEAR_RATIO); - - intakeArmMotor.configure(intakeArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - intakeArmEncoder.setPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); - targetPosition = IntakeConstants.INTAKE_ARM_RAISED_POSITION; // start with arm raised - } - - public void toggleIntake() { - if (!intakeOn) { - intakeOn = true; - intakeMotor.set(IntakeConstants.INTAKE_SPEED); - } - else { - intakeOn = false; - intakeMotor.set(0); - } - } - - public void setIntakePower(double power) { - double clampedPower = Math.max(-1.0, Math.min(1.0, power)); - intakeOn = Math.abs(clampedPower) > 0.0; - intakeMotor.set(clampedPower * IntakeConstants.INTAKE_SPEED); - } - - - public void setTargetPosition(double position) { - targetPosition = Math.max(IntakeConstants.INTAKE_ARM_MINIMUM, Math.min(IntakeConstants.INTAKE_ARM_MAXIMUM, position)); - } - - public void raiseIntake() { - setTargetPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); - intakeUp = true; - } - - public void lowerIntake() { - setTargetPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); - intakeUp = false; - } - - public void moveIntake() { - if (intakeUp){ - lowerIntake(); - } - else { - raiseIntake(); - } - } - - public double getArmPosition() { - return intakeArmEncoder.getPosition() * 360; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - double PIDOutput = intakeArmFeedForward.calculate( - Units.degreesToRadians(intakeArmEncoder.getPosition()),0) - + intakeArmPID.calculate(getArmPosition(), targetPosition); - intakeArmMotor.set(PIDOutput); - } -} diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 5c7c37f..c761f83 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -3,168 +3,15 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.Subsystems; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import frc.robot.Constants.ShooterConstants; - -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import frc.robot.Constants.ShooterConstants; +import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { - - public boolean isShooterActive = false; //Shooter True - - SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); - SparkMax feederMotor = new SparkMax(ShooterConstants.FEEDER_ID, MotorType.kBrushless); - SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); - - private final PIDController hoodController = new PIDController( - ShooterConstants.HOOD_KP, - 0.0, - 0.0 - ); - private double hoodTargetPosition = ShooterConstants.HOOD_ANGLE_LOW; - private boolean hoodActive = false; - - public enum HoodAngle { - LOW, - HIGH - } - - //LightSubsystem m_lightSubsystem = new LightSubsystem(); - - public ShooterSubsystem() { - SparkMaxConfig shootConfig = new SparkMaxConfig(); - shootConfig.inverted(false); - shootConfig.idleMode(IdleMode.kCoast); - - SparkMaxConfig feedConfig = new SparkMaxConfig(); - feedConfig.inverted(false); - feedConfig.idleMode(IdleMode.kBrake); - - SparkMaxConfig hoodConfig = new SparkMaxConfig(); - hoodConfig.inverted(false); - hoodConfig.idleMode(IdleMode.kBrake); - - shooterMotor.configure(shootConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - feederMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - hoodMotor.configure(hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - - hoodController.setTolerance(ShooterConstants.HOOD_TOLERANCE); - } - - public void toggleShooter() { - if (!isShooterActive) { - isShooterActive = true; - shooterMotor.set(ShooterConstants.SHOOTER_SPEED); - } - else { - isShooterActive = false; - shooterMotor.set(0); - } - } - - - public void runShooter(boolean shooterOn) { - if (shooterOn) { - isShooterActive = true; - shooterMotor.set(ShooterConstants.SHOOTER_SPEED); - } else { - isShooterActive = false; - shooterMotor.set(0); - } - } - - - public void runFeeder(boolean feederOn){ - runFeederSpeed(feederOn ? ShooterConstants.FEEDER_SPEED : 0); - } - - public void runFeederSpeed(double speed) { - feederMotor.set(speed); - } - - public void setHoodAngle(HoodAngle angle) { - switch (angle) { - case LOW: - hoodTargetPosition = ShooterConstants.HOOD_ANGLE_LOW; - break; - case HIGH: - hoodTargetPosition = ShooterConstants.HOOD_ANGLE_HIGH; - break; - default: - hoodTargetPosition = ShooterConstants.HOOD_ANGLE_HIGH; - } - hoodController.reset(); - hoodActive = true; - } - - public double getHoodPosition() { - return hoodMotor.getEncoder().getPosition(); - } - - public Command autoShoot() { - return new InstantCommand(() -> { - if (!isShooterActive) { - isShooterActive = true; - shooterMotor.set(ShooterConstants.SHOOTER_SPEED); - } - else { - isShooterActive = false; - shooterMotor.set(0); - } - }, this); - } - - public Command autoFeed() { - return new InstantCommand(() -> runFeeder(true), this); - } - - public Command autoStopFeed() { - return new InstantCommand(() -> runFeeder(false), this); - } - + /** Creates a new ShooterSubsystem. */ + public ShooterSubsystem() {} @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putBoolean("Is Shooter Active", isShooterActive); - SmartDashboard.putNumber("Hood Target Position", hoodTargetPosition); - SmartDashboard.putNumber("Hood Position", getHoodPosition()); - - - if (hoodActive) { - double output = hoodController.calculate(getHoodPosition(), hoodTargetPosition); - output = Math.max(-ShooterConstants.HOOD_MAX_OUTPUT, Math.min(ShooterConstants.HOOD_MAX_OUTPUT, output)); - - if (hoodController.atSetpoint()) { - hoodMotor.set(0); - hoodActive = false; - } else { - hoodMotor.set(output); - } - } else { - hoodMotor.set(0); - } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index f4f9bf7..7aab9b4 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -3,18 +3,14 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.Subsystems; + import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathPlannerPath; + import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -24,11 +20,9 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.LimelightHelpers; import frc.robot.Constants; -import frc.robot.Constants.AutoConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.SwerveConstants.ModuleData; @@ -84,94 +78,9 @@ public SwerveSubsystem() { //puts out the field field = new Field2d(); SmartDashboard.putData("Field", field); - - RobotConfig autoConfig; - try { - autoConfig = RobotConfig.fromGUISettings(); - } - catch (Exception e) { - e.printStackTrace(); - autoConfig = null; - } - - AutoBuilder.configure( - this::getPose, - this::resetOdometry, - this::getChassisSpeeds, - (speeds, feedforwards) -> driveFromChassisSpeeds(speeds, false), - new PPHolonomicDriveController( - new PIDConstants(SwerveConstants.driveKP, SwerveConstants.driveKI, SwerveConstants.driveKD), - new PIDConstants(SwerveConstants.angleKP, SwerveConstants.angleKI, SwerveConstants.angleKD) - ), - autoConfig, - () -> { - return FieldConstants.isRedAlliance(); - }, - this - ); } - - public Command autoDrive(String filename){ - try{ - PathPlannerPath path = PathPlannerPath.fromPathFile(filename); - return AutoBuilder.followPath(path); - } - catch (Exception e) { //exception e: see what the error was - DriverStation.reportError("Pathplanner Error: "+ e.getMessage(), e.getStackTrace()); - return null; - } - } - /** - * Creates a command that resets the robot's odometry to a specified starting position and orientation. - * - *

This method is used at the beginning of autonomous routines to tell the robot where it is - * physically located on the field. It does NOT move the robot - it only updates the software's - * position estimate (odometry). - * - *

IMPORTANT: The robot must be physically placed at the specified position before - * this command is executed. If the physical position doesn't match the coordinates passed to - * this method, autonomous paths will be incorrect and the robot may drive to wrong locations. - * - *

The method automatically handles alliance-aware coordinate flipping. If the robot is on the - * red alliance, the coordinates and rotation are automatically mirrored to account for field - * symmetry. - * - *

This command should typically be the first command in an autonomous sequence, before any - * path-following commands. - * - * @param x The X coordinate of the starting position in meters (field coordinates) - * @param y The Y coordinate of the starting position in meters (field coordinates) - * @param direction The starting heading in degrees (0° = east/right, 90° = north/up, 180° = west/left, 270° = south/down) - * @return A command that resets odometry to the specified pose when executed - * - *

Example usage: - * 

{@code
-   * // Robot is physically placed at (7.13, 7.276) facing 180° (south)
-   * // Then in autonomous command sequence:
-   * addCommands(
-   *     drive.startAutoAt(7.13, 7.276, 180),  // Reset odometry to match physical position
-   *     drive.autoDrive("MyPath")             // Follow path from this starting position
-   * );
-   * }
- */ - public Command startAutoAt(double x, double y, double direction) { - return runOnce(() -> { - // Create starting position and rotation - Translation2d startPos = new Translation2d(x, y); - Rotation2d startRotation = Rotation2d.fromDegrees(direction); - - // Apply alliance flip if on red side (field symmetry) - Pose2d startPose = new Pose2d( - FieldConstants.flipForAlliance(startPos), - FieldConstants.flipForAlliance(startRotation) - ); - - // Reset odometry to the starting position - resetOdometry(startPose); - }); - } private void updateOdometryWithVision (String limelightName){ boolean doRejectUpdate = false; @@ -303,17 +212,6 @@ public void saveModuleOffsets(Rotation2d desiredAngle){ } - public void saveModuleOffsets(Rotation2d desiredAngle){ - if(!DriverStation.isDisabled()){ - DriverStation.reportWarning( - "Attempted to save swerve module offsets while robot is enabled. Disable before calibrating.", - false); - return; - } - for (SwerveModule mod : mSwerveMods){ - mod.saveCanCoderOffset(desiredAngle); - } - } @Override public void periodic() { diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json deleted file mode 100644 index 4414707..0000000 --- a/vendordeps/PathplannerLib.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2026.1.2", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2026", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2026.1.2" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2026.1.2", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file From 245f9095c86ef8e6fcc65b93d086ceb007746868 Mon Sep 17 00:00:00 2001 From: Gavin Date: Sat, 14 Feb 2026 16:28:53 -0600 Subject: [PATCH 19/80] Add overloaded saveModuleOffsets method --- src/main/java/frc/robot/Subsystems/SwerveSubsystem.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 76b6c89..c3b14f0 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -195,6 +195,9 @@ public void resyncModuleEncoders(){ } } + public void saveModuleOffsets(){ + saveModuleOffsets(new Rotation2d()); + } public void saveModuleOffsets(Rotation2d desiredAngle){ if(!DriverStation.isDisabled()){ DriverStation.reportWarning( From 494530604e9c745794cc575ecdaefd5f5aa2e386 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sun, 15 Feb 2026 00:06:18 -0600 Subject: [PATCH 20/80] Pathplanner (autonomous) added the Pathplannerlib in vendor and the constants&swervesubsystem settings for autonomous, started the 8 fuel auto with a path code in. --- src/main/deploy/pathplanner/navgrid.json | 1 + .../deploy/pathplanner/paths/8FuelPath.path | 54 +++++++++++++++++ .../java/frc/robot/Auto/EightLemonAuto.java | 20 +++++++ src/main/java/frc/robot/Constants.java | 59 +++++++++++++++++-- .../frc/robot/Subsystems/SwerveSubsystem.java | 57 +++++++++++++----- vendordeps/PathplannerLib.json | 38 ++++++++++++ 6 files changed, 209 insertions(+), 20 deletions(-) create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/8FuelPath.path create mode 100644 src/main/java/frc/robot/Auto/EightLemonAuto.java create mode 100644 vendordeps/PathplannerLib.json diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..ac5f521 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/8FuelPath.path b/src/main/deploy/pathplanner/paths/8FuelPath.path new file mode 100644 index 0000000..c99e5dd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/8FuelPath.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5355439814814824, + "y": 7.129980034722222 + }, + "prevControl": null, + "nextControl": { + "x": 3.0651164641203708, + "y": 6.578326678240741 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1083180700231487, + "y": 5.600598524305556 + }, + "prevControl": { + "x": 2.638057508680556, + "y": 6.088609664351853 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 65.81773709758554 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 91.81907598250271 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/EightLemonAuto.java b/src/main/java/frc/robot/Auto/EightLemonAuto.java new file mode 100644 index 0000000..5f99ce3 --- /dev/null +++ b/src/main/java/frc/robot/Auto/EightLemonAuto.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + + +public class EightLemonAuto extends SequentialCommandGroup { + public EightLemonAuto (SwerveSubsystem drive, ShooterSubsystem shooter, IntakeSubsystem intake){ + addCommands( + drive.startAutoAt(3.53, 7.13, -130.45), + drive.autoDrive("8FuelPath") + ); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 318f829..48d4d07 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,17 +4,22 @@ package frc.robot; - - +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.system.plant.DCMotor; 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.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** Central location for robot-wide constants grouped by subsystem and feature */ public final class Constants { @@ -124,14 +129,56 @@ public record ModuleData( } +public static final class AutoConstants { + public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(SwerveConstants.wheelDiameter/2, + SwerveConstants.maxSpeed, + 1.2, + DCMotor.getNeoVortex(1).withReduction(SwerveConstants.driveGearRatio), + SwerveConstants.driveContinuousCurrentLimit, + 1); + + public static final RobotConfig ROBOT_CONFIG = new RobotConfig (52, 6.8, MODULE_CONFIG, + SwerveConstants.FRONT_LEFT, SwerveConstants.FRONT_RIGHT, SwerveConstants.BACK_LEFT, SwerveConstants.BACK_RIGHT); + + public static final PPHolonomicDriveController SWERV_DRIVE_CONTROLLER = new PPHolonomicDriveController(new PIDConstants(5.0,0.00001,0.0), + new PIDConstants(5.0, 0.005, 0.001) ); + + public enum AutoMode{ + EightLemonAuto + } + + private static SendableChooser sideChooser = new SendableChooser(); + private static SendableChooser autoModeChooser = new SendableChooser(); + private static SendableChooser eightLemonAutoChooser = new SendableChooser(); + static{ + sideChooser.addOption("RIGHT", true); + sideChooser.setDefaultOption("LEFT", false); + + for(AutoMode mode : AutoMode.values()){ + autoModeChooser.addOption(mode.toString(), mode); + } + + autoModeChooser.setDefaultOption(AutoMode.EightLemonAuto.toString(), AutoMode.EightLemonAuto); + SmartDashboard.putData("Eight_Lemon_Auto_Chooser", eightLemonAutoChooser); + SmartDashboard.putData("Auto Starting Location", sideChooser); + SmartDashboard.putData("Auto Mode", autoModeChooser); + } + + public static AutoMode getSelectedAutoMode(){ + AutoMode selection = autoModeChooser.getSelected(); + return selection != null ? selection : AutoMode.EightLemonAuto; + } + public static boolean isRightSideAuto(){ + return Boolean.TRUE.equals(sideChooser.getSelected()); + } +} + + public class FieldConstants { public static final double FIELD_LENGTH = 17.54824934; public static final double FIELD_WIDTH = 8.052; - public static final Translation2d BLUE_REEF_CENTER = new Translation2d(4.48933684,4.02587697); - - public static final Rotation2d RIGHT_CORAL_STATION_ANGLE = Rotation2d.fromDegrees(234.011392); - public static final Rotation2d LEFT_CORAL_STATION_ANGLE = Rotation2d.fromDegrees(-234.011392); + public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); public static boolean isRedAlliance(){ return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index c3b14f0..00f863b 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -6,6 +6,8 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -20,9 +22,12 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.LimelightHelpers; import frc.robot.Constants; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.SwerveConstants.ModuleData; @@ -38,23 +43,10 @@ public class SwerveSubsystem extends SubsystemBase { private Field2d field; - /* NetworkTable Publishers for Swerve Module State Monitoring - * These publishers send swerve module state data to NetworkTables for visualization - * and debugging. The data can be viewed in tools like AdvantageScope, Shuffleboard, - * or custom dashboards. Publishing actual vs. desired states allows comparison to - * diagnose control issues, tuning problems, or mechanical issues. - */ - // Publisher for actual/current swerve module states (speed and angle from encoders) - // * Get the default NetworkTable instance (shared across all NetworkTable operations) - // * Create a publisher for the "Swerve States" topic that sends arrays of SwerveModuleState - // * The struct format allows efficient serialization of the state data + private final StructArrayPublisher swerveDataPublisher = NetworkTableInstance.getDefault() .getStructArrayTopic("Swerve States", SwerveModuleState.struct).publish(); - // Publisher for desired/target swerve module states (commanded speed and angle) - // * Get the default NetworkTable instance - // * Create a publisher for the "Desired Swerve States" topic that sends arrays of SwerveModuleState - // * This shows what the robot is trying to achieve, useful for comparing against actual states private final StructArrayPublisher desiredSwerveDataPublisher = NetworkTableInstance.getDefault() .getStructArrayTopic("Desired Swerve States", SwerveModuleState.struct).publish(); @@ -79,6 +71,43 @@ public SwerveSubsystem() { field = new Field2d(); SmartDashboard.putData("Field", field); } + + private void configurePathPlanner(){ + AutoBuilder.configure(this::getPose, + this::resetOdometry, + this::getChassisSpeeds, + (speeds, feedforwards)->driveFromChassisSpeeds(speeds, false), + AutoConstants.SWERV_DRIVE_CONTROLLER, + AutoConstants.ROBOT_CONFIG, + FieldConstants::isRedAlliance, + this); + } + + public Command autoDrive(String filename){ + try{ + PathPlannerPath path = PathPlannerPath.fromPathFile(filename); + if (AutoConstants.isRightSideAuto()){ + path = path.mirrorPath(); + } + return AutoBuilder.followPath(path); + } + catch(Exception e){ + DriverStation.reportError("PATHPLANNER ERROR" + e.getMessage(), e.getStackTrace()); + return Commands.none(); + } + } + + public Command startAutoAt(double x, double y, double direction){ + return runOnce(()->{ + double newY = y; + if (AutoConstants.isRightSideAuto()){ + newY = FieldConstants.FIELD_WIDTH - y; + } + Pose2d startPose2d = FieldConstants.flipForAlliance(new Pose2d(x, newY, Rotation2d.fromDegrees(direction))); + pigeon.setYaw(startPose2d.getRotation().getDegrees()); + odometry.resetPosition(startPose2d.getRotation(),getPositions(),startPose2d); + }); + } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..4414707 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2026.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2026", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2026.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2026.1.2", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file From 813958265be5fb654da440b3e56b959d824b3e5a Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sun, 15 Feb 2026 00:13:50 -0600 Subject: [PATCH 21/80] Cleaned up things a little Got some big blocks of comments deleted for this branch for now (better going over code for coding auto) --- src/main/java/frc/robot/Robot.java | 21 +- src/main/java/frc/robot/RobotContainer.java | 21 +- .../frc/robot/Subsystems/IntakeSubsystem.java | 1 - .../robot/Subsystems/ShooterSubsystem.java | 14 -- .../frc/robot/Subsystems/SwerveSubsystem.java | 1 + src/main/java/frc/robot/SwerveModule.java | 204 +----------------- 6 files changed, 8 insertions(+), 254 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 82a7a76..e15cf3c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,34 +8,17 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -/** - * Main robot class that extends TimedRobot. This is the entry point for the robot program - * and manages the robot lifecycle across different modes (disabled, autonomous, teleop, test). - * - *

The Robot class: - *

    - *
  • Creates and initializes the RobotContainer which sets up subsystems and command bindings
    • - *
  • Runs the CommandScheduler every 20ms to execute active commands and check button bindings
    • - *
  • Handles mode transitions (autonomous, teleop, test) and manages command lifecycle
    • - *
- */ + public class Robot extends TimedRobot { private Command m_autonomousCommand; private final RobotContainer m_robotContainer; - /** - * Constructs the Robot. Initializes the RobotContainer which creates subsystems - * (subsystems configure themselves) and sets up command bindings. - */ public Robot() { m_robotContainer = new RobotContainer(); } - /** - * Called every 20ms during all robot modes. Runs the CommandScheduler which - * executes active commands, checks button/trigger bindings, and updates subsystems. - */ + @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index abaf19f..b31b011 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,18 +47,10 @@ public class RobotContainer { private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - /** - * Constructs the RobotContainer. Creates subsystems (which configure themselves) - * and sets up command bindings to map controller inputs to commands. - */ public RobotContainer() { configureBindings(); } - /** - * Configures command bindings for controller inputs. - * Maps buttons and triggers to commands and sets the default drive command. - */ private void configureBindings() { // Y Button = Zero gyro (reset heading to 0° or 180° based on alliance) @@ -137,24 +129,13 @@ private void configureBindings() { ); } - - /** - * Determines if the driver has requested speed reduction for precise positioning - * or delicate tasks. - * @return Speed multiplier - */ private double getSpeedMultiplier(){ // getHID() accesses the underlying XboxController to read button states directly. // CommandXboxController doesn't provide a method for stick button presses, so we use // the HID (Human Interface Device) object's getRawButton() method instead. return driveController.getHID().getRawButton(Button.kLeftStick.value)? 0.7: 1; } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 57533e6..18a7d89 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.TunableControllers.TunableArmFeedforward; import frc.robot.Constants.IntakeConstants; import com.revrobotics.spark.SparkMax; diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index ed792e5..caca87c 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -5,8 +5,6 @@ package frc.robot.Subsystems; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; @@ -17,18 +15,6 @@ import frc.robot.Constants.ShooterConstants; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import frc.robot.Constants.ShooterConstants; - public class ShooterSubsystem extends SubsystemBase { public boolean isShooterActive = false; //Shooter True diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 00f863b..fc012ab 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -72,6 +72,7 @@ public SwerveSubsystem() { SmartDashboard.putData("Field", field); } + private void configurePathPlanner(){ AutoBuilder.configure(this::getPose, this::resetOdometry, diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index e848cca..e9a3f9e 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -60,19 +60,6 @@ public class SwerveModule { //creates a feedforward for the swerve drive. feedforward does 90% of the work, estimating stuff //PID fixes the error - /** - * Constructs a swerve module with the specified module number and configuration data. - * Initializes and configures the angle encoder (CANcoder), angle motor (SparkMax), - * and drive motor (SparkFlex) according to the provided constants. - * - * @param moduleNumber The module identifier (typically 0-3 for a 4-module swerve drive) - * @param moduleConstants ModuleData record containing: - * - driveMotorID: CAN ID of the drive motor (SparkFlex) - * - angleMotorID: CAN ID of the angle motor (SparkMax) - * - encoderID: CAN ID of the absolute angle encoder (CANcoder) - * - angleOffset: Calibration offset in degrees to align encoder zero with module zero - * - location: Physical position of the module relative to robot center (Translation2d) - */ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ this.moduleNumber = moduleNumber; this.angleOffsetPreferenceKey = "Swerve/Module" + moduleNumber + "/AngleOffsetDegrees"; @@ -85,26 +72,13 @@ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ Preferences.getDouble(angleOffsetPreferenceKey, moduleConstants.angleOffset()); angleOffset = Rotation2d.fromDegrees(normalizeDegrees(storedOffset)); - /* Angle Encoder Configuration - * The CANcoder is an absolute encoder that provides the module's angle even after power loss. - * It's used to calibrate the integrated encoder on startup. - */ + // Create CANcoder instance with the encoder CAN ID from module constants angleEncoder = new CANcoder(moduleConstants.encoderID()); // Apply default configuration to the CANcoder (factory reset to known state) angleEncoder.getConfigurator().apply(new CANcoderConfiguration()); - // Set update frequency to 1 Hz (once per second) for absolute position readings. - // The CANcoder (absolute encoder) is only used once during robot startup to calibrate - // the integrated encoder (see resetToAbsolute() in configAngleMotor()). During normal - // operation, getAngle() reads from the integrated encoder every 20ms loop cycle, not - // the CANcoder. A low CANcoder update frequency reduces CAN bus traffic since we only - // need the absolute position once at startup, not continuously. angleEncoder.getAbsolutePosition().setUpdateFrequency(1); - /* Angle Motor Configuration - * The angle motor rotates the swerve module to the desired orientation. - * It uses a SparkMax with integrated encoder for position control. - */ // Create SparkMax motor controller for angle rotation (brushless motor) angleMotor = new SparkMax(moduleConstants.angleMotorID(), MotorType.kBrushless); // Get the integrated encoder (relative encoder) from the motor controller @@ -114,10 +88,6 @@ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ // Configure motor settings (current limits, PID, encoder conversion, etc.) configAngleMotor(); - /* Drive Motor Configuration - * The drive motor provides forward/backward motion for the swerve module. - * It uses a SparkFlex with integrated encoder for velocity control. - */ // Create SparkFlex motor controller for drive motion (brushless motor) driveMotor = new SparkFlex(moduleConstants.driveMotorID(), MotorType.kBrushless); // Get the integrated encoder (relative encoder) from the motor controller @@ -131,15 +101,6 @@ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ lastAngle = getState().angle; } - /** - * Sets the module to the desired state (speed and angle). - *

- * This is the main method for controlling the swerve module. It optimizes the desired - * state to minimize rotation distance, then sets both the wheel angle and drive speed. - * - * @param desiredState The target module state (speed in m/s and wheel angle) - * @param isOpenLoop If true, uses open loop control for drive motor; if false, uses closed loop velocity control - */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { // Optimize the desired state to minimize rotation (flip wheel 180° if needed) SwerveModuleState optimizedState = optimize(desiredState, getAngle()); @@ -148,99 +109,25 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) // Set the drive motor speed (open loop or closed loop based on parameter) setSpeed(optimizedState, isOpenLoop); } - - /** - * Gets the current state of the swerve module. - * @return SwerveModuleState containing the current drive velocity (in meters per second) - * and module angle (Rotation2d) - */ public SwerveModuleState getState(){ return new SwerveModuleState(driveEncoder.getVelocity(), getAngle()); } - - /** - * Gets the current position of the swerve module. - * @return SwerveModulePosition containing the current drive encoder position (in meters) - * and module angle (Rotation2d) - */ public SwerveModulePosition getPosition(){ return new SwerveModulePosition(driveEncoder.getPosition(), getAngle()); - } - - /** - * Gets the raw drive encoder position in encoder units (not converted to meters). - * @return Raw encoder position value from the drive motor's integrated encoder - */ + } public double getRawDriveEncoder(){ return driveEncoder.getPosition(); } - - /** - * Gets the raw turn encoder position in encoder units (not converted to degrees). - * @return Raw encoder position value from the angle motor's integrated encoder - */ public double getRawTurnEncoder(){ return integratedAngleEncoder.getPosition(); } - - /** - * Gets the current **absolute encoder** (CANcoder) position. - *

- * The CANcoder is an **absolute encoder** that retains its position even after power loss. - * This method reads the raw absolute position from the CANcoder and converts it to a - * Rotation2d representing the module's wheel angle. - *

- * Used primarily during module initialization in {@link #resetToAbsolute()} to calibrate - * the integrated encoder. Also used for debugging/logging to display the absolute encoder - * value on SmartDashboard for diagnostics. - * - * @return The current absolute encoder position as a Rotation2d - */ public Rotation2d getCanCoder(){ return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue().in(Units.Rotations)); } - - /** - * Checks if encoder data from both motors is valid (no errors). - *

- * Encoder errors can occur when: - *

    - *
  • CAN bus communication fails (disconnected cable, CAN bus overload, electrical interference)
    • - *
  • Motor controller configuration errors (invalid parameters, failed configuration write)
    • - *
  • Encoder hardware failure (damaged encoder, loose connections, sensor malfunction)
    • - *
  • Motor controller fault conditions (overcurrent, overvoltage, thermal shutdown)
    • - *
- *

- * When errors are detected, consider: - *

    - *
  • Logging the error to SmartDashboard or Driver Station for diagnostics
    • - *
  • Disabling the affected module to prevent unpredictable behavior
    • - *
  • Using fallback behavior (e.g., last known good encoder value, or disabling that module)
    • - *
  • Attempting recovery (re-initialization, reconfiguration, or recalibration)
    • - *
- *

- * Note: This method is currently not called anywhere in the codebase. Consider adding - * periodic error checking in {@code SwerveSubsystem.periodic()} to monitor module health. - * - * @return true if both drive motor and angle motor have no errors, false otherwise - */ public boolean isEncoderDataValid(){ return driveMotor.getLastError() == REVLibError.kOk && angleMotor.getLastError() == REVLibError.kOk; } - /** - * Optimizes the desired module state to minimize rotation distance. - *

- * Swerve modules can achieve the same direction of travel by rotating the wheel - * 180 degrees and reversing the drive speed. This method checks if the required - * rotation is greater than 90 degrees, and if so, flips the wheel direction - * and reverses speed to reduce the rotation needed. This minimizes wear and - * improves response time. - * - * @param desiredState The target module state (speed and angle) - * @param currentAngle The current module wheel angle - * @return Optimized module state that achieves the same direction with minimal rotation - */ private SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle){ // Calculate the angular difference between desired and current angle double difference = desiredState.angle.getDegrees() - currentAngle.getDegrees(); @@ -266,32 +153,12 @@ private SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d cu return new SwerveModuleState (speed, Rotation2d.fromDegrees(direction)); } - /** - * Sets the drive motor speed to achieve the desired velocity. - *

- * This is a private helper method used by {@link #setDesiredState(SwerveModuleState, boolean)}. - * Use {@code setDesiredState()} to control the module - do not call this method directly. - *

- * Supports two control modes: - *

    - *
  • Open loop: Direct percent output control (no feedback, less accurate)
    • - *
  • Closed loop: Velocity control with PID and feedforward (uses encoder feedback, more accurate)
    • - *
- * - * @param desiredState The target module state containing the desired speed in meters per second - * @param isOpenLoop If true, uses open loop control; if false, uses closed loop velocity control - */ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){ if (isOpenLoop) { - // Open loop: Convert desired speed to percent output (-1.0 to 1.0) - // No encoder feedback - motor runs at fixed percentage regardless of actual speed double percentOutput = desiredState.speedMetersPerSecond / Constants.SwerveConstants.maxSpeed; driveMotor.set(percentOutput); } else{ - // Closed loop: Use PID controller with feedforward for accurate velocity control - // Feedforward estimates motor output needed for desired speed (90% of work) - // PID controller corrects for any error between desired and actual speed driveController.setReference( desiredState.speedMetersPerSecond, ControlType.kVelocity, @@ -300,18 +167,6 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){ } } - /** - * Sets the wheel angle to the desired direction. - *

- * This is a private helper method used by {@link #setDesiredState(SwerveModuleState, boolean)}. - * Use {@code setDesiredState()} to control the module - do not call this method directly. - *

- * When the robot is moving very slowly (≤1% of max speed), the wheel angle is kept - * at the last position to prevent unnecessary rotation and reduce wear. When moving - * at significant speed, the wheel rotates to the desired angle. - * - * @param desiredState The target module state containing the desired wheel angle - */ private void setAngle(SwerveModuleState desiredState){ // If speed is very low (≤1% of max), keep last angle to avoid unnecessary rotation // This prevents jittery behavior and reduces wear when robot is barely moving @@ -323,42 +178,17 @@ private void setAngle(SwerveModuleState desiredState){ lastAngle = angle; } - /** - * Gets the current wheel angle from the **integrated encoder**. - *

- * This method reads the angle motor's integrated encoder position and converts it - * to a Rotation2d representing the current wheel orientation. - * - * @return The current wheel angle as a Rotation2d - */ + private Rotation2d getAngle(){ return Rotation2d.fromDegrees(integratedAngleEncoder.getPosition()); } - /** - * Points the wheel in a specific direction without changing drive speed. - *

- * This method rotates the wheel to the specified angle (in degrees) while keeping - * the drive motor stopped. Useful for testing, calibration, or positioning the wheel - * without moving the robot. Unlike {@link #setDesiredState(SwerveModuleState, boolean)}, - * this method only controls the angle motor, not the drive motor. - *

- * Note: This method is currently not called anywhere in the codebase. - * - * @param degrees The target wheel angle in degrees (0-360) - */ + public void pointInDirection(double degrees){ angleController.setReference(degrees, ControlType.kPosition); lastAngle = Rotation2d.fromDegrees(degrees); } - /** - * Configures the angle motor (SparkMax) with all necessary settings for position control. - * Called once during module initialization in the constructor. Configures current limits, - * motor inversion, brake mode, encoder conversion factors, PID values, and voltage - * compensation. After configuration, calibrates the integrated encoder to the absolute - * encoder (CANcoder) position. - */ private void configAngleMotor(){ SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); // Factory reset is commented out - only needed if motor needs to be reset to defaults @@ -389,18 +219,6 @@ private void configAngleMotor(){ resetToAbsolute(); } - /** - * Calibrates the integrated encoder to match the absolute encoder (CANcoder) position. - *

- * This method reads the absolute encoder position, subtracts the calibration offset - * (angleOffset), and sets the integrated encoder to this value. This ensures the - * integrated encoder starts at the correct position even after power loss, since the - * absolute encoder retains its position while the integrated encoder resets to zero. - *

- * Called once during module initialization in {@link #configAngleMotor()} after motor - * configuration is complete. This establishes the starting position for the integrated - * encoder, which is then used for all subsequent angle readings during normal operation. - */ private void resetToAbsolute() { double absolutePosition = getCanCoder().getDegrees() - angleOffset.getDegrees(); integratedAngleEncoder.setPosition(absolutePosition); //may need to change @@ -412,18 +230,10 @@ public void resyncToAbsolute(){ resetToAbsolute(); } - /*Saves the current CANcoder reading as the zero reference for this module. - Run this while the wheels are physically pointing straight to capture the - correct offset and persist it in WPILib Preferences. - */ public void saveCanCoderZero(){ saveCanCoderOffset(Rotation2d.fromDegrees(0.0)); } - /** - * Saves the current CANcoder reading as a reference for a desired heading. - * @param desiredAngle The field-relative angle that the wheel is currently aiming at. - */ public void saveCanCoderOffset(Rotation2d desiredAngle){ double absolute = getCanCoder().getDegrees(); double newOffset = normalizeDegrees(absolute - desiredAngle.getDegrees()); @@ -440,12 +250,6 @@ private double normalizeDegrees(double degrees){ return normalized; } - /** - * Configures the drive motor (SparkFlex) with all necessary settings for velocity control. - * Called once during module initialization in the constructor. Configures current limits, - * motor inversion, brake mode, encoder conversion factors, PID values, and voltage - * compensation. After configuration, resets the drive encoder position to zero. - */ private void configDriveMotor(){ SparkFlexConfig sparkFlexConfig = new SparkFlexConfig(); // Factory reset is commented out - only needed if motor needs to be reset to defaults From c493164ec21cf0b260845e191fe28fc7462eeb28 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sun, 15 Feb 2026 00:24:25 -0600 Subject: [PATCH 22/80] Add DriveTestAuto Added a drive test autonomous with both driving manually and driving by pathplanner (the pathplanner drive code is currently comment out but can bring back at any time, just switch the manual drive code) --- .../pathplanner/paths/DriveTestPath.path | 54 +++++++++++++++++++ .../java/frc/robot/Auto/DriveTestAuto.java | 32 +++++++++++ 2 files changed, 86 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/DriveTestPath.path create mode 100644 src/main/java/frc/robot/Auto/DriveTestAuto.java diff --git a/src/main/deploy/pathplanner/paths/DriveTestPath.path b/src/main/deploy/pathplanner/paths/DriveTestPath.path new file mode 100644 index 0000000..a932b07 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DriveTestPath.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1653634982638894, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.165363498263891, + "y": 6.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.547681929976852, + "y": 6.0 + }, + "prevControl": { + "x": 2.547681929976852, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java new file mode 100644 index 0000000..e7a9a80 --- /dev/null +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Subsystems.SwerveSubsystem; + + +public class DriveTestAuto extends SequentialCommandGroup { + public DriveTestAuto (SwerveSubsystem drive) { + addCommands( + new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive) + ); + } +} + +/* +public class DriveTestAuto extends SequentialCommandGroup { + public DriveTestAuto (SwerveSubsystem drive){ + addCommands( + drive.startAutoAt(1.165, 6.000, 0.000), + drive.autoDrive("DriveTestPath") + ); + } +} +*/ From 8cf1e38db19bcdd37f94b3195227adf139dffe79 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 16 Feb 2026 15:25:51 -0600 Subject: [PATCH 23/80] changed one id so it doesn't interfere --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 48d4d07..f6f01e4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -205,7 +205,7 @@ public static Pose2d flipForAlliance(Pose2d pose){ } /* Shooter Constants */ public class ShooterConstants { - public static final int SHOOTER_ID = 60; //Placeholder ID + public static final int SHOOTER_ID = 70; //Placeholder ID public static final int FEEDER_ID = 61; //Feeder ID public static final int HOOD_ID = 62; //Hood ID (NEED CHANGE) From 5c519d6ed099722bc7437b497b568ed771580969 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 19 Feb 2026 16:52:09 -0600 Subject: [PATCH 24/80] Fixed the problem with swerve module 1 Put driveInvert and angleInvert specifically for every single module in Constants. --- src/main/java/frc/robot/Constants.java | 27 +++++++++++++------ .../frc/robot/Subsystems/IntakeSubsystem.java | 1 - .../robot/Subsystems/ShooterSubsystem.java | 14 ---------- src/main/java/frc/robot/SwerveModule.java | 8 ++++-- 4 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 318f829..87770b1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,14 +111,23 @@ public static final class SwerveConstants{ /* Module Specific Constants */ public record ModuleData( - int driveMotorID, int angleMotorID, int encoderID, double angleOffset, Translation2d location + int driveMotorID, + int angleMotorID, + int encoderID, + double angleOffset, + Translation2d location, + boolean driveInvert, + boolean angleInvert ){} public static ModuleData[] moduleData = { - new ModuleData(6, 5, 7, 31.46, FRONT_LEFT), //Mod 0 Front left - new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT), //Mod 1 Front right - new ModuleData(12, 11, 13, 33.13, BACK_RIGHT), //Mod 2 Back right - new ModuleData(15, 14, 16, 8.52, BACK_LEFT) //Mod 3 Back left + new ModuleData(6, 5, 7, 31.46, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left + // Module 1 is currently the only module oscillating; flip its angle motor invert so its + // steering closed-loop sign matches the encoder direction. + // Module 1: also invert drive so +X command drives forward like the others. + new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right + new ModuleData(12, 11, 13, 33.13, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right + new ModuleData(15, 14, 16, 8.52, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left }; } @@ -172,10 +181,12 @@ public class ShooterConstants { public static final double HOOD_TOLERANCE = 0.02; } public class IntakeConstants { - public static int INTAKE_ID = 60; // placeholder + // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). + // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. + public static int INTAKE_ID = 63; // TODO: set to your intake motor CAN ID public static double INTAKE_SPEED = 50; //placeholder for percent power for intake - public static int INTAKE_ARM_ID = 62; //placeholder + public static int INTAKE_ARM_ID = 64; // TODO: set to your intake arm motor CAN ID public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later public static double INTAKE_ARM_LOWERED_POSITION = 0; public static double INTAKE_ARM_MINIMUM = 0; // placeholders @@ -186,4 +197,4 @@ public class IntakeConstants { public static double INTAKE_ARM_kI = 0; public static double INTAKE_ARM_kD = 0; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 57533e6..18a7d89 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.TunableControllers.TunableArmFeedforward; import frc.robot.Constants.IntakeConstants; import com.revrobotics.spark.SparkMax; diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index ed792e5..caca87c 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -5,8 +5,6 @@ package frc.robot.Subsystems; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; @@ -17,18 +15,6 @@ import frc.robot.Constants.ShooterConstants; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import frc.robot.Constants.ShooterConstants; - public class ShooterSubsystem extends SubsystemBase { public boolean isShooterActive = false; //Shooter True diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index e848cca..0b833b3 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -42,6 +42,8 @@ public class SwerveModule { private Rotation2d lastAngle; private Rotation2d angleOffset; private final String angleOffsetPreferenceKey; + private final boolean driveInvert; + private final boolean angleInvert; private SparkMax angleMotor; private SparkFlex driveMotor; @@ -76,6 +78,8 @@ public class SwerveModule { public SwerveModule(int moduleNumber, ModuleData moduleConstants){ this.moduleNumber = moduleNumber; this.angleOffsetPreferenceKey = "Swerve/Module" + moduleNumber + "/AngleOffsetDegrees"; + this.driveInvert = moduleConstants.driveInvert(); + this.angleInvert = moduleConstants.angleInvert(); this.m_angleKP = SwerveConstants.angleKP; this.m_angleKI = SwerveConstants.angleKI; this.m_angleKD = SwerveConstants.angleKD; @@ -369,7 +373,7 @@ private void configAngleMotor(){ // Set maximum current draw to protect motor and wiring sparkMaxConfig.smartCurrentLimit(SwerveConstants.angleContinuousCurrentLimit); // Set motor direction (may need to be inverted based on physical mounting) - sparkMaxConfig.inverted(SwerveConstants.angleInvert); + sparkMaxConfig.inverted(angleInvert); // Set idle mode: brake (holds position) or coast (free rotation) sparkMaxConfig.idleMode(SwerveConstants.angleNeutralMode); // Convert encoder counts to degrees so encoder position matches module rotation angle @@ -456,7 +460,7 @@ private void configDriveMotor(){ // Set maximum current draw to protect motor and wiring sparkFlexConfig.smartCurrentLimit(SwerveConstants.driveContinuousCurrentLimit); // Set motor direction (may need to be inverted based on physical mounting) - sparkFlexConfig.inverted(SwerveConstants.driveInvert); + sparkFlexConfig.inverted(driveInvert); // Set idle mode: brake (holds position) or coast (free rotation) sparkFlexConfig.idleMode(SwerveConstants.driveNeutralMode); // Convert encoder counts to meters per second for velocity readings From 5270cca389cf9db4aebd4eecf745ac45de810a6a Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 19 Feb 2026 22:47:39 -0600 Subject: [PATCH 25/80] Leave auto (coded manually) worked out great, still figuring out with the pathplanner auto --- src/main/java/frc/robot/Auto/DriveTestAuto.java | 7 ++++--- src/main/java/frc/robot/Constants.java | 7 +++---- src/main/java/frc/robot/RobotContainer.java | 13 +++++++++++-- .../java/frc/robot/Subsystems/SwerveSubsystem.java | 2 ++ 4 files changed, 20 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index e7a9a80..f563d08 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Subsystems.SwerveSubsystem; - +/* public class DriveTestAuto extends SequentialCommandGroup { public DriveTestAuto (SwerveSubsystem drive) { addCommands( @@ -19,8 +19,9 @@ public DriveTestAuto (SwerveSubsystem drive) { ); } } +*/ + -/* public class DriveTestAuto extends SequentialCommandGroup { public DriveTestAuto (SwerveSubsystem drive){ addCommands( @@ -29,4 +30,4 @@ public DriveTestAuto (SwerveSubsystem drive){ ); } } -*/ + diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 054b66d..bd3f532 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -153,12 +153,12 @@ public static final class AutoConstants { new PIDConstants(5.0, 0.005, 0.001) ); public enum AutoMode{ + DriveTestAuto, EightLemonAuto } private static SendableChooser sideChooser = new SendableChooser(); private static SendableChooser autoModeChooser = new SendableChooser(); - private static SendableChooser eightLemonAutoChooser = new SendableChooser(); static{ sideChooser.addOption("RIGHT", true); sideChooser.setDefaultOption("LEFT", false); @@ -167,15 +167,14 @@ public enum AutoMode{ autoModeChooser.addOption(mode.toString(), mode); } - autoModeChooser.setDefaultOption(AutoMode.EightLemonAuto.toString(), AutoMode.EightLemonAuto); - SmartDashboard.putData("Eight_Lemon_Auto_Chooser", eightLemonAutoChooser); + autoModeChooser.setDefaultOption(AutoMode.DriveTestAuto.toString(), AutoMode.DriveTestAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); } public static AutoMode getSelectedAutoMode(){ AutoMode selection = autoModeChooser.getSelected(); - return selection != null ? selection : AutoMode.EightLemonAuto; + return selection != null ? selection : AutoMode.DriveTestAuto; } public static boolean isRightSideAuto(){ return Boolean.TRUE.equals(sideChooser.getSelected()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b31b011..41b17f4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,9 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Auto.DriveTestAuto; +import frc.robot.Auto.EightLemonAuto; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Command.AutoAlign; import frc.robot.Command.TeleopSwerve; @@ -137,8 +140,14 @@ private double getSpeedMultiplier(){ } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + AutoConstants.AutoMode selected = AutoConstants.getSelectedAutoMode(); + + return switch (selected) { + case DriveTestAuto -> new DriveTestAuto(m_drive); + case EightLemonAuto -> new EightLemonAuto(m_drive, m_shooter, m_intake); + default -> Commands.none(); + }; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index fc012ab..0ff6f64 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -70,6 +70,8 @@ public SwerveSubsystem() { //puts out the field field = new Field2d(); SmartDashboard.putData("Field", field); + + configurePathPlanner(); } From 379ceccecae400e56df2e06fd24f6c42c90ec295 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 20 Feb 2026 17:53:53 -0600 Subject: [PATCH 26/80] Changed the translation2d for the drivebase --- src/main/java/frc/robot/Constants.java | 29 +++++++++++---------- src/main/java/frc/robot/RobotContainer.java | 11 ++++---- 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cadbc58..4c46a32 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,10 +46,11 @@ public static final class SwerveConstants{ public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( - new Translation2d(-halfTrackWidth, halfWheelBase), //Back Right - new Translation2d(halfTrackWidth,halfWheelBase), // Front Right - new Translation2d(halfTrackWidth,-halfWheelBase), // Front Left - new Translation2d(-halfTrackWidth,-halfWheelBase)); // Back Left + new Translation2d(-halfTrackWidth, -halfWheelBase), //Back Right + new Translation2d(halfTrackWidth,-halfWheelBase), // Front Right + new Translation2d(halfTrackWidth,halfWheelBase), // Front Left + new Translation2d(-halfTrackWidth,halfWheelBase)); // Back Left + //translation 2d locates the swerve module in cords //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html //SwerveDrive Kinematics converts between a ChassisSpeeds object and several SwerveModuleState objects, @@ -100,10 +101,10 @@ public static final class SwerveConstants{ public static final boolean angleInvert = true; //Location of modules - public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, halfTrackWidth); - public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, halfTrackWidth); - public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, -halfTrackWidth); - public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, -halfTrackWidth); + public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); + public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); + public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); + public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); /* Module Specific Constants */ public record ModuleData( @@ -111,10 +112,10 @@ public record ModuleData( ){} public static ModuleData[] moduleData = { - new ModuleData(11, 52, 19, 340.32, BACK_RIGHT), //Mod 0 Back right - new ModuleData(17, 53, 22, 51.59, FRONT_RIGHT), //Mod 1 Front right - new ModuleData(15, 16, 21, 130.16, FRONT_LEFT), //Mod 2 Front left - new ModuleData(13, 12, 20, 118.47, BACK_LEFT) //Mod 3 Back left + new ModuleData(11, 52, 19, 157.06, BACK_RIGHT), //Mod 0 Back right + new ModuleData(17, 53, 22, 54.94, FRONT_RIGHT), //Mod 1 Front right + new ModuleData(15, 16, 21, 132.28, FRONT_LEFT), //Mod 2 Front left + new ModuleData(13, 12, 20, 117.34, BACK_LEFT) //Mod 3 Back left }; } @@ -168,10 +169,10 @@ public class ShooterConstants { public static final double HOOD_TOLERANCE = 0.02; } public class IntakeConstants { - public static int INTAKE_ID = 60; // placeholder + public static int INTAKE_ID = 70; // placeholder public static double INTAKE_SPEED = 50; //placeholder for percent power for intake - public static int INTAKE_ARM_ID = 62; //placeholder + public static int INTAKE_ARM_ID = 71; //placeholder public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later public static double INTAKE_ARM_LOWERED_POSITION = 0; public static double INTAKE_ARM_MINIMUM = 0; // placeholders diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0512796..8328948 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -109,14 +109,15 @@ private void configureBindings() { () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag () -> driveController.getRightTriggerAxis() > 0.1 - )); + ) + ); //INTAKE // raises the intake using the A button on the helms controller - m_intake.setDefaultCommand( - new RunCommand( - () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), - m_intake)); + //m_intake.setDefaultCommand( + //new RunCommand( + // () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), + //m_intake)); //lowers the intake using the A button on the helms controller From d0187b503f021ab08b7a27ad71ebc69618bde60d Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 21 Feb 2026 13:20:39 -0600 Subject: [PATCH 27/80] fix module 0 for the fall-drivebase --- src/main/java/frc/robot/Constants.java | 18 ++++++++++++------ src/main/java/frc/robot/SwerveModule.java | 4 +++- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4c46a32..a236a91 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -108,14 +108,20 @@ public static final class SwerveConstants{ /* Module Specific Constants */ public record ModuleData( - int driveMotorID, int angleMotorID, int encoderID, double angleOffset, Translation2d location + int driveMotorID, + int angleMotorID, + int encoderID, + double angleOffset, + Translation2d location, + boolean driveMotorInverted ){} public static ModuleData[] moduleData = { - new ModuleData(11, 52, 19, 157.06, BACK_RIGHT), //Mod 0 Back right - new ModuleData(17, 53, 22, 54.94, FRONT_RIGHT), //Mod 1 Front right - new ModuleData(15, 16, 21, 132.28, FRONT_LEFT), //Mod 2 Front left - new ModuleData(13, 12, 20, 117.34, BACK_LEFT) //Mod 3 Back left + // If a single module drives "backward" vs the others, flip that module's driveMotorInverted. + new ModuleData(11, 52, 19, 157.06, BACK_RIGHT, true), //Mod 0 Back right + new ModuleData(17, 53, 22, 54.94, FRONT_RIGHT, false), //Mod 1 Front right + new ModuleData(15, 16, 21, 132.28, FRONT_LEFT, false), //Mod 2 Front left + new ModuleData(13, 12, 20, 117.34, BACK_LEFT, false) //Mod 3 Back left }; } @@ -183,4 +189,4 @@ public class IntakeConstants { public static double INTAKE_ARM_kI = 0; public static double INTAKE_ARM_kD = 0; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index e848cca..0d5d21d 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -42,6 +42,7 @@ public class SwerveModule { private Rotation2d lastAngle; private Rotation2d angleOffset; private final String angleOffsetPreferenceKey; + private final boolean driveMotorInverted; private SparkMax angleMotor; private SparkFlex driveMotor; @@ -76,6 +77,7 @@ public class SwerveModule { public SwerveModule(int moduleNumber, ModuleData moduleConstants){ this.moduleNumber = moduleNumber; this.angleOffsetPreferenceKey = "Swerve/Module" + moduleNumber + "/AngleOffsetDegrees"; + this.driveMotorInverted = moduleConstants.driveMotorInverted(); this.m_angleKP = SwerveConstants.angleKP; this.m_angleKI = SwerveConstants.angleKI; this.m_angleKD = SwerveConstants.angleKD; @@ -456,7 +458,7 @@ private void configDriveMotor(){ // Set maximum current draw to protect motor and wiring sparkFlexConfig.smartCurrentLimit(SwerveConstants.driveContinuousCurrentLimit); // Set motor direction (may need to be inverted based on physical mounting) - sparkFlexConfig.inverted(SwerveConstants.driveInvert); + sparkFlexConfig.inverted(driveMotorInverted); // Set idle mode: brake (holds position) or coast (free rotation) sparkFlexConfig.idleMode(SwerveConstants.driveNeutralMode); // Convert encoder counts to meters per second for velocity readings From 4091905cdb2fc897c1ece40266dc671245fdaff6 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 21 Feb 2026 13:29:30 -0600 Subject: [PATCH 28/80] Fix swerve --- .../java/frc/robot/Command/TeleopSwerve.java | 10 ++++----- src/main/java/frc/robot/Constants.java | 21 ++++++++----------- .../frc/robot/Subsystems/SwerveSubsystem.java | 2 +- src/main/java/frc/robot/SwerveModule.java | 2 +- 4 files changed, 15 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Command/TeleopSwerve.java b/src/main/java/frc/robot/Command/TeleopSwerve.java index d45c504..88e3981 100644 --- a/src/main/java/frc/robot/Command/TeleopSwerve.java +++ b/src/main/java/frc/robot/Command/TeleopSwerve.java @@ -58,10 +58,8 @@ public void execute() { double rotationVal = rotationLimiter.calculate( MathUtil.applyDeadband(m_rotationSupplier.getAsDouble(), SwerveConstants.inputDeadband)); - int invert = 1; - if (FieldConstants.isRedAlliance()){ - invert = -1; - } + boolean isFieldOriented = !m_robotCentricSupplier.getAsBoolean(); + int invert = (isFieldOriented && FieldConstants.isRedAlliance()) ? -1 : 1; /* Drive */ m_SwerveSubsystem.drive( @@ -70,7 +68,7 @@ public void execute() { //rotation value times max spin speed rotationVal * SwerveConstants.maxAngularVelocity, //whether or not in field centric mode - !m_robotCentricSupplier.getAsBoolean()); + isFieldOriented); } @@ -83,4 +81,4 @@ public void end(boolean interrupted) {} public boolean isFinished() { return false; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a236a91..de63ce3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -34,7 +34,8 @@ public static final class SwerveConstants{ public static final double halfWheelBase = Units.inchesToMeters(28/2.0);//to find public static final double wheelDiameter = Units.inchesToMeters(4.0); public static final double wheelCircumference = wheelDiameter * Math.PI; - public static final double driveBaseRadius = Math.hypot(halfTrackWidth/2, halfWheelBase/2); + // Distance from robot center to a module (used for max angular velocity scaling). + public static final double driveBaseRadius = Math.hypot(halfTrackWidth, halfWheelBase); public static final double openLoopRamp = 0.25; public static final double closedLoopRamp = 0.0; @@ -44,12 +45,14 @@ public static final class SwerveConstants{ public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1 MK4 SDS Modules //SDS Mk4 is 12.8:1, Mk4i is 21.4:1 + // Location of modules (x = forward, y = left). + public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); + public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); + public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); + public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); + public static final SwerveDriveKinematics swerveKinematics = - new SwerveDriveKinematics( - new Translation2d(-halfTrackWidth, -halfWheelBase), //Back Right - new Translation2d(halfTrackWidth,-halfWheelBase), // Front Right - new Translation2d(halfTrackWidth,halfWheelBase), // Front Left - new Translation2d(-halfTrackWidth,halfWheelBase)); // Back Left + new SwerveDriveKinematics(BACK_RIGHT, FRONT_RIGHT, FRONT_LEFT, BACK_LEFT); //translation 2d locates the swerve module in cords //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html @@ -100,12 +103,6 @@ public static final class SwerveConstants{ public static final boolean driveInvert = false; public static final boolean angleInvert = true; - //Location of modules - public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); - public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); - public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); - public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); - /* Module Specific Constants */ public record ModuleData( int driveMotorID, diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 53582ff..7169be9 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -124,7 +124,7 @@ public void driveFromChassisSpeeds(ChassisSpeeds driveSpeeds, boolean isOpenLoop desiredSwerveDataPublisher.set(desiredStates); for (SwerveModule mod : mSwerveMods) { - mod.setDesiredState(desiredStates[mod.moduleNumber], false); + mod.setDesiredState(desiredStates[mod.moduleNumber], isOpenLoop); } } diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index 0d5d21d..139dde0 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -458,7 +458,7 @@ private void configDriveMotor(){ // Set maximum current draw to protect motor and wiring sparkFlexConfig.smartCurrentLimit(SwerveConstants.driveContinuousCurrentLimit); // Set motor direction (may need to be inverted based on physical mounting) - sparkFlexConfig.inverted(driveMotorInverted); + sparkFlexConfig.inverted(SwerveConstants.driveInvert ^ driveMotorInverted); // Set idle mode: brake (holds position) or coast (free rotation) sparkFlexConfig.idleMode(SwerveConstants.driveNeutralMode); // Convert encoder counts to meters per second for velocity readings From 90541d4b815ac5e18b593273f5b564abcb78e484 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 21 Feb 2026 14:38:24 -0600 Subject: [PATCH 29/80] Revert "Merge branch 'practice-drivebase' into NonoAuto" This reverts commit ad4849483e3e4e01d19886110de8ca744a29f2ca, reversing changes made to 8b3122db6d4844704d50f843373f20629591c579. --- .../java/frc/robot/Command/TeleopSwerve.java | 8 +++++--- src/main/java/frc/robot/Constants.java | 20 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 11 +++++----- 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Command/TeleopSwerve.java b/src/main/java/frc/robot/Command/TeleopSwerve.java index 236cea3..c614e99 100644 --- a/src/main/java/frc/robot/Command/TeleopSwerve.java +++ b/src/main/java/frc/robot/Command/TeleopSwerve.java @@ -63,8 +63,10 @@ public void execute() { double rotationVal = rotationLimiter.calculate( MathUtil.applyDeadband(m_rotationSupplier.getAsDouble(), SwerveConstants.inputDeadband)); - boolean isFieldOriented = !m_robotCentricSupplier.getAsBoolean(); - int invert = (isFieldOriented && FieldConstants.isRedAlliance()) ? -1 : 1; + int invert = 1; + if (FieldConstants.isRedAlliance()){ + invert = -1; + } /* Command closed-loop swerve drive */ m_SwerveSubsystem.drive( @@ -86,4 +88,4 @@ public void end(boolean interrupted) {} public boolean isFinished() { return false; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2728315..99f33f6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,8 +39,10 @@ public static final class SwerveConstants{ public static final double halfWheelBase = Units.inchesToMeters(27/2.0);//to find public static final double wheelDiameter = Units.inchesToMeters(4.0); public static final double wheelCircumference = wheelDiameter * Math.PI; - // Distance from robot center to a module (used for max angular velocity scaling). - public static final double driveBaseRadius = Math.hypot(halfTrackWidth, halfWheelBase); + //halfTrackWidth/halfwheelBase are already "half" distances, so don't divide again. + //public static final double driveBaseRadius = Math.hypot(halfTrackWidth/2, halfWheelBase/2); + public static final double driveBaseRadius = Math.hypot(halfWheelBase, halfTrackWidth); + public static final double openLoopRamp = 0.25; public static final double closedLoopRamp = 0.0; @@ -50,15 +52,13 @@ public static final class SwerveConstants{ public static final double angleGearRatio = (21.4 / 1.0); // 21.4:1 MK4i Modules //SDS Mk4 is 12.8:1, Mk4i is 21.4:1 - // Location of modules (x = forward, y = left). - public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); - public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); - public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); - public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); - public static final SwerveDriveKinematics swerveKinematics = - new SwerveDriveKinematics(BACK_RIGHT, FRONT_RIGHT, FRONT_LEFT, BACK_LEFT); - + new SwerveDriveKinematics( + //WPILib coordinate system: +X = forward, +Y = left + new Translation2d(halfTrackWidth, halfWheelBase), //Front left + new Translation2d(halfTrackWidth, -halfWheelBase), //Front right + new Translation2d(-halfTrackWidth, -halfWheelBase), //Back right + new Translation2d(-halfTrackWidth, halfWheelBase)); //Back Left //translation 2d locates the swerve module in cords //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html //SwerveDrive Kinematics converts between a ChassisSpeeds object and several SwerveModuleState objects, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c00a9b2..ae8c531 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -117,15 +117,14 @@ private void configureBindings() { () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag () -> driveController.getRightTriggerAxis() > 0.1 - ) - ); + )); //INTAKE // raises the intake using the A button on the helms controller - //m_intake.setDefaultCommand( - //new RunCommand( - // () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), - //m_intake)); + m_intake.setDefaultCommand( + new RunCommand( + () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), + m_intake)); //lowers the intake using the A button on the helms controller From 5c63c21597b8485d8bd229eded20f588453ef520 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 21 Feb 2026 14:47:35 -0600 Subject: [PATCH 30/80] updated to 2026.2.1 --- build.gradle | 2 +- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 0 -> 2048 bytes 6 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat create mode 100644 ctre_sim/Pigeon 2 - 017 - 0 - ext.dat diff --git a/build.gradle b/build.gradle index 919fc7a..8c1b3c9 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.1.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..9ef3742f33e48045ce4e4d5237fc021d28aad376 GIT binary patch literal 2048 zcmdnU;Qyat2!TxuenZN=LnvrRojMuzG?x=3#SxxAnH?+`Ij;_IQ^{!$}q!a J85kU(@&I3^7>57= literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..a47290c37cec2191714f0cac3566779ea5b10426 GIT binary patch literal 2048 zcmdnY;QOCp2!TxuenZN=LnvrRojMu*O@(ZByejVo-4M+(9!&MiO literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..ceb1519bbe4502e3e4da39a0df1a8e5b818faf47 GIT binary patch literal 2048 zcmdnY;QOCp2!SmOK10gALnvrRojMu;~;M6(If% z8IGll4e|X6Xz~k?gYG5Eu=C(GVE^Az-kb#f}l^O-`s+8?@I{fcSqT uIF>RpsHPqG`ok7kegU$4gXMt-1`zd3Q2nfE^oLMZu=xyJCs;t{zyJVgFcuF0 literal 0 HcmV?d00001 From 742719113bc045e50d0e740ed563ce436d77fefd Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 23 Feb 2026 14:08:48 -0600 Subject: [PATCH 31/80] Created a center to depot auto. Starts in front of the hub, backup and then shoot. After shooting, drives to the depot for intake then drives closer to the hub to shoot again. --- .../frc/robot/Auto/CenterToDepotAuto.java | 86 +++++++++++++++++++ .../java/frc/robot/Auto/DriveTestAuto.java | 4 +- 2 files changed, 88 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/Auto/CenterToDepotAuto.java diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java new file mode 100644 index 0000000..28e2c91 --- /dev/null +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; + +public class CenterToDepotAuto extends SequentialCommandGroup { + public CenterToDepotAuto (SwerveSubsystem drive) { + final double[] startYawRad = new double[1]; + addCommands( + drive.startAutoAt(4.61, 4.03, 90.0), + new InstantCommand(()->drive.drive(0,0.5,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), + + Commands.waitSeconds(1), + + new InstantCommand(()-> drive.drive(0.9,0,0,false),drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0,false), drive), + + //Turn ~90 degrees in place (robot-centric) + Commands.runOnce(()->startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(()->{ + double targetYawRad = startYawRad[0] + (Math.PI / 2.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0,omegaRadiansPerSecond, false); + }, drive).until(()->{ + double targetYawRad = startYawRad[0] + (Math.PI / 2.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + // Move forward ~1m (0.5 m/s for 2s) after turning + new InstantCommand(() -> drive.drive(0.7,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + Commands.waitSeconds(2), + + // Back up ~0.5m, then turn 180 degrees + new InstantCommand(() -> drive.drive(-0.5,0,0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0,omegaRadiansPerSecond, false); + }, drive).until(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + new InstantCommand(() -> drive.drive(0.4, 0, 0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + //Turn 40 degrees left (counterclockwise) + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive) + ); + } +} + diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index f563d08..c58afdb 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -4,8 +4,7 @@ package frc.robot.Auto; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; + import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Subsystems.SwerveSubsystem; @@ -20,6 +19,7 @@ public DriveTestAuto (SwerveSubsystem drive) { } } */ + public class DriveTestAuto extends SequentialCommandGroup { From 067382c958df2edbbaed3f2665e8ffe77eb8cfca Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 23 Feb 2026 14:32:09 -0600 Subject: [PATCH 32/80] Coded an auto that start in front of the trench Start infront of the trench, move to shoot then drive to the depot for intake, drive back to shoot again. --- .../frc/robot/Auto/CenterToDepotAuto.java | 9 +- .../java/frc/robot/Auto/DriveTestAuto.java | 1 - .../java/frc/robot/Auto/EightLemonAuto.java | 2 +- .../frc/robot/Auto/TrenchToDepotAuto.java | 122 ++++++++++++++++++ 4 files changed, 130 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/Auto/TrenchToDepotAuto.java diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 28e2c91..b6d210f 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -22,6 +22,8 @@ public CenterToDepotAuto (SwerveSubsystem drive) { Commands.waitSeconds(1), + //SHOOT + new InstantCommand(()-> drive.drive(0.9,0,0,false),drive), Commands.waitSeconds(2), new InstantCommand(()->drive.drive(0,0,0,false), drive), @@ -40,14 +42,17 @@ public CenterToDepotAuto (SwerveSubsystem drive) { }), new InstantCommand(() -> drive.drive(0,0,0, false), drive), - // Move forward ~1m (0.5 m/s for 2s) after turning + // Move forward ~1m (0.5 m/s for 2s) after turning (to the depot) new InstantCommand(() -> drive.drive(0.7,0,0, false), drive), Commands.waitSeconds(2), new InstantCommand(() -> drive.drive(0,0,0, false), drive), Commands.waitSeconds(2), - // Back up ~0.5m, then turn 180 degrees + //INTAKE + + + // Back up ~0.5m, then turn 180 degrees new InstantCommand(() -> drive.drive(-0.5,0,0, false), drive), Commands.waitSeconds(1), new InstantCommand(() -> drive.drive(0,0,0, false), drive), diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index c58afdb..75dae1c 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -30,4 +30,3 @@ public DriveTestAuto (SwerveSubsystem drive){ ); } } - diff --git a/src/main/java/frc/robot/Auto/EightLemonAuto.java b/src/main/java/frc/robot/Auto/EightLemonAuto.java index 5f99ce3..1391f58 100644 --- a/src/main/java/frc/robot/Auto/EightLemonAuto.java +++ b/src/main/java/frc/robot/Auto/EightLemonAuto.java @@ -9,7 +9,7 @@ import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.SwerveSubsystem; - +//With PATHPLANNER public class EightLemonAuto extends SequentialCommandGroup { public EightLemonAuto (SwerveSubsystem drive, ShooterSubsystem shooter, IntakeSubsystem intake){ addCommands( diff --git a/src/main/java/frc/robot/Auto/TrenchToDepotAuto.java b/src/main/java/frc/robot/Auto/TrenchToDepotAuto.java new file mode 100644 index 0000000..6d65da2 --- /dev/null +++ b/src/main/java/frc/robot/Auto/TrenchToDepotAuto.java @@ -0,0 +1,122 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; + +public class TrenchToDepotAuto extends SequentialCommandGroup { + public TrenchToDepotAuto (SwerveSubsystem drive){ + final double[] startYawRad = new double[1]; + addCommands( + drive.startAutoAt(4.61, 6.9, 0), + new InstantCommand(()->drive.drive(-0.5,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), + + //Turn 40 degrees left (counterclockwise) + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + Commands.waitSeconds(2), + + //Turn back 40 degrees right (clockwise) to the starting heading + Commands.run(() -> { + double targetYawRad = startYawRad[0]; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp( + errorRad * 4.0, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0]; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + + //SHOOT + + + //Move to the right (infront of the depot) + new InstantCommand(() -> drive.drive(0, -0.4, 0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + //Turn 180 degrees + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0,omegaRadiansPerSecond, false); + }, drive).until(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Move forward to the depot + new InstantCommand(() -> drive.drive(0.7,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + Commands.waitSeconds(2), + + + //INTAKE + + + // Back up ~0.5m, then turn 180 degrees + new InstantCommand(() -> drive.drive(-0.5,0,0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0,omegaRadiansPerSecond, false); + }, drive).until(()->{ + double targetYawRad = startYawRad[0] + Math.PI; + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + new InstantCommand(() -> drive.drive(0.4, 0, 0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive), + + //Turn 40 degrees left (counterclockwise) + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + new InstantCommand(() -> drive.drive(0,0,0, false), drive) + + ); + } +} \ No newline at end of file From a656c6e564982e244e2029d38c2e8b8006f3ab44 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 23 Feb 2026 14:41:08 -0600 Subject: [PATCH 33/80] set the default alliance to BLUE alliance instead of red --- src/main/java/frc/robot/Constants.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99f33f6..63c23e3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -188,7 +188,20 @@ public class FieldConstants { public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); + /** + * If true, the robot will behave as if it is always on the Blue alliance (no field mirroring), + * even when connected to FMS / Driver Station reports Red. + * + * WARNING: Enabling this for real matches while actually on Red will make autos/field-oriented + * behavior mirror incorrectly. + */ + public static final boolean FORCE_BLUE_ALLIANCE = true; + public static boolean isRedAlliance(){ + if (FORCE_BLUE_ALLIANCE) { + return false; + } + // Default to Blue when alliance is unknown (common in sim/practice). return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; } From 1a5ec01ee30baec28b9b242f23643124aa5ef9fc Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 23 Feb 2026 16:54:03 -0600 Subject: [PATCH 34/80] RobotSimulation for this branch --- ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes .../frc/robot/Auto/TrenchToDepotAuto.java | 2 +- src/main/java/frc/robot/Constants.java | 33 ++++++--- src/main/java/frc/robot/Robot.java | 12 ++++ src/main/java/frc/robot/RobotContainer.java | 9 +++ src/main/java/frc/robot/RobotSimulation.java | 68 ++++++++++++++++++ .../frc/robot/Subsystems/SwerveSubsystem.java | 66 ++++++++++++++++- 10 files changed, 176 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/RobotSimulation.java diff --git a/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat index a47290c37cec2191714f0cac3566779ea5b10426..29cc10770bf45766d936637e6d36ea7fb459517c 100644 GIT binary patch delta 28 kcmZn=Xb@o8%HaKf;=wg6+Zeq5Z#=l1ZK4A2so^@o0uqM- E06P^6h5!Hn delta 76 zcmZn=Xb@mI$YB3}b0VV;drive.drive(-0.5,0,0, false), drive), Commands.waitSeconds(2), new InstantCommand(()->drive.drive(0,0,0, false),drive), diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 63c23e3..e3fa22d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -139,6 +139,8 @@ public record ModuleData( public static final class AutoConstants { + private static boolean dashboardInitialized = false; + public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(SwerveConstants.wheelDiameter/2, SwerveConstants.maxSpeed, 1.2, @@ -153,36 +155,47 @@ public static final class AutoConstants { new PIDConstants(5.0, 0.005, 0.001) ); public enum AutoMode{ + None, DriveTestAuto, - EightLemonAuto + EightLemonAuto, + TrenchToDepotAuto, + CenterToDepotAuto } private static SendableChooser sideChooser = new SendableChooser(); private static SendableChooser autoModeChooser = new SendableChooser(); - static{ + public static void initDashboard() { + if (dashboardInitialized) { + return; + } + dashboardInitialized = true; + sideChooser.addOption("RIGHT", true); sideChooser.setDefaultOption("LEFT", false); - for(AutoMode mode : AutoMode.values()){ - autoModeChooser.addOption(mode.toString(), mode); - } + autoModeChooser.setDefaultOption("TrenchToDepotAuto", AutoMode.TrenchToDepotAuto); + autoModeChooser.addOption("None", AutoMode.None); + autoModeChooser.addOption("EightLemonAuto (PathPlanner)", AutoMode.EightLemonAuto); + autoModeChooser.addOption("TrenchToDepotAuto", AutoMode.TrenchToDepotAuto); + autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); - autoModeChooser.setDefaultOption(AutoMode.DriveTestAuto.toString(), AutoMode.DriveTestAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); } public static AutoMode getSelectedAutoMode(){ + initDashboard(); AutoMode selection = autoModeChooser.getSelected(); return selection != null ? selection : AutoMode.DriveTestAuto; } public static boolean isRightSideAuto(){ + initDashboard(); return Boolean.TRUE.equals(sideChooser.getSelected()); } } -public class FieldConstants { +public static final class FieldConstants { public static final double FIELD_LENGTH = 17.54824934; public static final double FIELD_WIDTH = 8.052; @@ -225,7 +238,7 @@ public static Pose2d flipForAlliance(Pose2d pose){ } /* Shooter Constants */ - public class ShooterConstants { + public static final class ShooterConstants { public static final int SHOOTER_ID = 70; //Placeholder ID public static final int FEEDER_ID = 61; //Feeder ID public static final int HOOD_ID = 62; //Hood ID (NEED CHANGE) @@ -239,7 +252,7 @@ public class ShooterConstants { public static final double HOOD_MAX_OUTPUT = 0.4; public static final double HOOD_TOLERANCE = 0.02; } - public class IntakeConstants { + public static final class IntakeConstants { // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 63; // TODO: set to your intake motor CAN ID @@ -257,7 +270,7 @@ public class IntakeConstants { public static double INTAKE_ARM_kD = 0; } - public class CANdleConstants { + public static final class CANdleConstants { public static final int CANDLE_ID = 18; //Placeholder ID } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e15cf3c..a9f81d2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,9 +13,11 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private final RobotContainer m_robotContainer; + private final RobotSimulation m_robotSimulation; public Robot() { m_robotContainer = new RobotContainer(); + m_robotSimulation = new RobotSimulation(m_robotContainer); } @@ -74,4 +76,14 @@ public void testPeriodic() {} @Override public void testExit() {} + + @Override + public void simulationInit() { + m_robotSimulation.simulationInit(); + } + + @Override + public void simulationPeriodic() { + m_robotSimulation.simulationPeriodic(); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae8c531..999a873 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.DriveTestAuto; import frc.robot.Auto.EightLemonAuto; +import frc.robot.Auto.TrenchToDepotAuto; +import frc.robot.Auto.CenterToDepotAuto; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Command.AutoAlign; @@ -57,6 +59,7 @@ public class RobotContainer { private final ShooterSubsystem m_shooter = new ShooterSubsystem(); public RobotContainer() { + AutoConstants.initDashboard(); configureBindings(); } @@ -149,11 +152,17 @@ public Command getAutonomousCommand() { AutoConstants.AutoMode selected = AutoConstants.getSelectedAutoMode(); return switch (selected) { + case None -> Commands.none(); case DriveTestAuto -> new DriveTestAuto(m_drive); case EightLemonAuto -> new EightLemonAuto(m_drive, m_shooter, m_intake); + case TrenchToDepotAuto -> new TrenchToDepotAuto(m_drive); + case CenterToDepotAuto -> new CenterToDepotAuto(m_drive); default -> Commands.none(); }; } + public SwerveSubsystem getDriveSubsystem() { + return m_drive; + } } diff --git a/src/main/java/frc/robot/RobotSimulation.java b/src/main/java/frc/robot/RobotSimulation.java new file mode 100644 index 0000000..de37084 --- /dev/null +++ b/src/main/java/frc/robot/RobotSimulation.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; + +/** + * Desktop simulation hooks. + * + *

This class is called from {@link Robot#simulationInit()} and {@link Robot#simulationPeriodic()} + * so the "WPILib: Simulate Robot Code" action in VS Code can run your robot with a simple physics + * model. + */ +public class RobotSimulation { + private final SwerveSubsystem drive; + private double lastTimestampSeconds = Timer.getFPGATimestamp(); + + public RobotSimulation(RobotContainer robotContainer) { + this.drive = robotContainer.getDriveSubsystem(); + } + + public void simulationInit() { + if (!RobotBase.isSimulation()) { + return; + } + + // Leave the robot disabled by default so the Sim GUI Driver Station can control mode + // (Disabled / Auto / Teleop). + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(false); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(false); + DriverStationSim.notifyNewData(); + drive.simulationReset(); + lastTimestampSeconds = Timer.getFPGATimestamp(); + } + + public void simulationPeriodic() { + if (!RobotBase.isSimulation()) { + return; + } + + final double now = Timer.getFPGATimestamp(); + final double dtSeconds = MathUtil.clamp(now - lastTimestampSeconds, 0.0, 0.05); + lastTimestampSeconds = now; + + drive.simulationUpdate(dtSeconds); + + var speeds = drive.getLastCommandedSpeeds(); + double driveFraction = + Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) / SwerveConstants.maxSpeed; + double rotateFraction = + Math.abs(speeds.omegaRadiansPerSecond) / SwerveConstants.maxAngularVelocity; + double estimatedCurrentAmps = 8.0 + 80.0 * MathUtil.clamp(driveFraction, 0.0, 1.0) + + 40.0 * MathUtil.clamp(rotateFraction, 0.0, 1.0); + + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(estimatedCurrentAmps)); + } +} diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 0ff6f64..9af8284 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -42,6 +43,12 @@ public class SwerveSubsystem extends SubsystemBase { private SwerveModule[] mSwerveMods; private Field2d field; + private ChassisSpeeds lastCommandedSpeeds = new ChassisSpeeds(); + + private double simYawDegrees = 0.0; + private final double[] simWheelPositionsMeters = new double[4]; + private final Rotation2d[] simWheelAngles = + new Rotation2d[] {new Rotation2d(), new Rotation2d(), new Rotation2d(), new Rotation2d()}; private final StructArrayPublisher swerveDataPublisher = NetworkTableInstance.getDefault() @@ -74,6 +81,25 @@ public SwerveSubsystem() { configurePathPlanner(); } + public void simulationReset() { + if (!RobotBase.isSimulation()) { + return; + } + + simYawDegrees = getYaw().getDegrees(); + for (int i = 0; i < 4; i++) { + simWheelPositionsMeters[i] = 0.0; + simWheelAngles[i] = new Rotation2d(); + } + + pigeon.setYaw(simYawDegrees); + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = new SwerveModulePosition(0.0, simWheelAngles[i]); + } + odometry.resetPosition(Rotation2d.fromDegrees(simYawDegrees), positions, new Pose2d()); + } + private void configurePathPlanner(){ AutoBuilder.configure(this::getPose, @@ -152,6 +178,7 @@ public void drive(double xInput, double yInput, double rotationInput, boolean is } public void driveFromChassisSpeeds(ChassisSpeeds driveSpeeds, boolean isOpenLoop){ + lastCommandedSpeeds = driveSpeeds; SwerveModuleState[] desiredStates = SwerveConstants.swerveKinematics.toSwerveModuleStates(driveSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.maxSpeed); @@ -166,6 +193,10 @@ public ChassisSpeeds getChassisSpeeds(){ return SwerveConstants.swerveKinematics.toChassisSpeeds(getStates()); } + public ChassisSpeeds getLastCommandedSpeeds() { + return lastCommandedSpeeds; + } + public Pose2d getPose() { return odometry.getEstimatedPosition(); } @@ -246,9 +277,11 @@ public void saveModuleOffsets(Rotation2d desiredAngle){ @Override public void periodic() { - odometry.update(getYaw(), getPositions()); - updateOdometryWithVision("limelight-a"); - updateOdometryWithVision("limelight-b"); + if (!RobotBase.isSimulation()) { + odometry.update(getYaw(), getPositions()); + updateOdometryWithVision("limelight-a"); + updateOdometryWithVision("limelight-b"); + } field.setRobotPose(getPose()); SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble()); @@ -270,4 +303,31 @@ public void periodic() { swerveDataPublisher.set(getStates()); } + /** + * Simple swerve simulation: integrates the last commanded chassis speeds into wheel positions and + * a yaw angle, then updates odometry from those simulated sensors. + */ + public void simulationUpdate(double dtSeconds) { + if (!RobotBase.isSimulation()) { + return; + } + + ChassisSpeeds speeds = DriverStation.isDisabled() ? new ChassisSpeeds() : lastCommandedSpeeds; + + simYawDegrees += Math.toDegrees(speeds.omegaRadiansPerSecond * dtSeconds); + pigeon.setYaw(simYawDegrees); + + SwerveModuleState[] states = SwerveConstants.swerveKinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.maxSpeed); + + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + simWheelPositionsMeters[i] += states[i].speedMetersPerSecond * dtSeconds; + simWheelAngles[i] = states[i].angle; + positions[i] = new SwerveModulePosition(simWheelPositionsMeters[i], simWheelAngles[i]); + } + + odometry.update(Rotation2d.fromDegrees(simYawDegrees), positions); + } + } From 1c97a46fdc306a32a45c6f7272f495c179ad5cdd Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 23 Feb 2026 19:13:54 -0600 Subject: [PATCH 35/80] Adjusted drive speed & auto align position --- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes .../java/frc/robot/Command/AutoAlign.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- 7 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat index 9ef3742f33e48045ce4e4d5237fc021d28aad376..3007495c25b539647d15b1ed401f145f32577ce2 100644 GIT binary patch delta 28 kcmZn=Xb@o8%;5Wf;=wg6TNr%)Z#=l1ZK4A29Ef;=wf=Hw3T)05rY^+5i9m delta 16 YcmZn=Xb@o8&fxif;=wf=Hw3T)05rM=+5i9m diff --git a/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat index 94510af48520d56a204d6c4af37ad6eef7381658..caadd26ed74f91ebe51a5e015b7165664c691feb 100644 GIT binary patch delta 16 YcmZn=Xb@o8&*1of;=wf=Hw3T)05r-5+5i9m delta 16 YcmZn=Xb@o8%i#Qf;=wf=Hw3T)05rx1+5i9m diff --git a/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat index fd25c0440aaf2a5b51233358517ec648147d3272..31c730b7443a7bdc45882ecf361f6fe628a84c8b 100644 GIT binary patch delta 31 kcmZn=Xb@mI$YB3}b0VV;Bg+8>2M|l0ZK4A2 Date: Wed, 25 Feb 2026 18:11:02 -0600 Subject: [PATCH 36/80] Add indexer motor (will need speed tuning) --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 8 ++++++- .../robot/Subsystems/ShooterSubsystem.java | 21 ++++++++++++++++--- 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e3fa22d..339261d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -242,9 +242,11 @@ public static final class ShooterConstants { public static final int SHOOTER_ID = 70; //Placeholder ID public static final int FEEDER_ID = 61; //Feeder ID public static final int HOOD_ID = 62; //Hood ID (NEED CHANGE) + public static final int INDEXER_ID = 75; //Placeholder public static final double SHOOTER_SPEED = 0.5; //Placeholder speed public static final double FEEDER_SPEED = 0.5; + public static final double INDEXER_SPEED = 0.5; //placeholder public static final double HOOD_ANGLE_LOW = 0.0; public static final double HOOD_ANGLE_HIGH = 0.5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9a18dcb..5c12301 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,9 +90,15 @@ private void configureBindings() { if (Math.abs(feederAxis) > 0.1) { feederSpeed = -Math.signum(feederAxis) * ShooterConstants.FEEDER_SPEED; } - m_shooter.runFeederSpeed(feederSpeed); + m_shooter.setFeederSpeed(feederSpeed); }, m_shooter)); + + helmsController.button(Button.kX.value).onTrue(Commands.startEnd( + () -> m_shooter.runIndexer(true), + () -> m_shooter.runIndexer(false), + m_shooter)); + helmsController.button(Button.kB.value).onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); helmsController.button(Button.kY.value).onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index caca87c..bb77e2b 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -22,7 +22,8 @@ public class ShooterSubsystem extends SubsystemBase { SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); SparkMax feederMotor = new SparkMax(ShooterConstants.FEEDER_ID, MotorType.kBrushless); SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); - + SparkMax indexerMotor = new SparkMax(ShooterConstants.INDEXER_ID, MotorType.kBrushless); + private final PIDController hoodController = new PIDController( ShooterConstants.HOOD_KP, 0.0, @@ -51,6 +52,12 @@ public ShooterSubsystem() { hoodConfig.inverted(false); hoodConfig.idleMode(IdleMode.kBrake); + SparkMaxConfig indexConfig = new SparkMaxConfig(); + indexConfig.inverted(false); + indexConfig.idleMode(IdleMode.kBrake); + + + shooterMotor.configure(shootConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); feederMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodMotor.configure(hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); @@ -82,10 +89,10 @@ public void runShooter(boolean shooterOn) { public void runFeeder(boolean feederOn){ - runFeederSpeed(feederOn ? ShooterConstants.FEEDER_SPEED : 0); + setFeederSpeed(feederOn ? ShooterConstants.FEEDER_SPEED : 0); } - public void runFeederSpeed(double speed) { + public void setFeederSpeed(double speed) { feederMotor.set(speed); } @@ -108,6 +115,14 @@ public double getHoodPosition() { return hoodMotor.getEncoder().getPosition(); } + public void runIndexer(boolean indexerOn) { + setIndexerSpeed(indexerOn ? ShooterConstants.INDEXER_SPEED : 0); + } + + public void setIndexerSpeed(double speed) { + indexerMotor.set(speed); + } + @Override public void periodic() { // This method will be called once per scheduler run From cc346114572a9b8d6718c10cac98eeab00074163 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 26 Feb 2026 16:48:02 -0600 Subject: [PATCH 37/80] set up limelight --- src/main/java/frc/robot/Constants.java | 16 ++++++++++ src/main/java/frc/robot/RobotContainer.java | 29 ++++++++++++++++++- src/main/java/frc/robot/RobotSimulation.java | 8 +---- .../frc/robot/Subsystems/SwerveSubsystem.java | 25 +++++++++++++--- 4 files changed, 66 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 339261d..7da0ace 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -237,6 +237,22 @@ public static Pose2d flipForAlliance(Pose2d pose){ } } + + /** Vision constants (Limelight, etc). */ + public static final class VisionConstants { + public static final String[] LIMELIGHT_NAMES = {"limelight-a", "limelight-b"}; + + public static final String LIMELIGHT_STREAM_URL_FORMAT = "http://%s.local:5800/stream.mjpg"; + public static final boolean LIMELIGHT_STREAM_ENABLED_DEFAULT = true; + + public static final boolean VISION_ENABLED_DEFAULT = true; + public static final double MAX_VISION_ANGULAR_RATE_DEG_PER_SEC = 720.0; + + /** Standard deviations for vision measurements: (x meters, y meters, theta radians). */ + public static final double VISION_STD_DEV_X_METERS = 0.7; + public static final double VISION_STD_DEV_Y_METERS = 0.7; + public static final double VISION_STD_DEV_THETA_RADIANS = 99999.0; + } /* Shooter Constants */ public static final class ShooterConstants { public static final int SHOOTER_ID = 70; //Placeholder ID diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5c12301..9afbbb5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,14 @@ package frc.robot; - +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.cscore.HttpCamera; +import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -20,6 +23,7 @@ import frc.robot.Auto.TrenchToDepotAuto; import frc.robot.Auto.CenterToDepotAuto; import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.VisionConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Command.AutoAlign; import frc.robot.Command.TeleopSwerve; @@ -30,6 +34,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class RobotContainer { @@ -60,9 +65,31 @@ public class RobotContainer { public RobotContainer() { AutoConstants.initDashboard(); + startLimelightStreams(); configureBindings(); } + private void startLimelightStreams() { + SmartDashboard.putBoolean("Limelight Stream Enabled", VisionConstants.LIMELIGHT_STREAM_ENABLED_DEFAULT); + + if (RobotBase.isSimulation()) { + return; + } + + if (!SmartDashboard.getBoolean("Limelight Stream Enabled", VisionConstants.LIMELIGHT_STREAM_ENABLED_DEFAULT)) { + return; + } + + for (String limelightName : VisionConstants.LIMELIGHT_NAMES) { + String url = String.format(VisionConstants.LIMELIGHT_STREAM_URL_FORMAT, limelightName); + SmartDashboard.putString("Vision/" + limelightName + "/StreamURL", url); + + HttpCamera camera = new HttpCamera(limelightName, url); + camera.setConnectionStrategy(ConnectionStrategy.kKeepOpen); + CameraServer.startAutomaticCapture(camera); + } + } + private void configureBindings() { // Y Button = Zero gyro (reset heading to 0° or 180° based on alliance) diff --git a/src/main/java/frc/robot/RobotSimulation.java b/src/main/java/frc/robot/RobotSimulation.java index de37084..08b4007 100644 --- a/src/main/java/frc/robot/RobotSimulation.java +++ b/src/main/java/frc/robot/RobotSimulation.java @@ -13,13 +13,7 @@ import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; -/** - * Desktop simulation hooks. - * - *

This class is called from {@link Robot#simulationInit()} and {@link Robot#simulationPeriodic()} - * so the "WPILib: Simulate Robot Code" action in VS Code can run your robot with a simple physics - * model. - */ + public class RobotSimulation { private final SwerveSubsystem drive; private double lastTimestampSeconds = Timer.getFPGATimestamp(); diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 9af8284..25b8719 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -31,6 +31,7 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; +import frc.robot.Constants.VisionConstants; import frc.robot.Constants.SwerveConstants.ModuleData; import frc.robot.SwerveModule; @@ -77,6 +78,7 @@ public SwerveSubsystem() { //puts out the field field = new Field2d(); SmartDashboard.putData("Field", field); + SmartDashboard.putBoolean("Vision Enabled", VisionConstants.VISION_ENABLED_DEFAULT); configurePathPlanner(); } @@ -140,6 +142,10 @@ public Command startAutoAt(double x, double y, double direction){ + private boolean isVisionEnabled() { + return SmartDashboard.getBoolean("Vision Enabled", VisionConstants.VISION_ENABLED_DEFAULT); + } + private void updateOdometryWithVision (String limelightName){ boolean doRejectUpdate = false; LimelightHelpers.SetRobotOrientation(limelightName, odometry.getEstimatedPosition().getRotation().getDegrees(),0,0,0,0,0); @@ -147,7 +153,7 @@ private void updateOdometryWithVision (String limelightName){ if (mt2 == null){ return; } - if(Math.abs(pigeon.getAngularVelocityZWorld().getValueAsDouble())> 720) + if(Math.abs(pigeon.getAngularVelocityZWorld().getValueAsDouble()) > VisionConstants.MAX_VISION_ANGULAR_RATE_DEG_PER_SEC) { doRejectUpdate = true; } @@ -157,11 +163,19 @@ private void updateOdometryWithVision (String limelightName){ } if(!doRejectUpdate) { - odometry.setVisionMeasurementStdDevs(VecBuilder.fill (.7,.7,99999));// need to measure + odometry.setVisionMeasurementStdDevs( + VecBuilder.fill( + VisionConstants.VISION_STD_DEV_X_METERS, + VisionConstants.VISION_STD_DEV_Y_METERS, + VisionConstants.VISION_STD_DEV_THETA_RADIANS)); // need to measure odometry.addVisionMeasurement( mt2.pose, mt2.timestampSeconds); } + + SmartDashboard.putNumber("Vision/" + limelightName + "/TagCount", mt2.tagCount); + SmartDashboard.putNumber("Vision/" + limelightName + "/AvgTagDist", mt2.avgTagDist); + SmartDashboard.putNumber("Vision/" + limelightName + "/LatencyMs", mt2.latency); } @@ -279,8 +293,11 @@ public void saveModuleOffsets(Rotation2d desiredAngle){ public void periodic() { if (!RobotBase.isSimulation()) { odometry.update(getYaw(), getPositions()); - updateOdometryWithVision("limelight-a"); - updateOdometryWithVision("limelight-b"); + if (isVisionEnabled()) { + for (String limelightName : VisionConstants.LIMELIGHT_NAMES) { + updateOdometryWithVision(limelightName); + } + } } field.setRobotPose(getPose()); From 1dda195a408a84322a78d3bfba89169a6a14f52c Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 26 Feb 2026 20:37:58 -0600 Subject: [PATCH 38/80] Changed the controller a little --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9afbbb5..eae724b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -121,7 +121,7 @@ private void configureBindings() { }, m_shooter)); - helmsController.button(Button.kX.value).onTrue(Commands.startEnd( + helmsController.button(Button.kY.value).onTrue(Commands.startEnd( () -> m_shooter.runIndexer(true), () -> m_shooter.runIndexer(false), m_shooter)); @@ -156,17 +156,17 @@ private void configureBindings() { )); //INTAKE - // raises the intake using the A button on the helms controller m_intake.setDefaultCommand( new RunCommand( () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), m_intake)); - //lowers the intake using the A button on the helms controller + /*raise the intake using the A button on the helms controller helmsController.button(Button.kA.value).onTrue( new InstantCommand(() -> m_intake.raiseIntake(), m_intake) ); + */ // lowers the intake using the X button on the helms controller helmsController.button(Button.kX.value).onTrue( From 8eb46a99cd252ce3fd23572c0dcbe986fa263962 Mon Sep 17 00:00:00 2001 From: Seqi Date: Fri, 27 Feb 2026 16:22:57 -0600 Subject: [PATCH 39/80] Contants fix and potential alt auto align --- .../java/frc/robot/Command/AltAutoAlign.java | 123 ++++++++++++++++++ src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 5 +- 3 files changed, 129 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/Command/AltAutoAlign.java diff --git a/src/main/java/frc/robot/Command/AltAutoAlign.java b/src/main/java/frc/robot/Command/AltAutoAlign.java new file mode 100644 index 0000000..4307fa5 --- /dev/null +++ b/src/main/java/frc/robot/Command/AltAutoAlign.java @@ -0,0 +1,123 @@ +package frc.robot.Command; + +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.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; + +/* Drives the robot in an orbit around the hub while continuously facing the hub center */ + +public class AltAutoAlign extends Command { + + private SwerveSubsystem swerveSubsystem; + + private final PIDController headingController = new PIDController(kHeadingKp,0,0); + private final PIDController radiusController = new PIDController(kRadialKp, kRadialKi, kRadialKd); + + private static final double kDesiredOrbitRadiusMeters = 2; //placeholder + private static final double kMaxRadialSpeedMetersPerSecond = 1.0; // Max speed for correcting radius errors + private static final double kRadialKp = 1.6; //P-gain for radial distance correction + private static final double kRadialKi = 0.0; + private static final double kRadialKd = 0.0; + private static final double kHeadingKp = 4.5; //P-gain for yaw control that faces the hub + + public AltAutoAlign(SwerveSubsystem swerveSubsystem){ + this.swerveSubsystem = swerveSubsystem; + addRequirements(swerveSubsystem); + headingController.enableContinuousInput(-Math.PI, Math.PI); + radiusController.setSetpoint(kDesiredOrbitRadiusMeters); + } + + @Override + public void initialize(){ + headingController.reset(); //Reset yaw PID state every time the command starts + radiusController.reset(); + } + + + @Override + public void execute(){ + Pose2d FieldPosition = swerveSubsystem.getPose(); //Get robot position on field + + Translation2d HubLocation = new Translation2d(4.61,4.03); //Hub location + HubLocation = FieldConstants.flipForAlliance(HubLocation); //Mirror the hub point when we are Red + + Translation2d robotToHub = HubLocation.minus(FieldPosition.getTranslation()); //Vector from robot to hub. + double radialDistance = robotToHub.getNorm(); + /*translation2d that points from the robot to the hub + * getNorm() returns the vector's magnitude (length) + * this line computes how far the robot currently is from the hub + */ + // Stop driving if odometry is incorrect + if (radialDistance < 0.05){ + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(), true); + return; + } + + Translation2d radialDirection = robotToHub.div(radialDistance); //Unit vector that always points toward the hub + //Radial vector rotated 90 degrees counterclockwise + + double radialPidOutput = radiusController.calculate(radialDistance); + + double radialSpeed = MathUtil.clamp( + -radialPidOutput, + -kMaxRadialSpeedMetersPerSecond, + kMaxRadialSpeedMetersPerSecond + ); + + Translation2d fieldRelativeVelocity = radialDirection.times(radialSpeed); + + double speedMagnitude = fieldRelativeVelocity.getNorm(); // Total requested speed + if(speedMagnitude > SwerveConstants.maxSpeed){ + fieldRelativeVelocity = + fieldRelativeVelocity.times(SwerveConstants.maxSpeed / speedMagnitude); + // respect drivetrain max velocity + } + + double desiredHeadingRadians = radialDirection.getAngle().getRadians(); + //Face straight at the hub while moving + double headingFeedforward = 0.0; + if (radialDistance > 1e-3){ + headingFeedforward = (radialDirection.getY()*fieldRelativeVelocity.getX() + - radialDirection.getX() * fieldRelativeVelocity.getY()) / radialDistance; + } + + double headingRate = MathUtil.clamp( + headingFeedforward + + headingController.calculate((FieldPosition.getRotation().getRadians()), desiredHeadingRadians), + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + // Yaw PID output limited to drivetrain capabilities + + ChassisSpeeds requestedSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeVelocity.getX(), + fieldRelativeVelocity.getY(), + headingRate, + FieldPosition.getRotation()); + // Convert into chassis-relative speeds + + swerveSubsystem.driveFromChassisSpeeds(requestedSpeeds, false); + // Command the swerve in closed loop + + } + + + + @Override + public void end(boolean interrupted){ + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(), true); + // Stop the drivetrain + + } + + @Override + public boolean isFinished(){ + return false; + // Driver holds the trigger to stay in auto align + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7da0ace..516b192 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -96,7 +96,7 @@ public static final class SwerveConstants{ public static final double angleConversionFactor = 360.0 / angleGearRatio; /* Swerve Profiling Values */ - public static final double maxSpeed = 3; // meters per second + public static final double maxSpeed = 5; // meters per second public static final double maxAngularVelocity = maxSpeed/driveBaseRadius; //radians per second how fast the robot spin /* Neutral Modes */ @@ -196,7 +196,7 @@ public static boolean isRightSideAuto(){ public static final class FieldConstants { - public static final double FIELD_LENGTH = 17.54824934; + public static final double FIELD_LENGTH = 16.54; public static final double FIELD_WIDTH = 8.052; public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eae724b..43914be 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Command.AltAutoAlign; import frc.robot.Command.AutoAlign; import frc.robot.Command.TeleopSwerve; import frc.robot.Subsystems.IntakeSubsystem; @@ -135,6 +136,8 @@ private void configureBindings() { driveController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, true)); // Right Trigger = Auto-align to right scoring position driveController.axisGreaterThan(Axis.kRightTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, false)); + // Right Bumper = Alt-Auto-Align + driveController.button(Button.kRightBumper.value).whileTrue(new AltAutoAlign(m_drive)); // Default command runs continuously when no other command requires the subsystem. // It automatically pauses when commands like AutoAlign take control, then resumes @@ -198,4 +201,4 @@ public SwerveSubsystem getDriveSubsystem() { return m_drive; } -} +} \ No newline at end of file From bf4f8fe11ad0c3ebf637712a3389325709f1baf1 Mon Sep 17 00:00:00 2001 From: Seqi Date: Fri, 27 Feb 2026 16:37:29 -0600 Subject: [PATCH 40/80] update gitignore with sim files --- .gitignore | 3 +++ ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/Pigeon 2 - 023 - 0 - ext.dat | Bin 2048 -> 0 bytes 11 files changed, 3 insertions(+) delete mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat delete mode 100644 ctre_sim/Pigeon 2 - 017 - 0 - ext.dat delete mode 100644 ctre_sim/Pigeon 2 - 023 - 0 - ext.dat diff --git a/.gitignore b/.gitignore index f809adc..f788dde 100644 --- a/.gitignore +++ b/.gitignore @@ -53,3 +53,6 @@ Thumbs.db # VS Code Settings .vscode/ + +# sim files +.crte_sim/ diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat deleted file mode 100644 index 3007495c25b539647d15b1ed401f145f32577ce2..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdnY;QOCp2!SmOK10gALnvrRojMuzG?x=3#SxxAnH?+`Ij;_IQ^{!$}q!a J85kU(@&I417>57= diff --git a/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat deleted file mode 100644 index 8871b54f9a8c20e1d4d394250f438c22bbd16111..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdna;Q60n2!S089z)8#LnvrRojMu*O@(ZByejVo-4M+(9!*Lgq diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat deleted file mode 100644 index 2bbdfc967067bac8f73d741a6b04f5ec05ebf026..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdnU;Qyat6pV(zXb6mkz-S1JhQMeD49XB-Mt9YlWhIP2v5Lg_E)b0l6wu^rfeMh# IaDd7K0G)meegFUf diff --git a/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat deleted file mode 100644 index 28822ad2c78934e69bef571755c5c2e6aa5868cc..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdnU;Qyat2!TxuenZN=LnvrRojMu;~;M6(If% z8IGll4e|X6Xz~k?gY3h9}iRbJU>G5Eu=C(GVE^Az-kb#f}l^O-`s+8?@I{fcSqT zIF>RpsHPqG`ok7kegU$4gXICJHUyQkrwXJ#C7B$Yut~a-CoSiNgQ@R?8N& diff --git a/ctre_sim/Pigeon 2 - 023 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 023 - 0 - ext.dat deleted file mode 100644 index 95ad7c56327066f2017fc21678b8c7e998d2902f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmX@eVE>wCpz;8k C& Date: Fri, 27 Feb 2026 20:11:26 -0600 Subject: [PATCH 41/80] robot container code --- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 0 -> 2048 bytes .../frc/robot/Auto/CenterToDepotAuto.java | 2 +- src/main/java/frc/robot/Constants.java | 21 +++---- src/main/java/frc/robot/RobotContainer.java | 57 ++++++------------ .../robot/Subsystems/ShooterSubsystem.java | 20 +++--- 9 files changed, 43 insertions(+), 57 deletions(-) create mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat create mode 100644 ctre_sim/Pigeon 2 - 017 - 0 - ext.dat diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..b40e50d301af77c71776d37a2176211ebe318c65 GIT binary patch literal 2048 zcmdnU;Qyat6pV(zXb6mkz-S1JhQMeDU=M-Z)CfkPi~&^EVfBs@5dX%VEq#m(CSS2@ zLWo`{)6@s4=Y$G1AnR{1d^SNuBNADD0gAjf4>Q6j2x|?*{uJdseT)r(*J^>H$Z8aj N~~lLF%JL{1S5X{ literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..a8a740cb8b94f1532c1dbc9fc0a929e2f184b8ff GIT binary patch literal 2048 zcmdnU;Qyat6pV(zXb6mkz-S1JhQMeDU=M-Z)CfkPi~&^EVfBs@5dX%VEq#m(CSS2@ zLWn--SJVfo=Y$G1AnR{1d=|jC8by8qRQ}Hmkoml;9&CWv#{|{Sibg}Za7tkfM16|# U9-w}P1GPXIX1FW^g9B6^0NfKJssI20 literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..d0d993e3355567851b38b36ad3e32f63491f79ee GIT binary patch literal 2048 zcmdnU;Qyat2!TxuenZN=LnvrRojMuc^+(l*atEL1Xv*yoQ87Yl)@T_ X`V{3oK=&~ms0GR}!(|y59H8<5ve70O literal 0 HcmV?d00001 diff --git a/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..0d5521e38988239e6c1a2b3aa5cf879d64ef741f GIT binary patch literal 2048 zcmX@iVEdn86i_V$4l&q`h9}iRbJU>G5Eu=C(GVB{A&{FI!3cDw0o17ut9O)u_&4rs z>0@Lt`AWzK6TAz+`Z=Kr8<6!k7(RQTw_G1&9!!1#RQ}Hmkoml;9&CWv2T}t9=(KO>jAoIsk_8Aol drive.drive(0.9,0,0,false),drive), Commands.waitSeconds(2), diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 516b192..27def89 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -197,16 +197,13 @@ public static boolean isRightSideAuto(){ public static final class FieldConstants { public static final double FIELD_LENGTH = 16.54; - public static final double FIELD_WIDTH = 8.052; + public static final double FIELD_WIDTH = 8.07; public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); /** * If true, the robot will behave as if it is always on the Blue alliance (no field mirroring), - * even when connected to FMS / Driver Station reports Red. - * - * WARNING: Enabling this for real matches while actually on Red will make autos/field-oriented - * behavior mirror incorrectly. + * even when connected to FMS / Driver Station reports Red.1 */ public static final boolean FORCE_BLUE_ALLIANCE = true; @@ -255,13 +252,13 @@ public static final class VisionConstants { } /* Shooter Constants */ public static final class ShooterConstants { - public static final int SHOOTER_ID = 70; //Placeholder ID - public static final int FEEDER_ID = 61; //Feeder ID - public static final int HOOD_ID = 62; //Hood ID (NEED CHANGE) - public static final int INDEXER_ID = 75; //Placeholder + public static final int SHOOTER_ID = 22; + public static final int KICKER_ID = 21; + public static final int HOOD_ID = 20; + public static final int INDEXER_ID = 23; public static final double SHOOTER_SPEED = 0.5; //Placeholder speed - public static final double FEEDER_SPEED = 0.5; + public static final double KICKER_SPEED = 0.5; public static final double INDEXER_SPEED = 0.5; //placeholder public static final double HOOD_ANGLE_LOW = 0.0; @@ -273,10 +270,10 @@ public static final class ShooterConstants { public static final class IntakeConstants { // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. - public static int INTAKE_ID = 63; // TODO: set to your intake motor CAN ID + public static int INTAKE_ID = 19; // TODO: set to your intake motor CAN ID public static double INTAKE_SPEED = 50; //placeholder for percent power for intake - public static int INTAKE_ARM_ID = 64; // TODO: set to your intake arm motor CAN ID + public static int INTAKE_ARM_ID = 18; // TODO: set to your intake arm motor CAN ID public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later public static double INTAKE_ARM_LOWERED_POSITION = 0; public static double INTAKE_ARM_MINIMUM = 0; // placeholders diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 43914be..1e33a57 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.DriveTestAuto; import frc.robot.Auto.EightLemonAuto; @@ -31,10 +32,6 @@ import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -104,32 +101,24 @@ private void configureBindings() { // SHOOTER CONTROLLER - helmsController.axisGreaterThan(Axis.kRightTrigger.value, 0.1) - .whileTrue(Commands.startEnd( - () -> m_shooter.runShooter(true), - () -> m_shooter.runShooter(false), - m_shooter)); - m_shooter.setDefaultCommand( Commands.run( () -> { - double feederAxis = helmsController.getRawAxis(Axis.kRightY.value); - double feederSpeed = 0.0; - if (Math.abs(feederAxis) > 0.1) { - feederSpeed = -Math.signum(feederAxis) * ShooterConstants.FEEDER_SPEED; - } - m_shooter.setFeederSpeed(feederSpeed); + // Right stick Y controls shooter + indexer together. + // Invert so stick-up (negative on Xbox) produces positive motor output. + double shooterAxis = -MathUtil.applyDeadband( + helmsController.getRawAxis(Axis.kRightY.value), + 0.1); + + m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); + m_shooter.setIndexerSpeed(shooterAxis * ShooterConstants.INDEXER_SPEED); + + // Right bumper runs the kicker while held. + m_shooter.setKickerSpeed(helmsController.getHID().getRightBumper() + ? ShooterConstants.KICKER_SPEED + : 0.0); }, m_shooter)); - - helmsController.button(Button.kY.value).onTrue(Commands.startEnd( - () -> m_shooter.runIndexer(true), - () -> m_shooter.runIndexer(false), - m_shooter)); - - - helmsController.button(Button.kB.value).onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); - helmsController.button(Button.kY.value).onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); // Left Trigger = Auto-align to left scoring position @@ -164,17 +153,11 @@ private void configureBindings() { () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), m_intake)); - - /*raise the intake using the A button on the helms controller - helmsController.button(Button.kA.value).onTrue( - new InstantCommand(() -> m_intake.raiseIntake(), m_intake) - ); - */ - - // lowers the intake using the X button on the helms controller - helmsController.button(Button.kX.value).onTrue( - new InstantCommand(() -> m_intake.lowerIntake(), m_intake) - ); + // Intake arm on helms D-pad. + new POVButton(helmsController.getHID(), 0) + .onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); + new POVButton(helmsController.getHID(), 180) + .onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); } private double getSpeedMultiplier(){ @@ -201,4 +184,4 @@ public SwerveSubsystem getDriveSubsystem() { return m_drive; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index bb77e2b..3e1aed1 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -20,7 +20,7 @@ public class ShooterSubsystem extends SubsystemBase { public boolean isShooterActive = false; //Shooter True SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); - SparkMax feederMotor = new SparkMax(ShooterConstants.FEEDER_ID, MotorType.kBrushless); + SparkMax kickerMotor = new SparkMax(ShooterConstants.KICKER_ID, MotorType.kBrushless); SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); SparkMax indexerMotor = new SparkMax(ShooterConstants.INDEXER_ID, MotorType.kBrushless); @@ -59,8 +59,9 @@ public ShooterSubsystem() { shooterMotor.configure(shootConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - feederMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + kickerMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodMotor.configure(hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + indexerMotor.configure(indexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodController.setTolerance(ShooterConstants.HOOD_TOLERANCE); } @@ -87,13 +88,18 @@ public void runShooter(boolean shooterOn) { } } + public void setShooterSpeed(double speed) { + isShooterActive = Math.abs(speed) > 0.0; + shooterMotor.set(speed); + } + - public void runFeeder(boolean feederOn){ - setFeederSpeed(feederOn ? ShooterConstants.FEEDER_SPEED : 0); + public void runKicker(boolean kickerOn){ + setKickerSpeed(kickerOn ? ShooterConstants.KICKER_SPEED : 0); } - public void setFeederSpeed(double speed) { - feederMotor.set(speed); + public void setKickerSpeed(double speed) { + kickerMotor.set(speed); } public void setHoodAngle(HoodAngle angle) { @@ -145,4 +151,4 @@ public void periodic() { hoodMotor.set(0); } } -} \ No newline at end of file +} From 0c4d359767ced73b9dcb3f386d42d3bcc84d46ec Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 27 Feb 2026 20:36:09 -0600 Subject: [PATCH 42/80] changed the IDs for the kicker and the indexer and the helms controller --- src/main/java/frc/robot/Constants.java | 14 +++++------ src/main/java/frc/robot/RobotContainer.java | 10 +++++--- .../frc/robot/Subsystems/IntakeSubsystem.java | 25 ++++++++++++------- 3 files changed, 29 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 27def89..1d4ffb5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -253,11 +253,11 @@ public static final class VisionConstants { /* Shooter Constants */ public static final class ShooterConstants { public static final int SHOOTER_ID = 22; - public static final int KICKER_ID = 21; + public static final int KICKER_ID = 23; public static final int HOOD_ID = 20; - public static final int INDEXER_ID = 23; + public static final int INDEXER_ID = 21; - public static final double SHOOTER_SPEED = 0.5; //Placeholder speed + public static final double SHOOTER_SPEED = 0.75; //Placeholder speed public static final double KICKER_SPEED = 0.5; public static final double INDEXER_SPEED = 0.5; //placeholder @@ -270,17 +270,17 @@ public static final class ShooterConstants { public static final class IntakeConstants { // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. - public static int INTAKE_ID = 19; // TODO: set to your intake motor CAN ID + public static int INTAKE_ID = 19; public static double INTAKE_SPEED = 50; //placeholder for percent power for intake - public static int INTAKE_ARM_ID = 18; // TODO: set to your intake arm motor CAN ID + public static int INTAKE_ARM_ID = 18; public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later public static double INTAKE_ARM_LOWERED_POSITION = 0; public static double INTAKE_ARM_MINIMUM = 0; // placeholders public static double INTAKE_ARM_MAXIMUM = 90; - public static int GEAR_RATIO = 3; + public static int GEAR_RATIO = 25; - public static double INTAKE_ARM_kP = 0.01; + public static double INTAKE_ARM_kP = 0.000001; public static double INTAKE_ARM_kI = 0; public static double INTAKE_ARM_kD = 0; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1e33a57..cbe8de6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.DriveTestAuto; import frc.robot.Auto.EightLemonAuto; @@ -152,11 +151,14 @@ private void configureBindings() { new RunCommand( () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), m_intake)); + + - // Intake arm on helms D-pad. - new POVButton(helmsController.getHID(), 0) + // Intake arm on helms controller buttons. + // X = raise arm, A = lower arm. + helmsController.button(Button.kX.value) .onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); - new POVButton(helmsController.getHID(), 180) + helmsController.button(Button.kA.value) .onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); } diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 18a7d89..bf727c4 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -32,7 +32,7 @@ public class IntakeSubsystem extends SubsystemBase { public double targetPosition; private boolean intakeOn = false; - private boolean intakeUp = true; + private boolean intakeUp = false; /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { @@ -45,11 +45,14 @@ public IntakeSubsystem() { SparkMaxConfig intakeArmConfig = new SparkMaxConfig(); intakeArmConfig.inverted(false); intakeArmConfig.idleMode(IdleMode.kBrake); - intakeArmConfig.encoder.positionConversionFactor(360/IntakeConstants.GEAR_RATIO); + // Convert motor rotations -> arm degrees (assumes INTAKEConstants.GEAR_RATIO is motor:arm reduction). + intakeArmConfig.encoder.positionConversionFactor(360.0 / IntakeConstants.GEAR_RATIO); intakeArmMotor.configure(intakeArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - intakeArmEncoder.setPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); - targetPosition = IntakeConstants.INTAKE_ARM_RAISED_POSITION; // start with arm raised + // On enable, assume the arm is sitting on the floor at 0° and hold there. + intakeArmEncoder.setPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); + intakeArmPID.reset(); + targetPosition = IntakeConstants.INTAKE_ARM_LOWERED_POSITION; } public void toggleIntake() { @@ -94,15 +97,19 @@ public void moveIntake() { } public double getArmPosition() { - return intakeArmEncoder.getPosition() * 360; + // With positionConversionFactor set, encoder position is already in degrees. + return intakeArmEncoder.getPosition(); } @Override public void periodic() { // This method will be called once per scheduler run - double PIDOutput = intakeArmFeedForward.calculate( - Units.degreesToRadians(intakeArmEncoder.getPosition()),0) - + intakeArmPID.calculate(getArmPosition(), targetPosition); - intakeArmMotor.set(PIDOutput); + double currentDegrees = getArmPosition(); + double output = intakeArmFeedForward.calculate(Units.degreesToRadians(currentDegrees), 0) + + intakeArmPID.calculate(currentDegrees, targetPosition); + + // SparkMax expects [-1, 1]; clamp to avoid slamming the arm on startup/tuning mistakes. + output = Math.max(-1.0, Math.min(1.0, output)); + intakeArmMotor.set(output); } } From 598357aaec4d237fed0a648975d5cdcd2a1a99ac Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 27 Feb 2026 20:45:10 -0600 Subject: [PATCH 43/80] testing --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Subsystems/ShooterSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1d4ffb5..c5f62d9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -280,7 +280,7 @@ public static final class IntakeConstants { public static double INTAKE_ARM_MAXIMUM = 90; public static int GEAR_RATIO = 25; - public static double INTAKE_ARM_kP = 0.000001; + public static double INTAKE_ARM_kP = 0.001; public static double INTAKE_ARM_kI = 0; public static double INTAKE_ARM_kD = 0; } diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 3e1aed1..28b98b7 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -53,7 +53,7 @@ public ShooterSubsystem() { hoodConfig.idleMode(IdleMode.kBrake); SparkMaxConfig indexConfig = new SparkMaxConfig(); - indexConfig.inverted(false); + indexConfig.inverted(true); indexConfig.idleMode(IdleMode.kBrake); From d058c61dae44f44a6f7222b31ab22112448fd257 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 27 Feb 2026 20:50:58 -0600 Subject: [PATCH 44/80] testing --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/Subsystems/ShooterSubsystem.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cbe8de6..eec8896 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -110,11 +110,11 @@ private void configureBindings() { 0.1); m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); - m_shooter.setIndexerSpeed(shooterAxis * ShooterConstants.INDEXER_SPEED); + m_shooter.setKickerSpeed(shooterAxis * ShooterConstants.KICKER_SPEED); // Right bumper runs the kicker while held. - m_shooter.setKickerSpeed(helmsController.getHID().getRightBumper() - ? ShooterConstants.KICKER_SPEED + m_shooter.setIndexerSpeed(helmsController.getHID().getRightBumper() + ? ShooterConstants.INDEXER_SPEED : 0.0); }, m_shooter)); diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 28b98b7..c798870 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -45,7 +45,7 @@ public ShooterSubsystem() { shootConfig.idleMode(IdleMode.kCoast); SparkMaxConfig feedConfig = new SparkMaxConfig(); - feedConfig.inverted(false); + feedConfig.inverted(true); feedConfig.idleMode(IdleMode.kBrake); SparkMaxConfig hoodConfig = new SparkMaxConfig(); @@ -53,7 +53,7 @@ public ShooterSubsystem() { hoodConfig.idleMode(IdleMode.kBrake); SparkMaxConfig indexConfig = new SparkMaxConfig(); - indexConfig.inverted(true); + indexConfig.inverted(false); indexConfig.idleMode(IdleMode.kBrake); From a3997ab98dd9801c57b3a5d7dd09b218ea0f9175 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 27 Feb 2026 20:54:41 -0600 Subject: [PATCH 45/80] testing --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 15 ++++++++------- .../frc/robot/Subsystems/IntakeSubsystem.java | 7 +++++++ 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c5f62d9..36ded10 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -252,8 +252,8 @@ public static final class VisionConstants { } /* Shooter Constants */ public static final class ShooterConstants { - public static final int SHOOTER_ID = 22; - public static final int KICKER_ID = 23; + public static final int SHOOTER_ID = 23; + public static final int KICKER_ID = 22; public static final int HOOD_ID = 20; public static final int INDEXER_ID = 21; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eec8896..6c3ba6e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,7 +103,7 @@ private void configureBindings() { m_shooter.setDefaultCommand( Commands.run( () -> { - // Right stick Y controls shooter + indexer together. + // Right stick Y controls shooter + kicker together. // Invert so stick-up (negative on Xbox) produces positive motor output. double shooterAxis = -MathUtil.applyDeadband( helmsController.getRawAxis(Axis.kRightY.value), @@ -112,7 +112,7 @@ private void configureBindings() { m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); m_shooter.setKickerSpeed(shooterAxis * ShooterConstants.KICKER_SPEED); - // Right bumper runs the kicker while held. + // Right bumper runs the indexer while held. m_shooter.setIndexerSpeed(helmsController.getHID().getRightBumper() ? ShooterConstants.INDEXER_SPEED : 0.0); @@ -154,12 +154,13 @@ private void configureBindings() { - // Intake arm on helms controller buttons. + // Intake arm buttons. // X = raise arm, A = lower arm. - helmsController.button(Button.kX.value) - .onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); - helmsController.button(Button.kA.value) - .onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); + // Bound on both controllers so it works regardless of which one you're pressing. + helmsController.x().onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); + helmsController.a().onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); + driveController.x().onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); + driveController.a().onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); } private double getSpeedMultiplier(){ diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index bf727c4..9479e02 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; @@ -79,11 +80,13 @@ public void setTargetPosition(double position) { public void raiseIntake() { setTargetPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); + intakeArmPID.reset(); intakeUp = true; } public void lowerIntake() { setTargetPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); + intakeArmPID.reset(); intakeUp = false; } @@ -111,5 +114,9 @@ public void periodic() { // SparkMax expects [-1, 1]; clamp to avoid slamming the arm on startup/tuning mistakes. output = Math.max(-1.0, Math.min(1.0, output)); intakeArmMotor.set(output); + + SmartDashboard.putNumber("IntakeArm/TargetDeg", targetPosition); + SmartDashboard.putNumber("IntakeArm/PositionDeg", currentDegrees); + SmartDashboard.putNumber("IntakeArm/Output", output); } } From f68db0ca6face08cdf86024c82ec28b7d30f09b7 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 27 Feb 2026 21:00:04 -0600 Subject: [PATCH 46/80] ID change --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 15 +++++++++++++ .../robot/Subsystems/ShooterSubsystem.java | 21 +++++++++++++++---- 3 files changed, 35 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 36ded10..9e38dc8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -252,10 +252,10 @@ public static final class VisionConstants { } /* Shooter Constants */ public static final class ShooterConstants { - public static final int SHOOTER_ID = 23; - public static final int KICKER_ID = 22; + public static final int SHOOTER_ID = 22; + public static final int KICKER_ID = 21; public static final int HOOD_ID = 20; - public static final int INDEXER_ID = 21; + public static final int INDEXER_ID = 23; public static final double SHOOTER_SPEED = 0.75; //Placeholder speed public static final double KICKER_SPEED = 0.5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c3ba6e..bc685ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -119,6 +119,21 @@ private void configureBindings() { }, m_shooter)); + // Helms Y = run shooter+kicker+indexer while held (easy "does it work?" test). + helmsController.y().whileTrue( + Commands.runEnd( + () -> { + m_shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + m_shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + m_shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, + () -> { + m_shooter.setShooterSpeed(0.0); + m_shooter.setKickerSpeed(0.0); + m_shooter.setIndexerSpeed(0.0); + }, + m_shooter)); + // Left Trigger = Auto-align to left scoring position driveController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, true)); diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index c798870..9a97e14 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -19,10 +19,14 @@ public class ShooterSubsystem extends SubsystemBase { public boolean isShooterActive = false; //Shooter True - SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); - SparkMax kickerMotor = new SparkMax(ShooterConstants.KICKER_ID, MotorType.kBrushless); - SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); - SparkMax indexerMotor = new SparkMax(ShooterConstants.INDEXER_ID, MotorType.kBrushless); + private final SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); + private final SparkMax kickerMotor = new SparkMax(ShooterConstants.KICKER_ID, MotorType.kBrushless); + private final SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); + private final SparkMax indexerMotor = new SparkMax(ShooterConstants.INDEXER_ID, MotorType.kBrushless); + + private double shooterCmd = 0.0; + private double kickerCmd = 0.0; + private double indexerCmd = 0.0; private final PIDController hoodController = new PIDController( ShooterConstants.HOOD_KP, @@ -90,6 +94,7 @@ public void runShooter(boolean shooterOn) { public void setShooterSpeed(double speed) { isShooterActive = Math.abs(speed) > 0.0; + shooterCmd = speed; shooterMotor.set(speed); } @@ -99,6 +104,7 @@ public void runKicker(boolean kickerOn){ } public void setKickerSpeed(double speed) { + kickerCmd = speed; kickerMotor.set(speed); } @@ -126,6 +132,7 @@ public void runIndexer(boolean indexerOn) { } public void setIndexerSpeed(double speed) { + indexerCmd = speed; indexerMotor.set(speed); } @@ -135,6 +142,12 @@ public void periodic() { SmartDashboard.putBoolean("Is Shooter Active", isShooterActive); SmartDashboard.putNumber("Hood Target Position", hoodTargetPosition); SmartDashboard.putNumber("Hood Position", getHoodPosition()); + SmartDashboard.putNumber("Shooter/Cmd", shooterCmd); + SmartDashboard.putNumber("Shooter/VelocityRPM", shooterMotor.getEncoder().getVelocity()); + SmartDashboard.putNumber("Kicker/Cmd", kickerCmd); + SmartDashboard.putNumber("Kicker/VelocityRPM", kickerMotor.getEncoder().getVelocity()); + SmartDashboard.putNumber("Indexer/Cmd", indexerCmd); + SmartDashboard.putNumber("Indexer/VelocityRPM", indexerMotor.getEncoder().getVelocity()); if (hoodActive) { From b1fa4527a844a1afc79088b1af7ab0849fe2b9bb Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 27 Feb 2026 21:33:04 -0600 Subject: [PATCH 47/80] made intake arm work --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/Subsystems/IntakeSubsystem.java | 2 +- src/main/java/frc/robot/Subsystems/ShooterSubsystem.java | 6 +++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e38dc8..2429244 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -257,8 +257,8 @@ public static final class ShooterConstants { public static final int HOOD_ID = 20; public static final int INDEXER_ID = 23; - public static final double SHOOTER_SPEED = 0.75; //Placeholder speed - public static final double KICKER_SPEED = 0.5; + public static final double SHOOTER_SPEED = 0.60; //Placeholder speed + public static final double KICKER_SPEED = 0.60; public static final double INDEXER_SPEED = 0.5; //placeholder public static final double HOOD_ANGLE_LOW = 0.0; @@ -280,7 +280,7 @@ public static final class IntakeConstants { public static double INTAKE_ARM_MAXIMUM = 90; public static int GEAR_RATIO = 25; - public static double INTAKE_ARM_kP = 0.001; + public static double INTAKE_ARM_kP = 0.01; public static double INTAKE_ARM_kI = 0; public static double INTAKE_ARM_kD = 0; } diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 9479e02..7d7e72d 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -44,7 +44,7 @@ public IntakeSubsystem() { intakeMotor.configure(intakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); SparkMaxConfig intakeArmConfig = new SparkMaxConfig(); - intakeArmConfig.inverted(false); + intakeArmConfig.inverted(true); intakeArmConfig.idleMode(IdleMode.kBrake); // Convert motor rotations -> arm degrees (assumes INTAKEConstants.GEAR_RATIO is motor:arm reduction). intakeArmConfig.encoder.positionConversionFactor(360.0 / IntakeConstants.GEAR_RATIO); diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 9a97e14..57b09f2 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -45,11 +45,11 @@ public enum HoodAngle { public ShooterSubsystem() { SparkMaxConfig shootConfig = new SparkMaxConfig(); - shootConfig.inverted(false); + shootConfig.inverted(true); shootConfig.idleMode(IdleMode.kCoast); SparkMaxConfig feedConfig = new SparkMaxConfig(); - feedConfig.inverted(true); + feedConfig.inverted(false); feedConfig.idleMode(IdleMode.kBrake); SparkMaxConfig hoodConfig = new SparkMaxConfig(); @@ -57,7 +57,7 @@ public ShooterSubsystem() { hoodConfig.idleMode(IdleMode.kBrake); SparkMaxConfig indexConfig = new SparkMaxConfig(); - indexConfig.inverted(false); + indexConfig.inverted(true); indexConfig.idleMode(IdleMode.kBrake); From 789a3d097ee4737db1fb43b81b8dd93cca2da0c9 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 27 Feb 2026 23:47:11 -0600 Subject: [PATCH 48/80] Controller, intake, shooter Bound the hood to the controller (button Y & B); coded the intake arm so it doesn't slam on the floor, and made the intake go the correct direction for intake (push stick forward); setup the shooter to go up to speed right away instead of increasing the speed as the stick gets pushed --- src/main/java/frc/robot/Constants.java | 399 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 56 +-- .../frc/robot/Subsystems/IntakeSubsystem.java | 35 +- .../robot/Subsystems/ShooterSubsystem.java | 7 + 4 files changed, 272 insertions(+), 225 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2429244..4e1ba2d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,113 +29,113 @@ public final class Constants { // Swerve Constants - public static final class SwerveConstants{ - public static final double inputDeadband = .1; // Deadzone for joystick inputs to prevent drift - public static final int PIGEON_ID = 17; //CAN ID for Pigeon gyro sensor - public static final boolean invertPigeon = false; // Whether to invert gyro readings - - /* Drivetrain Constants */ - public static final double halfTrackWidth = Units.inchesToMeters(27/2.0);//to find - public static final double halfWheelBase = Units.inchesToMeters(27/2.0);//to find - public static final double wheelDiameter = Units.inchesToMeters(4.0); - public static final double wheelCircumference = wheelDiameter * Math.PI; - //halfTrackWidth/halfwheelBase are already "half" distances, so don't divide again. - //public static final double driveBaseRadius = Math.hypot(halfTrackWidth/2, halfWheelBase/2); - public static final double driveBaseRadius = Math.hypot(halfWheelBase, halfTrackWidth); - - - public static final double openLoopRamp = 0.25; - public static final double closedLoopRamp = 0.0; - - public static final double driveGearRatio = (6.75 / 1.0); // 6.75:1 L2 Mk4 Modules - //L1 is 8.14:1, L2 is 6.75:1, L3 is 6.12:1, L4 is 5.14:1 - public static final double angleGearRatio = (21.4 / 1.0); // 21.4:1 MK4i Modules - //SDS Mk4 is 12.8:1, Mk4i is 21.4:1 - - public static final SwerveDriveKinematics swerveKinematics = - new SwerveDriveKinematics( - //WPILib coordinate system: +X = forward, +Y = left - new Translation2d(halfTrackWidth, halfWheelBase), //Front left - new Translation2d(halfTrackWidth, -halfWheelBase), //Front right - new Translation2d(-halfTrackWidth, -halfWheelBase), //Back right - new Translation2d(-halfTrackWidth, halfWheelBase)); //Back Left - //translation 2d locates the swerve module in cords - //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html - //SwerveDrive Kinematics converts between a ChassisSpeeds object and several SwerveModuleState objects, - //which contains velocities and angles for each swerve module of a swerve drive robot. - - /* Swerve Voltage Compensation */ - public static final double voltageComp = 12.0; +public static final class SwerveConstants{ + public static final double inputDeadband = .1; // Deadzone for joystick inputs to prevent drift + public static final int PIGEON_ID = 17; //CAN ID for Pigeon gyro sensor + public static final boolean invertPigeon = false; // Whether to invert gyro readings + + /* Drivetrain Constants */ + public static final double halfTrackWidth = Units.inchesToMeters(27/2.0);//to find + public static final double halfWheelBase = Units.inchesToMeters(27/2.0);//to find + public static final double wheelDiameter = Units.inchesToMeters(4.0); + public static final double wheelCircumference = wheelDiameter * Math.PI; + //halfTrackWidth/halfwheelBase are already "half" distances, so don't divide again. + //public static final double driveBaseRadius = Math.hypot(halfTrackWidth/2, halfWheelBase/2); + public static final double driveBaseRadius = Math.hypot(halfWheelBase, halfTrackWidth); + + + public static final double openLoopRamp = 0.25; + public static final double closedLoopRamp = 0.0; + + public static final double driveGearRatio = (6.75 / 1.0); // 6.75:1 L2 Mk4 Modules + //L1 is 8.14:1, L2 is 6.75:1, L3 is 6.12:1, L4 is 5.14:1 + public static final double angleGearRatio = (21.4 / 1.0); // 21.4:1 MK4i Modules + //SDS Mk4 is 12.8:1, Mk4i is 21.4:1 + + public static final SwerveDriveKinematics swerveKinematics = + new SwerveDriveKinematics( + //WPILib coordinate system: +X = forward, +Y = left + new Translation2d(halfTrackWidth, halfWheelBase), //Front left + new Translation2d(halfTrackWidth, -halfWheelBase), //Front right + new Translation2d(-halfTrackWidth, -halfWheelBase), //Back right + new Translation2d(-halfTrackWidth, halfWheelBase)); //Back Left + //translation 2d locates the swerve module in cords + //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html + //SwerveDrive Kinematics converts between a ChassisSpeeds object and several SwerveModuleState objects, + //which contains velocities and angles for each swerve module of a swerve drive robot. + + /* Swerve Voltage Compensation */ + public static final double voltageComp = 12.0; - //Swerve Current Limiting for neos - public static final int angleContinuousCurrentLimit = 20; //limits current draw of turning motor - public static final int driveContinuousCurrentLimit = 40; //limits current draw of drive motor - - - - /* Drive Motor PID Values */ - public static final double driveKP = 0.1; //to tune - public static final double driveKI = 0.0; //to tune - public static final double driveKD = 0.0; //to tune + //Swerve Current Limiting for neos + public static final int angleContinuousCurrentLimit = 20; //limits current draw of turning motor + public static final int driveContinuousCurrentLimit = 40; //limits current draw of drive motor + + + + /* Drive Motor PID Values */ + public static final double driveKP = 0.1; //to tune + public static final double driveKI = 0.0; //to tune + public static final double driveKD = 0.0; //to tune + + /* Drive Motor Characterization Values */ + //values to calculate the drive feedforward (KFF) + public static final double driveKS = 0.667; //to calculate + public static final double driveKV = 2.4; //to calculate + public static final double driveKA = 0.5; //to calculate + + /* Angle Motor PID Values */ + public static final double angleKP = 0.01; //to tune + public static final double angleKI = 0.0; //to tune, keep it at zero unless you see a persistent offset + public static final double angleKD = 0.0; //to tune + + /* Drive Motor Conversion Factors */ + public static final double driveConversionPositionFactor = + (wheelDiameter * Math.PI) / driveGearRatio; + public static final double driveConversionVelocityFactor = driveConversionPositionFactor / 60.0; + public static final double angleConversionFactor = 360.0 / angleGearRatio; + + /* Swerve Profiling Values */ + public static final double maxSpeed = 5; // meters per second + public static final double maxAngularVelocity = maxSpeed/driveBaseRadius; //radians per second how fast the robot spin + + /* Neutral Modes */ + public static final IdleMode angleNeutralMode = IdleMode.kBrake; + public static final IdleMode driveNeutralMode = IdleMode.kBrake; + + /* Motor Inverts */ + public static final boolean canCoderInvert = false; + public static final boolean driveInvert = false; + public static final boolean angleInvert = true; + + //Location of modules + public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); + public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); + public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); + public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); + + /* Module Specific Constants */ + public record ModuleData( + int driveMotorID, + int angleMotorID, + int encoderID, + double angleOffset, + Translation2d location, + boolean driveInvert, + boolean angleInvert + ){} + + public static ModuleData[] moduleData = { + new ModuleData(6, 5, 7, 31.46, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left + // Module 1 is currently the only module oscillating; flip its angle motor invert so its + // steering closed-loop sign matches the encoder direction. + // Module 1: also invert drive so +X command drives forward like the others. + new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right + new ModuleData(12, 11, 13, 33.13, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right + new ModuleData(15, 14, 16, 8.52, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left + }; - /* Drive Motor Characterization Values */ - //values to calculate the drive feedforward (KFF) - public static final double driveKS = 0.667; //to calculate - public static final double driveKV = 2.4; //to calculate - public static final double driveKA = 0.5; //to calculate - - /* Angle Motor PID Values */ - public static final double angleKP = 0.01; //to tune - public static final double angleKI = 0.0; //to tune, keep it at zero unless you see a persistent offset - public static final double angleKD = 0.0; //to tune - - /* Drive Motor Conversion Factors */ - public static final double driveConversionPositionFactor = - (wheelDiameter * Math.PI) / driveGearRatio; - public static final double driveConversionVelocityFactor = driveConversionPositionFactor / 60.0; - public static final double angleConversionFactor = 360.0 / angleGearRatio; - - /* Swerve Profiling Values */ - public static final double maxSpeed = 5; // meters per second - public static final double maxAngularVelocity = maxSpeed/driveBaseRadius; //radians per second how fast the robot spin - - /* Neutral Modes */ - public static final IdleMode angleNeutralMode = IdleMode.kBrake; - public static final IdleMode driveNeutralMode = IdleMode.kBrake; - - /* Motor Inverts */ - public static final boolean canCoderInvert = false; - public static final boolean driveInvert = false; - public static final boolean angleInvert = true; - - //Location of modules - public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); - public static final Translation2d FRONT_RIGHT = new Translation2d(halfWheelBase, -halfTrackWidth); - public static final Translation2d BACK_RIGHT = new Translation2d(-halfWheelBase, -halfTrackWidth); - public static final Translation2d BACK_LEFT = new Translation2d(-halfWheelBase, halfTrackWidth); - - /* Module Specific Constants */ - public record ModuleData( - int driveMotorID, - int angleMotorID, - int encoderID, - double angleOffset, - Translation2d location, - boolean driveInvert, - boolean angleInvert - ){} - - public static ModuleData[] moduleData = { - new ModuleData(6, 5, 7, 31.46, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left - // Module 1 is currently the only module oscillating; flip its angle motor invert so its - // steering closed-loop sign matches the encoder direction. - // Module 1: also invert drive so +X command drives forward like the others. - new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right - new ModuleData(12, 11, 13, 33.13, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right - new ModuleData(15, 14, 16, 8.52, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left - }; - - } +} public static final class AutoConstants { @@ -196,96 +196,117 @@ public static boolean isRightSideAuto(){ public static final class FieldConstants { - public static final double FIELD_LENGTH = 16.54; - public static final double FIELD_WIDTH = 8.07; - - public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); - - /** - * If true, the robot will behave as if it is always on the Blue alliance (no field mirroring), - * even when connected to FMS / Driver Station reports Red.1 - */ - public static final boolean FORCE_BLUE_ALLIANCE = true; - - public static boolean isRedAlliance(){ - if (FORCE_BLUE_ALLIANCE) { - return false; - } - // Default to Blue when alliance is unknown (common in sim/practice). - return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - } - - public static Rotation2d flipForAlliance(Rotation2d rotation){ - if(isRedAlliance()){ - return Rotation2d.fromDegrees(rotation.getDegrees() + 180); - }else{ - return rotation; - } - } - public static Translation2d flipForAlliance(Translation2d pos){ - if(isRedAlliance()){ - return new Translation2d(FIELD_LENGTH - pos.getX(), FIELD_WIDTH - pos.getY()); - }else{ - return pos; - } - } - public static Pose2d flipForAlliance(Pose2d pose){ - return new Pose2d(flipForAlliance(pose.getTranslation()), flipForAlliance(pose.getRotation())); - } - - } + public static final double FIELD_LENGTH = 16.54; + public static final double FIELD_WIDTH = 8.07; - /** Vision constants (Limelight, etc). */ - public static final class VisionConstants { - public static final String[] LIMELIGHT_NAMES = {"limelight-a", "limelight-b"}; - - public static final String LIMELIGHT_STREAM_URL_FORMAT = "http://%s.local:5800/stream.mjpg"; - public static final boolean LIMELIGHT_STREAM_ENABLED_DEFAULT = true; + public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); - public static final boolean VISION_ENABLED_DEFAULT = true; - public static final double MAX_VISION_ANGULAR_RATE_DEG_PER_SEC = 720.0; + /** + * If true, the robot will behave as if it is always on the Blue alliance (no field mirroring), + * even when connected to FMS / Driver Station reports Red.1 + */ + public static final boolean FORCE_BLUE_ALLIANCE = true; - /** Standard deviations for vision measurements: (x meters, y meters, theta radians). */ - public static final double VISION_STD_DEV_X_METERS = 0.7; - public static final double VISION_STD_DEV_Y_METERS = 0.7; - public static final double VISION_STD_DEV_THETA_RADIANS = 99999.0; + public static boolean isRedAlliance(){ + if (FORCE_BLUE_ALLIANCE) { + return false; + } + // Default to Blue when alliance is unknown (common in sim/practice). + return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; + } + + public static Rotation2d flipForAlliance(Rotation2d rotation){ + if(isRedAlliance()){ + return Rotation2d.fromDegrees(rotation.getDegrees() + 180); + }else{ + return rotation; + } } - /* Shooter Constants */ - public static final class ShooterConstants { - public static final int SHOOTER_ID = 22; - public static final int KICKER_ID = 21; - public static final int HOOD_ID = 20; - public static final int INDEXER_ID = 23; - - public static final double SHOOTER_SPEED = 0.60; //Placeholder speed - public static final double KICKER_SPEED = 0.60; - public static final double INDEXER_SPEED = 0.5; //placeholder - - public static final double HOOD_ANGLE_LOW = 0.0; - public static final double HOOD_ANGLE_HIGH = 0.5; - public static final double HOOD_KP = 1.2; - public static final double HOOD_MAX_OUTPUT = 0.4; - public static final double HOOD_TOLERANCE = 0.02; + public static Translation2d flipForAlliance(Translation2d pos){ + if(isRedAlliance()){ + return new Translation2d(FIELD_LENGTH - pos.getX(), FIELD_WIDTH - pos.getY()); + }else{ + return pos; + } } - public static final class IntakeConstants { - // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). - // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. - public static int INTAKE_ID = 19; - public static double INTAKE_SPEED = 50; //placeholder for percent power for intake - - public static int INTAKE_ARM_ID = 18; - public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later - public static double INTAKE_ARM_LOWERED_POSITION = 0; - public static double INTAKE_ARM_MINIMUM = 0; // placeholders - public static double INTAKE_ARM_MAXIMUM = 90; - public static int GEAR_RATIO = 25; - - public static double INTAKE_ARM_kP = 0.01; - public static double INTAKE_ARM_kI = 0; - public static double INTAKE_ARM_kD = 0; + public static Pose2d flipForAlliance(Pose2d pose){ + return new Pose2d(flipForAlliance(pose.getTranslation()), flipForAlliance(pose.getRotation())); } + +} + +/** Vision constants (Limelight, etc). */ +public static final class VisionConstants { + public static final String[] LIMELIGHT_NAMES = {"limelight-a", "limelight-b"}; + + public static final String LIMELIGHT_STREAM_URL_FORMAT = "http://%s.local:5800/stream.mjpg"; + public static final boolean LIMELIGHT_STREAM_ENABLED_DEFAULT = true; + + public static final boolean VISION_ENABLED_DEFAULT = true; + public static final double MAX_VISION_ANGULAR_RATE_DEG_PER_SEC = 720.0; + + /** Standard deviations for vision measurements: (x meters, y meters, theta radians). */ + public static final double VISION_STD_DEV_X_METERS = 0.7; + public static final double VISION_STD_DEV_Y_METERS = 0.7; + public static final double VISION_STD_DEV_THETA_RADIANS = 99999.0; +} + +/* Shooter Constants */ +public static final class ShooterConstants { + public static final int SHOOTER_ID = 22; + public static final int KICKER_ID = 21; + public static final int HOOD_ID = 20; + public static final int INDEXER_ID = 23; + + // Percent output caps ([-1..1]). Higher = faster spin-up but more current draw. + public static final double SHOOTER_SPEED = 1.0; + public static final double KICKER_SPEED = 1.0; + public static final double INDEXER_SPEED = 0.5; //placeholder + + // Electrical limits/compensation. + public static final double SHOOTER_VOLTAGE_COMP = 12.0; + public static final int SHOOTER_CURRENT_LIMIT_AMPS = 60; + public static final int KICKER_CURRENT_LIMIT_AMPS = 60; + + // Hood position units are motor rotations (NEO internal encoder). + // Max travel is 3 rotations = 1080 degrees. + public static final double HOOD_MIN_ROTATIONS = 0.0; + public static final double HOOD_MAX_ROTATIONS = 3.0; + + // Preset positions. + public static final double HOOD_ANGLE_LOW = HOOD_MIN_ROTATIONS; + public static final double HOOD_ANGLE_HIGH = HOOD_MAX_ROTATIONS; // "up" (about 2 inches) + public static final double HOOD_KP = 1.2; + public static final double HOOD_MAX_OUTPUT = 0.4; + public static final double HOOD_TOLERANCE = 0.02; +} + +public static final class IntakeConstants { + // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). + // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. + public static int INTAKE_ID = 19; + public static double INTAKE_SPEED = 50; //placeholder for percent power for intake + + public static int INTAKE_ARM_ID = 18; + public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later + public static double INTAKE_ARM_LOWERED_POSITION = 0; + public static double INTAKE_ARM_MINIMUM = 0; // placeholders + public static double INTAKE_ARM_MAXIMUM = 90; + public static int GEAR_RATIO = 25; + + public static double INTAKE_ARM_kP = 0.01; + public static double INTAKE_ARM_kI = 0; + public static double INTAKE_ARM_kD = 0; + + // Intake arm motion limits (tune to be slower/gentler). + public static double INTAKE_ARM_MAX_VEL_DEG_PER_SEC = 60.0; //Degrees + public static double INTAKE_ARM_MAX_ACCEL_DEG_PER_SEC2 = 120.0; //Degrees/sec + public static double INTAKE_ARM_MAX_OUTPUT_UP = 0.25; //Degrees/sec^2 + public static double INTAKE_ARM_MAX_OUTPUT_DOWN = 0.18; //Percent output (0.1) +} + +public static final class CANdleConstants { + public static final int CANDLE_ID = 18; //Placeholder ID - public static final class CANdleConstants { - public static final int CANDLE_ID = 18; //Placeholder ID } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bc685ae..65efa19 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -101,38 +101,28 @@ private void configureBindings() { // SHOOTER CONTROLLER m_shooter.setDefaultCommand( - Commands.run( - () -> { - // Right stick Y controls shooter + kicker together. - // Invert so stick-up (negative on Xbox) produces positive motor output. - double shooterAxis = -MathUtil.applyDeadband( - helmsController.getRawAxis(Axis.kRightY.value), - 0.1); - - m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); - m_shooter.setKickerSpeed(shooterAxis * ShooterConstants.KICKER_SPEED); - - // Right bumper runs the indexer while held. - m_shooter.setIndexerSpeed(helmsController.getHID().getRightBumper() - ? ShooterConstants.INDEXER_SPEED - : 0.0); - }, - m_shooter)); - - // Helms Y = run shooter+kicker+indexer while held (easy "does it work?" test). - helmsController.y().whileTrue( - Commands.runEnd( - () -> { - m_shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); - m_shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - m_shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - }, - () -> { - m_shooter.setShooterSpeed(0.0); - m_shooter.setKickerSpeed(0.0); - m_shooter.setIndexerSpeed(0.0); - }, - m_shooter)); + Commands.run( + () -> { + // Right stick Y controls shooter + kicker together. + // Invert so stick-up (negative on Xbox) produces positive motor output. + double shooterAxis = -MathUtil.applyDeadband( + helmsController.getRawAxis(Axis.kRightY.value), + 0.1); + + m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); + m_shooter.setKickerSpeed(shooterAxis * ShooterConstants.KICKER_SPEED); + + // Right bumper runs the indexer while held. + m_shooter.setIndexerSpeed(helmsController.getHID().getRightBumper() + ? ShooterConstants.INDEXER_SPEED + : 0.0); + }, + m_shooter)); + + // Hood controls (helms controller). + // Y = hood up (2 inches / max travel), B = hood down. + helmsController.y().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); + helmsController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); // Left Trigger = Auto-align to left scoring position @@ -164,7 +154,7 @@ private void configureBindings() { //INTAKE m_intake.setDefaultCommand( new RunCommand( - () -> m_intake.setIntakePower(-MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), + () -> m_intake.setIntakePower(MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), m_intake)); diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 7d7e72d..470ecfb 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -4,9 +4,12 @@ package frc.robot.Subsystems; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; @@ -35,6 +38,15 @@ public class IntakeSubsystem extends SubsystemBase { private boolean intakeOn = false; private boolean intakeUp = false; + private final TrapezoidProfile.Constraints armConstraints = + new TrapezoidProfile.Constraints( + IntakeConstants.INTAKE_ARM_MAX_VEL_DEG_PER_SEC, + IntakeConstants.INTAKE_ARM_MAX_ACCEL_DEG_PER_SEC2); + + private TrapezoidProfile.State armSetpoint = new TrapezoidProfile.State(0.0, 0.0); + private TrapezoidProfile.State armGoal = new TrapezoidProfile.State(0.0, 0.0); + private double lastTimestampSec = 0.0; + /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { SparkMaxConfig intakeConfig = new SparkMaxConfig(); @@ -54,6 +66,10 @@ public IntakeSubsystem() { intakeArmEncoder.setPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); intakeArmPID.reset(); targetPosition = IntakeConstants.INTAKE_ARM_LOWERED_POSITION; + + armSetpoint = new TrapezoidProfile.State(targetPosition, 0.0); + armGoal = new TrapezoidProfile.State(targetPosition, 0.0); + lastTimestampSec = Timer.getFPGATimestamp(); } public void toggleIntake() { @@ -76,6 +92,7 @@ public void setIntakePower(double power) { public void setTargetPosition(double position) { targetPosition = Math.max(IntakeConstants.INTAKE_ARM_MINIMUM, Math.min(IntakeConstants.INTAKE_ARM_MAXIMUM, position)); + armGoal = new TrapezoidProfile.State(targetPosition, 0.0); } public void raiseIntake() { @@ -107,15 +124,27 @@ public double getArmPosition() { @Override public void periodic() { // This method will be called once per scheduler run + double nowSec = Timer.getFPGATimestamp(); + double dtSec = nowSec - lastTimestampSec; + lastTimestampSec = nowSec; + dtSec = MathUtil.clamp(dtSec, 0.0, 0.05); + double currentDegrees = getArmPosition(); + armSetpoint = new TrapezoidProfile(armConstraints).calculate(dtSec, armGoal, armSetpoint); + double output = intakeArmFeedForward.calculate(Units.degreesToRadians(currentDegrees), 0) - + intakeArmPID.calculate(currentDegrees, targetPosition); + + intakeArmPID.calculate(currentDegrees, armSetpoint.position); - // SparkMax expects [-1, 1]; clamp to avoid slamming the arm on startup/tuning mistakes. - output = Math.max(-1.0, Math.min(1.0, output)); + // Limit output so the arm moves slower/gentler (especially on the way down). + double maxOutput = armSetpoint.velocity >= 0.0 + ? IntakeConstants.INTAKE_ARM_MAX_OUTPUT_UP + : IntakeConstants.INTAKE_ARM_MAX_OUTPUT_DOWN; + output = MathUtil.clamp(output, -maxOutput, maxOutput); intakeArmMotor.set(output); SmartDashboard.putNumber("IntakeArm/TargetDeg", targetPosition); + SmartDashboard.putNumber("IntakeArm/SetpointDeg", armSetpoint.position); + SmartDashboard.putNumber("IntakeArm/SetpointVelDegPerSec", armSetpoint.velocity); SmartDashboard.putNumber("IntakeArm/PositionDeg", currentDegrees); SmartDashboard.putNumber("IntakeArm/Output", output); } diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 57b09f2..92da2af 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -45,12 +45,16 @@ public enum HoodAngle { public ShooterSubsystem() { SparkMaxConfig shootConfig = new SparkMaxConfig(); + shootConfig.smartCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT_AMPS); shootConfig.inverted(true); shootConfig.idleMode(IdleMode.kCoast); + shootConfig.voltageCompensation(ShooterConstants.SHOOTER_VOLTAGE_COMP); SparkMaxConfig feedConfig = new SparkMaxConfig(); + feedConfig.smartCurrentLimit(ShooterConstants.KICKER_CURRENT_LIMIT_AMPS); feedConfig.inverted(false); feedConfig.idleMode(IdleMode.kBrake); + feedConfig.voltageCompensation(ShooterConstants.SHOOTER_VOLTAGE_COMP); SparkMaxConfig hoodConfig = new SparkMaxConfig(); hoodConfig.inverted(false); @@ -119,6 +123,9 @@ public void setHoodAngle(HoodAngle angle) { default: hoodTargetPosition = ShooterConstants.HOOD_ANGLE_HIGH; } + hoodTargetPosition = Math.max( + ShooterConstants.HOOD_MIN_ROTATIONS, + Math.min(ShooterConstants.HOOD_MAX_ROTATIONS, hoodTargetPosition)); hoodController.reset(); hoodActive = true; } From 2503ba8e8293e2a1ed3233a2eca72f578d073dfe Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 28 Feb 2026 10:27:43 -0600 Subject: [PATCH 49/80] week 0 --- src/main/java/frc/robot/Constants.java | 23 +++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 6 ++--- .../frc/robot/Subsystems/IntakeSubsystem.java | 8 +++++-- .../robot/Subsystems/ShooterSubsystem.java | 2 +- 4 files changed, 22 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4e1ba2d..334bca9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -259,9 +259,9 @@ public static final class ShooterConstants { public static final int INDEXER_ID = 23; // Percent output caps ([-1..1]). Higher = faster spin-up but more current draw. - public static final double SHOOTER_SPEED = 1.0; - public static final double KICKER_SPEED = 1.0; - public static final double INDEXER_SPEED = 0.5; //placeholder + public static final double SHOOTER_SPEED = 0.6; + public static final double KICKER_SPEED = 0.6; + public static final double INDEXER_SPEED = 0.4; //placeholder // Electrical limits/compensation. public static final double SHOOTER_VOLTAGE_COMP = 12.0; @@ -271,12 +271,12 @@ public static final class ShooterConstants { // Hood position units are motor rotations (NEO internal encoder). // Max travel is 3 rotations = 1080 degrees. public static final double HOOD_MIN_ROTATIONS = 0.0; - public static final double HOOD_MAX_ROTATIONS = 3.0; + public static final double HOOD_MAX_ROTATIONS = 24.0; // Preset positions. public static final double HOOD_ANGLE_LOW = HOOD_MIN_ROTATIONS; public static final double HOOD_ANGLE_HIGH = HOOD_MAX_ROTATIONS; // "up" (about 2 inches) - public static final double HOOD_KP = 1.2; + public static final double HOOD_KP = 0.1; public static final double HOOD_MAX_OUTPUT = 0.4; public static final double HOOD_TOLERANCE = 0.02; } @@ -285,7 +285,7 @@ public static final class IntakeConstants { // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; - public static double INTAKE_SPEED = 50; //placeholder for percent power for intake + public static double INTAKE_SPEED = 75; //placeholder for percent power for intake public static int INTAKE_ARM_ID = 18; public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later @@ -299,10 +299,13 @@ public static final class IntakeConstants { public static double INTAKE_ARM_kD = 0; // Intake arm motion limits (tune to be slower/gentler). - public static double INTAKE_ARM_MAX_VEL_DEG_PER_SEC = 60.0; //Degrees - public static double INTAKE_ARM_MAX_ACCEL_DEG_PER_SEC2 = 120.0; //Degrees/sec - public static double INTAKE_ARM_MAX_OUTPUT_UP = 0.25; //Degrees/sec^2 - public static double INTAKE_ARM_MAX_OUTPUT_DOWN = 0.18; //Percent output (0.1) + public static double INTAKE_ARM_MAX_VEL_DEG_PER_SEC = 30.0; //Degrees/sec + public static double INTAKE_ARM_MAX_ACCEL_DEG_PER_SEC2 = 5.0; //Degrees/sec^2 + + public static double INTAKE_ARM_MAX_OUTPUT_UP = 0.25; + public static double INTAKE_ARM_MAX_OUTPUT_DOWN = 0.25; //Percent output (0.1) + public static double INTAKE_ARM_FLOOR_SLOW_ZONE_DEG = 10.0; + public static double INTAKE_ARM_MAX_OUTPUT_DOWN_NEAR_FLOOR = 0.12; } public static final class CANdleConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 65efa19..80bf4dd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -121,8 +121,8 @@ private void configureBindings() { // Hood controls (helms controller). // Y = hood up (2 inches / max travel), B = hood down. - helmsController.y().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); - helmsController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); + driveController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); + driveController.a().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); // Left Trigger = Auto-align to left scoring position @@ -164,8 +164,6 @@ private void configureBindings() { // Bound on both controllers so it works regardless of which one you're pressing. helmsController.x().onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); helmsController.a().onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); - driveController.x().onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); - driveController.a().onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); } private double getSpeedMultiplier(){ diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 470ecfb..a3c3064 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -36,7 +36,7 @@ public class IntakeSubsystem extends SubsystemBase { public double targetPosition; private boolean intakeOn = false; - private boolean intakeUp = false; + private boolean intakeUp = true; private final TrapezoidProfile.Constraints armConstraints = new TrapezoidProfile.Constraints( @@ -136,9 +136,13 @@ public void periodic() { + intakeArmPID.calculate(currentDegrees, armSetpoint.position); // Limit output so the arm moves slower/gentler (especially on the way down). - double maxOutput = armSetpoint.velocity >= 0.0 + boolean movingUp = armSetpoint.position > currentDegrees; + double maxOutput = movingUp ? IntakeConstants.INTAKE_ARM_MAX_OUTPUT_UP : IntakeConstants.INTAKE_ARM_MAX_OUTPUT_DOWN; + if (!movingUp && currentDegrees <= IntakeConstants.INTAKE_ARM_FLOOR_SLOW_ZONE_DEG){ + maxOutput = Math.min(maxOutput, IntakeConstants.INTAKE_ARM_MAX_OUTPUT_DOWN_NEAR_FLOOR); + } output = MathUtil.clamp(output, -maxOutput, maxOutput); intakeArmMotor.set(output); diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 92da2af..88a7311 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -57,7 +57,7 @@ public ShooterSubsystem() { feedConfig.voltageCompensation(ShooterConstants.SHOOTER_VOLTAGE_COMP); SparkMaxConfig hoodConfig = new SparkMaxConfig(); - hoodConfig.inverted(false); + hoodConfig.inverted(true); hoodConfig.idleMode(IdleMode.kBrake); SparkMaxConfig indexConfig = new SparkMaxConfig(); From 79b030af06ef43468506b6f43b74862aab9c2e5b Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 28 Feb 2026 16:17:13 -0600 Subject: [PATCH 50/80] week0 --- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes src/main/deploy/pathplanner/navgrid.json | 2 +- src/main/java/frc/robot/Constants.java | 19 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/Subsystems/CandleSubsystem.java | 2 +- .../frc/robot/Subsystems/IntakeSubsystem.java | 14 ++++++++----- 10 files changed, 22 insertions(+), 19 deletions(-) diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat index b40e50d301af77c71776d37a2176211ebe318c65..6202248dd0a78ee422f2d6fb5bdf5bf236c3a0ea 100644 GIT binary patch delta 25 hcmZn=Xb@o8%;5WfqTm{qO$>hjHwu=rP1a<;4ghp72}}S0 delta 20 ccmZn=Xb@o8$l(8fhjHwu=rP1a<;4ghp72}}S0 delta 20 ccmZn=Xb@o8$l(8fhjHwu=rP1a<;4ghp72}}S0 delta 20 ccmZn=Xb@o8$l(8ft<8 delta 28 kcmZn=Xb@o8$l(8f;=wg6n;885Z#t<8 diff --git a/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat index 0d5521e38988239e6c1a2b3aa5cf879d64ef741f..fceb1ffd735d31487a91ec9043c24c593dc95de1 100644 GIT binary patch delta 31 kcmZn=Xb@mI%V7L}b0VV;Bg+{E6AqG_K$)4=n0jg^X;{X5v delta 31 kcmZn=Xb@mI%wYR}b0VV;Bg-KMI}qyy>qG_K$)4=n0jfg^;{X5v diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index ac5f521..23e0db9 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 334bca9..b0fd2b9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -288,22 +288,21 @@ public static final class IntakeConstants { public static double INTAKE_SPEED = 75; //placeholder for percent power for intake public static int INTAKE_ARM_ID = 18; - public static double INTAKE_ARM_RAISED_POSITION = 90; //to do later - public static double INTAKE_ARM_LOWERED_POSITION = 0; - public static double INTAKE_ARM_MINIMUM = 0; // placeholders - public static double INTAKE_ARM_MAXIMUM = 90; + public static double INTAKE_ARM_RAISED_POSITION = 0; //to do later + public static double INTAKE_ARM_LOWERED_POSITION = 90; + public static double INTAKE_ARM_MINIMUM = 5; // placeholders + public static double INTAKE_ARM_MAXIMUM = 85; public static int GEAR_RATIO = 25; - public static double INTAKE_ARM_kP = 0.01; - public static double INTAKE_ARM_kI = 0; - public static double INTAKE_ARM_kD = 0; - + public static double INTAKE_ARM_kP = 0.03; + public static double INTAKE_ARM_kI = 0.0; + public static double INTAKE_ARM_kD = 0.0; // Intake arm motion limits (tune to be slower/gentler). public static double INTAKE_ARM_MAX_VEL_DEG_PER_SEC = 30.0; //Degrees/sec public static double INTAKE_ARM_MAX_ACCEL_DEG_PER_SEC2 = 5.0; //Degrees/sec^2 - public static double INTAKE_ARM_MAX_OUTPUT_UP = 0.25; - public static double INTAKE_ARM_MAX_OUTPUT_DOWN = 0.25; //Percent output (0.1) + public static double INTAKE_ARM_MAX_OUTPUT_UP = 0.1; + public static double INTAKE_ARM_MAX_OUTPUT_DOWN = 0.35; //Percent output (0.1) public static double INTAKE_ARM_FLOOR_SLOW_ZONE_DEG = 10.0; public static double INTAKE_ARM_MAX_OUTPUT_DOWN_NEAR_FLOOR = 0.12; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 80bf4dd..5fb9ca7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -162,8 +162,8 @@ private void configureBindings() { // Intake arm buttons. // X = raise arm, A = lower arm. // Bound on both controllers so it works regardless of which one you're pressing. - helmsController.x().onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); - helmsController.a().onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); + helmsController.a().onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); + helmsController.x().onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); } private double getSpeedMultiplier(){ diff --git a/src/main/java/frc/robot/Subsystems/CandleSubsystem.java b/src/main/java/frc/robot/Subsystems/CandleSubsystem.java index 9e9f1de..4424fdb 100644 --- a/src/main/java/frc/robot/Subsystems/CandleSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/CandleSubsystem.java @@ -11,7 +11,7 @@ import com.ctre.phoenix6.controls.EmptyAnimation; import com.ctre.phoenix6.controls.SolidColor; import com.ctre.phoenix6.hardware.CANdle; -import com.ctre.phoenix6.signals.RGBWColor; +import com.ctre.phoenix6.signals.RGBWColor; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.CANdleConstants; diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index a3c3064..301a377 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -74,11 +74,11 @@ public IntakeSubsystem() { public void toggleIntake() { if (!intakeOn) { - intakeOn = true; + intakeOn = false; intakeMotor.set(IntakeConstants.INTAKE_SPEED); } else { - intakeOn = false; + intakeOn = true; intakeMotor.set(0); } } @@ -98,13 +98,13 @@ public void setTargetPosition(double position) { public void raiseIntake() { setTargetPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); intakeArmPID.reset(); - intakeUp = true; + intakeUp = false; } public void lowerIntake() { setTargetPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); intakeArmPID.reset(); - intakeUp = false; + intakeUp = true; } public void moveIntake() { @@ -124,6 +124,10 @@ public double getArmPosition() { @Override public void periodic() { // This method will be called once per scheduler run + + System.out.println(intakeArmEncoder.getPosition()); + System.out.println(armSetpoint.position); + double nowSec = Timer.getFPGATimestamp(); double dtSec = nowSec - lastTimestampSec; lastTimestampSec = nowSec; @@ -136,7 +140,7 @@ public void periodic() { + intakeArmPID.calculate(currentDegrees, armSetpoint.position); // Limit output so the arm moves slower/gentler (especially on the way down). - boolean movingUp = armSetpoint.position > currentDegrees; + boolean movingUp = armSetpoint.position < currentDegrees; double maxOutput = movingUp ? IntakeConstants.INTAKE_ARM_MAX_OUTPUT_UP : IntakeConstants.INTAKE_ARM_MAX_OUTPUT_DOWN; From 9d52ee70b87e9431f11e3ab4f94c12ed2cb732b1 Mon Sep 17 00:00:00 2001 From: Seqi Date: Sat, 28 Feb 2026 22:38:37 -0600 Subject: [PATCH 51/80] Changed wpilib language settings to java --- .wpilib/wpilib_preferences.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 130b3fc..e570ff3 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,5 +1,5 @@ { - "currentLanguage": "none", + "currentLanguage": "java", "enableCppIntellisense": false, "projectYear": "none", "teamNumber": 3926 From df47ce2ebc6e9ef1f4f7c6706f39bafa9db8e567 Mon Sep 17 00:00:00 2001 From: Seqi Date: Sun, 1 Mar 2026 11:02:58 -0600 Subject: [PATCH 52/80] Added offsetDepotAuto, fixed gitignore spelling mistake. --- .gitignore | 2 +- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 2048 -> 0 bytes .../deploy/pathplanner/paths/GoToDepot.path | 54 ++++++++++++++++++ .../pathplanner/paths/ShootAfterDepot.path | 54 ++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 34 +++++++++++ .../java/frc/robot/Auto/OffsetDepotAuto.java | 39 +++++++++++++ src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 2 + .../robot/Subsystems/ShooterSubsystem.java | 5 ++ 13 files changed, 192 insertions(+), 2 deletions(-) delete mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat delete mode 100644 ctre_sim/Pigeon 2 - 017 - 0 - ext.dat create mode 100644 src/main/deploy/pathplanner/paths/GoToDepot.path create mode 100644 src/main/deploy/pathplanner/paths/ShootAfterDepot.path create mode 100644 src/main/deploy/pathplanner/settings.json create mode 100644 src/main/java/frc/robot/Auto/OffsetDepotAuto.java diff --git a/.gitignore b/.gitignore index f788dde..2c7dcf3 100644 --- a/.gitignore +++ b/.gitignore @@ -55,4 +55,4 @@ Thumbs.db .vscode/ # sim files -.crte_sim/ +.ctre_sim/ diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat deleted file mode 100644 index 6202248dd0a78ee422f2d6fb5bdf5bf236c3a0ea..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdnY;QOCp2!TxuenZN=LnvrRojMut#IC4K+^ diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat deleted file mode 100644 index abd4b46bc5c679a3d9d99bb16fc74b98516e5c53..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdnY;QOCp2!TxuenZN=LnvrRojMuQj{W0QEB*s0GR}!(|y59H8<5at|e` diff --git a/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat deleted file mode 100644 index 7717118da420ab2c6fcad14fef1d319cf6d8680f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdnY;QOCp2!SmOK10gALnvrRojMuc^+(l*atEL1Xv*yoQ87Yl)@T_ X`V{3oK=&~ms0GR}!(|y59H8<5ve_mW diff --git a/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat deleted file mode 100644 index fceb1ffd735d31487a91ec9043c24c593dc95de1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmX@hVEmt96i_V$&M=sah9}iRbJU>G5Eu=C(GVB{A&{FI!3cDw0o17ut9O)u_&4rs z>0@Lt`AWzK6TAz+`Z=Kr8<6!k7(RQTw_G1&9!!1#RQ}Hmkoml;9&CWv2T}t9=(KO>jAoIsk_mAol intake.raiseIntake(), intake), + new InstantCommand(() -> intake.setIntakePower(IntakeConstants.INTAKE_SPEED), intake), + Commands.waitSeconds(3), + new InstantCommand(() -> intake.setIntakePower(0), intake), + drive.autoDrive("ShootAfterDepot"), + new InstantCommand(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + new InstantCommand(() -> shooter.AutoToggleShootKick(false)), + Commands.waitSeconds(3), + new InstantCommand(() -> intake.lowerIntake(), intake), + new InstantCommand(() -> intake.raiseIntake(), intake), + Commands.waitSeconds(1), + new InstantCommand(() -> intake.lowerIntake(), intake), + new InstantCommand(() -> intake.raiseIntake(), intake), + Commands.waitSeconds(4), + new InstantCommand(() -> shooter.AutoToggleShootKick(true)) + ); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b0fd2b9..e2315a0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -159,7 +159,8 @@ public enum AutoMode{ DriveTestAuto, EightLemonAuto, TrenchToDepotAuto, - CenterToDepotAuto + CenterToDepotAuto, + OffsetDepotAuto } private static SendableChooser sideChooser = new SendableChooser(); @@ -178,6 +179,7 @@ public static void initDashboard() { autoModeChooser.addOption("EightLemonAuto (PathPlanner)", AutoMode.EightLemonAuto); autoModeChooser.addOption("TrenchToDepotAuto", AutoMode.TrenchToDepotAuto); autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); + autoModeChooser.addOption("Offset Depot Auto", AutoMode.OffsetDepotAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5fb9ca7..18e5556 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.DriveTestAuto; import frc.robot.Auto.EightLemonAuto; +import frc.robot.Auto.OffsetDepotAuto; import frc.robot.Auto.TrenchToDepotAuto; import frc.robot.Auto.CenterToDepotAuto; import frc.robot.Constants.AutoConstants; @@ -182,6 +183,7 @@ public Command getAutonomousCommand() { case EightLemonAuto -> new EightLemonAuto(m_drive, m_shooter, m_intake); case TrenchToDepotAuto -> new TrenchToDepotAuto(m_drive); case CenterToDepotAuto -> new CenterToDepotAuto(m_drive); + case OffsetDepotAuto -> new OffsetDepotAuto(m_drive, m_intake, m_shooter); default -> Commands.none(); }; } diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 88a7311..b0ced2e 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -143,6 +143,11 @@ public void setIndexerSpeed(double speed) { indexerMotor.set(speed); } + public void AutoToggleShootKick (boolean AutoShootOn) { + setKickerSpeed(AutoShootOn ? 0 : ShooterConstants.KICKER_SPEED); + setShooterSpeed(AutoShootOn ? 0 : ShooterConstants.SHOOTER_SPEED); + } + @Override public void periodic() { // This method will be called once per scheduler run From e31447f21772ba7b4df1c4479675f491717eea37 Mon Sep 17 00:00:00 2001 From: Seqi Date: Sun, 1 Mar 2026 11:24:53 -0600 Subject: [PATCH 53/80] fixed no project year in wpilib_preferences --- .wpilib/wpilib_preferences.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index e570ff3..8cc61a0 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "currentLanguage": "java", "enableCppIntellisense": false, - "projectYear": "none", + "projectYear": "2026", "teamNumber": 3926 } \ No newline at end of file From a9b92a4817e694777396194606a62bd7a6d18222 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 2 Mar 2026 18:07:15 -0600 Subject: [PATCH 54/80] changed the intake arm to how the hood is moving --- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 0 -> 2048 bytes src/main/java/frc/robot/Constants.java | 34 ++--- src/main/java/frc/robot/RobotContainer.java | 32 ++--- .../frc/robot/Subsystems/IntakeSubsystem.java | 116 ++++++++---------- 8 files changed, 88 insertions(+), 94 deletions(-) create mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat create mode 100644 ctre_sim/Pigeon 2 - 017 - 0 - ext.dat diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..77854d2130ab9de38e159dc88f062c90234bfba1 GIT binary patch literal 2048 zcmdnU;Qyat6pV(zXb6mkz-S1JhQMeD&?E#FT=?S82sD`!YI;MqWig2VXU4w?j12L- z1@ph5$S;7(-d-Jm1v<qE+1yK1vGl238k;xAVAnKW*`dQFuC>KsCG5Eu=C(GVE^A+SK}pg$wfo19RuHe_2CgZMY* w{F}hokS5!ZEUt+xzW_zPU_CQ>5amGhr#xnz$jIRNt`;Z{R>Nh*1TqH(0NfrJ00000 literal 0 HcmV?d00001 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e2315a0..48c9ce6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -287,26 +287,28 @@ public static final class IntakeConstants { // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; - public static double INTAKE_SPEED = 75; //placeholder for percent power for intake + public static double INTAKE_SPEED = 75; //percent output scaling for intake motor public static int INTAKE_ARM_ID = 18; - public static double INTAKE_ARM_RAISED_POSITION = 0; //to do later - public static double INTAKE_ARM_LOWERED_POSITION = 90; - public static double INTAKE_ARM_MINIMUM = 5; // placeholders - public static double INTAKE_ARM_MAXIMUM = 85; public static int GEAR_RATIO = 25; - public static double INTAKE_ARM_kP = 0.03; - public static double INTAKE_ARM_kI = 0.0; - public static double INTAKE_ARM_kD = 0.0; - // Intake arm motion limits (tune to be slower/gentler). - public static double INTAKE_ARM_MAX_VEL_DEG_PER_SEC = 30.0; //Degrees/sec - public static double INTAKE_ARM_MAX_ACCEL_DEG_PER_SEC2 = 5.0; //Degrees/sec^2 - - public static double INTAKE_ARM_MAX_OUTPUT_UP = 0.1; - public static double INTAKE_ARM_MAX_OUTPUT_DOWN = 0.35; //Percent output (0.1) - public static double INTAKE_ARM_FLOOR_SLOW_ZONE_DEG = 10.0; - public static double INTAKE_ARM_MAX_OUTPUT_DOWN_NEAR_FLOOR = 0.12; + //Intake arm position units are degrees + public static final double INTAKE_ARM_MIN_DEG = 0.0; + public static final double INTAKE_ARM_MAX_DEG = 90.0; + + //Preset positions + public static final double INTAKE_ARM_LOWERED_POSITION = INTAKE_ARM_MIN_DEG; + public static final double INTAKE_ARM_RAISED_POSITION = INTAKE_ARM_MAX_DEG; + + //PID constants for intake arm (degrees). + public static final double INTAKE_ARM_kP = 0.01; + public static final double INTAKE_ARM_kI = 0.0; + public static final double INTAKE_ARM_kD = 0.0; + public static final double INTAKE_ARM_TOLERANCE_DEG = 2.0; + + //Percent output cap (0..1) for gentler motion + //duty-cycle / percent output for SparkMax.set(...), which expects a value in [-1.0, 1.0] + public static final double INTAKE_ARM_MAX_OUTPUT = 0.25; } public static final class CANdleConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18e5556..d5fbb41 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,27 +103,28 @@ private void configureBindings() { // SHOOTER CONTROLLER m_shooter.setDefaultCommand( Commands.run( - () -> { - // Right stick Y controls shooter + kicker together. + () -> { + // Right stick Y controls shooter. // Invert so stick-up (negative on Xbox) produces positive motor output. double shooterAxis = -MathUtil.applyDeadband( helmsController.getRawAxis(Axis.kRightY.value), - 0.1); - + 0.1); + m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); - m_shooter.setKickerSpeed(shooterAxis * ShooterConstants.KICKER_SPEED); + + // Right bumper runs the indexer and kicker while held. + Boolean rightBumperPressed = helmsController.getHID().getRightBumper(); - // Right bumper runs the indexer while held. - m_shooter.setIndexerSpeed(helmsController.getHID().getRightBumper() - ? ShooterConstants.INDEXER_SPEED - : 0.0); + m_shooter.setKickerSpeed(rightBumperPressed ? ShooterConstants.KICKER_SPEED : 0.0); + m_shooter.setIndexerSpeed(rightBumperPressed ? ShooterConstants.INDEXER_SPEED : 0.0); }, - m_shooter)); + m_shooter)); + // Hood controls (helms controller). // Y = hood up (2 inches / max travel), B = hood down. - driveController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); - driveController.a().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); + helmsController.y().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); + helmsController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); // Left Trigger = Auto-align to left scoring position @@ -161,10 +162,10 @@ private void configureBindings() { // Intake arm buttons. - // X = raise arm, A = lower arm. + // Left Trigger = lower arm, Right Trigger = Raise arm. // Bound on both controllers so it works regardless of which one you're pressing. - helmsController.a().onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); - helmsController.x().onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); + helmsController.axisGreaterThan(Axis.kRightTrigger.value, 0.1).onTrue(new InstantCommand(() -> m_intake.raiseIntake(), m_intake)); + helmsController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).onTrue(new InstantCommand(() -> m_intake.lowerIntake(), m_intake)); } private double getSpeedMultiplier(){ @@ -191,5 +192,4 @@ public Command getAutonomousCommand() { public SwerveSubsystem getDriveSubsystem() { return m_drive; } - } diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 301a377..89c8b02 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -27,25 +27,23 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkMax intakeMotor = new SparkMax(IntakeConstants.INTAKE_ID, MotorType.kBrushless); private final SparkMax intakeArmMotor = new SparkMax(IntakeConstants.INTAKE_ARM_ID, MotorType.kBrushless); - private RelativeEncoder intakeArmEncoder = intakeArmMotor.getEncoder(); + private final RelativeEncoder intakeArmEncoder = intakeArmMotor.getEncoder(); - private PIDController intakeArmPID = new PIDController(IntakeConstants.INTAKE_ARM_kP, IntakeConstants.INTAKE_ARM_kI, IntakeConstants.INTAKE_ARM_kD); - - private ArmFeedforward intakeArmFeedForward = new ArmFeedforward(0,0,0); - - public double targetPosition; + private final PIDController intakeArmController = new PIDController( + IntakeConstants.INTAKE_ARM_kP, + IntakeConstants.INTAKE_ARM_kI, + IntakeConstants.INTAKE_ARM_kD); + + private double intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; + private boolean intakeArmActive = false; private boolean intakeOn = false; private boolean intakeUp = true; - private final TrapezoidProfile.Constraints armConstraints = - new TrapezoidProfile.Constraints( - IntakeConstants.INTAKE_ARM_MAX_VEL_DEG_PER_SEC, - IntakeConstants.INTAKE_ARM_MAX_ACCEL_DEG_PER_SEC2); - - private TrapezoidProfile.State armSetpoint = new TrapezoidProfile.State(0.0, 0.0); - private TrapezoidProfile.State armGoal = new TrapezoidProfile.State(0.0, 0.0); - private double lastTimestampSec = 0.0; + public enum IntakeArmAngle { + DOWN, + UP + } /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { @@ -58,18 +56,15 @@ public IntakeSubsystem() { SparkMaxConfig intakeArmConfig = new SparkMaxConfig(); intakeArmConfig.inverted(true); intakeArmConfig.idleMode(IdleMode.kBrake); - // Convert motor rotations -> arm degrees (assumes INTAKEConstants.GEAR_RATIO is motor:arm reduction). + intakeArmConfig.encoder.positionConversionFactor(360.0 / IntakeConstants.GEAR_RATIO); intakeArmMotor.configure(intakeArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - // On enable, assume the arm is sitting on the floor at 0° and hold there. - intakeArmEncoder.setPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); - intakeArmPID.reset(); - targetPosition = IntakeConstants.INTAKE_ARM_LOWERED_POSITION; - - armSetpoint = new TrapezoidProfile.State(targetPosition, 0.0); - armGoal = new TrapezoidProfile.State(targetPosition, 0.0); - lastTimestampSec = Timer.getFPGATimestamp(); + //On enable, assume the arm starts raised at 90 degrees + intakeArmEncoder.setPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); + intakeArmController.setTolerance(IntakeConstants.INTAKE_ARM_TOLERANCE_DEG); + intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; + intakeArmActive = false; } public void toggleIntake() { @@ -90,21 +85,32 @@ public void setIntakePower(double power) { } - public void setTargetPosition(double position) { - targetPosition = Math.max(IntakeConstants.INTAKE_ARM_MINIMUM, Math.min(IntakeConstants.INTAKE_ARM_MAXIMUM, position)); - armGoal = new TrapezoidProfile.State(targetPosition, 0.0); + public void setIntakeArmAngle(IntakeArmAngle angle){ + switch (angle){ + case DOWN: + intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_LOWERED_POSITION; + intakeUp = false; + break; + case UP: + default: + intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; + intakeUp = true; + break; + } + + intakeArmTargetDeg = Math.max( + IntakeConstants.INTAKE_ARM_MIN_DEG, + Math.min(IntakeConstants.INTAKE_ARM_MAX_DEG, intakeArmTargetDeg)); + intakeArmController.reset(); + intakeArmActive = true; } public void raiseIntake() { - setTargetPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); - intakeArmPID.reset(); - intakeUp = false; + setIntakeArmAngle (IntakeArmAngle.UP); } public void lowerIntake() { - setTargetPosition(IntakeConstants.INTAKE_ARM_LOWERED_POSITION); - intakeArmPID.reset(); - intakeUp = true; + setIntakeArmAngle(IntakeArmAngle.DOWN); } public void moveIntake() { @@ -116,44 +122,30 @@ public void moveIntake() { } } - public double getArmPosition() { - // With positionConversionFactor set, encoder position is already in degrees. + public double getArmPositionDeg() { return intakeArmEncoder.getPosition(); } @Override public void periodic() { - // This method will be called once per scheduler run - - System.out.println(intakeArmEncoder.getPosition()); - System.out.println(armSetpoint.position); + double currentDeg = getArmPositionDeg(); - double nowSec = Timer.getFPGATimestamp(); - double dtSec = nowSec - lastTimestampSec; - lastTimestampSec = nowSec; - dtSec = MathUtil.clamp(dtSec, 0.0, 0.05); + SmartDashboard.putNumber("IntakeArm/TargetDeg", intakeArmTargetDeg); + SmartDashboard.putNumber("IntakeArm/PostionDeg", currentDeg); + SmartDashboard.putBoolean("IntakeArm/Active", intakeArmActive); - double currentDegrees = getArmPosition(); - armSetpoint = new TrapezoidProfile(armConstraints).calculate(dtSec, armGoal, armSetpoint); + if (intakeArmActive){ + double output = intakeArmController.calculate(currentDeg, intakeArmTargetDeg); + output = Math.max(-IntakeConstants.INTAKE_ARM_MAX_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output)); - double output = intakeArmFeedForward.calculate(Units.degreesToRadians(currentDegrees), 0) - + intakeArmPID.calculate(currentDegrees, armSetpoint.position); - - // Limit output so the arm moves slower/gentler (especially on the way down). - boolean movingUp = armSetpoint.position < currentDegrees; - double maxOutput = movingUp - ? IntakeConstants.INTAKE_ARM_MAX_OUTPUT_UP - : IntakeConstants.INTAKE_ARM_MAX_OUTPUT_DOWN; - if (!movingUp && currentDegrees <= IntakeConstants.INTAKE_ARM_FLOOR_SLOW_ZONE_DEG){ - maxOutput = Math.min(maxOutput, IntakeConstants.INTAKE_ARM_MAX_OUTPUT_DOWN_NEAR_FLOOR); + if (intakeArmController.atSetpoint()){ + intakeArmMotor.set(0.0); + intakeArmActive = false; + } else { + intakeArmMotor.set(output); + } + } else{ + intakeArmMotor.set(0.0); } - output = MathUtil.clamp(output, -maxOutput, maxOutput); - intakeArmMotor.set(output); - - SmartDashboard.putNumber("IntakeArm/TargetDeg", targetPosition); - SmartDashboard.putNumber("IntakeArm/SetpointDeg", armSetpoint.position); - SmartDashboard.putNumber("IntakeArm/SetpointVelDegPerSec", armSetpoint.velocity); - SmartDashboard.putNumber("IntakeArm/PositionDeg", currentDegrees); - SmartDashboard.putNumber("IntakeArm/Output", output); } } From b185a960cc0ea6993c3b8a8840b468f8c58caf32 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 2 Mar 2026 18:30:45 -0600 Subject: [PATCH 55/80] changed limelight name --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 48c9ce6..ed1c54c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -239,7 +239,7 @@ public static Pose2d flipForAlliance(Pose2d pose){ /** Vision constants (Limelight, etc). */ public static final class VisionConstants { - public static final String[] LIMELIGHT_NAMES = {"limelight-a", "limelight-b"}; + public static final String[] LIMELIGHT_NAMES = {"limelightA", "limelightB"}; public static final String LIMELIGHT_STREAM_URL_FORMAT = "http://%s.local:5800/stream.mjpg"; public static final boolean LIMELIGHT_STREAM_ENABLED_DEFAULT = true; From 993efcf78ec184d59fb5f5168119c09331d0bb48 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Mon, 2 Mar 2026 19:45:19 -0600 Subject: [PATCH 56/80] tuned the intake arm --- src/main/java/frc/robot/Constants.java | 13 +++++++------ src/main/java/frc/robot/Robot.java | 14 +++++++++++++- .../java/frc/robot/Subsystems/IntakeSubsystem.java | 2 +- 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed1c54c..3b331e2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -241,7 +241,7 @@ public static Pose2d flipForAlliance(Pose2d pose){ public static final class VisionConstants { public static final String[] LIMELIGHT_NAMES = {"limelightA", "limelightB"}; - public static final String LIMELIGHT_STREAM_URL_FORMAT = "http://%s.local:5800/stream.mjpg"; + public static final String LIMELIGHT_STREAM_URL_FORMAT = "http://10.39.26.20"; public static final boolean LIMELIGHT_STREAM_ENABLED_DEFAULT = true; public static final boolean VISION_ENABLED_DEFAULT = true; @@ -293,7 +293,7 @@ public static final class IntakeConstants { public static int GEAR_RATIO = 25; //Intake arm position units are degrees - public static final double INTAKE_ARM_MIN_DEG = 0.0; + public static final double INTAKE_ARM_MIN_DEG = 20.0; public static final double INTAKE_ARM_MAX_DEG = 90.0; //Preset positions @@ -301,14 +301,15 @@ public static final class IntakeConstants { public static final double INTAKE_ARM_RAISED_POSITION = INTAKE_ARM_MAX_DEG; //PID constants for intake arm (degrees). - public static final double INTAKE_ARM_kP = 0.01; - public static final double INTAKE_ARM_kI = 0.0; - public static final double INTAKE_ARM_kD = 0.0; + public static final double INTAKE_ARM_kP = 6.0; + public static final double INTAKE_ARM_kI = 1.5; + public static final double INTAKE_ARM_kD = 0.15; public static final double INTAKE_ARM_TOLERANCE_DEG = 2.0; //Percent output cap (0..1) for gentler motion //duty-cycle / percent output for SparkMax.set(...), which expects a value in [-1.0, 1.0] - public static final double INTAKE_ARM_MAX_OUTPUT = 0.25; + public static final double INTAKE_ARM_MAX_OUTPUT = 0.20; + public static final double INTAKE_ARM_MIN_OUTPUT = -0.10; } public static final class CANdleConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a9f81d2..254fc4b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.HttpCamera; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -27,7 +29,12 @@ public void robotPeriodic() { } @Override - public void disabledInit() {} + public void disabledInit() { + HttpCamera limelightA = new HttpCamera("LimelightA", "http://10.39.26.201"); + HttpCamera limelightB = new HttpCamera("LimelightB", "http://10.39.26.202"); + CameraServer.startAutomaticCapture(limelightA); + CameraServer.startAutomaticCapture(limelightB); + } @Override public void disabledPeriodic() {} @@ -57,6 +64,11 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + + HttpCamera limelightA = new HttpCamera("LimelightA", "http://10.39.26.201"); + HttpCamera limelightB = new HttpCamera("LimelightB", "http://10.39.26.202"); + CameraServer.startAutomaticCapture(limelightA); + CameraServer.startAutomaticCapture(limelightB); } @Override diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 89c8b02..8df1be9 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -136,7 +136,7 @@ public void periodic() { if (intakeArmActive){ double output = intakeArmController.calculate(currentDeg, intakeArmTargetDeg); - output = Math.max(-IntakeConstants.INTAKE_ARM_MAX_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output)); + output = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output)); if (intakeArmController.atSetpoint()){ intakeArmMotor.set(0.0); From 47fb2ac113674c5e37381d103ab1413472694fa9 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 3 Mar 2026 19:26:34 -0600 Subject: [PATCH 57/80] limelight --- src/main/java/frc/robot/Constants.java | 19 +++++++++++++++++-- src/main/java/frc/robot/Robot.java | 10 ---------- src/main/java/frc/robot/RobotContainer.java | 2 +- 3 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b331e2..012a17b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -240,8 +240,11 @@ public static Pose2d flipForAlliance(Pose2d pose){ /** Vision constants (Limelight, etc). */ public static final class VisionConstants { public static final String[] LIMELIGHT_NAMES = {"limelightA", "limelightB"}; - - public static final String LIMELIGHT_STREAM_URL_FORMAT = "http://10.39.26.20"; + + // Limelight MJPEG stream endpoints. + // Using fixed IPs avoids mDNS/DNS resolution issues on the roboRIO. + public static final String LIMELIGHT_A_STREAM_URL = "http://10.39.26.201:5801/stream.mjpg"; + public static final String LIMELIGHT_B_STREAM_URL = "http://10.39.26.202:5801/stream.mjpg"; public static final boolean LIMELIGHT_STREAM_ENABLED_DEFAULT = true; public static final boolean VISION_ENABLED_DEFAULT = true; @@ -251,6 +254,18 @@ public static final class VisionConstants { public static final double VISION_STD_DEV_X_METERS = 0.7; public static final double VISION_STD_DEV_Y_METERS = 0.7; public static final double VISION_STD_DEV_THETA_RADIANS = 99999.0; + + public static String getLimelightStreamUrl(String limelightName) { + switch (limelightName) { + case "limelightA": + return LIMELIGHT_A_STREAM_URL; + case "limelightB": + return LIMELIGHT_B_STREAM_URL; + default: + // Fallback for any future Limelight names. + return "http://" + limelightName + ".local:5801/stream.mjpg"; + } + } } /* Shooter Constants */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 254fc4b..b617b64 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,6 @@ package frc.robot; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.HttpCamera; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -30,10 +28,6 @@ public void robotPeriodic() { @Override public void disabledInit() { - HttpCamera limelightA = new HttpCamera("LimelightA", "http://10.39.26.201"); - HttpCamera limelightB = new HttpCamera("LimelightB", "http://10.39.26.202"); - CameraServer.startAutomaticCapture(limelightA); - CameraServer.startAutomaticCapture(limelightB); } @Override @@ -65,10 +59,6 @@ public void teleopInit() { m_autonomousCommand.cancel(); } - HttpCamera limelightA = new HttpCamera("LimelightA", "http://10.39.26.201"); - HttpCamera limelightB = new HttpCamera("LimelightB", "http://10.39.26.202"); - CameraServer.startAutomaticCapture(limelightA); - CameraServer.startAutomaticCapture(limelightB); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d5fbb41..9295e62 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ private void startLimelightStreams() { } for (String limelightName : VisionConstants.LIMELIGHT_NAMES) { - String url = String.format(VisionConstants.LIMELIGHT_STREAM_URL_FORMAT, limelightName); + String url = VisionConstants.getLimelightStreamUrl(limelightName); SmartDashboard.putString("Vision/" + limelightName + "/StreamURL", url); HttpCamera camera = new HttpCamera(limelightName, url); From 093d2f3e0a28b9e6bbf225d2e163a4b1fba2ab97 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 4 Mar 2026 15:35:53 -0600 Subject: [PATCH 58/80] add shooting auto and limelight vision --- .../java/frc/robot/Auto/RealLemonAuto.java | 27 +++++++++++++++++++ src/main/java/frc/robot/Constants.java | 4 ++- src/main/java/frc/robot/Robot.java | 8 ++++-- src/main/java/frc/robot/RobotContainer.java | 2 ++ 4 files changed, 38 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/Auto/RealLemonAuto.java diff --git a/src/main/java/frc/robot/Auto/RealLemonAuto.java b/src/main/java/frc/robot/Auto/RealLemonAuto.java new file mode 100644 index 0000000..e34cdb3 --- /dev/null +++ b/src/main/java/frc/robot/Auto/RealLemonAuto.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class RealLemonAuto extends SequentialCommandGroup { + + public RealLemonAuto(IntakeSubsystem intake, ShooterSubsystem shooter) { + addCommands( + new InstantCommand(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + new InstantCommand(() -> shooter.AutoToggleShootKick(false)), + Commands.waitSeconds(3), + new InstantCommand(() -> intake.lowerIntake(), intake), + new InstantCommand(() -> intake.raiseIntake(), intake) + ); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 012a17b..e5b680d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -160,7 +160,8 @@ public enum AutoMode{ EightLemonAuto, TrenchToDepotAuto, CenterToDepotAuto, - OffsetDepotAuto + OffsetDepotAuto, + RealLemonAuto } private static SendableChooser sideChooser = new SendableChooser(); @@ -180,6 +181,7 @@ public static void initDashboard() { autoModeChooser.addOption("TrenchToDepotAuto", AutoMode.TrenchToDepotAuto); autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); autoModeChooser.addOption("Offset Depot Auto", AutoMode.OffsetDepotAuto); + autoModeChooser.addOption("RealLemonAuto", AutoMode.RealLemonAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b617b64..b4ec70d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -20,6 +21,10 @@ public Robot() { m_robotSimulation = new RobotSimulation(m_robotContainer); } + @Override + public void robotInit() { + CameraServer.startAutomaticCapture(); + } @Override public void robotPeriodic() { @@ -27,8 +32,7 @@ public void robotPeriodic() { } @Override - public void disabledInit() { - } + public void disabledInit() {} @Override public void disabledPeriodic() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9295e62..b223e3a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,7 @@ import frc.robot.Auto.DriveTestAuto; import frc.robot.Auto.EightLemonAuto; import frc.robot.Auto.OffsetDepotAuto; +import frc.robot.Auto.RealLemonAuto; import frc.robot.Auto.TrenchToDepotAuto; import frc.robot.Auto.CenterToDepotAuto; import frc.robot.Constants.AutoConstants; @@ -185,6 +186,7 @@ public Command getAutonomousCommand() { case TrenchToDepotAuto -> new TrenchToDepotAuto(m_drive); case CenterToDepotAuto -> new CenterToDepotAuto(m_drive); case OffsetDepotAuto -> new OffsetDepotAuto(m_drive, m_intake, m_shooter); + case RealLemonAuto -> new RealLemonAuto(m_intake, m_shooter); default -> Commands.none(); }; } From e78d2e301adb25711d743cd44677dcd44e139a33 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 4 Mar 2026 15:46:02 -0600 Subject: [PATCH 59/80] commit --- src/main/java/frc/robot/Auto/RealLemonAuto.java | 16 +++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RealLemonAuto.java b/src/main/java/frc/robot/Auto/RealLemonAuto.java index e34cdb3..aea5cb6 100644 --- a/src/main/java/frc/robot/Auto/RealLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RealLemonAuto.java @@ -4,19 +4,33 @@ package frc.robot.Auto; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; public class RealLemonAuto extends SequentialCommandGroup { - public RealLemonAuto(IntakeSubsystem intake, ShooterSubsystem shooter) { + public RealLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + final double[] startYawRad = new double[1]; addCommands( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(30.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(30.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), new InstantCommand(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), new InstantCommand(() -> shooter.AutoToggleShootKick(false)), Commands.waitSeconds(3), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b223e3a..a7875fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -186,7 +186,7 @@ public Command getAutonomousCommand() { case TrenchToDepotAuto -> new TrenchToDepotAuto(m_drive); case CenterToDepotAuto -> new CenterToDepotAuto(m_drive); case OffsetDepotAuto -> new OffsetDepotAuto(m_drive, m_intake, m_shooter); - case RealLemonAuto -> new RealLemonAuto(m_intake, m_shooter); + case RealLemonAuto -> new RealLemonAuto(m_drive, m_intake, m_shooter); default -> Commands.none(); }; } From ec1f053bb464c9401713920926fdbab15cc2868e Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Wed, 4 Mar 2026 15:55:38 -0600 Subject: [PATCH 60/80] auto change --- src/main/java/frc/robot/Auto/OffsetDepotAuto.java | 5 +++-- src/main/java/frc/robot/Auto/RealLemonAuto.java | 5 ++--- src/main/java/frc/robot/Subsystems/ShooterSubsystem.java | 7 ++++++- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Auto/OffsetDepotAuto.java b/src/main/java/frc/robot/Auto/OffsetDepotAuto.java index da66e1d..8172ff0 100644 --- a/src/main/java/frc/robot/Auto/OffsetDepotAuto.java +++ b/src/main/java/frc/robot/Auto/OffsetDepotAuto.java @@ -25,15 +25,16 @@ public OffsetDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSub new InstantCommand(() -> intake.setIntakePower(0), intake), drive.autoDrive("ShootAfterDepot"), new InstantCommand(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), - new InstantCommand(() -> shooter.AutoToggleShootKick(false)), + new InstantCommand(() -> shooter.AutoToggleShoot(false)), Commands.waitSeconds(3), + new InstantCommand(() -> shooter.AutoToggleKickIndex(false)), new InstantCommand(() -> intake.lowerIntake(), intake), new InstantCommand(() -> intake.raiseIntake(), intake), Commands.waitSeconds(1), new InstantCommand(() -> intake.lowerIntake(), intake), new InstantCommand(() -> intake.raiseIntake(), intake), Commands.waitSeconds(4), - new InstantCommand(() -> shooter.AutoToggleShootKick(true)) + new InstantCommand(() -> shooter.AutoToggleShoot(true)) ); } } diff --git a/src/main/java/frc/robot/Auto/RealLemonAuto.java b/src/main/java/frc/robot/Auto/RealLemonAuto.java index aea5cb6..1b35982 100644 --- a/src/main/java/frc/robot/Auto/RealLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RealLemonAuto.java @@ -32,10 +32,9 @@ public RealLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy return Math.abs(errorRad) < Math.toRadians(3.0); }), new InstantCommand(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), - new InstantCommand(() -> shooter.AutoToggleShootKick(false)), + new InstantCommand(() -> shooter.AutoToggleShoot(false)), Commands.waitSeconds(3), - new InstantCommand(() -> intake.lowerIntake(), intake), - new InstantCommand(() -> intake.raiseIntake(), intake) + new InstantCommand(() -> shooter.AutoToggleKickIndex(false)) ); } } diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index b0ced2e..64b95a1 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -143,11 +143,16 @@ public void setIndexerSpeed(double speed) { indexerMotor.set(speed); } - public void AutoToggleShootKick (boolean AutoShootOn) { + public void AutoToggleShoot (boolean AutoShootOn) { setKickerSpeed(AutoShootOn ? 0 : ShooterConstants.KICKER_SPEED); setShooterSpeed(AutoShootOn ? 0 : ShooterConstants.SHOOTER_SPEED); } + public void AutoToggleKickIndex (boolean AutoIndexKickOn) { + setKickerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.KICKER_SPEED); + setIndexerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.SHOOTER_SPEED); + } + @Override public void periodic() { // This method will be called once per scheduler run From 297dbc4d96abaeb0d4e8d8cafebb468617088cfe Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Wed, 4 Mar 2026 15:57:42 -0600 Subject: [PATCH 61/80] e --- src/main/java/frc/robot/Subsystems/ShooterSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 64b95a1..a489028 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -144,7 +144,6 @@ public void setIndexerSpeed(double speed) { } public void AutoToggleShoot (boolean AutoShootOn) { - setKickerSpeed(AutoShootOn ? 0 : ShooterConstants.KICKER_SPEED); setShooterSpeed(AutoShootOn ? 0 : ShooterConstants.SHOOTER_SPEED); } From 4d2d043de2be37c4fd408a39c958c732b0463c8e Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Wed, 4 Mar 2026 16:03:29 -0600 Subject: [PATCH 62/80] Revert "e" This reverts commit 297dbc4d96abaeb0d4e8d8cafebb468617088cfe. --- src/main/java/frc/robot/Subsystems/ShooterSubsystem.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index a489028..64b95a1 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -144,6 +144,7 @@ public void setIndexerSpeed(double speed) { } public void AutoToggleShoot (boolean AutoShootOn) { + setKickerSpeed(AutoShootOn ? 0 : ShooterConstants.KICKER_SPEED); setShooterSpeed(AutoShootOn ? 0 : ShooterConstants.SHOOTER_SPEED); } From 69a976083c0c6e34c05c878690c80ae341b9dd45 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 4 Mar 2026 16:03:46 -0600 Subject: [PATCH 63/80] auto --- .../java/frc/robot/Auto/RealLemonAuto.java | 25 ++++++++++++------- src/main/java/frc/robot/Constants.java | 3 +++ .../robot/Subsystems/ShooterSubsystem.java | 6 ++++- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RealLemonAuto.java b/src/main/java/frc/robot/Auto/RealLemonAuto.java index 1b35982..6990383 100644 --- a/src/main/java/frc/robot/Auto/RealLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RealLemonAuto.java @@ -6,10 +6,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; @@ -22,19 +20,28 @@ public RealLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy addCommands( Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(30.0); + double targetYawRad = startYawRad[0] + Math.toRadians(25.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); drive.drive(0,0, omegaRadiansPerSecond, false); }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(30.0); + double targetYawRad = startYawRad[0] + Math.toRadians(25.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); return Math.abs(errorRad) < Math.toRadians(3.0); }), - new InstantCommand(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), - new InstantCommand(() -> shooter.AutoToggleShoot(false)), - Commands.waitSeconds(3), - new InstantCommand(() -> shooter.AutoToggleKickIndex(false)) + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + shooter.runShooter(true); + }, shooter), + Commands.waitUntil(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(3.0), + Commands.runOnce(() -> { + shooter.runIndexer(true); + shooter.runKicker(true); + }, shooter) ); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e5b680d..c6e1b9e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -282,6 +282,9 @@ public static final class ShooterConstants { public static final double KICKER_SPEED = 0.6; public static final double INDEXER_SPEED = 0.4; //placeholder + // Shooter readiness (SparkMax encoder velocity is RPM). Tune on the real robot. + public static final double SHOOTER_READY_RPM = 30.0; + // Electrical limits/compensation. public static final double SHOOTER_VOLTAGE_COMP = 12.0; public static final int SHOOTER_CURRENT_LIMIT_AMPS = 60; diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 64b95a1..5fa11b2 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -102,6 +102,10 @@ public void setShooterSpeed(double speed) { shooterMotor.set(speed); } + public double getShooterVelocityRpm() { + return shooterMotor.getEncoder().getVelocity(); + } + public void runKicker(boolean kickerOn){ setKickerSpeed(kickerOn ? ShooterConstants.KICKER_SPEED : 0); @@ -160,7 +164,7 @@ public void periodic() { SmartDashboard.putNumber("Hood Target Position", hoodTargetPosition); SmartDashboard.putNumber("Hood Position", getHoodPosition()); SmartDashboard.putNumber("Shooter/Cmd", shooterCmd); - SmartDashboard.putNumber("Shooter/VelocityRPM", shooterMotor.getEncoder().getVelocity()); + SmartDashboard.putNumber("Shooter/VelocityRPM", getShooterVelocityRpm()); SmartDashboard.putNumber("Kicker/Cmd", kickerCmd); SmartDashboard.putNumber("Kicker/VelocityRPM", kickerMotor.getEncoder().getVelocity()); SmartDashboard.putNumber("Indexer/Cmd", indexerCmd); From b820738cca3193b0150c88807ee628a1052dfcfb Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 4 Mar 2026 16:12:52 -0600 Subject: [PATCH 64/80] reallemonauto --- .../java/frc/robot/Auto/RealLemonAuto.java | 18 ++++++++++++------ src/main/java/frc/robot/Constants.java | 2 +- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RealLemonAuto.java b/src/main/java/frc/robot/Auto/RealLemonAuto.java index 6990383..3252704 100644 --- a/src/main/java/frc/robot/Auto/RealLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RealLemonAuto.java @@ -34,14 +34,20 @@ public RealLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy Commands.runOnce(() -> { shooter.runIndexer(false); shooter.runKicker(false); - shooter.runShooter(true); }, shooter), - Commands.waitUntil(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) - .withTimeout(3.0), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(10.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + }, shooter).withTimeout(2.0), Commands.runOnce(() -> { - shooter.runIndexer(true); - shooter.runKicker(true); - }, shooter) + shooter.setIndexerSpeed(0.4); + shooter.setKickerSpeed(0.6); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) ); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c6e1b9e..3e0a859 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -283,7 +283,7 @@ public static final class ShooterConstants { public static final double INDEXER_SPEED = 0.4; //placeholder // Shooter readiness (SparkMax encoder velocity is RPM). Tune on the real robot. - public static final double SHOOTER_READY_RPM = 30.0; + public static final double SHOOTER_READY_RPM = 3000.0; // Electrical limits/compensation. public static final double SHOOTER_VOLTAGE_COMP = 12.0; From 781bfb83949757eb73633c477f38e9191fe2990c Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 5 Mar 2026 09:13:46 -0600 Subject: [PATCH 65/80] changed for the indexer to go 1 second after the kicker, added the limelight and the livecam --- .../java/frc/robot/Auto/RealLemonAuto.java | 28 ++++++---- src/main/java/frc/robot/RobotContainer.java | 56 ++++++++++++++++--- 2 files changed, 64 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RealLemonAuto.java b/src/main/java/frc/robot/Auto/RealLemonAuto.java index 3252704..758fe31 100644 --- a/src/main/java/frc/robot/Auto/RealLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RealLemonAuto.java @@ -38,16 +38,20 @@ public RealLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) .withTimeout(10.0), - Commands.run(() -> { - shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); - shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - }, shooter).withTimeout(2.0), - Commands.runOnce(() -> { - shooter.setIndexerSpeed(0.4); - shooter.setKickerSpeed(0.6); - }, shooter), - Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) - ); - } + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a7875fb..21c585d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,9 +9,11 @@ import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.cscore.HttpCamera; +import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -61,10 +63,17 @@ public class RobotContainer { //ShooterSubsystem for shooter private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + + private boolean lastHelmsRightBumperPressed = false; + private double helmsRightBumperPressTimestampSec = 0.0; + + private final java.util.Map limelightCameras = new java.util.HashMap<>(); + private UsbCamera driverCamera; public RobotContainer() { AutoConstants.initDashboard(); startLimelightStreams(); + startDriverCameraStream(); configureBindings(); } @@ -83,12 +92,34 @@ private void startLimelightStreams() { String url = VisionConstants.getLimelightStreamUrl(limelightName); SmartDashboard.putString("Vision/" + limelightName + "/StreamURL", url); - HttpCamera camera = new HttpCamera(limelightName, url); + HttpCamera camera = limelightCameras.computeIfAbsent(limelightName, (name) -> new HttpCamera(name, url)); camera.setConnectionStrategy(ConnectionStrategy.kKeepOpen); CameraServer.startAutomaticCapture(camera); } } + private void startDriverCameraStream() { + SmartDashboard.putBoolean("Driver Camera Enabled", true); + + if (RobotBase.isSimulation()) { + return; + } + + if (!SmartDashboard.getBoolean("Driver Camera Enabled", true)) { + return; + } + + if (driverCamera != null) { + return; + } + + // Microsoft LifeCam HD-3000 (or any USB UVC camera) connected to the roboRIO. + driverCamera = CameraServer.startAutomaticCapture("DriverCam", 0); + driverCamera.setConnectionStrategy(ConnectionStrategy.kKeepOpen); + driverCamera.setResolution(640, 480); + driverCamera.setFPS(30); + } + private void configureBindings() { // Y Button = Zero gyro (reset heading to 0° or 180° based on alliance) @@ -108,16 +139,25 @@ private void configureBindings() { // Right stick Y controls shooter. // Invert so stick-up (negative on Xbox) produces positive motor output. double shooterAxis = -MathUtil.applyDeadband( - helmsController.getRawAxis(Axis.kRightY.value), - 0.1); + helmsController.getRawAxis(Axis.kRightY.value), + 0.1); m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); // Right bumper runs the indexer and kicker while held. - Boolean rightBumperPressed = helmsController.getHID().getRightBumper(); + boolean rightBumperPressed = helmsController.getHID().getRightBumper(); + if (rightBumperPressed && !lastHelmsRightBumperPressed) { + helmsRightBumperPressTimestampSec = Timer.getFPGATimestamp(); + } + + boolean indexerEnabled = + rightBumperPressed + && (Timer.getFPGATimestamp() - helmsRightBumperPressTimestampSec) >= 1.0; m_shooter.setKickerSpeed(rightBumperPressed ? ShooterConstants.KICKER_SPEED : 0.0); - m_shooter.setIndexerSpeed(rightBumperPressed ? ShooterConstants.INDEXER_SPEED : 0.0); + m_shooter.setIndexerSpeed(indexerEnabled ? ShooterConstants.INDEXER_SPEED : 0.0); + + lastHelmsRightBumperPressed = rightBumperPressed; }, m_shooter)); @@ -156,9 +196,9 @@ private void configureBindings() { //INTAKE m_intake.setDefaultCommand( - new RunCommand( - () -> m_intake.setIntakePower(MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), - m_intake)); + new RunCommand( + () -> m_intake.setIntakePower(MathUtil.applyDeadband(helmsController.getLeftY(), 0.1)), + m_intake)); From 64511b682438c87d28345aa7fc28010f16a85561 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Thu, 5 Mar 2026 11:42:44 -0600 Subject: [PATCH 66/80] changed from mt2 back to mt1 --- src/main/java/frc/robot/Constants.java | 10 +++++----- .../frc/robot/Subsystems/SwerveSubsystem.java | 16 ++++++++-------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3e0a859..19f0787 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -241,12 +241,12 @@ public static Pose2d flipForAlliance(Pose2d pose){ /** Vision constants (Limelight, etc). */ public static final class VisionConstants { - public static final String[] LIMELIGHT_NAMES = {"limelightA", "limelightB"}; + public static final String[] LIMELIGHT_NAMES = {"limelight-a", "limelight-b"}; // Limelight MJPEG stream endpoints. // Using fixed IPs avoids mDNS/DNS resolution issues on the roboRIO. - public static final String LIMELIGHT_A_STREAM_URL = "http://10.39.26.201:5801/stream.mjpg"; - public static final String LIMELIGHT_B_STREAM_URL = "http://10.39.26.202:5801/stream.mjpg"; + public static final String LIMELIGHT_A_STREAM_URL = "http://10.39.26.4:5801/stream.mjpg"; + public static final String LIMELIGHT_B_STREAM_URL = "http://10.39.26.5:5801/stream.mjpg"; public static final boolean LIMELIGHT_STREAM_ENABLED_DEFAULT = true; public static final boolean VISION_ENABLED_DEFAULT = true; @@ -259,9 +259,9 @@ public static final class VisionConstants { public static String getLimelightStreamUrl(String limelightName) { switch (limelightName) { - case "limelightA": + case "limelight-a": return LIMELIGHT_A_STREAM_URL; - case "limelightB": + case "limelight-b": return LIMELIGHT_B_STREAM_URL; default: // Fallback for any future Limelight names. diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 25b8719..27244d3 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -149,15 +149,15 @@ private boolean isVisionEnabled() { private void updateOdometryWithVision (String limelightName){ boolean doRejectUpdate = false; LimelightHelpers.SetRobotOrientation(limelightName, odometry.getEstimatedPosition().getRotation().getDegrees(),0,0,0,0,0); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - if (mt2 == null){ + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); + if (mt1 == null){ return; } if(Math.abs(pigeon.getAngularVelocityZWorld().getValueAsDouble()) > VisionConstants.MAX_VISION_ANGULAR_RATE_DEG_PER_SEC) { doRejectUpdate = true; } - if(mt2.tagCount == 0) + if(mt1.tagCount == 0) { doRejectUpdate = true; } @@ -169,13 +169,13 @@ private void updateOdometryWithVision (String limelightName){ VisionConstants.VISION_STD_DEV_Y_METERS, VisionConstants.VISION_STD_DEV_THETA_RADIANS)); // need to measure odometry.addVisionMeasurement( - mt2.pose, - mt2.timestampSeconds); + mt1.pose, + mt1.timestampSeconds); } - SmartDashboard.putNumber("Vision/" + limelightName + "/TagCount", mt2.tagCount); - SmartDashboard.putNumber("Vision/" + limelightName + "/AvgTagDist", mt2.avgTagDist); - SmartDashboard.putNumber("Vision/" + limelightName + "/LatencyMs", mt2.latency); + SmartDashboard.putNumber("Vision/" + limelightName + "/TagCount", mt1.tagCount); + SmartDashboard.putNumber("Vision/" + limelightName + "/AvgTagDist", mt1.avgTagDist); + SmartDashboard.putNumber("Vision/" + limelightName + "/LatencyMs", mt1.latency); } From a3d7c58d623ad332502ff87e7b4524c1c6a5bf09 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Thu, 5 Mar 2026 14:45:31 -0600 Subject: [PATCH 67/80] duluth --- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes .../java/frc/robot/Auto/RealLemonAuto.java | 2 +- .../java/frc/robot/Command/AltAutoAlign.java | 3 ++- src/main/java/frc/robot/RobotContainer.java | 2 +- 8 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat index 77854d2130ab9de38e159dc88f062c90234bfba1..f2fb661fb6305db0f41b741b50ba196b7f2b0f79 100644 GIT binary patch delta 25 hcmZn=Xb@o8%;5WfqTm{qO$>hjHwtcHo1DQu0|0g>35x&# delta 20 ccmZn=Xb@o8$l(8fhjHwtcHo1DQu0|0g>35x&# delta 20 ccmZn=Xb@o8$l(8fhjHwtcHo1DQu0|0g>35x&# delta 20 ccmZn=Xb@o8$l(8fhjHwtcHo1DQu0|0g>35x&# delta 20 ccmZn=Xb@o8$l(8f2M|l0ZK4A2 shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) - .withTimeout(10.0), + .withTimeout(2.0), Commands.sequence( // Start kicker first, then start indexer 1 second later (kicker keeps running). diff --git a/src/main/java/frc/robot/Command/AltAutoAlign.java b/src/main/java/frc/robot/Command/AltAutoAlign.java index 4307fa5..d528ad0 100644 --- a/src/main/java/frc/robot/Command/AltAutoAlign.java +++ b/src/main/java/frc/robot/Command/AltAutoAlign.java @@ -79,7 +79,8 @@ public void execute(){ // respect drivetrain max velocity } - double desiredHeadingRadians = radialDirection.getAngle().getRadians(); + double desiredHeadingRadians = radialDirection.getAngle().getRadians() + Math.PI / 2.0; + //Face straight at the hub while moving double headingFeedforward = 0.0; if (radialDistance > 1e-3){ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21c585d..36cc7d4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -213,7 +213,7 @@ private double getSpeedMultiplier(){ // getHID() accesses the underlying XboxController to read button states directly. // CommandXboxController doesn't provide a method for stick button presses, so we use // the HID (Human Interface Device) object's getRawButton() method instead. - return driveController.getHID().getRawButton(Button.kLeftStick.value)? 0.85: 1; + return driveController.getHID().getRawButton(Button.kLeftStick.value)? 1: 1; } public Command getAutonomousCommand() { From 8094e8005afb8d10d117c38235c306201530d696 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 5 Mar 2026 14:56:57 -0600 Subject: [PATCH 68/80] add left bumper to go reverse --- src/main/java/frc/robot/RobotContainer.java | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 36cc7d4..e9bd8c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -144,7 +144,9 @@ private void configureBindings() { m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); - // Right bumper runs the indexer and kicker while held. + // Right bumper runs the indexer and kicker forward while held. + // Left bumper runs the indexer and kicker in reverse while held. + boolean leftBumperPressed = helmsController.getHID().getLeftBumper(); boolean rightBumperPressed = helmsController.getHID().getRightBumper(); if (rightBumperPressed && !lastHelmsRightBumperPressed) { helmsRightBumperPressTimestampSec = Timer.getFPGATimestamp(); @@ -154,8 +156,19 @@ private void configureBindings() { rightBumperPressed && (Timer.getFPGATimestamp() - helmsRightBumperPressTimestampSec) >= 1.0; - m_shooter.setKickerSpeed(rightBumperPressed ? ShooterConstants.KICKER_SPEED : 0.0); - m_shooter.setIndexerSpeed(indexerEnabled ? ShooterConstants.INDEXER_SPEED : 0.0); + double kickerSpeed = 0.0; + double indexerSpeed = 0.0; + + if (leftBumperPressed) { + kickerSpeed = -ShooterConstants.KICKER_SPEED; + indexerSpeed = -ShooterConstants.INDEXER_SPEED; + } else if (rightBumperPressed) { + kickerSpeed = ShooterConstants.KICKER_SPEED; + indexerSpeed = indexerEnabled ? ShooterConstants.INDEXER_SPEED : 0.0; + } + + m_shooter.setKickerSpeed(kickerSpeed); + m_shooter.setIndexerSpeed(indexerSpeed); lastHelmsRightBumperPressed = rightBumperPressed; }, From ee01b0f85690475b3c48df3df4f5a319c0699b1a Mon Sep 17 00:00:00 2001 From: Seqi Date: Thu, 5 Mar 2026 21:13:30 -0600 Subject: [PATCH 69/80] fixed auto align and added hood angle and made auto move --- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 2048 -> 0 bytes .../java/frc/robot/Auto/RealLemonAuto.java | 8 ++- .../java/frc/robot/Auto/RightLemonAuto.java | 57 ++++++++++++++++++ .../java/frc/robot/Command/AltAutoAlign.java | 18 ++++-- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 13 ++-- .../robot/Subsystems/ShooterSubsystem.java | 6 +- 11 files changed, 94 insertions(+), 12 deletions(-) delete mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat delete mode 100644 ctre_sim/Pigeon 2 - 017 - 0 - ext.dat create mode 100644 src/main/java/frc/robot/Auto/RightLemonAuto.java diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat deleted file mode 100644 index f2fb661fb6305db0f41b741b50ba196b7f2b0f79..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdnY;QOCp2!TxuenZN=LnvrRojMuRZWzW^$KV-8ThpR diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat deleted file mode 100644 index bfd42b21dd866bbb87d7e0950709f173dc1e0893..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdnY;QOCp2!TxuenZN=LnvrRojMu3h9}iRbJU>G5Eu=C(GVE^A+SK}pg$wfo19RuHe_2CgZMY* w{F}hokS5!ZEUt+xzW_zPU_CQ>5amGhr#xnz$jIRNt`;Z{R>Nh*1TqH(0NfrJ00000 diff --git a/src/main/java/frc/robot/Auto/RealLemonAuto.java b/src/main/java/frc/robot/Auto/RealLemonAuto.java index 0086576..df36ad5 100644 --- a/src/main/java/frc/robot/Auto/RealLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RealLemonAuto.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; @@ -18,14 +19,17 @@ public class RealLemonAuto extends SequentialCommandGroup { public RealLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { final double[] startYawRad = new double[1]; addCommands( + new InstantCommand(()->drive.drive(-0.5, 0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(25.0); + double targetYawRad = startYawRad[0] + Math.toRadians(30.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); drive.drive(0,0, omegaRadiansPerSecond, false); }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(25.0); + double targetYawRad = startYawRad[0] + Math.toRadians(30.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); return Math.abs(errorRad) < Math.toRadians(3.0); }), diff --git a/src/main/java/frc/robot/Auto/RightLemonAuto.java b/src/main/java/frc/robot/Auto/RightLemonAuto.java new file mode 100644 index 0000000..2336417 --- /dev/null +++ b/src/main/java/frc/robot/Auto/RightLemonAuto.java @@ -0,0 +1,57 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class RightLemonAuto extends SequentialCommandGroup { + + public RightLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + final double[] startYawRad = new double[1]; + addCommands( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-20.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-20.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + Commands.runOnce(() -> drive.drive(0.5, 0, 0, false), drive), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } +} diff --git a/src/main/java/frc/robot/Command/AltAutoAlign.java b/src/main/java/frc/robot/Command/AltAutoAlign.java index d528ad0..34d1ede 100644 --- a/src/main/java/frc/robot/Command/AltAutoAlign.java +++ b/src/main/java/frc/robot/Command/AltAutoAlign.java @@ -7,7 +7,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; /* Drives the robot in an orbit around the hub while continuously facing the hub center */ @@ -15,20 +17,22 @@ public class AltAutoAlign extends Command { private SwerveSubsystem swerveSubsystem; + private ShooterSubsystem shooterSubsystem; private final PIDController headingController = new PIDController(kHeadingKp,0,0); private final PIDController radiusController = new PIDController(kRadialKp, kRadialKi, kRadialKd); private static final double kDesiredOrbitRadiusMeters = 2; //placeholder private static final double kMaxRadialSpeedMetersPerSecond = 1.0; // Max speed for correcting radius errors - private static final double kRadialKp = 1.6; //P-gain for radial distance correction + private static final double kRadialKp = 0.1; //P-gain for radial distance correction private static final double kRadialKi = 0.0; private static final double kRadialKd = 0.0; - private static final double kHeadingKp = 4.5; //P-gain for yaw control that faces the hub + private static final double kHeadingKp = 0.1; //P-gain for yaw control that faces the hub - public AltAutoAlign(SwerveSubsystem swerveSubsystem){ + public AltAutoAlign(SwerveSubsystem swerveSubsystem, ShooterSubsystem shooterSubsystem){ this.swerveSubsystem = swerveSubsystem; - addRequirements(swerveSubsystem); + this.shooterSubsystem = shooterSubsystem; + addRequirements(swerveSubsystem, shooterSubsystem); headingController.enableContinuousInput(-Math.PI, Math.PI); radiusController.setSetpoint(kDesiredOrbitRadiusMeters); } @@ -105,6 +109,12 @@ public void execute(){ swerveSubsystem.driveFromChassisSpeeds(requestedSpeeds, false); // Command the swerve in closed loop + shooterSubsystem.setHoodAngle(ShooterSubsystem.HoodAngle.MED); + //put up hood angle + + shooterSubsystem.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + //Start shooter motor + } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 19f0787..f5910b4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -293,10 +293,12 @@ public static final class ShooterConstants { // Hood position units are motor rotations (NEO internal encoder). // Max travel is 3 rotations = 1080 degrees. public static final double HOOD_MIN_ROTATIONS = 0.0; - public static final double HOOD_MAX_ROTATIONS = 24.0; + public static final double HOOD_MED_ROTATIONS = 24.0; + public static final double HOOD_MAX_ROTATIONS = 36.0; // Preset positions. public static final double HOOD_ANGLE_LOW = HOOD_MIN_ROTATIONS; + public static final double HOOD_ANGLE_MED = HOOD_MED_ROTATIONS; public static final double HOOD_ANGLE_HIGH = HOOD_MAX_ROTATIONS; // "up" (about 2 inches) public static final double HOOD_KP = 0.1; public static final double HOOD_MAX_OUTPUT = 0.4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9bd8c3..bd36d64 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -140,9 +141,12 @@ private void configureBindings() { // Invert so stick-up (negative on Xbox) produces positive motor output. double shooterAxis = -MathUtil.applyDeadband( helmsController.getRawAxis(Axis.kRightY.value), - 0.1); - + 0.1); + m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); + if (shooterAxis > 0.2) { + m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED); + } // Right bumper runs the indexer and kicker forward while held. // Left bumper runs the indexer and kicker in reverse while held. @@ -178,7 +182,8 @@ private void configureBindings() { // Hood controls (helms controller). // Y = hood up (2 inches / max travel), B = hood down. helmsController.y().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); - helmsController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); + helmsController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), m_shooter)); + helmsController.a().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); // Left Trigger = Auto-align to left scoring position @@ -186,7 +191,7 @@ private void configureBindings() { // Right Trigger = Auto-align to right scoring position driveController.axisGreaterThan(Axis.kRightTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, false)); // Right Bumper = Alt-Auto-Align - driveController.button(Button.kRightBumper.value).whileTrue(new AltAutoAlign(m_drive)); + driveController.button(Button.kRightBumper.value).whileTrue(new AltAutoAlign(m_drive, m_shooter)); // Default command runs continuously when no other command requires the subsystem. // It automatically pauses when commands like AutoAlign take control, then resumes diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 5fa11b2..4f78c76 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -19,7 +19,7 @@ public class ShooterSubsystem extends SubsystemBase { public boolean isShooterActive = false; //Shooter True - private final SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); + public final SparkMax shooterMotor = new SparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless); private final SparkMax kickerMotor = new SparkMax(ShooterConstants.KICKER_ID, MotorType.kBrushless); private final SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); private final SparkMax indexerMotor = new SparkMax(ShooterConstants.INDEXER_ID, MotorType.kBrushless); @@ -38,6 +38,7 @@ public class ShooterSubsystem extends SubsystemBase { public enum HoodAngle { LOW, + MED, HIGH } @@ -121,6 +122,9 @@ public void setHoodAngle(HoodAngle angle) { case LOW: hoodTargetPosition = ShooterConstants.HOOD_ANGLE_LOW; break; + case MED: + hoodTargetPosition = ShooterConstants.HOOD_ANGLE_MED; + break; case HIGH: hoodTargetPosition = ShooterConstants.HOOD_ANGLE_HIGH; break; From 10a547e0567e74f2f84a5c2e63714194aa3f8812 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 6 Mar 2026 08:18:24 -0600 Subject: [PATCH 70/80] fix shooter --- src/main/java/frc/robot/Auto/OffsetDepotAuto.java | 1 - src/main/java/frc/robot/RobotContainer.java | 4 ---- src/main/java/frc/robot/Subsystems/IntakeSubsystem.java | 5 ----- 3 files changed, 10 deletions(-) diff --git a/src/main/java/frc/robot/Auto/OffsetDepotAuto.java b/src/main/java/frc/robot/Auto/OffsetDepotAuto.java index 8172ff0..025e3c4 100644 --- a/src/main/java/frc/robot/Auto/OffsetDepotAuto.java +++ b/src/main/java/frc/robot/Auto/OffsetDepotAuto.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.IntakeConstants; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bd36d64..c9930c7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,7 +12,6 @@ import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -144,9 +143,6 @@ private void configureBindings() { 0.1); m_shooter.setShooterSpeed(shooterAxis * ShooterConstants.SHOOTER_SPEED); - if (shooterAxis > 0.2) { - m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED); - } // Right bumper runs the indexer and kicker forward while held. // Left bumper runs the indexer and kicker in reverse while held. diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 8df1be9..412243c 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -4,12 +4,7 @@ package frc.robot.Subsystems; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; From 70d6d7822d3216de0f8c321e77a3e9e6db60c5ae Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 6 Mar 2026 09:53:35 -0600 Subject: [PATCH 71/80] increased intake speed --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f5910b4..727646a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -309,7 +309,7 @@ public static final class IntakeConstants { // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; - public static double INTAKE_SPEED = 75; //percent output scaling for intake motor + public static double INTAKE_SPEED = 90; //percent output scaling for intake motor public static int INTAKE_ARM_ID = 18; public static int GEAR_RATIO = 25; From 680378f2581f4d6b7fed94a0766793b371f6770b Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 6 Mar 2026 11:31:10 -0600 Subject: [PATCH 72/80] added auto --- ...{RealLemonAuto.java => LeftLemonAuto.java} | 8 ++-- .../java/frc/robot/Auto/RightLemonAuto.java | 10 +++-- .../java/frc/robot/Auto/ShootEightAuto.java | 43 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 4 +- 4 files changed, 56 insertions(+), 9 deletions(-) rename src/main/java/frc/robot/Auto/{RealLemonAuto.java => LeftLemonAuto.java} (95%) create mode 100644 src/main/java/frc/robot/Auto/ShootEightAuto.java diff --git a/src/main/java/frc/robot/Auto/RealLemonAuto.java b/src/main/java/frc/robot/Auto/LeftLemonAuto.java similarity index 95% rename from src/main/java/frc/robot/Auto/RealLemonAuto.java rename to src/main/java/frc/robot/Auto/LeftLemonAuto.java index df36ad5..a23d4e4 100644 --- a/src/main/java/frc/robot/Auto/RealLemonAuto.java +++ b/src/main/java/frc/robot/Auto/LeftLemonAuto.java @@ -14,9 +14,9 @@ import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -public class RealLemonAuto extends SequentialCommandGroup { +public class LeftLemonAuto extends SequentialCommandGroup { - public RealLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + public LeftLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { final double[] startYawRad = new double[1]; addCommands( new InstantCommand(()->drive.drive(-0.5, 0,0, false), drive), @@ -24,12 +24,12 @@ public RealLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy new InstantCommand(()->drive.drive(0,0,0, false),drive), Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(30.0); + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); drive.drive(0,0, omegaRadiansPerSecond, false); }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(30.0); + double targetYawRad = startYawRad[0] + Math.toRadians(40.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); return Math.abs(errorRad) < Math.toRadians(3.0); }), diff --git a/src/main/java/frc/robot/Auto/RightLemonAuto.java b/src/main/java/frc/robot/Auto/RightLemonAuto.java index 2336417..ea4bbc5 100644 --- a/src/main/java/frc/robot/Auto/RightLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RightLemonAuto.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; @@ -18,18 +19,21 @@ public class RightLemonAuto extends SequentialCommandGroup { public RightLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { final double[] startYawRad = new double[1]; addCommands( + new InstantCommand(()->drive.drive(-0.5, 0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(-20.0); + double targetYawRad = startYawRad[0] + Math.toRadians(320.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); drive.drive(0,0, omegaRadiansPerSecond, false); }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(-20.0); + double targetYawRad = startYawRad[0] + Math.toRadians(320.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); return Math.abs(errorRad) < Math.toRadians(3.0); }), - Commands.runOnce(() -> drive.drive(0.5, 0, 0, false), drive), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), Commands.runOnce(() -> { shooter.runIndexer(false); diff --git a/src/main/java/frc/robot/Auto/ShootEightAuto.java b/src/main/java/frc/robot/Auto/ShootEightAuto.java new file mode 100644 index 0000000..3ec5bf5 --- /dev/null +++ b/src/main/java/frc/robot/Auto/ShootEightAuto.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class ShootEightAuto extends SequentialCommandGroup { + + public ShootEightAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + final double[] startYawRad = new double[1]; + addCommands( + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c9930c7..9d095d4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,7 @@ import frc.robot.Auto.DriveTestAuto; import frc.robot.Auto.EightLemonAuto; import frc.robot.Auto.OffsetDepotAuto; -import frc.robot.Auto.RealLemonAuto; +import frc.robot.Auto.LeftLemonAuto; import frc.robot.Auto.TrenchToDepotAuto; import frc.robot.Auto.CenterToDepotAuto; import frc.robot.Constants.AutoConstants; @@ -240,7 +240,7 @@ public Command getAutonomousCommand() { case TrenchToDepotAuto -> new TrenchToDepotAuto(m_drive); case CenterToDepotAuto -> new CenterToDepotAuto(m_drive); case OffsetDepotAuto -> new OffsetDepotAuto(m_drive, m_intake, m_shooter); - case RealLemonAuto -> new RealLemonAuto(m_drive, m_intake, m_shooter); + case RealLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); default -> Commands.none(); }; } From 82c8db8b14892b5827b9bef77cddff68df479b75 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 6 Mar 2026 13:19:09 -0600 Subject: [PATCH 73/80] changed speed, auto setup on constants --- src/main/java/frc/robot/Constants.java | 10 +++++++--- src/main/java/frc/robot/RobotContainer.java | 15 ++++++++++----- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 727646a..3b91bce 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -161,7 +161,9 @@ public enum AutoMode{ TrenchToDepotAuto, CenterToDepotAuto, OffsetDepotAuto, - RealLemonAuto + LeftLemonAuto, + RightLemonAuto, + ShootEightAuto } private static SendableChooser sideChooser = new SendableChooser(); @@ -175,13 +177,15 @@ public static void initDashboard() { sideChooser.addOption("RIGHT", true); sideChooser.setDefaultOption("LEFT", false); - autoModeChooser.setDefaultOption("TrenchToDepotAuto", AutoMode.TrenchToDepotAuto); + autoModeChooser.setDefaultOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("None", AutoMode.None); autoModeChooser.addOption("EightLemonAuto (PathPlanner)", AutoMode.EightLemonAuto); autoModeChooser.addOption("TrenchToDepotAuto", AutoMode.TrenchToDepotAuto); autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); autoModeChooser.addOption("Offset Depot Auto", AutoMode.OffsetDepotAuto); - autoModeChooser.addOption("RealLemonAuto", AutoMode.RealLemonAuto); + autoModeChooser.addOption("ShootEightAuto", AutoMode.ShootEightAuto); + autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); + autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d095d4..ab2b4f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,8 @@ import frc.robot.Auto.LeftLemonAuto; import frc.robot.Auto.TrenchToDepotAuto; import frc.robot.Auto.CenterToDepotAuto; +import frc.robot.Auto.RightLemonAuto; +import frc.robot.Auto.ShootEightAuto; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Constants.ShooterConstants; @@ -197,11 +199,11 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.5, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.7, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.5, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.7, // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.5, + () -> -driveController.getRawAxis(rotationAxis) * 0.7, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag @@ -237,10 +239,13 @@ public Command getAutonomousCommand() { case None -> Commands.none(); case DriveTestAuto -> new DriveTestAuto(m_drive); case EightLemonAuto -> new EightLemonAuto(m_drive, m_shooter, m_intake); + case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); + case OffsetDepotAuto -> new OffsetDepotAuto(m_drive, m_intake, m_shooter); + case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); + case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case TrenchToDepotAuto -> new TrenchToDepotAuto(m_drive); case CenterToDepotAuto -> new CenterToDepotAuto(m_drive); - case OffsetDepotAuto -> new OffsetDepotAuto(m_drive, m_intake, m_shooter); - case RealLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); + default -> Commands.none(); }; } From baa0a9070b34201201fa30461892c60f2619e290 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 6 Mar 2026 14:23:30 -0600 Subject: [PATCH 74/80] tuned the rotation speed down --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab2b4f1..17477bb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -203,7 +203,7 @@ private void configureBindings() { // strafeSupplier - Side-to-side speed () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.7, // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.7, + () -> -driveController.getRawAxis(rotationAxis) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag From adec40b5602044fba8fad59e428785ef17abdb6b Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 6 Mar 2026 15:28:20 -0600 Subject: [PATCH 75/80] edit hood angle --- src/main/java/frc/robot/Auto/RightLemonAuto.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightLemonAuto.java b/src/main/java/frc/robot/Auto/RightLemonAuto.java index ea4bbc5..d361b89 100644 --- a/src/main/java/frc/robot/Auto/RightLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RightLemonAuto.java @@ -19,17 +19,17 @@ public class RightLemonAuto extends SequentialCommandGroup { public RightLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { final double[] startYawRad = new double[1]; addCommands( - new InstantCommand(()->drive.drive(-0.5, 0,0, false), drive), + new InstantCommand(()->drive.drive(0.5, 0,0, false), drive), Commands.waitSeconds(2), new InstantCommand(()->drive.drive(0,0,0, false),drive), Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(320.0); + double targetYawRad = startYawRad[0] + Math.toRadians(-30.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); drive.drive(0,0, omegaRadiansPerSecond, false); }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(320.0); + double targetYawRad = startYawRad[0] + Math.toRadians(-30.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); return Math.abs(errorRad) < Math.toRadians(3.0); }), From 6cb76fb44c1d7edcd98fe26fc27f4f0a880db9ff Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 6 Mar 2026 15:38:43 -0600 Subject: [PATCH 76/80] Moved unused autonomous into unused folder, add a centerlemonauto --- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 0 -> 2048 bytes .../java/frc/robot/Auto/CenterLemonAuto.java | 49 ++++++++++++++++++ src/main/java/frc/robot/Constants.java | 15 ++---- src/main/java/frc/robot/RobotContainer.java | 13 ++--- .../CenterToDepotAuto.java | 2 +- .../{Auto => UnusedAuto}/DriveTestAuto.java | 2 +- .../{Auto => UnusedAuto}/EightLemonAuto.java | 2 +- .../{Auto => UnusedAuto}/OffsetDepotAuto.java | 2 +- .../TrenchToDepotAuto.java | 2 +- 13 files changed, 61 insertions(+), 26 deletions(-) create mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat create mode 100644 ctre_sim/Pigeon 2 - 017 - 0 - ext.dat create mode 100644 src/main/java/frc/robot/Auto/CenterLemonAuto.java rename src/main/java/frc/robot/{Auto => UnusedAuto}/CenterToDepotAuto.java (99%) rename src/main/java/frc/robot/{Auto => UnusedAuto}/DriveTestAuto.java (96%) rename src/main/java/frc/robot/{Auto => UnusedAuto}/EightLemonAuto.java (95%) rename src/main/java/frc/robot/{Auto => UnusedAuto}/OffsetDepotAuto.java (98%) rename src/main/java/frc/robot/{Auto => UnusedAuto}/TrenchToDepotAuto.java (99%) diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..25d3d5d73a2ead81f7ebb266baf83ccf23e9713d GIT binary patch literal 2048 zcmdT^O-NKx6u$G`*dIe!AQ2T=xM(2C7J@@i*G+{{1};RRU!+x0M7c_wu*EDa23lm) zt_Fr7NG-QE$hezmz!vUGY7s8li8UH_&(GKMU8!9QFFels?)lC+-#PcbH)gt4%sN`* z=IpxCo6y;g-JN@~JZQ}IO1gc=(bWSFcKveupIa{MA)6TNVf+0>2V3mtbsv!dUXC?A z=yydfzO&57_W!cBcYc`a^@UQ1!-Uvl;;X|r(L|uiCG7qD9 zzrI&&A{{h8_>u7xUewyF5ufq{R|47Jb}-pQ)Vvo5Y<1smjq-TZ7(|>C#{FzoFU~Kg z*zaeP?!zBT`VRp8pmg3#Hqg7D=iJ6ad~Z+(`YtbZy}so5FRLQ)k-ug{&lb&F z4h17W&M8~Wux^~k?92_u;D1X=|3&RDo0L4@qYllF=8t&jr}8`xXLH*lKk}2G7Wx#l z7GJw5^FW`-4;wUWDA7Xv-z??>KXBvY{oFU^WADc?_?Rc~!kyGl>JR*dp_V%OC63Q8 zXC?3OvfQJ*+I^qGYXqOx69n zuE>=amigHJU)Ek&AESEx39+a%kQV*0sRTS?i-GVnsyFHlQ5ctaKy^;4Yx=#+!)V@5 zuN9k62h9(DWbDL?T6-hpQ-0u5T=pxDC98;<_u_!9?%VP(k4KF_#5rNy&wlmd{Bnx@ ze)j1;{E?*p0MHLg&%IK%dADn=xjF7UKVAF(3GWQx6-sZ_LMzL^1f7C-BB#@SXYte`B<96a5m$=NGe* zcWg`UQC@8-Sl(0V_~GwN^5C95r4PV8n6H0dfwy@X<70e3zMeN<`#w%TDb=qYm2>}g dTj~N&jB9_c{0a92L*loJo#BN%K3-ZA{x8sLAua#_ literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..b47546c156d4dcbbad23f4e382a0c00938b43680 GIT binary patch literal 2048 zcmdT@O-NKx6uvX_LW5CE5Rr;R3>pX>K|u-Rwy8)`!G%QhW4Lr7f~^#v=wcQM)5@Tw z3L=3&B(~B)#+{N-ipc5?tM$9Sg zO~`}vLmr6t@S)Z|59<}o!n|=G?|uxhkNR&(*?&&=^F6^qJ?4;pq(Az>pT@Hufz6bMdEir?4)zr9q~G{b z_0V?)eAu)x^K{VvuNHbyJ?%g`5??!7!#Y4O%z>URKI(st)$U_os&VUczTi$A$~(%c zZ8W|Mc_yE@ug5&oT_X2IKAj^z#^AlWeBS@+`ZD`EU%gY1d*2%n{_suidSsvEu;guk ST~7n_0EalxZ0xhLEA_|AkrRag literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..e752b82f3d9583a8610986f249d0dd1486ed3d34 GIT binary patch literal 2048 zcmdT@O-NKx6u$H3*%S>j8WLH-5H1>su!WEWaody>sWe1{ltr^_AyKXZU#Kt(gMk(a zElZFH8b!A?)VMP$YT;6`AY8bU$_%~d=bPtTaM7ZL7ar$+=YHp$@1Aqtlufp}n1Yu=I3e$e6p;-06d7wl_RD{MY#x+in~sn;5#rcJ=u=w(ytKkH`R^b)94GqsDFO2;D)#jgkM!dl zd3_B3P8Rz8xZc!BekZmTDEfN;^t|qOz8CY-Ptr*k3%Y?{y{mt}c;BX+f4MzwdiD~( z$J>SBH=zrLE~|-v%f*#Lbnni@0$bF<9QW27vazc7%iNK1`k%nduFE~3_u`DykAe=q z_k8X`k@@yA>#SkT-_d@a_3l^PcU<`I`$k#6-@l#o!;k!*jMQ(^yp^P2^ z{7NKw$2R00mDFa^b9)YWx&H;y`%OIz8#Z|vgZB)Kruce3{>(2=Jga8Q-7=S7_v-b3 h{;v0P`Dvc5i9GkoI*d8HZS5V92C9ww+-kVO{4K$M5l#RA literal 0 HcmV?d00001 diff --git a/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..6434212e5b49afbe74ca1f8fa59143223c4faf4e GIT binary patch literal 2048 zcmeHGzf0Ru5P$gru{Mekq=*W(I4DSLF+#=VxoA!ztPX zQY!K=&!SCJsTY!KZNIbb?xO)dcATaKtMgCqJ?zWBmoW7TA7b@e<|n;u-8j_juOjNa zJFZ;#F25V*;(W8D>yZ$=Vq;?6`b&(T^y6M=_b~j0EcCncL<8X&(xbD1tS@@=`um9S zt)_UMq?0fXbOV3;)%5a72ar-SdZiKrU-RRBZ1mcFy&0X}@*6sIX$b{R#Lla!?{?3D zwb{(FXPQg?(V};6r*`?)#Nj>EUvnSO+q2H~9dDcMJJr-3vwYoWb!M?7Z+qTl^*$Cn z+{pZ=@dc~jJ--8J zBAwd&fGZZC*?O3lo??u7O1uil2l#*A1?~I_@#hyf&*Uw?8%J#9o-|bWKTGd*+mmY> z?>A$QQrHvpJ4?$p@ore&=N;=goMZioe%{OFM8maxzarzy3f~7(o8v9U7yQ8Y+I$lx b4ODygV&G)$bFzM{rp(Tx)cELZ95eqPIMMJ% literal 0 HcmV?d00001 diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java new file mode 100644 index 0000000..870c9c1 --- /dev/null +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class CenterLemonAuto extends SequentialCommandGroup { + + public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addCommands( + new InstantCommand(()->drive.drive(0, 0.5,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(()->drive.drive(0,0,0, false),drive), + + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b91bce..8e28383 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -156,14 +156,10 @@ public static final class AutoConstants { public enum AutoMode{ None, - DriveTestAuto, - EightLemonAuto, - TrenchToDepotAuto, - CenterToDepotAuto, - OffsetDepotAuto, LeftLemonAuto, RightLemonAuto, - ShootEightAuto + ShootEightAuto, + CenterLemonAuto } private static SendableChooser sideChooser = new SendableChooser(); @@ -179,13 +175,10 @@ public static void initDashboard() { autoModeChooser.setDefaultOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("None", AutoMode.None); - autoModeChooser.addOption("EightLemonAuto (PathPlanner)", AutoMode.EightLemonAuto); - autoModeChooser.addOption("TrenchToDepotAuto", AutoMode.TrenchToDepotAuto); - autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); - autoModeChooser.addOption("Offset Depot Auto", AutoMode.OffsetDepotAuto); autoModeChooser.addOption("ShootEightAuto", AutoMode.ShootEightAuto); autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); + autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); @@ -194,7 +187,7 @@ public static void initDashboard() { public static AutoMode getSelectedAutoMode(){ initDashboard(); AutoMode selection = autoModeChooser.getSelected(); - return selection != null ? selection : AutoMode.DriveTestAuto; + return selection != null ? selection : AutoMode.LeftLemonAuto; } public static boolean isRightSideAuto(){ initDashboard(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17477bb..7c0b076 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,14 +20,10 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Auto.DriveTestAuto; -import frc.robot.Auto.EightLemonAuto; -import frc.robot.Auto.OffsetDepotAuto; import frc.robot.Auto.LeftLemonAuto; -import frc.robot.Auto.TrenchToDepotAuto; -import frc.robot.Auto.CenterToDepotAuto; import frc.robot.Auto.RightLemonAuto; import frc.robot.Auto.ShootEightAuto; +import frc.robot.Auto.CenterLemonAuto; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Constants.ShooterConstants; @@ -237,14 +233,11 @@ public Command getAutonomousCommand() { return switch (selected) { case None -> Commands.none(); - case DriveTestAuto -> new DriveTestAuto(m_drive); - case EightLemonAuto -> new EightLemonAuto(m_drive, m_shooter, m_intake); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); - case OffsetDepotAuto -> new OffsetDepotAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); - case TrenchToDepotAuto -> new TrenchToDepotAuto(m_drive); - case CenterToDepotAuto -> new CenterToDepotAuto(m_drive); + case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); + default -> Commands.none(); }; diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java similarity index 99% rename from src/main/java/frc/robot/Auto/CenterToDepotAuto.java rename to src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java index 9d69ce7..34a761f 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Commands; diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java similarity index 96% rename from src/main/java/frc/robot/Auto/DriveTestAuto.java rename to src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java index 75dae1c..105578a 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/frc/robot/Auto/EightLemonAuto.java b/src/main/java/frc/robot/UnusedAuto/EightLemonAuto.java similarity index 95% rename from src/main/java/frc/robot/Auto/EightLemonAuto.java rename to src/main/java/frc/robot/UnusedAuto/EightLemonAuto.java index 1391f58..5677ef4 100644 --- a/src/main/java/frc/robot/Auto/EightLemonAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/EightLemonAuto.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Subsystems.ShooterSubsystem; diff --git a/src/main/java/frc/robot/Auto/OffsetDepotAuto.java b/src/main/java/frc/robot/UnusedAuto/OffsetDepotAuto.java similarity index 98% rename from src/main/java/frc/robot/Auto/OffsetDepotAuto.java rename to src/main/java/frc/robot/UnusedAuto/OffsetDepotAuto.java index 025e3c4..53e9c4e 100644 --- a/src/main/java/frc/robot/Auto/OffsetDepotAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/OffsetDepotAuto.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/frc/robot/Auto/TrenchToDepotAuto.java b/src/main/java/frc/robot/UnusedAuto/TrenchToDepotAuto.java similarity index 99% rename from src/main/java/frc/robot/Auto/TrenchToDepotAuto.java rename to src/main/java/frc/robot/UnusedAuto/TrenchToDepotAuto.java index fb306fe..d2a72cd 100644 --- a/src/main/java/frc/robot/Auto/TrenchToDepotAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/TrenchToDepotAuto.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Commands; From b60c7ef46186c050983735d2c897b46efb2a190f Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 7 Mar 2026 08:09:54 -0600 Subject: [PATCH 77/80] center auto --- src/main/java/frc/robot/Auto/CenterLemonAuto.java | 4 ++-- src/main/java/frc/robot/Constants.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java index 870c9c1..8c6c080 100644 --- a/src/main/java/frc/robot/Auto/CenterLemonAuto.java +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -18,11 +18,11 @@ public class CenterLemonAuto extends SequentialCommandGroup { public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addCommands( - new InstantCommand(()->drive.drive(0, 0.5,0, false), drive), + new InstantCommand(()->drive.drive(0, 0.6,0, false), drive), Commands.waitSeconds(2), new InstantCommand(()->drive.drive(0,0,0, false),drive), - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), Commands.runOnce(() -> { shooter.runIndexer(false); shooter.runKicker(false); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8e28383..416a1ed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -290,7 +290,7 @@ public static final class ShooterConstants { // Hood position units are motor rotations (NEO internal encoder). // Max travel is 3 rotations = 1080 degrees. public static final double HOOD_MIN_ROTATIONS = 0.0; - public static final double HOOD_MED_ROTATIONS = 24.0; + public static final double HOOD_MED_ROTATIONS = 20.0; public static final double HOOD_MAX_ROTATIONS = 36.0; // Preset positions. From 17e8da0845ccedf412c7cdbab805ed27b2631922 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 7 Mar 2026 09:08:30 -0600 Subject: [PATCH 78/80] changed auto distance --- src/main/java/frc/robot/Auto/CenterLemonAuto.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java index 8c6c080..af4a8e8 100644 --- a/src/main/java/frc/robot/Auto/CenterLemonAuto.java +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -18,7 +18,7 @@ public class CenterLemonAuto extends SequentialCommandGroup { public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addCommands( - new InstantCommand(()->drive.drive(0, 0.6,0, false), drive), + new InstantCommand(()->drive.drive(0, 0.5,0, false), drive), Commands.waitSeconds(2), new InstantCommand(()->drive.drive(0,0,0, false),drive), From 750b679adbc903e5ccc47543f84c73a0d7f972c8 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 7 Mar 2026 10:09:59 -0600 Subject: [PATCH 79/80] tuned auto distance --- src/main/java/frc/robot/Auto/CenterLemonAuto.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java index af4a8e8..f9b2712 100644 --- a/src/main/java/frc/robot/Auto/CenterLemonAuto.java +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -18,7 +18,7 @@ public class CenterLemonAuto extends SequentialCommandGroup { public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addCommands( - new InstantCommand(()->drive.drive(0, 0.5,0, false), drive), + new InstantCommand(()->drive.drive(0, 0.4,0, false), drive), Commands.waitSeconds(2), new InstantCommand(()->drive.drive(0,0,0, false),drive), From a49cfda44c55a3f049e797181681ab9a249f4612 Mon Sep 17 00:00:00 2001 From: Seqi Date: Mon, 9 Mar 2026 22:30:31 -0500 Subject: [PATCH 80/80] removed ctre_sim files --- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat | Bin 2048 -> 0 bytes ctre_sim/Pigeon 2 - 017 - 0 - ext.dat | Bin 2048 -> 0 bytes 5 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat delete mode 100644 ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat delete mode 100644 ctre_sim/Pigeon 2 - 017 - 0 - ext.dat diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat deleted file mode 100644 index 25d3d5d73a2ead81f7ebb266baf83ccf23e9713d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdT^O-NKx6u$G`*dIe!AQ2T=xM(2C7J@@i*G+{{1};RRU!+x0M7c_wu*EDa23lm) zt_Fr7NG-QE$hezmz!vUGY7s8li8UH_&(GKMU8!9QFFels?)lC+-#PcbH)gt4%sN`* z=IpxCo6y;g-JN@~JZQ}IO1gc=(bWSFcKveupIa{MA)6TNVf+0>2V3mtbsv!dUXC?A z=yydfzO&57_W!cBcYc`a^@UQ1!-Uvl;;X|r(L|uiCG7qD9 zzrI&&A{{h8_>u7xUewyF5ufq{R|47Jb}-pQ)Vvo5Y<1smjq-TZ7(|>C#{FzoFU~Kg z*zaeP?!zBT`VRp8pmg3#Hqg7D=iJ6ad~Z+(`YtbZy}so5FRLQ)k-ug{&lb&F z4h17W&M8~Wux^~k?92_u;D1X=|3&RDo0L4@qYllF=8t&jr}8`xXLH*lKk}2G7Wx#l z7GJw5^FW`-4;wUWDA7Xv-z??>KXBvY{oFU^WADc?_?Rc~!kyGl>JR*dp_V%OC63Q8 zXC?3OvfQJ*+I^qGYXqOx69n zuE>=amigHJU)Ek&AESEx39+a%kQV*0sRTS?i-GVnsyFHlQ5ctaKy^;4Yx=#+!)V@5 zuN9k62h9(DWbDL?T6-hpQ-0u5T=pxDC98;<_u_!9?%VP(k4KF_#5rNy&wlmd{Bnx@ ze)j1;{E?*p0MHLg&%IK%dADn=xjF7UKVAF(3GWQx6-sZ_LMzL^1f7C-BB#@SXYte`B<96a5m$=NGe* zcWg`UQC@8-Sl(0V_~GwN^5C95r4PV8n6H0dfwy@X<70e3zMeN<`#w%TDb=qYm2>}g dTj~N&jB9_c{0a92L*loJo#BN%K3-ZA{x8sLAua#_ diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat deleted file mode 100644 index b47546c156d4dcbbad23f4e382a0c00938b43680..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdT@O-NKx6uvX_LW5CE5Rr;R3>pX>K|u-Rwy8)`!G%QhW4Lr7f~^#v=wcQM)5@Tw z3L=3&B(~B)#+{N-ipc5?tM$9Sg zO~`}vLmr6t@S)Z|59<}o!n|=G?|uxhkNR&(*?&&=^F6^qJ?4;pq(Az>pT@Hufz6bMdEir?4)zr9q~G{b z_0V?)eAu)x^K{VvuNHbyJ?%g`5??!7!#Y4O%z>URKI(st)$U_os&VUczTi$A$~(%c zZ8W|Mc_yE@ug5&oT_X2IKAj^z#^AlWeBS@+`ZD`EU%gY1d*2%n{_suidSsvEu;guk ST~7n_0EalxZ0xhLEA_|AkrRag diff --git a/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat deleted file mode 100644 index e752b82f3d9583a8610986f249d0dd1486ed3d34..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmdT@O-NKx6u$H3*%S>j8WLH-5H1>su!WEWaody>sWe1{ltr^_AyKXZU#Kt(gMk(a zElZFH8b!A?)VMP$YT;6`AY8bU$_%~d=bPtTaM7ZL7ar$+=YHp$@1Aqtlufp}n1Yu=I3e$e6p;-06d7wl_RD{MY#x+in~sn;5#rcJ=u=w(ytKkH`R^b)94GqsDFO2;D)#jgkM!dl zd3_B3P8Rz8xZc!BekZmTDEfN;^t|qOz8CY-Ptr*k3%Y?{y{mt}c;BX+f4MzwdiD~( z$J>SBH=zrLE~|-v%f*#Lbnni@0$bF<9QW27vazc7%iNK1`k%nduFE~3_u`DykAe=q z_k8X`k@@yA>#SkT-_d@a_3l^PcU<`I`$k#6-@l#o!;k!*jMQ(^yp^P2^ z{7NKw$2R00mDFa^b9)YWx&H;y`%OIz8#Z|vgZB)Kruce3{>(2=Jga8Q-7=S7_v-b3 h{;v0P`Dvc5i9GkoI*d8HZS5V92C9ww+-kVO{4K$M5l#RA diff --git a/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat deleted file mode 100644 index 6434212e5b49afbe74ca1f8fa59143223c4faf4e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2048 zcmeHGzf0Ru5P$gru{Mekq=*W(I4DSLF+#=VxoA!ztPX zQY!K=&!SCJsTY!KZNIbb?xO)dcATaKtMgCqJ?zWBmoW7TA7b@e<|n;u-8j_juOjNa zJFZ;#F25V*;(W8D>yZ$=Vq;?6`b&(T^y6M=_b~j0EcCncL<8X&(xbD1tS@@=`um9S zt)_UMq?0fXbOV3;)%5a72ar-SdZiKrU-RRBZ1mcFy&0X}@*6sIX$b{R#Lla!?{?3D zwb{(FXPQg?(V};6r*`?)#Nj>EUvnSO+q2H~9dDcMJJr-3vwYoWb!M?7Z+qTl^*$Cn z+{pZ=@dc~jJ--8J zBAwd&fGZZC*?O3lo??u7O1uil2l#*A1?~I_@#hyf&*Uw?8%J#9o-|bWKTGd*+mmY> z?>A$QQrHvpJ4?$p@ore&=N;=goMZioe%{OFM8maxzarzy3f~7(o8v9U7yQ8Y+I$lx b4ODygV&G)$bFzM{rp(Tx)cELZ95eqPIMMJ%