From b95611f8f93de0947c776a95989da048dd8c5d3e Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Fri, 20 Mar 2026 02:21:34 -0700 Subject: [PATCH 01/32] 3/20 - Quick Save Vision --- src/main/java/frc/robot/BuildConstants.java | 12 +- .../frc/robot/subsystems/vision/Vision.java | 82 ++++++-- .../subsystems/vision/VisionConstants.java | 110 +++++++---- .../frc/robot/subsystems/vision/VisionIO.java | 8 +- .../subsystems/vision/VisionIOLimelight.java | 184 +++++++++++++----- .../vision/VisionIOPhotonVision.java | 4 +- 6 files changed, 290 insertions(+), 110 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 08b618a..6370b22 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 117; - public static final String GIT_SHA = "a9a3bb9b6ba0417636c4e7c371002dc50cb3c379"; - public static final String GIT_DATE = "2026-03-14 19:05:40 EDT"; - public static final String GIT_BRANCH = "test/2026/FullRobotV2"; - public static final String BUILD_DATE = "2026-03-15 17:39:28 EDT"; - public static final long BUILD_UNIX_TIME = 1773610768489L; + public static final int GIT_REVISION = 121; + public static final String GIT_SHA = "c105f4a8cc878287d6b1aaf670d7e93aa4f9f506"; + public static final String GIT_DATE = "2026-03-16 02:02:34 EDT"; + public static final String GIT_BRANCH = "test/2026/FullRobotV3"; + public static final String BUILD_DATE = "2026-03-20 03:39:14 EDT"; + public static final long BUILD_UNIX_TIME = 1773992354576L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index f6e3e71..db857c8 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,9 +1,6 @@ -// Copyright (c) 2021-2025 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. +/* + * Unifying the best parts from FRC 1678 (2024), FRC 254 (FRC 2025), FRC 2910 (FRC 2025), FRC 6328 (FRC 2026), etc. + */ package frc.robot.subsystems.vision; @@ -13,6 +10,15 @@ import static frc.robot.subsystems.vision.VisionConstants.linearStdDevBaseline; import static frc.robot.subsystems.vision.VisionConstants.maxZError; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.LinkedList; +import java.util.List; +import java.util.Map; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -24,26 +30,72 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import frc.robot.util.FullSubsystem; -import java.util.LinkedList; -import java.util.List; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; public class Vision extends FullSubsystem { + /* + * Pipeline: + * 1. Read all cameras. Send single NT flush after all cameras. + * 2. For each observation from each camera, run the rejection checks. + * 3. For single-tag observations that pass intial, run quality gate check. + * 4. For multi-tag observations, run the std. dev. euqation + * + * Then, accumulate accepted observations into a list. + * Sort by timestamps. + * Submit all sorted observations to the pose estimator consumer. + */ private final VisionConsumer consumer; private final Supplier gyroRotationSupplier; private final Supplier robotSpeedsSupplier; private final VisionIO[] io; private final VisionIOInputsAutoLogged[] inputs; + + // NT check: latencySubscriber not updated (handled in VisionIOLimelight). + // Frame-based check: no frames received for frameDisconnectedTimeoutSec. + private final Timer[] frameDisconnectTimers; private final Alert[] disconnectedAlerts; - // Initialize logging values - // List allTagPoses = new LinkedList<>(); - List allRobotPoses = new LinkedList<>(); - List allRobotPosesAccepted = new LinkedList<>(); - // List allRobotPosesRejected = new LinkedList<>(); + // Logging key strings + private final String[] cameraInputKeys; + private final String[] cameraPosesAcceptedKeys; + private final String[] cameraLogPrefixes; + + // There ought to be a better way to do this, bruh + // Tag Detection time persistence for AdvantageKit/Scope visualisation + private final Map lastTagDetectionTimes = new HashMap<>(); + + // Note: When exclusiveTagId != NO_EXCLUSIVE_TAG, only observations containing that + // tag ID are accepted. Volatile bc I might need in command thread. + private volatile int exclusiveTagId = VisionConstants.NO_EXCLUSIVE_TAG; + + // Per-LL accepted and all pose lists. Cleared and reused each loop. + private final ArrayList[] perCameraRobotPoses; + private final ArrayList[] perCameraRobotPosesAccepted; + + // Global summary lists. Cleared and reused each loop. + private final ArrayList allRobotPoses; + private final ArrayList allRobotPosesAccepted; + + // Output buffers + private Pose3d[] allRobotPosesBuffer = new Pose3d[0]; + private Pose3d[] allRobotPosesAcceptedBuffer = new Pose3d[0]; + private Pose3d[] perCameraBuffer = new Pose3d[8]; + private static final Pose3d[] EMPTY_POSE3D = new Pose3d[0]; + + private final ArrayList pendingObservations = new ArrayList<>(16); + + private Rotation3d cachedGyroRotation3d = new Rotation3d(); + private double lastGyroRadians = Double.NaN; // DO NOT USE 0.0 + + /* + * One fully-processed observation, send this boy + */ + private record PendingObservation( + double timestamp, + Pose2d pose, + Matrix stdDevs) {} public Vision( VisionConsumer consumer, diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 6820c3b..1680669 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -7,31 +7,36 @@ package frc.robot.subsystems.vision; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -public class VisionConstants { - // AprilTag layout - Test FieldConstants from 6328 - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); // Fix to 2026 Layout - // FieldConstants(); +public final class VisionConstants { + + private VisionConstants() {} + + // ------- APRILTAG FIELD CONSTANTS -------- \\ + +public static final AprilTagFieldLayout aprilTagLayout; + static { + aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + // All California District Events and DCMP should be WELDED fields. + } - // Camera names, must match names configured on coprocessor - public static String cameraPurple = "limelight-purple"; // BL - public static String cameraOrange = "limelight-orange"; // BR - public static String cameraYellow = "limelight-yellow"; // FL - public static String cameraPink = "limelight-pink"; // FR + // ------- LIMELIGHT NAME -------- \\ + + // The User MUST get this right + public static final String cameraPurple = "limelight-purple"; + public static final String cameraOrange = "limelight-orange"; + public static final String cameraYellow = "limelight-yellow"; + public static final String cameraPink = "limelight-pink"; // Unused public static String cameraPlaceholder = "limelight-placeholder"; - // Robot to camera transforms - // (Not used by Limelight, configure in LL Finder UI instead) + // ------- LIMELIGHT TRANSFORM (SIM) -------- \\ + public static Transform3d cameraTransformToPurple = new Transform3d(0.1397, -0.3302, 0.1778, new Rotation3d(0.0, 0.38, 90.0)); // updated 1/22 public static Transform3d cameraTransformToOrange = @@ -41,33 +46,72 @@ public class VisionConstants { public static Transform3d cameraTransformToBlue = new Transform3d(0.127, -0.3302, 0.18415, new Rotation3d(0.0, 0.38, -90.0)); // updated 1/22 - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - // Standard deviation baselines at 1 meter - public static double linearStdDevBaseline = 0.04; // Meters - public static double angularStdDevBaseline = Double.POSITIVE_INFINITY; // Radians + // ------- PER-LIMELIGHT TRUST MULTIPLIERS -------- \\ + // Note: This is indexed in the same order that the cameras are passed into the Vision constructor + // Increase the factor equals less trust in that limelight - // Standard deviation multipliers for each camera - // Manual Variance Weighting: (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = + public static final double[] cameraStdDevFactors = new double[] { 1.0, // Camera 0 - Yellow - L 1.0, // Camera 1 - Purple - L - 2.0, // Camera 2 - Pink - R - 2.0, // Camera 3 - Orange - L + 1.0, // Camera 2 - Pink - R + 1.0, // Camera 3 - Orange - L }; + // ------- BUMPER CLIPPING MARGIN -------- \\ + + public static final double fieldBorderMargin = 0.5; // meters + + // ------- DISCONNECTION UTIL -------- \\ + + public static final double ntDisconnectedTimeoutMs = 250.0; // please work + public static final double frameDisconnectedTimeoutSec = 0.5; + + // ------- POSE FILTERING CONSTANTS -------- \\ + + public static final double maxZError = 0.5; // meters + + public static final double maxLinearSpeed = 4.0; // m/s + + public static final double maxAngularSpeed = Math.toRadians(720); // rad/s (~2 full rotations) + + public static final double maxGyroError = 1.0; // Degrees + + // Only for when only one tag is present + public static final double singleTagMinAreaPercent = 0.10; // Percent of Image frame + public static final double singleTagMaxAngularVelocityRadPerSec = Math.toRadians(360); // rad/s + public static final double singleTagThetaStdDev = Double.POSITIVE_INFINITY; // Redundant but wtv + + // Standard deviation baselines at 1 meter + public static final double linearStdDevBaseline = 0.04; // Meters + public static final double angularStdDevBaseline = Double.POSITIVE_INFINITY; // Radians + // Multipliers to apply for MegaTag 2 observations public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // Never Trust - // Clamping ranges for vision estimates (see units) - static final double maxLinearSpeed = 2.0; // Meters per second - static final double maxAngularSpeed = - DegreesPerSecond.of(270).in(RadiansPerSecond); // Radians per second - static final double maxGyroError = 1.0; // Degrees - static final double maxTranslationError = 1.0; // Meters - static final int LOCK_MODE = 10; + // Scale factor applied to Std. Devs. for Limelight's native Std. Devs. + // Set to 0.0 to disable the native std dev influence. + public static final double limelightStdDevWeight = 0.5; + + // ------- MISC / UTIL -------- \\ + + // Index constants for limelight NT entry for std. dev. + // Layout: [MT1x, MT1y, MT1z, MT1roll, MT1pitch, MT1yaw, MT2x, MT2y, MT2z, MT2roll, MT2pitch, MT2yaw] - IIRC + public static final int kMT2XStdDevIndex = 6; + public static final int kMT2YStdDevIndex = 7; + public static final int kMT2YawStdDevIndex = 11; + public static final int kExpectedStdDevLen = 12; + + + // Used for avoiding goofy floating point overflow + public static final double LARGE_VARIANCE = 1e9; + + // Default (-1): No Tag ID filtering + public static final int NO_EXCLUSIVE_TAG = -1; + + // How long AdvnatageScope holds onto the pose visualizer + // So I don't need super-ision to see the poses :skull: + public static final double tagLogPersistenceSeconds = 0.1; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 6428210..62128eb 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -13,9 +13,12 @@ public interface VisionIO { @AutoLog public static class VisionIOInputs { + public boolean connected = false; + public double lastFrameTimestampSec = 0.0; public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; + public double[] limelightStdDevs = new double[0]; } /** Represents a robot pose sample used for pose estimation. */ @@ -25,6 +28,7 @@ public static record PoseObservation( double ambiguity, int tagCount, double averageTagDistance, + double averageTagArea, PoseObservationType type) {} public static enum PoseObservationType { @@ -32,5 +36,7 @@ public static enum PoseObservationType { PHOTONVISION } - public default void updateInputs(VisionIOInputs inputs) {} + default void updateInputs(VisionIOInputs inputs) {} + + default void setThrottleValue(int throttleValue) {} } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 36b69ed..089636b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -7,6 +7,13 @@ package frc.robot.subsystems.vision; +import java.util.ArrayList; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -16,19 +23,36 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; -import java.util.HashSet; -import java.util.LinkedList; -import java.util.List; -import java.util.Set; -import java.util.function.Supplier; /** IO implementation for real Limelight hardware. */ public class VisionIOLimelight implements VisionIO { + // Storing NT once avoids repeated static lookup + private static final NetworkTableInstance NT = NetworkTableInstance.getDefault(); + private final Supplier rotationSupplier; + private final Supplier yawRateSupplier; // rad/s + private final DoubleArrayPublisher orientationPublisher; + private final DoubleSubscriber latencySubscriber; // pipeline latency (ms) + private final DoubleArraySubscriber megatag2Subscriber; // botpose_orb_wpiblue + private final DoubleArraySubscriber limelightStdDevsSubscriber; // native units by LL + private final DoubleSubscriber throttlePublisher; + + // Pre-allocate buffers + private final ArrayList poseObservationCache = new ArrayList<>(8); + + // Rm Hashset method for preventing tagId duplication + // 32 tag is totally unrealistic but let's just not get a overflow error + private static final int MAX_TAGS = 32; + private final int[] tagIdScratchLottery = new int[MAX_TAGS]; + private int tagIdCount = 0; - private final DoubleSubscriber latencySubscriber; - private final DoubleArraySubscriber megatag2Subscriber; + // Output array for inputs.poseObservations + // Rm the PoseObservation[n] memory allocation required + private PoseObservation[] poseObservationOutput = new PoseObservation[8]; + + // Default a blank array to store the std dev in case it doesn't work + private static final double[] stdDevCache = new double[0]; /** * Creates a new VisionIOLimelight. @@ -36,79 +60,126 @@ public class VisionIOLimelight implements VisionIO { * @param name The configured name of the Limelight. * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. */ - public VisionIOLimelight(String name, Supplier rotationSupplier) { - var table = NetworkTableInstance.getDefault().getTable(name); + public VisionIOLimelight(String name, Supplier rotationSupplier, Supplier yawRateSupplier) { + + var table = NT.getTable(name); + this.rotationSupplier = rotationSupplier; + this.yawRateSupplier = yawRateSupplier; + orientationPublisher = - table - .getDoubleArrayTopic("robot_orientation_set") - .publish(); // [yaw,yawrate,pitch,pitchrate,roll,rollrate] - Degrees / Degrees per - // second + table.getDoubleArrayTopic("robot_orientation_set").publish(); + // [yaw,yawrate,pitch,pitchrate,roll,rollrate] - Degrees / Degrees per second latencySubscriber = - table - .getDoubleTopic("tl") - .subscribe(0.0); // Select pipeline's latency in ms. Total Latency = tl + cl. - // Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees, total latency (cl+tl), - // tag count, tag span, average tag distance from camera, average tag area () - // Should be "botpose_wpiblue", but don't fix what isn't broken - // double[] megatag2Cache = new double[] {}; + table.getDoubleTopic("tl").subscribe(0.0); + // Select pipeline's latency in ms. Total Latency = tl + cl. megatag2Subscriber = - table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[]{}); + // Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees + + limelightStdDevsSubscriber = + table.getDoubleArrayTopic("stddevs").subscribe(stdDevCache); + + // Not to sure if this is going to work + // NT topics are write only so im goofing a DoubleSubscriber to publish changes + throttlePublisher = + table.getDoubleTopic("throttle_set").subscribe(0.0); + } @Override public void updateInputs(VisionIOInputs inputs) { - // Update connection status based on whether an update has been seen in the last 250ms + + // ------- NT CHECK -------- \\ + + // Latency topic not updated in 250ms = NT disconnected inputs.connected = - ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) + < VisionConstants.ntDisconnectedTimeoutMs; + + // ------- ROBOT ORIENTATION PUBLISHING -------- \\ + + // Publish BEFORE reading poses, the User must get this right + // Added yawRate bc it helps with latency compensation + // Note: flush() is NOT called here // Update orientation for MegaTag 2 + double yawDeg = rotationSupplier.get().getDegrees(); + double yawRateDegPerSec = Units.radiansToDegrees(yawRateSupplier.get()); orientationPublisher.accept( - new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); - NetworkTableInstance.getDefault() - .flush(); // Increases network traffic but recommended by Limelight + new double[] {yawDeg, yawRateDegPerSec, 0.0, 0.0, 0.0, 0.0}); + + // ------- LL NATIVE STD DEV -------- \\ + inputs.limelightStdDevs = limelightStdDevsSubscriber.get(stdDevCache); - // Read new pose observations from NetworkTables - Set tagIds = new HashSet<>(); - List poseObservations = new LinkedList<>(); + // ------- PROCESS THE POSE OBSERVATION -------- \\ + // **insert pain + + // Clear Pre-allocated buffers + poseObservationCache.clear(); + tagIdCount = 0; + + // Process Buffer Frames + //readQueue() returns all frames captured since the last call (0 or multiple) for (var rawSample : megatag2Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; + + // Use the NT server timestamp of this sample (multiply conversion for seconds) + inputs.lastFrameTimestampSec = rawSample.timestamp * 1.0e-6; + + // I might go insane but here we go + // Collect frame tagIds without duplicates (without HashSets) + // botpose_orb_wpiblue data starts at index 11, 7 values per tag for (int i = 11; i < rawSample.value.length; i += 7) { - tagIds.add((int) rawSample.value[i]); + int id = (int) rawSample.value[i]; + boolean found = false; + for (int j = 0; j < tagIdCount; j++) { + if (tagIdScratchLottery[j] == id) { found = true; break; } + } + if (!found && tagIdCount < MAX_TAGS) { + tagIdScratchLottery[tagIdCount++] = id; + } } - poseObservations.add( + + // Build Pose Observation + // botpose_orb_wpiblue array layout: + // [0-2] : X, Y, Z (meters) + // [3-5] : roll, pitch, yaw (degrees) + // [6] : total latency in ms (tl + cl. Use this, not just tl) + // [7] : tag count + // [8] : tag span (i don't care about this) + // [9] : average tag distance (meters) + // [10] : average tag area (percent of image, 0–100) -> convert to fraction + // [11...]: tag data (7 values per tag) + poseObservationCache.add( new PoseObservation( - // Timestamp, based on server timestamp of publish and latency + // Timestamp: NT server publish time minus total pipeline latency rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, - - // 3D pose estimate parsePose(rawSample.value), - - // Ambiguity, zeroed because the pose is already disambiguated 0.0, - - // Tag count - (int) rawSample.value[7], - - // Average tag distance - rawSample.value[9], - - // Observation type + (int) rawSample.value[7], // tagCount + rawSample.value[9], // averageTagDistance (meters) + rawSample.value[10] / 100.0, // averageTagArea: convert % to fraction PoseObservationType.MEGATAG_2)); } - // Save pose observations to inputs object - inputs.poseObservations = new PoseObservation[poseObservations.size()]; - for (int i = 0; i < poseObservations.size(); i++) { - inputs.poseObservations[i] = poseObservations.get(i); + // Write Pose Observations + int observationSize = poseObservationCache.size(); + if (poseObservationOutput.length < observationSize) { + poseObservationOutput = new PoseObservation[observationSize * 2]; + // If not enough space, double that size. } - // Save tag IDs to inputs objects - inputs.tagIds = new int[tagIds.size()]; - int i = 0; - for (int id : tagIds) { - inputs.tagIds[i++] = id; + // Note: toArray writes into the provided array and returns it + inputs.poseObservations = poseObservationCache.toArray(poseObservationOutput); + + // Write Tag ID's + // Only reallocate inputs.tagIds if the count changed + if (inputs.tagIds.length != tagIdCount) { + inputs.tagIds = new int[tagIdCount]; } + System.arraycopy(tagIdScratchLottery, 0, inputs.tagIds, 0, tagIdCount); + } /** @@ -125,4 +196,11 @@ private static Pose3d parsePose(double[] rawLLArray) { Units.degreesToRadians(rawLLArray[4]), Units.degreesToRadians(rawLLArray[5]))); } + + @Override + public void setThrottleValue(int throttleValue) { + NT.getTable(orientationPublisher.getTopic().getName().split("/")[0]) + .getEntry("throttle_set") + .setNumber(throttleValue); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index fd24106..ff750e2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -67,7 +67,7 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate multitagResult.estimatedPose.ambiguity, // Ambiguity multitagResult.fiducialIDsUsed.size(), // Tag count - totalTagDistance / result.targets.size(), // Average tag distance + totalTagDistance / result.targets.size(), 0.5, // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } else if (!result.targets.isEmpty()) { // Single tag result @@ -93,7 +93,7 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate target.poseAmbiguity, // Ambiguity 1, // Tag count - cameraToTarget.getTranslation().getNorm(), // Average tag distance + cameraToTarget.getTranslation().getNorm(), 0.5, // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } } From aef1b33bd4059bf5390be019550538cd69f55795 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Fri, 20 Mar 2026 12:43:38 -0700 Subject: [PATCH 02/32] 3/20 - Quick Save Vision --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 79 ++-- .../frc/robot/subsystems/vision/Vision.java | 402 +++++++++++++----- .../subsystems/vision/VisionConstants.java | 24 +- .../subsystems/vision/VisionIOLimelight.java | 68 ++- .../vision/VisionIOPhotonVision.java | 6 +- 6 files changed, 374 insertions(+), 215 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 6370b22..8cdf94d 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 121; - public static final String GIT_SHA = "c105f4a8cc878287d6b1aaf670d7e93aa4f9f506"; - public static final String GIT_DATE = "2026-03-16 02:02:34 EDT"; + public static final int GIT_REVISION = 122; + public static final String GIT_SHA = "b95611f8f93de0947c776a95989da048dd8c5d3e"; + public static final String GIT_DATE = "2026-03-20 05:21:34 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-20 03:39:14 EDT"; - public static final long BUILD_UNIX_TIME = 1773992354576L; + public static final String BUILD_DATE = "2026-03-20 15:40:56 EDT"; + public static final long BUILD_UNIX_TIME = 1774035656896L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5daf686..207b762 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,11 +64,6 @@ import frc.robot.subsystems.swerve.ModuleIOSim; import frc.robot.subsystems.swerve.ModuleIOSpark; import frc.robot.subsystems.swerve.SwerveSubsystem; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionConstants; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOLimelight; -import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -87,7 +82,7 @@ public class RobotContainer { private final KickerSubsystem kickerSubsystem; private final FlywheelSubsystem flywheelSubsystem; private final HoodSubsystem hoodSubsystem; - private final Vision vision; + // private final Vision vision; private final ShotCalculator shotCalculator; private Alliance lastAppliedAlliance = null; @@ -120,13 +115,15 @@ public RobotContainer() { new ModuleIOSpark(2), new ModuleIOSpark(3)); - vision = - new Vision( - swerveSubsystem::addVisionMeasurement, - swerveSubsystem::getRotation, - swerveSubsystem::getChassisSpeeds, - new VisionIOLimelight(VisionConstants.cameraYellow, swerveSubsystem::getRotation), - new VisionIOLimelight(VisionConstants.cameraPurple, swerveSubsystem::getRotation)); + // vision = + // new Vision( + // swerveSubsystem::addVisionMeasurement, + // swerveSubsystem::getRotation, + // swerveSubsystem::getChassisSpeeds, + // new VisionIOLimelight(VisionConstants.cameraYellow, + // swerveSubsystem::getRotation), + // new VisionIOLimelight(VisionConstants.cameraPurple, + // swerveSubsystem::getRotation)); // new VisionIOLimelight(VisionConstants.cameraPink, swerveSubsystem::getRotation)); // new VisionIOLimelight(VisionConstants.cameraOrange, swerveSubsystem::getRotation)); @@ -152,27 +149,27 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); - vision = - new Vision( - swerveSubsystem::addVisionMeasurement, - swerveSubsystem::getRotation, - swerveSubsystem::getChassisSpeeds, - new VisionIOPhotonVisionSim( - VisionConstants.cameraPurple, - VisionConstants.cameraTransformToPurple, - swerveSubsystem::getPose), - new VisionIOPhotonVisionSim( - VisionConstants.cameraOrange, - VisionConstants.cameraTransformToOrange, - swerveSubsystem::getPose), - new VisionIOPhotonVisionSim( - VisionConstants.cameraPlaceholder, - VisionConstants.cameraTransformToGreen, - swerveSubsystem::getPose), - new VisionIOPhotonVisionSim( - VisionConstants.cameraPlaceholder, - VisionConstants.cameraTransformToBlue, - swerveSubsystem::getPose)); + // vision = + // new Vision( + // swerveSubsystem::addVisionMeasurement, + // swerveSubsystem::getRotation, + // swerveSubsystem::getChassisSpeeds, + // new VisionIOPhotonVisionSim( + // VisionConstants.cameraPurple, + // VisionConstants.cameraTransformToPurple, + // swerveSubsystem::getPose), + // new VisionIOPhotonVisionSim( + // VisionConstants.cameraOrange, + // VisionConstants.cameraTransformToOrange, + // swerveSubsystem::getPose), + // new VisionIOPhotonVisionSim( + // VisionConstants.cameraPlaceholder, + // VisionConstants.cameraTransformToGreen, + // swerveSubsystem::getPose), + // new VisionIOPhotonVisionSim( + // VisionConstants.cameraPlaceholder, + // VisionConstants.cameraTransformToBlue, + // swerveSubsystem::getPose)); shotCalculator = new ShotCalculator(swerveSubsystem); @@ -197,13 +194,13 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - vision = - new Vision( - swerveSubsystem::addVisionMeasurement, - swerveSubsystem::getRotation, - swerveSubsystem::getChassisSpeeds, - new VisionIO() {}, - new VisionIO() {}); + // vision = + // new Vision( + // swerveSubsystem::addVisionMeasurement, + // swerveSubsystem::getRotation, + // swerveSubsystem::getChassisSpeeds, + // new VisionIO() {}, + // new VisionIO() {}); shotCalculator = new ShotCalculator(swerveSubsystem); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index db857c8..bfd52a5 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -4,21 +4,6 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.angularStdDevBaseline; -import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; -import static frc.robot.subsystems.vision.VisionConstants.cameraStdDevFactors; -import static frc.robot.subsystems.vision.VisionConstants.linearStdDevBaseline; -import static frc.robot.subsystems.vision.VisionConstants.maxZError; - -import java.util.ArrayList; -import java.util.HashMap; -import java.util.LinkedList; -import java.util.List; -import java.util.Map; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -28,11 +13,17 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import frc.robot.util.FullSubsystem; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; public class Vision extends FullSubsystem { /* @@ -41,10 +32,17 @@ public class Vision extends FullSubsystem { * 2. For each observation from each camera, run the rejection checks. * 3. For single-tag observations that pass intial, run quality gate check. * 4. For multi-tag observations, run the std. dev. euqation - * + * * Then, accumulate accepted observations into a list. * Sort by timestamps. * Submit all sorted observations to the pose estimator consumer. + * + * Memory Estimations (I am not a memory expert): + * 8 - Per-LL Oberservations: I am expecting maybe 1-2 frames per 50 hz, maybe 3-4. Double for safety. + * Java's backend defaults ArrayList internal array length to an Object sized at [10]. I don't think that + * matter too much but yeah. + * 16 - Per-LL Oberservation x Cameras (4 poseObs x 4 LL) + * Don't overcommit memory. */ private final VisionConsumer consumer; private final Supplier gyroRotationSupplier; @@ -71,7 +69,12 @@ public class Vision extends FullSubsystem { private volatile int exclusiveTagId = VisionConstants.NO_EXCLUSIVE_TAG; // Per-LL accepted and all pose lists. Cleared and reused each loop. + // JVM has type-erasure for casted ArrayLists + // Bad programming habits, fix later. + @SuppressWarnings("unchecked") private final ArrayList[] perCameraRobotPoses; + + @SuppressWarnings("unchecked") private final ArrayList[] perCameraRobotPosesAccepted; // Global summary lists. Cleared and reused each loop. @@ -79,24 +82,26 @@ public class Vision extends FullSubsystem { private final ArrayList allRobotPosesAccepted; // Output buffers - private Pose3d[] allRobotPosesBuffer = new Pose3d[0]; + private Pose3d[] allRobotPosesBuffer = new Pose3d[0]; private Pose3d[] allRobotPosesAcceptedBuffer = new Pose3d[0]; - private Pose3d[] perCameraBuffer = new Pose3d[8]; - private static final Pose3d[] EMPTY_POSE3D = new Pose3d[0]; - - private final ArrayList pendingObservations = new ArrayList<>(16); + private Pose3d[] perCameraBuffer = new Pose3d[PER_CAMERA_CAPACITY]; + private static final Pose3d[] EMPTY_POSE3D = new Pose3d[0]; private Rotation3d cachedGyroRotation3d = new Rotation3d(); private double lastGyroRadians = Double.NaN; // DO NOT USE 0.0 + private static final int PER_CAMERA_CAPACITY = 8; + private static final int PENDING_OBSERVATION_CAPACITY = 16; + + private final ArrayList pendingObservations = + new ArrayList<>(PENDING_OBSERVATION_CAPACITY); + /* * One fully-processed observation, send this boy */ - private record PendingObservation( - double timestamp, - Pose2d pose, - Matrix stdDevs) {} + private record PendingObservation(double timestamp, Pose2d pose, Matrix stdDevs) {} + // GG public Vision( VisionConsumer consumer, Supplier gyroRotationSupplier, @@ -109,139 +114,251 @@ public Vision( // Initialize inputs this.inputs = new VisionIOInputsAutoLogged[io.length]; + frameDisconnectTimers = new Timer[io.length]; + disconnectedAlerts = new Alert[io.length]; + cameraInputKeys = new String[io.length]; + cameraPosesAcceptedKeys = new String[io.length]; + cameraLogPrefixes = new String[io.length]; + perCameraRobotPoses = new ArrayList[io.length]; + perCameraRobotPosesAccepted = new ArrayList[io.length]; + for (int i = 0; i < inputs.length; i++) { inputs[i] = new VisionIOInputsAutoLogged(); - } - // Initialize disconnected alerts - this.disconnectedAlerts = new Alert[io.length]; - for (int i = 0; i < inputs.length; i++) { disconnectedAlerts[i] = new Alert( "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + cameraInputKeys[i] = "Vision/Camera" + i; + cameraPosesAcceptedKeys[i] = "Vision/Camera" + i + "/RobotPosesAccepted"; + cameraLogPrefixes[i] = "Vision/Camera" + i; + + // Initialize disconnected alerts + // I don't think this should be too memory intensive + frameDisconnectTimers[i] = new Timer(); + frameDisconnectTimers[i].start(); + disconnectedAlerts[i] = + new Alert("Vision camera " + i + " is disconnected.", AlertType.kWarning); + + // Pre-allocate per-camera lists with initial capacity. + // Size up bc we want to avoid re-allocation + perCameraRobotPoses[i] = new ArrayList<>(PER_CAMERA_CAPACITY); + perCameraRobotPosesAccepted[i] = new ArrayList<>(PER_CAMERA_CAPACITY); + } + + // Initial capacity for the camera summary lists + allRobotPoses = new ArrayList<>(io.length * PER_CAMERA_CAPACITY); + allRobotPosesAccepted = new ArrayList<>(io.length * PER_CAMERA_CAPACITY); + } + + // ------- TAG EXCLUSION UTIL -------- \\ + + public void setExclusiveTag(int tagId) { + this.exclusiveTagId = tagId; + } + + public void clearExclusiveTag() { + this.exclusiveTagId = VisionConstants.NO_EXCLUSIVE_TAG; + } + + public int getExclusiveTag() { + return exclusiveTagId; + } + + // ------- THROTTLING UTIL -------- \\ + // Stolen from 2910 + + public void setThrottleValue(int throttleValue) { + for (VisionIO camera : io) { + camera.setThrottleValue(throttleValue); } } @Override public void periodic() { - // Instead of creating new lists each loop, clear allocated lists to reuse memory and reduce GC - // overhead - // allTagPoses.clear(); - allRobotPoses.clear(); - allRobotPosesAccepted.clear(); - // allRobotPosesRejected.clear(); - for (int i = 0; i < io.length; i++) { io[i].updateInputs(inputs[i]); - Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + Logger.processInputs(cameraInputKeys[i], inputs[i]); } - // Update Robot State - Rotation3d currentGyroRotation = new Rotation3d(gyroRotationSupplier.get()); - ChassisSpeeds currentSpeeds = robotSpeedsSupplier.get(); + // Updated all 4 cameras then flush + NetworkTableInstance.getDefault().flush(); + + // Cache Gyro Rotation + double currentGyroRad = gyroRotationSupplier.get().getRadians(); + // The value of the lastGyroRadians intializes at NaN. + if (Double.isNaN(lastGyroRadians) || currentGyroRad != lastGyroRadians) { + cachedGyroRotation3d = new Rotation3d(gyroRotationSupplier.get()); + lastGyroRadians = currentGyroRad; + } + // Check if Robot is lightning McQueen + // I swear, do this calc once, not per camera. + ChassisSpeeds currentSpeeds = robotSpeedsSupplier.get(); // Calculate Speed Magnitudes double linearSpeed = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); double angularSpeed = Math.abs(currentSpeeds.omegaRadiansPerSecond); + // Clear buffers + allRobotPoses.clear(); + allRobotPosesAccepted.clear(); + pendingObservations.clear(); + // Loop over cameras for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { - // Update disconnected alert - disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); - - // Initialize logging values - // List tagPoses = new LinkedList<>(); - List robotPoses = new LinkedList<>(); - List robotPosesAccepted = new LinkedList<>(); - // List robotPosesRejected = new LinkedList<>(); - - // Add tag poses - // for (int tagId : inputs[cameraIndex].tagIds) { - // var tagPose = aprilTagLayout.getTagPose(tagId); - // if (tagPose.isPresent()) { - // tagPoses.add(tagPose.get()); - // } - // } - - // Loop over pose observations - for (var observation : inputs[cameraIndex].poseObservations) { - // Check whether to reject pose - boolean rejectPose = - observation.tagCount() == 0 // Must have at least one tag - || Math.abs(observation.pose().getZ()) - > maxZError // Must have realistic Z coordinate - - // Must be within the field boundaries - || observation.pose().getX() < 0.0 - || observation.pose().getX() > aprilTagLayout.getFieldLength() - || observation.pose().getY() < 0.0 - || observation.pose().getY() > aprilTagLayout.getFieldWidth(); - - if (!rejectPose) { - if (linearSpeed > VisionConstants.maxLinearSpeed - || angularSpeed > VisionConstants.maxAngularSpeed) { - rejectPose = true; + // Update dual disconnected alert + // Reset timer when new frame arrives + // NT check handled by [cameraIndex].connected + + // Stolen from 6328 + if (inputs[cameraIndex].lastFrameTimestampSec > 0 + && inputs[cameraIndex].poseObservations.length > 0) { + frameDisconnectTimers[cameraIndex].reset(); + } + + boolean frameDisconnected = + frameDisconnectTimers[cameraIndex].hasElapsed( + VisionConstants.frameDisconnectedTimeoutSec); + boolean disconnected = !inputs[cameraIndex].connected || frameDisconnected; + + if (disconnected) { + String reason = + !inputs[cameraIndex].connected + ? "NT disconnected" + : "No Frames Received for " + + VisionConstants.frameDisconnectedTimeoutSec + + " seconds"; + disconnectedAlerts[cameraIndex].setText( + "Vision camera " + cameraIndex + " disconnected (" + reason + ")."); + } + disconnectedAlerts[cameraIndex].set(disconnected); + // --- + + // TO-DO: Add util to update when DS is disabled + + // If exclyusive tags are included and not seen, skip all observations. + int exclusion = exclusiveTagId; + if (exclusion != VisionConstants.NO_EXCLUSIVE_TAG) { + boolean cameraSeesExclusive = false; + for (int id : inputs[cameraIndex].tagIds) { + if (id == exclusion) { + cameraSeesExclusive = true; + break; } } + if (!cameraSeesExclusive) { + // Log that we're filtering this camera, then skip. + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/ExclusiveTagFiltered", true); + continue; + } + } + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/ExclusiveTagFiltered", false); - if (!rejectPose) { - if (Math.abs( + // Clear Per-LL Pose lists + ArrayList robotPoses = perCameraRobotPoses[cameraIndex]; + ArrayList robotPosesAccepted = perCameraRobotPosesAccepted[cameraIndex]; + robotPoses.clear(); + robotPosesAccepted.clear(); + + // Update Per-Observation filtering + for (var observation : inputs[cameraIndex].poseObservations) { + + // See pipeline above + boolean reject = (observation.tagCount() == 0); + + if (!reject) reject = (Math.abs(observation.pose().getZ()) > VisionConstants.maxZError); + + // Margin added recommended by 6328 + if (!reject) { + double x = observation.pose().getX(); + double y = observation.pose().getY(); + reject = + x < -VisionConstants.fieldBorderMargin + || x + > VisionConstants.aprilTagLayout.getFieldLength() + + VisionConstants.fieldBorderMargin + || y < -VisionConstants.fieldBorderMargin + || y + > VisionConstants.aprilTagLayout.getFieldWidth() + + VisionConstants.fieldBorderMargin; + } + + // Technically this shouldnt really be needed + if (!reject) reject = (linearSpeed > VisionConstants.maxLinearSpeed); + + // Reject if yawRate is too high = timestamp might not be as reliable + if (!reject) reject = (angularSpeed > VisionConstants.maxAngularSpeed); + + // Check reprojection error for MT2 measurements + if (!reject) { + double gyroVisionDifferenceDeg = + Math.abs( Math.toDegrees( - (observation.pose().getRotation().minus(currentGyroRotation).getAngle()))) - > VisionConstants.maxGyroError) { - rejectPose = true; - } + observation.pose().getRotation().minus(cachedGyroRotation3d).getAngle())); + reject = (gyroVisionDifferenceDeg > VisionConstants.maxGyroError); } - // Add pose to log + // Log all pose observation robotPoses.add(observation.pose()); - if (rejectPose) { - // robotPosesRejected.add(observation.pose()); - } else { - robotPosesAccepted.add(observation.pose()); - } - // Skip if rejected - if (rejectPose) { + if (reject) { + // Observation logged but not accepted. continue; } - // Calculate dynamically standard deviation scalar factor - double stdDevFactor = - Math.pow(observation.averageTagDistance(), 2.0) - / observation.tagCount(); // Square tag count if vision is more trustworthy + // Single Tag and Multi-Tag need to be weighted differently + // Taken from 254 + final boolean isSingleTag = (observation.tagCount() == 1); + Pose2d acceptedPose2d; + double xyStdDev; + double thetaStdDev; + + if (isSingleTag) { + if (observation.averageTagArea() < VisionConstants.singleTagMinAreaPercent) { + // Rm if too laggy + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagArea", true); + continue; + } - double linearStdDev = linearStdDevBaseline * stdDevFactor; - double angularStdDev = angularStdDevBaseline * stdDevFactor; + if (angularSpeed > VisionConstants.singleTagMaxAngularVelocityRadPerSec) { + Logger.recordOutput( + cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagAngularVel", true); + continue; + } - if (observation.type() == PoseObservationType.MEGATAG_2) { - linearStdDev *= VisionConstants.linearStdDevMegatag2Factor; - angularStdDev *= VisionConstants.angularStdDevMegatag2Factor; - } + // If singleTag is accepted, use MT2 translation + acceptedPose2d = observation.pose().toPose2d(); + xyStdDev = calculateXYStdDev(observation, cameraIndex, inputs[cameraIndex]); + thetaStdDev = VisionConstants.singleTagThetaStdDev; + + } else { + acceptedPose2d = observation.pose().toPose2d(); + xyStdDev = calculateXYStdDev(observation, cameraIndex, inputs[cameraIndex]); + thetaStdDev = Double.POSITIVE_INFINITY; + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/AcceptedMultiTag", acceptedPose2d); + + // Utility for forcing AdvantageScope to keep Tag visible + for (int tagId : inputs[cameraIndex].tagIds) { + lastTagDetectionTimes.put(tagId, Timer.getTimestamp()); + } - if (cameraIndex < cameraStdDevFactors.length) { - linearStdDev *= cameraStdDevFactors[cameraIndex]; - angularStdDev *= cameraStdDevFactors[cameraIndex]; + // Aggregate accepted poses into a list + robotPosesAccepted.add(observation.pose()); + + // Sorting by timestamp is done at the end of the loop + pendingObservations.add( + new PendingObservation( + observation.timestamp(), + acceptedPose2d, + VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); } - // Send vision observation - consumer.accept( - observation.pose().toPose2d(), - observation.timestamp(), - VecBuilder.fill(linearStdDev, linearStdDev, 9999999999999999999.0)); + Logger.recordOutput(cameraPosesAcceptedKeys[cameraIndex], toArray(robotPosesAccepted)); + Logger.recordOutput( + cameraLogPrefixes[cameraIndex] + "/TagCount", inputs[cameraIndex].tagIds.length); + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/Connected", !disconnected); } - - // Log camera data (Disable in Match to save Bandwidth) - // Use Limelight Replay Feature Instead of Logging Poses to Dashboard - Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", - robotPosesAccepted.toArray(new Pose3d[0])); - // allTagPoses.addAll(tagPoses); - allRobotPoses.addAll(robotPoses); - allRobotPosesAccepted.addAll(robotPosesAccepted); - // allRobotPosesRejected.addAll(robotPosesRejected); } } @@ -253,6 +370,45 @@ public void accept( Matrix visionMeasurementStdDevs); } + private double calculateXYStdDev( + VisionIO.PoseObservation observation, + int cameraIndex, + VisionIOInputsAutoLogged cameraInputs) { + + // Original method + double modelStdDev = + VisionConstants.linearStdDevBaseline + * Math.pow(observation.averageTagDistance(), 2) // Modify to Tune + / Math.pow(observation.tagCount(), 1); // Modify to Tune + + // Per-camera trust multiplier + if (cameraIndex < VisionConstants.cameraStdDevFactors.length) { + modelStdDev *= VisionConstants.cameraStdDevFactors[cameraIndex]; + } + + // Quality scaling from Limelight's native stddevs (254) + // If Limelight provides MT2 std devs and they're larger, + // take a weighted blend in the next calculation. + + double[] llStdDevs = cameraInputs.limelightStdDevs; + if (llStdDevs.length >= VisionConstants.kExpectedStdDevLen + && observation.type() == PoseObservationType.MEGATAG_2) { + double llXStd = llStdDevs[VisionConstants.kMT2XStdDevIndex]; + double llYStd = llStdDevs[VisionConstants.kMT2YStdDevIndex]; + // Take the higher uncertainty + double llXYMax = Math.max(llXStd, llYStd); + + // Blend-- if Limelight is more pessimistic, weight its view more. + // Inverse Variance Weighting between two estimates + double blended = + modelStdDev * (1.0 - VisionConstants.limelightStdDevWeight) + + llXYMax * VisionConstants.limelightStdDevWeight; + modelStdDev = Math.max(modelStdDev, blended); // never trust more than our model alone + } + + return modelStdDev; + } + @Override public void periodicAfterScheduler() { // Log summary data @@ -263,4 +419,16 @@ public void periodicAfterScheduler() { // Logger.recordOutput( // "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(new Pose3d[0])); } + + // ------- UTIL -------- \\ + + // Because the buffer length grows, this util helps alleviate some of the array allocation + private Pose3d[] toArray(ArrayList list) { + int size = list.size(); + if (size == 0) return EMPTY_POSE3D; + if (perCameraBuffer.length < size) { + perCameraBuffer = new Pose3d[size * 2]; + } + return list.toArray(perCameraBuffer); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 1680669..84f846a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -18,19 +18,20 @@ private VisionConstants() {} // ------- APRILTAG FIELD CONSTANTS -------- \\ -public static final AprilTagFieldLayout aprilTagLayout; + public static final AprilTagFieldLayout aprilTagLayout; + static { - aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); - // All California District Events and DCMP should be WELDED fields. + aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + // All California District Events and DCMP should be WELDED fields. } // ------- LIMELIGHT NAME -------- \\ // The User MUST get this right - public static final String cameraPurple = "limelight-purple"; - public static final String cameraOrange = "limelight-orange"; - public static final String cameraYellow = "limelight-yellow"; - public static final String cameraPink = "limelight-pink"; + public static final String cameraPurple = "limelight-purple"; + public static final String cameraOrange = "limelight-orange"; + public static final String cameraYellow = "limelight-yellow"; + public static final String cameraPink = "limelight-pink"; // Unused public static String cameraPlaceholder = "limelight-placeholder"; @@ -46,7 +47,6 @@ private VisionConstants() {} public static Transform3d cameraTransformToBlue = new Transform3d(0.127, -0.3302, 0.18415, new Rotation3d(0.0, 0.38, -90.0)); // updated 1/22 - // ------- PER-LIMELIGHT TRUST MULTIPLIERS -------- \\ // Note: This is indexed in the same order that the cameras are passed into the Vision constructor // Increase the factor equals less trust in that limelight @@ -98,13 +98,13 @@ private VisionConstants() {} // ------- MISC / UTIL -------- \\ // Index constants for limelight NT entry for std. dev. - // Layout: [MT1x, MT1y, MT1z, MT1roll, MT1pitch, MT1yaw, MT2x, MT2y, MT2z, MT2roll, MT2pitch, MT2yaw] - IIRC - public static final int kMT2XStdDevIndex = 6; - public static final int kMT2YStdDevIndex = 7; + // Layout: [MT1x, MT1y, MT1z, MT1roll, MT1pitch, MT1yaw, MT2x, MT2y, MT2z, MT2roll, MT2pitch, + // MT2yaw] - IIRC + public static final int kMT2XStdDevIndex = 6; + public static final int kMT2YStdDevIndex = 7; public static final int kMT2YawStdDevIndex = 11; public static final int kExpectedStdDevLen = 12; - // Used for avoiding goofy floating point overflow public static final double LARGE_VARIANCE = 1e9; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 089636b..f71334f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -7,13 +7,6 @@ package frc.robot.subsystems.vision; -import java.util.ArrayList; -import java.util.HashSet; -import java.util.LinkedList; -import java.util.List; -import java.util.Set; -import java.util.function.Supplier; - import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -23,20 +16,22 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.function.Supplier; /** IO implementation for real Limelight hardware. */ public class VisionIOLimelight implements VisionIO { // Storing NT once avoids repeated static lookup private static final NetworkTableInstance NT = NetworkTableInstance.getDefault(); - + private final Supplier rotationSupplier; private final Supplier yawRateSupplier; // rad/s private final DoubleArrayPublisher orientationPublisher; - private final DoubleSubscriber latencySubscriber; // pipeline latency (ms) - private final DoubleArraySubscriber megatag2Subscriber; // botpose_orb_wpiblue + private final DoubleSubscriber latencySubscriber; // pipeline latency (ms) + private final DoubleArraySubscriber megatag2Subscriber; // botpose_orb_wpiblue private final DoubleArraySubscriber limelightStdDevsSubscriber; // native units by LL - private final DoubleSubscriber throttlePublisher; + private final DoubleSubscriber throttlePublisher; // Pre-allocate buffers private final ArrayList poseObservationCache = new ArrayList<>(8); @@ -44,7 +39,7 @@ public class VisionIOLimelight implements VisionIO { // Rm Hashset method for preventing tagId duplication // 32 tag is totally unrealistic but let's just not get a overflow error private static final int MAX_TAGS = 32; - private final int[] tagIdScratchLottery = new int[MAX_TAGS]; + private final int[] tagIdScratchLottery = new int[MAX_TAGS]; private int tagIdCount = 0; // Output array for inputs.poseObservations @@ -60,31 +55,27 @@ public class VisionIOLimelight implements VisionIO { * @param name The configured name of the Limelight. * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. */ - public VisionIOLimelight(String name, Supplier rotationSupplier, Supplier yawRateSupplier) { + public VisionIOLimelight( + String name, Supplier rotationSupplier, Supplier yawRateSupplier) { var table = NT.getTable(name); this.rotationSupplier = rotationSupplier; - this.yawRateSupplier = yawRateSupplier; - - orientationPublisher = - table.getDoubleArrayTopic("robot_orientation_set").publish(); - // [yaw,yawrate,pitch,pitchrate,roll,rollrate] - Degrees / Degrees per second - latencySubscriber = - table.getDoubleTopic("tl").subscribe(0.0); - // Select pipeline's latency in ms. Total Latency = tl + cl. + this.yawRateSupplier = yawRateSupplier; + + orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); + // [yaw,yawrate,pitch,pitchrate,roll,rollrate] - Degrees / Degrees per second + latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + // Select pipeline's latency in ms. Total Latency = tl + cl. megatag2Subscriber = - table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[]{}); - // Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + // Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees - limelightStdDevsSubscriber = - table.getDoubleArrayTopic("stddevs").subscribe(stdDevCache); + limelightStdDevsSubscriber = table.getDoubleArrayTopic("stddevs").subscribe(stdDevCache); // Not to sure if this is going to work // NT topics are write only so im goofing a DoubleSubscriber to publish changes - throttlePublisher = - table.getDoubleTopic("throttle_set").subscribe(0.0); - + throttlePublisher = table.getDoubleTopic("throttle_set").subscribe(0.0); } @Override @@ -106,8 +97,7 @@ public void updateInputs(VisionIOInputs inputs) { // Update orientation for MegaTag 2 double yawDeg = rotationSupplier.get().getDegrees(); double yawRateDegPerSec = Units.radiansToDegrees(yawRateSupplier.get()); - orientationPublisher.accept( - new double[] {yawDeg, yawRateDegPerSec, 0.0, 0.0, 0.0, 0.0}); + orientationPublisher.accept(new double[] {yawDeg, yawRateDegPerSec, 0.0, 0.0, 0.0, 0.0}); // ------- LL NATIVE STD DEV -------- \\ inputs.limelightStdDevs = limelightStdDevsSubscriber.get(stdDevCache); @@ -120,11 +110,11 @@ public void updateInputs(VisionIOInputs inputs) { tagIdCount = 0; // Process Buffer Frames - //readQueue() returns all frames captured since the last call (0 or multiple) + // readQueue() returns all frames captured since the last call (0 or multiple) for (var rawSample : megatag2Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; - // Use the NT server timestamp of this sample (multiply conversion for seconds) + // Use the NT server timestamp of this sample (multiply conversion for seconds) inputs.lastFrameTimestampSec = rawSample.timestamp * 1.0e-6; // I might go insane but here we go @@ -134,7 +124,10 @@ public void updateInputs(VisionIOInputs inputs) { int id = (int) rawSample.value[i]; boolean found = false; for (int j = 0; j < tagIdCount; j++) { - if (tagIdScratchLottery[j] == id) { found = true; break; } + if (tagIdScratchLottery[j] == id) { + found = true; + break; + } } if (!found && tagIdCount < MAX_TAGS) { tagIdScratchLottery[tagIdCount++] = id; @@ -157,9 +150,9 @@ public void updateInputs(VisionIOInputs inputs) { rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, parsePose(rawSample.value), 0.0, - (int) rawSample.value[7], // tagCount - rawSample.value[9], // averageTagDistance (meters) - rawSample.value[10] / 100.0, // averageTagArea: convert % to fraction + (int) rawSample.value[7], // tagCount + rawSample.value[9], // averageTagDistance (meters) + rawSample.value[10] / 100.0, // averageTagArea: convert % to fraction PoseObservationType.MEGATAG_2)); } @@ -167,7 +160,7 @@ public void updateInputs(VisionIOInputs inputs) { int observationSize = poseObservationCache.size(); if (poseObservationOutput.length < observationSize) { poseObservationOutput = new PoseObservation[observationSize * 2]; - // If not enough space, double that size. + // If not enough space, double that size. } // Note: toArray writes into the provided array and returns it @@ -179,7 +172,6 @@ public void updateInputs(VisionIOInputs inputs) { inputs.tagIds = new int[tagIdCount]; } System.arraycopy(tagIdScratchLottery, 0, inputs.tagIds, 0, tagIdCount); - } /** diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index ff750e2..ddb2a2a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -67,7 +67,8 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate multitagResult.estimatedPose.ambiguity, // Ambiguity multitagResult.fiducialIDsUsed.size(), // Tag count - totalTagDistance / result.targets.size(), 0.5, // Average tag distance + totalTagDistance / result.targets.size(), + 0.5, // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } else if (!result.targets.isEmpty()) { // Single tag result @@ -93,7 +94,8 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate target.poseAmbiguity, // Ambiguity 1, // Tag count - cameraToTarget.getTranslation().getNorm(), 0.5, // Average tag distance + cameraToTarget.getTranslation().getNorm(), + 0.5, // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } } From fea37d650e263679681249cabdcb49b43b86b864 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Fri, 20 Mar 2026 13:20:34 -0700 Subject: [PATCH 03/32] 3/20 - Quick Save Vision --- src/main/java/frc/robot/BuildConstants.java | 10 +-- .../frc/robot/subsystems/vision/Vision.java | 61 +++++++++++++++---- 2 files changed, 54 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8cdf94d..42a7047 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 122; - public static final String GIT_SHA = "b95611f8f93de0947c776a95989da048dd8c5d3e"; - public static final String GIT_DATE = "2026-03-20 05:21:34 EDT"; + public static final int GIT_REVISION = 123; + public static final String GIT_SHA = "aef1b33bd4059bf5390be019550538cd69f55795"; + public static final String GIT_DATE = "2026-03-20 15:43:38 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-20 15:40:56 EDT"; - public static final long BUILD_UNIX_TIME = 1774035656896L; + public static final String BUILD_DATE = "2026-03-20 16:20:23 EDT"; + public static final long BUILD_UNIX_TIME = 1774038023647L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index bfd52a5..471bee0 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -20,6 +20,7 @@ import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import frc.robot.util.FullSubsystem; import java.util.ArrayList; +import java.util.Comparator; import java.util.HashMap; import java.util.Map; import java.util.function.Supplier; @@ -44,6 +45,7 @@ public class Vision extends FullSubsystem { * 16 - Per-LL Oberservation x Cameras (4 poseObs x 4 LL) * Don't overcommit memory. */ + private final VisionConsumer consumer; private final Supplier gyroRotationSupplier; private final Supplier robotSpeedsSupplier; @@ -69,7 +71,7 @@ public class Vision extends FullSubsystem { private volatile int exclusiveTagId = VisionConstants.NO_EXCLUSIVE_TAG; // Per-LL accepted and all pose lists. Cleared and reused each loop. - // JVM has type-erasure for casted ArrayLists + // JVM has type-erasure for generic casting on ArrayLists // Bad programming habits, fix later. @SuppressWarnings("unchecked") private final ArrayList[] perCameraRobotPoses; @@ -102,6 +104,7 @@ public class Vision extends FullSubsystem { private record PendingObservation(double timestamp, Pose2d pose, Matrix stdDevs) {} // GG + @SuppressWarnings("unchecked") public Vision( VisionConsumer consumer, Supplier gyroRotationSupplier, @@ -123,11 +126,8 @@ public Vision( perCameraRobotPosesAccepted = new ArrayList[io.length]; for (int i = 0; i < inputs.length; i++) { - inputs[i] = new VisionIOInputsAutoLogged(); - disconnectedAlerts[i] = - new Alert( - "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + inputs[i] = new VisionIOInputsAutoLogged(); cameraInputKeys[i] = "Vision/Camera" + i; cameraPosesAcceptedKeys[i] = "Vision/Camera" + i + "/RobotPosesAccepted"; cameraLogPrefixes[i] = "Vision/Camera" + i; @@ -151,6 +151,7 @@ public Vision( } // ------- TAG EXCLUSION UTIL -------- \\ + // Based on 254's sorting public void setExclusiveTag(int tagId) { this.exclusiveTagId = tagId; @@ -165,7 +166,7 @@ public int getExclusiveTag() { } // ------- THROTTLING UTIL -------- \\ - // Stolen from 2910 + // Based on 2910's sorting public void setThrottleValue(int throttleValue) { for (VisionIO camera : io) { @@ -269,7 +270,7 @@ public void periodic() { if (!reject) reject = (Math.abs(observation.pose().getZ()) > VisionConstants.maxZError); - // Margin added recommended by 6328 + // Margin added, recommended by 6328 if (!reject) { double x = observation.pose().getX(); double y = observation.pose().getY(); @@ -358,6 +359,18 @@ public void periodic() { Logger.recordOutput( cameraLogPrefixes[cameraIndex] + "/TagCount", inputs[cameraIndex].tagIds.length); Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/Connected", !disconnected); + + allRobotPoses.addAll(robotPoses); + allRobotPosesAccepted.addAll(robotPosesAccepted); + } + + // Sort accumalated tag poses by timestamp + // Taken from 6328 + pendingObservations.sort(Comparator.comparingDouble(PendingObservation::timestamp)); + + // Submit ordered accpeted pose estimations to vision consumer + for (var observation : pendingObservations) { + consumer.accept(observation.pose(), observation.timestamp(), observation.stdDevs()); } } } @@ -412,12 +425,36 @@ private double calculateXYStdDev( @Override public void periodicAfterScheduler() { // Log summary data - // Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[0])); - Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[0])); + int allSize = allRobotPoses.size(); + int allAcceptedSize = allRobotPosesAccepted.size(); + + // Resizing if needed + if (allRobotPosesBuffer.length < allSize) { + allRobotPosesBuffer = new Pose3d[allSize * 2]; + } + if (allRobotPosesAcceptedBuffer.length < allAcceptedSize) { + allRobotPosesAcceptedBuffer = new Pose3d[allAcceptedSize * 2]; + } + + Logger.recordOutput( + "Vision/Summary/RobotPoses", + allSize == 0 ? EMPTY_POSE3D : allRobotPoses.toArray(allRobotPosesBuffer)); Logger.recordOutput( - "Vision/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(new Pose3d[0])); - // Logger.recordOutput( - // "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(new Pose3d[0])); + "Vision/Summary/RobotPosesAccepted", + allAcceptedSize == 0 + ? EMPTY_POSE3D + : allRobotPosesAccepted.toArray(allRobotPosesAcceptedBuffer)); + + // Taken from 6328 + ArrayList recentTagPoses = new ArrayList<>(); + double timestampNow = Timer.getTimestamp(); + for (var entry : lastTagDetectionTimes.entrySet()) { + if (timestampNow - entry.getValue() < VisionConstants.tagLogPersistenceSeconds) { + VisionConstants.aprilTagLayout.getTagPose(entry.getKey()).ifPresent(recentTagPoses::add); + } + } + // Log persistent tags + Logger.recordOutput("Vision/Summary/RecentTagPoses", recentTagPoses.toArray(EMPTY_POSE3D)); } // ------- UTIL -------- \\ From f26fc28fe47c09661306a8f8e602e0f24cd2d3f1 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Mon, 23 Mar 2026 00:20:06 -0700 Subject: [PATCH 04/32] 3/23 - Fix Throttling and Clarify Camera "For" Loops --- .../frc/robot/subsystems/vision/Vision.java | 35 +++++++++++++------ .../subsystems/vision/VisionIOLimelight.java | 5 --- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 471bee0..ec7555e 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -4,6 +4,14 @@ package frc.robot.subsystems.vision; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -19,12 +27,6 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import frc.robot.util.FullSubsystem; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Map; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; public class Vision extends FullSubsystem { /* @@ -46,6 +48,11 @@ public class Vision extends FullSubsystem { * Don't overcommit memory. */ + // Capacity Constants - See memory estimation notes in the class-level comment above + // Check these by logging .size() at peak usage and adjusting if resizes occur + private static final int PER_CAMERA_CAPACITY = 8; + private static final int PENDING_OBSERVATION_CAPACITY = 16; + private final VisionConsumer consumer; private final Supplier gyroRotationSupplier; private final Supplier robotSpeedsSupplier; @@ -92,9 +99,6 @@ public class Vision extends FullSubsystem { private Rotation3d cachedGyroRotation3d = new Rotation3d(); private double lastGyroRadians = Double.NaN; // DO NOT USE 0.0 - private static final int PER_CAMERA_CAPACITY = 8; - private static final int PENDING_OBSERVATION_CAPACITY = 16; - private final ArrayList pendingObservations = new ArrayList<>(PENDING_OBSERVATION_CAPACITY); @@ -328,15 +332,23 @@ public void periodic() { continue; } + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagArea", false); + // If singleTag is accepted, use MT2 translation + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagAngularVel", false); + // If singleTag is accepted, use MT2 translation acceptedPose2d = observation.pose().toPose2d(); xyStdDev = calculateXYStdDev(observation, cameraIndex, inputs[cameraIndex]); thetaStdDev = VisionConstants.singleTagThetaStdDev; + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/AcceptedSingleTag", acceptedPose2d); + } else { + acceptedPose2d = observation.pose().toPose2d(); xyStdDev = calculateXYStdDev(observation, cameraIndex, inputs[cameraIndex]); thetaStdDev = Double.POSITIVE_INFINITY; + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/AcceptedMultiTag", acceptedPose2d); // Utility for forcing AdvantageScope to keep Tag visible @@ -353,8 +365,9 @@ public void periodic() { observation.timestamp(), acceptedPose2d, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); - } + } // End of Pose Oberservation Loop + // Per-Camera Summary logging Logger.recordOutput(cameraPosesAcceptedKeys[cameraIndex], toArray(robotPosesAccepted)); Logger.recordOutput( cameraLogPrefixes[cameraIndex] + "/TagCount", inputs[cameraIndex].tagIds.length); @@ -362,7 +375,7 @@ public void periodic() { allRobotPoses.addAll(robotPoses); allRobotPosesAccepted.addAll(robotPosesAccepted); - } + } // End of Camera Loop // Sort accumalated tag poses by timestamp // Taken from 6328 diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index f71334f..d2ff2d5 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -31,7 +31,6 @@ public class VisionIOLimelight implements VisionIO { private final DoubleSubscriber latencySubscriber; // pipeline latency (ms) private final DoubleArraySubscriber megatag2Subscriber; // botpose_orb_wpiblue private final DoubleArraySubscriber limelightStdDevsSubscriber; // native units by LL - private final DoubleSubscriber throttlePublisher; // Pre-allocate buffers private final ArrayList poseObservationCache = new ArrayList<>(8); @@ -72,10 +71,6 @@ public VisionIOLimelight( // Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees limelightStdDevsSubscriber = table.getDoubleArrayTopic("stddevs").subscribe(stdDevCache); - - // Not to sure if this is going to work - // NT topics are write only so im goofing a DoubleSubscriber to publish changes - throttlePublisher = table.getDoubleTopic("throttle_set").subscribe(0.0); } @Override From ba3356cd0544b1b6401bd529d24eed067fa4a076 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Mon, 23 Mar 2026 00:39:08 -0700 Subject: [PATCH 05/32] 3/23 - Fix Single-Tag Omission --- src/main/java/frc/robot/BuildConstants.java | 10 +- .../frc/robot/subsystems/vision/Vision.java | 110 +++++++++--------- 2 files changed, 60 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 42a7047..a0dc085 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 123; - public static final String GIT_SHA = "aef1b33bd4059bf5390be019550538cd69f55795"; - public static final String GIT_DATE = "2026-03-20 15:43:38 EDT"; + public static final int GIT_REVISION = 125; + public static final String GIT_SHA = "f26fc28fe47c09661306a8f8e602e0f24cd2d3f1"; + public static final String GIT_DATE = "2026-03-23 03:20:06 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-20 16:20:23 EDT"; - public static final long BUILD_UNIX_TIME = 1774038023647L; + public static final String BUILD_DATE = "2026-03-23 03:38:48 EDT"; + public static final long BUILD_UNIX_TIME = 1774251528547L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index ec7555e..7c3eca1 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -4,14 +4,6 @@ package frc.robot.subsystems.vision; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.Map; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -27,25 +19,31 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import frc.robot.util.FullSubsystem; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; public class Vision extends FullSubsystem { /* * Pipeline: - * 1. Read all cameras. Send single NT flush after all cameras. - * 2. For each observation from each camera, run the rejection checks. - * 3. For single-tag observations that pass intial, run quality gate check. - * 4. For multi-tag observations, run the std. dev. euqation + * 1. Read all cameras. Send single NT flush after all cameras. + * 2. For each observation from each camera, run the rejection checks. + * 3. For single-tag observations that pass intial, run quality gate check. + * 4. For multi-tag observations, run the std. dev. euqation * - * Then, accumulate accepted observations into a list. - * Sort by timestamps. - * Submit all sorted observations to the pose estimator consumer. + * Then, accumulate accepted observations into a list. + * Sort by timestamps. + * Submit all sorted observations to the pose estimator consumer. * - * Memory Estimations (I am not a memory expert): - * 8 - Per-LL Oberservations: I am expecting maybe 1-2 frames per 50 hz, maybe 3-4. Double for safety. - * Java's backend defaults ArrayList internal array length to an Object sized at [10]. I don't think that - * matter too much but yeah. - * 16 - Per-LL Oberservation x Cameras (4 poseObs x 4 LL) - * Don't overcommit memory. + * Memory Estimations (I am not a memory expert): + * 8 - Per-LL Oberservations: I am expecting maybe 1-2 frames per 50 hz, maybe 3-4. Double for safety. + * Java's backend defaults ArrayList internal array length to an Object sized at [10]. I don't think that + * matter too much but yeah. + * 16 - Per-LL Oberservation x Cameras (4 poseObs x 4 LL) + * Don't overcommit memory. */ // Capacity Constants - See memory estimation notes in the class-level comment above @@ -334,14 +332,16 @@ public void periodic() { Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagArea", false); // If singleTag is accepted, use MT2 translation - Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagAngularVel", false); + Logger.recordOutput( + cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagAngularVel", false); // If singleTag is accepted, use MT2 translation acceptedPose2d = observation.pose().toPose2d(); xyStdDev = calculateXYStdDev(observation, cameraIndex, inputs[cameraIndex]); thetaStdDev = VisionConstants.singleTagThetaStdDev; - Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/AcceptedSingleTag", acceptedPose2d); + Logger.recordOutput( + cameraLogPrefixes[cameraIndex] + "/AcceptedSingleTag", acceptedPose2d); } else { @@ -350,41 +350,41 @@ public void periodic() { thetaStdDev = Double.POSITIVE_INFINITY; Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/AcceptedMultiTag", acceptedPose2d); + } - // Utility for forcing AdvantageScope to keep Tag visible - for (int tagId : inputs[cameraIndex].tagIds) { - lastTagDetectionTimes.put(tagId, Timer.getTimestamp()); - } + // Utility for forcing AdvantageScope to keep Tag visible + for (int tagId : inputs[cameraIndex].tagIds) { + lastTagDetectionTimes.put(tagId, Timer.getTimestamp()); + } + + // Aggregate accepted poses into a list + robotPosesAccepted.add(observation.pose()); - // Aggregate accepted poses into a list - robotPosesAccepted.add(observation.pose()); - - // Sorting by timestamp is done at the end of the loop - pendingObservations.add( - new PendingObservation( - observation.timestamp(), - acceptedPose2d, - VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); - } // End of Pose Oberservation Loop - - // Per-Camera Summary logging - Logger.recordOutput(cameraPosesAcceptedKeys[cameraIndex], toArray(robotPosesAccepted)); - Logger.recordOutput( - cameraLogPrefixes[cameraIndex] + "/TagCount", inputs[cameraIndex].tagIds.length); - Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/Connected", !disconnected); - - allRobotPoses.addAll(robotPoses); - allRobotPosesAccepted.addAll(robotPosesAccepted); - } // End of Camera Loop - - // Sort accumalated tag poses by timestamp - // Taken from 6328 - pendingObservations.sort(Comparator.comparingDouble(PendingObservation::timestamp)); - - // Submit ordered accpeted pose estimations to vision consumer - for (var observation : pendingObservations) { - consumer.accept(observation.pose(), observation.timestamp(), observation.stdDevs()); + // Sorting by timestamp is done at the end of the loop + pendingObservations.add( + new PendingObservation( + observation.timestamp(), + acceptedPose2d, + VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); } + + // Per-Camera Summary logging + Logger.recordOutput(cameraPosesAcceptedKeys[cameraIndex], toArray(robotPosesAccepted)); + Logger.recordOutput( + cameraLogPrefixes[cameraIndex] + "/TagCount", inputs[cameraIndex].tagIds.length); + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/Connected", !disconnected); + + allRobotPoses.addAll(robotPoses); + allRobotPosesAccepted.addAll(robotPosesAccepted); + } // End of Camera Loop + + // Sort accumalated tag poses by timestamp + // Taken from 6328 + pendingObservations.sort(Comparator.comparingDouble(PendingObservation::timestamp)); + + // Submit ordered accpeted pose estimations to vision consumer + for (var observation : pendingObservations) { + consumer.accept(observation.pose(), observation.timestamp(), observation.stdDevs()); } } From 5499e3b4bcb361ac72222cddb68aa0bb577a3382 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Wed, 25 Mar 2026 23:44:05 -0700 Subject: [PATCH 06/32] 3/25 - Fix toArray Helper Method causing NPE on descending fiducial observations due to pre-allocated buffers set last elements to null instead of clearing, Fix lastFrameTimestampSec being stuck on the last published timestamp instead of resetting causing a logic hard-lock. --- src/main/java/frc/robot/BuildConstants.java | 10 ++-- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 11 ++++ .../frc/robot/subsystems/vision/Vision.java | 52 ++++++------------- .../subsystems/vision/VisionConstants.java | 2 +- .../subsystems/vision/VisionIOLimelight.java | 12 +---- 6 files changed, 36 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index a0dc085..fdccd54 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 125; - public static final String GIT_SHA = "f26fc28fe47c09661306a8f8e602e0f24cd2d3f1"; - public static final String GIT_DATE = "2026-03-23 03:20:06 EDT"; + public static final int GIT_REVISION = 126; + public static final String GIT_SHA = "ba3356cd0544b1b6401bd529d24eed067fa4a076"; + public static final String GIT_DATE = "2026-03-23 03:39:08 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-23 03:38:48 EDT"; - public static final long BUILD_UNIX_TIME = 1774251528547L; + public static final String BUILD_DATE = "2026-03-26 02:41:37 EDT"; + public static final long BUILD_UNIX_TIME = 1774507297587L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cc038e5..c889f22 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -189,6 +189,7 @@ public void teleopInit() { // this line or comment it out. if (autonomousCommand != null) { autonomousCommand.cancel(); + autonomousCommand = null; } // Handle edge case where alliance data isn't recieved before enabled diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 207b762..73a3a02 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -423,6 +423,17 @@ public Command getAutonomousCommand() { return autoChooser.get(); } + /** + * See: https://www.chiefdelphi.com/t/fatal-robot-code-crash/427074/26 + * + *

Use this to clear the autonomous command connected to main {@link Robot} class. + * + * @return + */ + // public void killAutonomousBuilder(){ + // autoChooser. + // } + // Only use for Driver Reset Button Binding public Rotation2d returnGlobalSwerveOffset() { if (!DriverStation.getAlliance().isPresent()) { diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 7c3eca1..cd43ec8 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -61,6 +61,7 @@ public class Vision extends FullSubsystem { // Frame-based check: no frames received for frameDisconnectedTimeoutSec. private final Timer[] frameDisconnectTimers; private final Alert[] disconnectedAlerts; + private final double[] lastFrameTimestamps; // Tracks previous timestamps to detect new timestamps // Logging key strings private final String[] cameraInputKeys; @@ -78,6 +79,7 @@ public class Vision extends FullSubsystem { // Per-LL accepted and all pose lists. Cleared and reused each loop. // JVM has type-erasure for generic casting on ArrayLists // Bad programming habits, fix later. + // Can be fixed by List> @SuppressWarnings("unchecked") private final ArrayList[] perCameraRobotPoses; @@ -88,12 +90,6 @@ public class Vision extends FullSubsystem { private final ArrayList allRobotPoses; private final ArrayList allRobotPosesAccepted; - // Output buffers - private Pose3d[] allRobotPosesBuffer = new Pose3d[0]; - private Pose3d[] allRobotPosesAcceptedBuffer = new Pose3d[0]; - private Pose3d[] perCameraBuffer = new Pose3d[PER_CAMERA_CAPACITY]; - private static final Pose3d[] EMPTY_POSE3D = new Pose3d[0]; - private Rotation3d cachedGyroRotation3d = new Rotation3d(); private double lastGyroRadians = Double.NaN; // DO NOT USE 0.0 @@ -121,6 +117,7 @@ public Vision( this.inputs = new VisionIOInputsAutoLogged[io.length]; frameDisconnectTimers = new Timer[io.length]; disconnectedAlerts = new Alert[io.length]; + lastFrameTimestamps = new double[io.length]; cameraInputKeys = new String[io.length]; cameraPosesAcceptedKeys = new String[io.length]; cameraLogPrefixes = new String[io.length]; @@ -138,6 +135,7 @@ public Vision( // I don't think this should be too memory intensive frameDisconnectTimers[i] = new Timer(); frameDisconnectTimers[i].start(); + lastFrameTimestamps[i] = 0.0; disconnectedAlerts[i] = new Alert("Vision camera " + i + " is disconnected.", AlertType.kWarning); @@ -184,7 +182,7 @@ public void periodic() { Logger.processInputs(cameraInputKeys[i], inputs[i]); } - // Updated all 4 cameras then flush + // Updated all cameras then flush NetworkTableInstance.getDefault().flush(); // Cache Gyro Rotation @@ -215,9 +213,11 @@ public void periodic() { // NT check handled by [cameraIndex].connected // Stolen from 6328 - if (inputs[cameraIndex].lastFrameTimestampSec > 0 - && inputs[cameraIndex].poseObservations.length > 0) { + if (inputs[cameraIndex].lastFrameTimestampSec != lastFrameTimestamps[cameraIndex]) { frameDisconnectTimers[cameraIndex].reset(); + lastFrameTimestamps[cameraIndex] = inputs[cameraIndex].lastFrameTimestampSec; + // Only reset the disconnect timer if the current timestamp is different from the last + // recorded timestamp } boolean frameDisconnected = @@ -293,7 +293,7 @@ public void periodic() { // Reject if yawRate is too high = timestamp might not be as reliable if (!reject) reject = (angularSpeed > VisionConstants.maxAngularSpeed); - // Check reprojection error for MT2 measurements + // MT2 measurements should already be pre-seeded, disable if redundant if (!reject) { double gyroVisionDifferenceDeg = Math.abs( @@ -438,25 +438,10 @@ private double calculateXYStdDev( @Override public void periodicAfterScheduler() { // Log summary data - int allSize = allRobotPoses.size(); - int allAcceptedSize = allRobotPosesAccepted.size(); - - // Resizing if needed - if (allRobotPosesBuffer.length < allSize) { - allRobotPosesBuffer = new Pose3d[allSize * 2]; - } - if (allRobotPosesAcceptedBuffer.length < allAcceptedSize) { - allRobotPosesAcceptedBuffer = new Pose3d[allAcceptedSize * 2]; - } + Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/Summary/RobotPoses", - allSize == 0 ? EMPTY_POSE3D : allRobotPoses.toArray(allRobotPosesBuffer)); - Logger.recordOutput( - "Vision/Summary/RobotPosesAccepted", - allAcceptedSize == 0 - ? EMPTY_POSE3D - : allRobotPosesAccepted.toArray(allRobotPosesAcceptedBuffer)); + "Vision/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(new Pose3d[0])); // Taken from 6328 ArrayList recentTagPoses = new ArrayList<>(); @@ -467,18 +452,15 @@ public void periodicAfterScheduler() { } } // Log persistent tags - Logger.recordOutput("Vision/Summary/RecentTagPoses", recentTagPoses.toArray(EMPTY_POSE3D)); + Logger.recordOutput("Vision/Summary/RecentTagPoses", recentTagPoses.toArray(new Pose3d[0])); } // ------- UTIL -------- \\ - // Because the buffer length grows, this util helps alleviate some of the array allocation + // Refactored toArray Helper method after removing output buffer logic + // See + // https://stackoverflow.com/questions/70154349/why-java-inserts-null-to-array-when-using-list-toarray private Pose3d[] toArray(ArrayList list) { - int size = list.size(); - if (size == 0) return EMPTY_POSE3D; - if (perCameraBuffer.length < size) { - perCameraBuffer = new Pose3d[size * 2]; - } - return list.toArray(perCameraBuffer); + return list.toArray(new Pose3d[0]); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 84f846a..ffa81c6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -76,7 +76,7 @@ private VisionConstants() {} public static final double maxAngularSpeed = Math.toRadians(720); // rad/s (~2 full rotations) - public static final double maxGyroError = 1.0; // Degrees + public static final double maxGyroError = 5.0; // Degrees // Only for when only one tag is present public static final double singleTagMinAreaPercent = 0.10; // Percent of Image frame diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index d2ff2d5..c3c6a5b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -41,10 +41,6 @@ public class VisionIOLimelight implements VisionIO { private final int[] tagIdScratchLottery = new int[MAX_TAGS]; private int tagIdCount = 0; - // Output array for inputs.poseObservations - // Rm the PoseObservation[n] memory allocation required - private PoseObservation[] poseObservationOutput = new PoseObservation[8]; - // Default a blank array to store the std dev in case it doesn't work private static final double[] stdDevCache = new double[0]; @@ -152,14 +148,8 @@ public void updateInputs(VisionIOInputs inputs) { } // Write Pose Observations - int observationSize = poseObservationCache.size(); - if (poseObservationOutput.length < observationSize) { - poseObservationOutput = new PoseObservation[observationSize * 2]; - // If not enough space, double that size. - } - // Note: toArray writes into the provided array and returns it - inputs.poseObservations = poseObservationCache.toArray(poseObservationOutput); + inputs.poseObservations = poseObservationCache.toArray(new PoseObservation[0]); // Write Tag ID's // Only reallocate inputs.tagIds if the count changed From 8f2c2e35c1713ff148b1d736f447b7f11c93c296 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Thu, 26 Mar 2026 01:02:04 -0700 Subject: [PATCH 07/32] 3/26 - Rm Field Constants to prevent AprilTagFieldLayout JSON fetching as a possible source of lag --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/FieldConstants.java | 744 ++++++++++---------- 2 files changed, 382 insertions(+), 372 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index fdccd54..c925c77 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 126; - public static final String GIT_SHA = "ba3356cd0544b1b6401bd529d24eed067fa4a076"; - public static final String GIT_DATE = "2026-03-23 03:39:08 EDT"; + public static final int GIT_REVISION = 127; + public static final String GIT_SHA = "5499e3b4bcb361ac72222cddb68aa0bb577a3382"; + public static final String GIT_DATE = "2026-03-26 02:44:05 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-26 02:41:37 EDT"; - public static final long BUILD_UNIX_TIME = 1774507297587L; + public static final String BUILD_DATE = "2026-03-26 04:00:05 EDT"; + public static final long BUILD_UNIX_TIME = 1774512005813L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index e6c670e..86f98b8 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,367 +1,377 @@ -// Copyright (c) 2025-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot; - -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.IOException; -import java.nio.file.Path; -import lombok.Getter; -import lombok.RequiredArgsConstructor; - -/** - * Contains information for location of field element and other useful reference points. - * - *

NOTE: All constants are defined relative to the field coordinate system, and from the - * perspective of the blue alliance station - */ -public class FieldConstants { - public static final FieldType fieldType = FieldType.WELDED; - - // AprilTag related constants - public static final int aprilTagCount = AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); - public static final double aprilTagWidth = Units.inchesToMeters(6.5); - public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; - - // Field dimensions - public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); - public static final double fieldWidth = AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); - - /** - * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) - */ - public static class LinesVertical { - public static final double center = fieldLength / 2.0; - public static final double starting = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX(); - public static final double allianceZone = starting; - public static final double hubCenter = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; - public static final double neutralZoneNear = center - Units.inchesToMeters(120); - public static final double neutralZoneFar = center + Units.inchesToMeters(120); - public static final double oppHubCenter = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; - public static final double oppAllianceZone = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(10).get().getX(); - } - - /** - * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) - * - *

NOTE: The field element start and end are always left to right from the perspective of the - * alliance station - */ - public static class LinesHorizontal { - - public static final double center = fieldWidth / 2.0; - - // Right of hub - public static final double rightBumpStart = Hub.nearRightCorner.getY(); - public static final double rightBumpEnd = rightBumpStart - RightBump.width; - public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); - public static final double rightTrenchOpenEnd = 0; - - // Left of hub - public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); - public static final double leftBumpStart = leftBumpEnd + LeftBump.width; - public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); - public static final double leftTrenchOpenStart = fieldWidth; - } - - /** Hub related constants */ - public static class Hub { - - // Dimensions - public static final double width = Units.inchesToMeters(47.0); - public static final double height = - Units.inchesToMeters(72.0); // includes the catcher at the top - public static final double innerWidth = Units.inchesToMeters(41.7); - public static final double innerHeight = Units.inchesToMeters(56.5); - - // Relevant reference points on alliance side - public static final Translation3d topCenterPoint = - new Translation3d( - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, - fieldWidth / 2.0, - height); - public static final Translation3d innerCenterPoint = - new Translation3d( - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, - fieldWidth / 2.0, - innerHeight); - - public static final Translation2d nearLeftCorner = - new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d nearRightCorner = - new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); - public static final Translation2d farLeftCorner = - new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d farRightCorner = - new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); - - // Relevant reference points on the opposite side - public static final Translation3d oppTopCenterPoint = - new Translation3d( - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + width / 2.0, - fieldWidth / 2.0, - height); - public static final Translation2d oppNearLeftCorner = - new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d oppNearRightCorner = - new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); - public static final Translation2d oppFarLeftCorner = - new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d oppFarRightCorner = - new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); - - // Hub faces - public static final Pose2d nearFace = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().toPose2d(); - public static final Pose2d farFace = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(20).get().toPose2d(); - public static final Pose2d rightFace = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(18).get().toPose2d(); - public static final Pose2d leftFace = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d(); - } - - /** Left Bump related constants */ - public static class LeftBump { - - // Dimensions - public static final double width = Units.inchesToMeters(73.0); - public static final double height = Units.inchesToMeters(6.513); - public static final double depth = Units.inchesToMeters(44.4); - - // Relevant reference points on alliance side - public static final Translation2d nearLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); - public static final Translation2d nearRightCorner = Hub.nearLeftCorner; - public static final Translation2d farLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); - public static final Translation2d farRightCorner = Hub.farLeftCorner; - - // Relevant reference points on opposing side - public static final Translation2d oppNearLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); - public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; - public static final Translation2d oppFarLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); - public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; - } - - /** Right Bump related constants */ - public static class RightBump { - // Dimensions - public static final double width = Units.inchesToMeters(73.0); - public static final double height = Units.inchesToMeters(6.513); - public static final double depth = Units.inchesToMeters(44.4); - - // Relevant reference points on alliance side - public static final Translation2d nearLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); - public static final Translation2d nearRightCorner = Hub.nearLeftCorner; - public static final Translation2d farLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); - public static final Translation2d farRightCorner = Hub.farLeftCorner; - - // Relevant reference points on opposing side - public static final Translation2d oppNearLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); - public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; - public static final Translation2d oppFarLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); - public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; - } - - /** Left Trench related constants */ - public static class LeftTrench { - // Dimensions - public static final double width = Units.inchesToMeters(65.65); - public static final double depth = Units.inchesToMeters(47.0); - public static final double height = Units.inchesToMeters(40.25); - public static final double openingWidth = Units.inchesToMeters(50.34); - public static final double openingHeight = Units.inchesToMeters(22.25); - - // Relevant reference points on alliance side - public static final Translation3d openingTopLeft = - new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); - public static final Translation3d openingTopRight = - new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); - - // Relevant reference points on opposing side - public static final Translation3d oppOpeningTopLeft = - new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); - public static final Translation3d oppOpeningTopRight = - new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); - } - - public static class RightTrench { - - // Dimensions - public static final double width = Units.inchesToMeters(65.65); - public static final double depth = Units.inchesToMeters(47.0); - public static final double height = Units.inchesToMeters(40.25); - public static final double openingWidth = Units.inchesToMeters(50.34); - public static final double openingHeight = Units.inchesToMeters(22.25); - - // Relevant reference points on alliance side - public static final Translation3d openingTopLeft = - new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); - public static final Translation3d openingTopRight = - new Translation3d(LinesVertical.hubCenter, 0, openingHeight); - - // Relevant reference points on opposing side - public static final Translation3d oppOpeningTopLeft = - new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); - public static final Translation3d oppOpeningTopRight = - new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); - } - - /** Tower related constants */ - public static class Tower { - // Dimensions - public static final double width = Units.inchesToMeters(49.25); - public static final double depth = Units.inchesToMeters(45.0); - public static final double height = Units.inchesToMeters(78.25); - public static final double innerOpeningWidth = Units.inchesToMeters(32.250); - public static final double frontFaceX = Units.inchesToMeters(43.51); - - public static final double uprightHeight = Units.inchesToMeters(72.1); - - // Rung heights from the floor - public static final double lowRungHeight = Units.inchesToMeters(27.0); - public static final double midRungHeight = Units.inchesToMeters(45.0); - public static final double highRungHeight = Units.inchesToMeters(63.0); - - // Relevant reference points on alliance side - public static final Translation2d centerPoint = - new Translation2d( - frontFaceX, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()); - public static final Translation2d leftUpright = - new Translation2d( - frontFaceX, - (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) - + innerOpeningWidth / 2 - + Units.inchesToMeters(0.75)); - public static final Translation2d rightUpright = - new Translation2d( - frontFaceX, - (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) - - innerOpeningWidth / 2 - - Units.inchesToMeters(0.75)); - - // Relevant reference points on opposing side - public static final Translation2d oppCenterPoint = - new Translation2d( - fieldLength - frontFaceX, - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()); - public static final Translation2d oppLeftUpright = - new Translation2d( - fieldLength - frontFaceX, - (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) - + innerOpeningWidth / 2 - + Units.inchesToMeters(0.75)); - public static final Translation2d oppRightUpright = - new Translation2d( - fieldLength - frontFaceX, - (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) - - innerOpeningWidth / 2 - - Units.inchesToMeters(0.75)); - } - - public static class Depot { - // Dimensions - public static final double width = Units.inchesToMeters(42.0); - public static final double depth = Units.inchesToMeters(27.0); - public static final double height = Units.inchesToMeters(1.125); - public static final double distanceFromCenterY = Units.inchesToMeters(75.93); - - // Relevant reference points on alliance side - public static final Translation3d depotCenter = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); - public static final Translation3d leftCorner = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); - public static final Translation3d rightCorner = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); - } - - public static class Outpost { - // Dimensions - public static final double width = Units.inchesToMeters(31.8); - public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); - public static final double height = Units.inchesToMeters(7.0); - - // Relevant reference points on alliance side - public static final Translation2d centerPoint = - new Translation2d(0, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); - } - - @RequiredArgsConstructor - public enum FieldType { - ANDYMARK("andymark"), - WELDED("welded"); - - @Getter private final String jsonFolder; - } - - public enum AprilTagLayoutType { - OFFICIAL("2026-official"), - NONE("2026-none"); - - private final String name; - private volatile AprilTagFieldLayout layout; - private volatile String layoutString; - - AprilTagLayoutType(String name) { - this.name = name; - } - - public AprilTagFieldLayout getLayout() { - if (layout == null) { - synchronized (this) { - if (layout == null) { - try { - Path p = - Constants.disableHAL - ? Path.of( - "src", - "main", - "deploy", - "apriltags", - fieldType.getJsonFolder(), - name + ".json") - : Path.of( - Filesystem.getDeployDirectory().getPath(), - "apriltags", - fieldType.getJsonFolder(), - name + ".json"); - layout = new AprilTagFieldLayout(p); - layoutString = new ObjectMapper().writeValueAsString(layout); - } catch (IOException e) { - throw new RuntimeException(e); - } - } - } - } - return layout; - } - - public String getLayoutString() { - if (layoutString == null) { - getLayout(); - } - return layoutString; - } - } -} +// // Copyright (c) 2025-2026 Littleton Robotics +// // http://github.com/Mechanical-Advantage +// // +// // Use of this source code is governed by an MIT-style +// // license that can be found in the LICENSE file at +// // the root directory of this project. + +// package frc.robot; + +// import com.fasterxml.jackson.databind.ObjectMapper; +// import edu.wpi.first.apriltag.AprilTagFieldLayout; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.geometry.Translation3d; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.Filesystem; +// import java.io.IOException; +// import java.nio.file.Path; +// import lombok.Getter; +// import lombok.RequiredArgsConstructor; + +// /** +// * Contains information for location of field element and other useful reference points. +// * +// *

NOTE: All constants are defined relative to the field coordinate system, and from the +// * perspective of the blue alliance station +// */ +// public class FieldConstants { +// public static final FieldType fieldType = FieldType.WELDED; + +// // AprilTag related constants +// public static final int aprilTagCount = +// AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); +// public static final double aprilTagWidth = Units.inchesToMeters(6.5); +// public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + +// // Field dimensions +// public static final double fieldLength = +// AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); +// public static final double fieldWidth = +// AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); + +// /** +// * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) +// */ +// public static class LinesVertical { +// public static final double center = fieldLength / 2.0; +// public static final double starting = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX(); +// public static final double allianceZone = starting; +// public static final double hubCenter = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; +// public static final double neutralZoneNear = center - Units.inchesToMeters(120); +// public static final double neutralZoneFar = center + Units.inchesToMeters(120); +// public static final double oppHubCenter = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; +// public static final double oppAllianceZone = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(10).get().getX(); +// } + +// /** +// * Officially defined and relevant horizontal lines found on the field (defined by Y-axis +// offset) +// * +// *

NOTE: The field element start and end are always left to right from the perspective of +// the +// * alliance station +// */ +// public static class LinesHorizontal { + +// public static final double center = fieldWidth / 2.0; + +// // Right of hub +// public static final double rightBumpStart = Hub.nearRightCorner.getY(); +// public static final double rightBumpEnd = rightBumpStart - RightBump.width; +// public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); +// public static final double rightTrenchOpenEnd = 0; + +// // Left of hub +// public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); +// public static final double leftBumpStart = leftBumpEnd + LeftBump.width; +// public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); +// public static final double leftTrenchOpenStart = fieldWidth; +// } + +// /** Hub related constants */ +// public static class Hub { + +// // Dimensions +// public static final double width = Units.inchesToMeters(47.0); +// public static final double height = +// Units.inchesToMeters(72.0); // includes the catcher at the top +// public static final double innerWidth = Units.inchesToMeters(41.7); +// public static final double innerHeight = Units.inchesToMeters(56.5); + +// // Relevant reference points on alliance side +// public static final Translation3d topCenterPoint = +// new Translation3d( +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, +// fieldWidth / 2.0, +// height); +// public static final Translation3d innerCenterPoint = +// new Translation3d( +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, +// fieldWidth / 2.0, +// innerHeight); + +// public static final Translation2d nearLeftCorner = +// new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); +// public static final Translation2d nearRightCorner = +// new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); +// public static final Translation2d farLeftCorner = +// new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); +// public static final Translation2d farRightCorner = +// new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + +// // Relevant reference points on the opposite side +// public static final Translation3d oppTopCenterPoint = +// new Translation3d( +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + width / 2.0, +// fieldWidth / 2.0, +// height); +// public static final Translation2d oppNearLeftCorner = +// new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / +// 2.0); +// public static final Translation2d oppNearRightCorner = +// new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / +// 2.0); +// public static final Translation2d oppFarLeftCorner = +// new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / +// 2.0); +// public static final Translation2d oppFarRightCorner = +// new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / +// 2.0); + +// // Hub faces +// public static final Pose2d nearFace = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().toPose2d(); +// public static final Pose2d farFace = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(20).get().toPose2d(); +// public static final Pose2d rightFace = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(18).get().toPose2d(); +// public static final Pose2d leftFace = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d(); +// } + +// /** Left Bump related constants */ +// public static class LeftBump { + +// // Dimensions +// public static final double width = Units.inchesToMeters(73.0); +// public static final double height = Units.inchesToMeters(6.513); +// public static final double depth = Units.inchesToMeters(44.4); + +// // Relevant reference points on alliance side +// public static final Translation2d nearLeftCorner = +// new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); +// public static final Translation2d nearRightCorner = Hub.nearLeftCorner; +// public static final Translation2d farLeftCorner = +// new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); +// public static final Translation2d farRightCorner = Hub.farLeftCorner; + +// // Relevant reference points on opposing side +// public static final Translation2d oppNearLeftCorner = +// new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); +// public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; +// public static final Translation2d oppFarLeftCorner = +// new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); +// public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; +// } + +// /** Right Bump related constants */ +// public static class RightBump { +// // Dimensions +// public static final double width = Units.inchesToMeters(73.0); +// public static final double height = Units.inchesToMeters(6.513); +// public static final double depth = Units.inchesToMeters(44.4); + +// // Relevant reference points on alliance side +// public static final Translation2d nearLeftCorner = +// new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); +// public static final Translation2d nearRightCorner = Hub.nearLeftCorner; +// public static final Translation2d farLeftCorner = +// new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); +// public static final Translation2d farRightCorner = Hub.farLeftCorner; + +// // Relevant reference points on opposing side +// public static final Translation2d oppNearLeftCorner = +// new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); +// public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; +// public static final Translation2d oppFarLeftCorner = +// new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); +// public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; +// } + +// /** Left Trench related constants */ +// public static class LeftTrench { +// // Dimensions +// public static final double width = Units.inchesToMeters(65.65); +// public static final double depth = Units.inchesToMeters(47.0); +// public static final double height = Units.inchesToMeters(40.25); +// public static final double openingWidth = Units.inchesToMeters(50.34); +// public static final double openingHeight = Units.inchesToMeters(22.25); + +// // Relevant reference points on alliance side +// public static final Translation3d openingTopLeft = +// new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); +// public static final Translation3d openingTopRight = +// new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + +// // Relevant reference points on opposing side +// public static final Translation3d oppOpeningTopLeft = +// new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); +// public static final Translation3d oppOpeningTopRight = +// new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); +// } + +// public static class RightTrench { + +// // Dimensions +// public static final double width = Units.inchesToMeters(65.65); +// public static final double depth = Units.inchesToMeters(47.0); +// public static final double height = Units.inchesToMeters(40.25); +// public static final double openingWidth = Units.inchesToMeters(50.34); +// public static final double openingHeight = Units.inchesToMeters(22.25); + +// // Relevant reference points on alliance side +// public static final Translation3d openingTopLeft = +// new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); +// public static final Translation3d openingTopRight = +// new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + +// // Relevant reference points on opposing side +// public static final Translation3d oppOpeningTopLeft = +// new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); +// public static final Translation3d oppOpeningTopRight = +// new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); +// } + +// /** Tower related constants */ +// public static class Tower { +// // Dimensions +// public static final double width = Units.inchesToMeters(49.25); +// public static final double depth = Units.inchesToMeters(45.0); +// public static final double height = Units.inchesToMeters(78.25); +// public static final double innerOpeningWidth = Units.inchesToMeters(32.250); +// public static final double frontFaceX = Units.inchesToMeters(43.51); + +// public static final double uprightHeight = Units.inchesToMeters(72.1); + +// // Rung heights from the floor +// public static final double lowRungHeight = Units.inchesToMeters(27.0); +// public static final double midRungHeight = Units.inchesToMeters(45.0); +// public static final double highRungHeight = Units.inchesToMeters(63.0); + +// // Relevant reference points on alliance side +// public static final Translation2d centerPoint = +// new Translation2d( +// frontFaceX, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()); +// public static final Translation2d leftUpright = +// new Translation2d( +// frontFaceX, +// (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) +// + innerOpeningWidth / 2 +// + Units.inchesToMeters(0.75)); +// public static final Translation2d rightUpright = +// new Translation2d( +// frontFaceX, +// (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) +// - innerOpeningWidth / 2 +// - Units.inchesToMeters(0.75)); + +// // Relevant reference points on opposing side +// public static final Translation2d oppCenterPoint = +// new Translation2d( +// fieldLength - frontFaceX, +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()); +// public static final Translation2d oppLeftUpright = +// new Translation2d( +// fieldLength - frontFaceX, +// (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) +// + innerOpeningWidth / 2 +// + Units.inchesToMeters(0.75)); +// public static final Translation2d oppRightUpright = +// new Translation2d( +// fieldLength - frontFaceX, +// (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) +// - innerOpeningWidth / 2 +// - Units.inchesToMeters(0.75)); +// } + +// public static class Depot { +// // Dimensions +// public static final double width = Units.inchesToMeters(42.0); +// public static final double depth = Units.inchesToMeters(27.0); +// public static final double height = Units.inchesToMeters(1.125); +// public static final double distanceFromCenterY = Units.inchesToMeters(75.93); + +// // Relevant reference points on alliance side +// public static final Translation3d depotCenter = +// new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); +// public static final Translation3d leftCorner = +// new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); +// public static final Translation3d rightCorner = +// new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); +// } + +// public static class Outpost { +// // Dimensions +// public static final double width = Units.inchesToMeters(31.8); +// public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); +// public static final double height = Units.inchesToMeters(7.0); + +// // Relevant reference points on alliance side +// public static final Translation2d centerPoint = +// new Translation2d(0, +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); +// } + +// @RequiredArgsConstructor +// public enum FieldType { +// ANDYMARK("andymark"), +// WELDED("welded"); + +// @Getter private final String jsonFolder; +// } + +// public enum AprilTagLayoutType { +// OFFICIAL("2026-official"), +// NONE("2026-none"); + +// private final String name; +// private volatile AprilTagFieldLayout layout; +// private volatile String layoutString; + +// AprilTagLayoutType(String name) { +// this.name = name; +// } + +// public AprilTagFieldLayout getLayout() { +// if (layout == null) { +// synchronized (this) { +// if (layout == null) { +// try { +// Path p = +// Constants.disableHAL +// ? Path.of( +// "src", +// "main", +// "deploy", +// "apriltags", +// fieldType.getJsonFolder(), +// name + ".json") +// : Path.of( +// Filesystem.getDeployDirectory().getPath(), +// "apriltags", +// fieldType.getJsonFolder(), +// name + ".json"); +// layout = new AprilTagFieldLayout(p); +// layoutString = new ObjectMapper().writeValueAsString(layout); +// } catch (IOException e) { +// throw new RuntimeException(e); +// } +// } +// } +// } +// return layout; +// } + +// public String getLayoutString() { +// if (layoutString == null) { +// getLayout(); +// } +// return layoutString; +// } +// } +// } From 9d254147039f4de0cf4558d3bd905a0e249e3753 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Thu, 26 Mar 2026 11:10:43 -0700 Subject: [PATCH 08/32] 3/26 - Autoscheduled Forced Warmup command in Disabled to avoid issues in warming up the PathPlanner library when AutoInit enables. --- .../pathplanner/autos/Force Warmup.auto | 19 +++++++ .../pathplanner/paths/Force Warmup.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/BuildConstants.java | 10 ++-- src/main/java/frc/robot/Robot.java | 13 ++++- 5 files changed, 93 insertions(+), 7 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Force Warmup.auto create mode 100644 src/main/deploy/pathplanner/paths/Force Warmup.path diff --git a/src/main/deploy/pathplanner/autos/Force Warmup.auto b/src/main/deploy/pathplanner/autos/Force Warmup.auto new file mode 100644 index 0000000..a15c30b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Force Warmup.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Force Warmup" + } + } + ] + } + }, + "resetOdom": false, + "folder": "Force Warmup", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Force Warmup.path b/src/main/deploy/pathplanner/paths/Force Warmup.path new file mode 100644 index 0000000..600e861 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Force Warmup.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3144935805991436, + "y": 7.0 + }, + "prevControl": { + "x": 2.3144935805991436, + "y": 7.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": "Force Warmup", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 2c87ad1..9f3c0c5 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,14 +4,16 @@ "holonomicMode": true, "pathFolders": [ "CLB Citrus Auto", + "CRB Citrus Auto", "Default NZ Auto", "Depot Auto", - "CRB Citrus Auto", + "Force Warmup", "Testing" ], "autoFolders": [ "Citrus Auto", "Default NZ Auto", + "Force Warmup", "TestingSpartan" ], "defaultMaxVel": 3.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c925c77..59d410b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 127; - public static final String GIT_SHA = "5499e3b4bcb361ac72222cddb68aa0bb577a3382"; - public static final String GIT_DATE = "2026-03-26 02:44:05 EDT"; + public static final int GIT_REVISION = 128; + public static final String GIT_SHA = "8f2c2e35c1713ff148b1d736f447b7f11c93c296"; + public static final String GIT_DATE = "2026-03-26 04:02:04 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-26 04:00:05 EDT"; - public static final long BUILD_UNIX_TIME = 1774512005813L; + public static final String BUILD_DATE = "2026-03-26 14:08:38 EDT"; + public static final long BUILD_UNIX_TIME = 1774548518066L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c889f22..a9e6ca8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,10 +13,12 @@ package frc.robot; +import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.Mode; import frc.robot.subsystems.vision.VisionConstants; import frc.robot.util.FullSubsystem; @@ -148,6 +150,12 @@ public void disabledInit() { } else { hasInitializedAlliancePose = false; } + + // Handle PathPlanner States Initialization by Running an Auto Routine + // Should not actually drive, only warmups up the PathPlanner library + // See: https://www.chiefdelphi.com/t/pathplanner-initial-lag-after-deploying-code/455068/7 + Command forcedAutoInitCommand = new PathPlannerAuto("Force Warmup").ignoringDisable(true); + CommandScheduler.getInstance().schedule(forcedAutoInitCommand); } /** This function is called periodically when disabled. */ @@ -170,9 +178,12 @@ public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); + // 3847: Time Delay to prevent Auto from firing with a delay + Command delaggerWaitCommand = new WaitCommand(0.01); + // schedule the autonomous command (example) if (autonomousCommand != null) { - CommandScheduler.getInstance().schedule(autonomousCommand); + CommandScheduler.getInstance().schedule(delaggerWaitCommand.andThen(autonomousCommand)); } } From b4bf31de528e680f3ab60cd95bc06db26c2efbd3 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Thu, 26 Mar 2026 16:39:37 -0700 Subject: [PATCH 09/32] 3/26 - Working State with Intaking and Vision --- src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/RobotContainer.java | 87 ++++++++++--------- .../intake/pivot/PivotConstants.java | 2 +- .../subsystems/swerve/SwerveSubsystem.java | 8 ++ 4 files changed, 61 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 59d410b..d64cd04 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 128; - public static final String GIT_SHA = "8f2c2e35c1713ff148b1d736f447b7f11c93c296"; - public static final String GIT_DATE = "2026-03-26 04:02:04 EDT"; + public static final int GIT_REVISION = 129; + public static final String GIT_SHA = "9d254147039f4de0cf4558d3bd905a0e249e3753"; + public static final String GIT_DATE = "2026-03-26 14:10:43 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-26 14:08:38 EDT"; - public static final long BUILD_UNIX_TIME = 1774548518066L; + public static final String BUILD_DATE = "2026-03-26 19:37:57 EDT"; + public static final long BUILD_UNIX_TIME = 1774568277920L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73a3a02..2d2e283 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,6 +64,11 @@ import frc.robot.subsystems.swerve.ModuleIOSim; import frc.robot.subsystems.swerve.ModuleIOSpark; import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionConstants; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIOLimelight; +import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -82,7 +87,7 @@ public class RobotContainer { private final KickerSubsystem kickerSubsystem; private final FlywheelSubsystem flywheelSubsystem; private final HoodSubsystem hoodSubsystem; - // private final Vision vision; + private final Vision vision; private final ShotCalculator shotCalculator; private Alliance lastAppliedAlliance = null; @@ -115,17 +120,19 @@ public RobotContainer() { new ModuleIOSpark(2), new ModuleIOSpark(3)); - // vision = - // new Vision( - // swerveSubsystem::addVisionMeasurement, - // swerveSubsystem::getRotation, - // swerveSubsystem::getChassisSpeeds, - // new VisionIOLimelight(VisionConstants.cameraYellow, - // swerveSubsystem::getRotation), - // new VisionIOLimelight(VisionConstants.cameraPurple, - // swerveSubsystem::getRotation)); - // new VisionIOLimelight(VisionConstants.cameraPink, swerveSubsystem::getRotation)); - // new VisionIOLimelight(VisionConstants.cameraOrange, swerveSubsystem::getRotation)); + vision = + new Vision( + swerveSubsystem::addVisionMeasurement, + swerveSubsystem::getRotation, + swerveSubsystem::getChassisSpeeds, + new VisionIOLimelight( + VisionConstants.cameraYellow, + swerveSubsystem::getRotation, + swerveSubsystem::getYawVelocityRate), + new VisionIOLimelight( + VisionConstants.cameraPink, + swerveSubsystem::getRotation, + swerveSubsystem::getYawVelocityRate)); shotCalculator = new ShotCalculator(swerveSubsystem); @@ -149,27 +156,27 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); - // vision = - // new Vision( - // swerveSubsystem::addVisionMeasurement, - // swerveSubsystem::getRotation, - // swerveSubsystem::getChassisSpeeds, - // new VisionIOPhotonVisionSim( - // VisionConstants.cameraPurple, - // VisionConstants.cameraTransformToPurple, - // swerveSubsystem::getPose), - // new VisionIOPhotonVisionSim( - // VisionConstants.cameraOrange, - // VisionConstants.cameraTransformToOrange, - // swerveSubsystem::getPose), - // new VisionIOPhotonVisionSim( - // VisionConstants.cameraPlaceholder, - // VisionConstants.cameraTransformToGreen, - // swerveSubsystem::getPose), - // new VisionIOPhotonVisionSim( - // VisionConstants.cameraPlaceholder, - // VisionConstants.cameraTransformToBlue, - // swerveSubsystem::getPose)); + vision = + new Vision( + swerveSubsystem::addVisionMeasurement, + swerveSubsystem::getRotation, + swerveSubsystem::getChassisSpeeds, + new VisionIOPhotonVisionSim( + VisionConstants.cameraPurple, + VisionConstants.cameraTransformToPurple, + swerveSubsystem::getPose), + new VisionIOPhotonVisionSim( + VisionConstants.cameraOrange, + VisionConstants.cameraTransformToOrange, + swerveSubsystem::getPose), + new VisionIOPhotonVisionSim( + VisionConstants.cameraPlaceholder, + VisionConstants.cameraTransformToGreen, + swerveSubsystem::getPose), + new VisionIOPhotonVisionSim( + VisionConstants.cameraPlaceholder, + VisionConstants.cameraTransformToBlue, + swerveSubsystem::getPose)); shotCalculator = new ShotCalculator(swerveSubsystem); @@ -194,13 +201,13 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - // vision = - // new Vision( - // swerveSubsystem::addVisionMeasurement, - // swerveSubsystem::getRotation, - // swerveSubsystem::getChassisSpeeds, - // new VisionIO() {}, - // new VisionIO() {}); + vision = + new Vision( + swerveSubsystem::addVisionMeasurement, + swerveSubsystem::getRotation, + swerveSubsystem::getChassisSpeeds, + new VisionIO() {}, + new VisionIO() {}); shotCalculator = new ShotCalculator(swerveSubsystem); diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java index 16e96c4..ba1d4b4 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java @@ -24,7 +24,7 @@ public class PivotConstants { // Temporary Values (Confirm Functionality) public static final double stowAngle = 0.0; - public static final double deployedAngle = -2.198803; // //-2.23 + public static final double deployedAngle = -2.06; // Not full stow prototype public static final double halfDeployedAngle = -2.198803 / 2; // //-2.23 public static final double jugglingAngle = Units.degreesToRadians(0.0); public static final double debuggingAngle = Units.degreesToRadians(0.0); // Change as needed diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 98b9003..d2e50da 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -71,6 +71,7 @@ public class SwerveSubsystem extends FullSubsystem { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); private Rotation2d rawGyroRotation = new Rotation2d(); + private double rawGyroYawRate = 0.0; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { new SwerveModulePosition(), @@ -208,10 +209,12 @@ public void periodic() { if (gyroInputs.connected) { // Use the real gyro angle rawGyroRotation = gyroInputs.odometryYawPositions[i]; + rawGyroYawRate = gyroInputs.yawVelocityRadPerSec; } else { // Use the angle delta from the kinematics and module deltas Twist2d twist = kinematics.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + rawGyroYawRate = 0.0; } // Apply update @@ -347,6 +350,11 @@ public Rotation2d getRotation() { return getPose().getRotation(); } + /** Returns the current yaw velocity rate */ + public double getYawVelocityRate() { + return rawGyroYawRate; + } + /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); From dd6a8bff3ce58acd0cf742f2c8506b4a943282f6 Mon Sep 17 00:00:00 2001 From: Theo-253 Date: Fri, 27 Mar 2026 15:54:09 -0700 Subject: [PATCH 10/32] 3/27 - Updated pivotCurrentLimit to 70 --- .../java/frc/robot/subsystems/intake/pivot/PivotConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java index ba1d4b4..b7dcf1f 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java @@ -4,7 +4,7 @@ public class PivotConstants { public static final int sparkMasterPivotCanId = 9; - public static final int pivotCurrentLimit = 50; // amps + public static final int pivotCurrentLimit = 70; // amps public static final double pivotkP = 0.8; // big distance, big speed; small distance, small speed // start small 0.001, then step up slowly From aa1feb136ff6fc710a89bac1e700091f00e4317e Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sat, 28 Mar 2026 11:14:15 -0700 Subject: [PATCH 11/32] 3/28 - Fix Swapped Pivot Current Limit Constants --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../robot/subsystems/intake/pivot/PivotConstants.java | 2 +- .../subsystems/intake/roller/RollerConstants.java | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index d64cd04..cb624aa 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 129; - public static final String GIT_SHA = "9d254147039f4de0cf4558d3bd905a0e249e3753"; - public static final String GIT_DATE = "2026-03-26 14:10:43 EDT"; + public static final int GIT_REVISION = 131; + public static final String GIT_SHA = "dd6a8bff3ce58acd0cf742f2c8506b4a943282f6"; + public static final String GIT_DATE = "2026-03-27 18:54:09 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-26 19:37:57 EDT"; - public static final long BUILD_UNIX_TIME = 1774568277920L; + public static final String BUILD_DATE = "2026-03-28 14:08:38 EDT"; + public static final long BUILD_UNIX_TIME = 1774721318410L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java index b7dcf1f..ba1d4b4 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java @@ -4,7 +4,7 @@ public class PivotConstants { public static final int sparkMasterPivotCanId = 9; - public static final int pivotCurrentLimit = 70; // amps + public static final int pivotCurrentLimit = 50; // amps public static final double pivotkP = 0.8; // big distance, big speed; small distance, small speed // start small 0.001, then step up slowly diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java index a5480d9..3cd387b 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java @@ -2,7 +2,7 @@ public class RollerConstants { public static final int sparkMasterRollerCanId = 10; - public static final int masterCurrentLimit = 45; // amps + public static final int masterCurrentLimit = 70; // amps // Temporary Values (Confirm Functionality) public static final double stowVolts = 0.0; From a1e6ddbec86fddf93a820cbbb2f9895b59331194 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sat, 28 Mar 2026 20:02:02 -0700 Subject: [PATCH 12/32] 3/28 - Quick Save --- .../pathplanner/autos/CLB_Citrus_V2.auto | 204 ++++++++++++++++++ .../deploy/pathplanner/autos/JustAim.auto | 25 +++ .../deploy/pathplanner/autos/JustKick.auto | 56 +++++ .../deploy/pathplanner/paths/CLB_Entry.path | 2 +- .../deploy/pathplanner/paths/CLB_Intake.path | 4 +- .../pathplanner/paths/CLB_Intake_V2.path | 105 +++++++++ .../pathplanner/paths/CLB_StageShoot1_V2.path | 59 +++++ .../pathplanner/paths/CLB_TrenchShoot.path | 2 +- .../deploy/pathplanner/paths/CLR_Intake.path | 2 +- .../pathplanner/paths/Copy of CLB_Intake.path | 2 +- .../deploy/pathplanner/paths/New Path.path | 70 ++++++ src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../subsystems/indexer/IndexerConstants.java | 6 +- .../subsystems/indexer/IndexerIOSpark.java | 1 + .../intake/pivot/PivotConstants.java | 4 +- .../intake/pivot/PivotSubsystem.java | 2 +- .../subsystems/swerve/SwerveConstants.java | 8 +- .../subsystems/vision/VisionIOLimelight.java | 2 +- 20 files changed, 546 insertions(+), 24 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/CLB_Citrus_V2.auto create mode 100644 src/main/deploy/pathplanner/autos/JustAim.auto create mode 100644 src/main/deploy/pathplanner/autos/JustKick.auto create mode 100644 src/main/deploy/pathplanner/paths/CLB_Intake_V2.path create mode 100644 src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/autos/CLB_Citrus_V2.auto b/src/main/deploy/pathplanner/autos/CLB_Citrus_V2.auto new file mode 100644 index 0000000..0f98a38 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CLB_Citrus_V2.auto @@ -0,0 +1,204 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleWarm" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CLB_Entry" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CLB_Intake_V2" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CLB_StageShoot1_V2" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + }, + { + "type": "named", + "data": { + "name": "flywheelLayup" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "Citrus Auto", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/JustAim.auto b/src/main/deploy/pathplanner/autos/JustAim.auto new file mode 100644 index 0000000..1cc468e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/JustAim.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + } + ] + } + }, + "resetOdom": false, + "folder": "TestingSpartan", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/JustKick.auto b/src/main/deploy/pathplanner/autos/JustKick.auto new file mode 100644 index 0000000..043497e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/JustKick.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + }, + { + "type": "named", + "data": { + "name": "flywheelLayup" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "TestingSpartan", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CLB_Entry.path b/src/main/deploy/pathplanner/paths/CLB_Entry.path index 952286d..3d59e6f 100644 --- a/src/main/deploy/pathplanner/paths/CLB_Entry.path +++ b/src/main/deploy/pathplanner/paths/CLB_Entry.path @@ -62,7 +62,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 2.0, + "velocity": 0.5, "rotation": -107.64200000000001 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/CLB_Intake.path b/src/main/deploy/pathplanner/paths/CLB_Intake.path index 6867b3b..1d1efb8 100644 --- a/src/main/deploy/pathplanner/paths/CLB_Intake.path +++ b/src/main/deploy/pathplanner/paths/CLB_Intake.path @@ -53,8 +53,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.811910669975181, - "maxWaypointRelativePos": 0.3513647642680001, + "minWaypointRelativePos": 1.1505739365293643, + "maxWaypointRelativePos": 0.2808912896691613, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path b/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path new file mode 100644 index 0000000..f2551e0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.13, + "y": 7.017 + }, + "prevControl": null, + "nextControl": { + "x": 8.126000184885955, + "y": 6.575320550590469 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.13, + "y": 5.054753442150735 + }, + "prevControl": { + "x": 8.157996058825054, + "y": 5.354131000583097 + }, + "nextControl": { + "x": 8.09464321482716, + "y": 4.676663508663737 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.846747503566333, + "y": 5.607 + }, + "prevControl": { + "x": 7.198343383255684, + "y": 5.160743691163518 + }, + "nextControl": { + "x": 6.510342368045648, + "y": 6.0339757489301 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.342, + "y": 7.37 + }, + "prevControl": { + "x": 6.445509272467903, + "y": 6.771526390870186 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.854771784232354, + "rotationDegrees": -110.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.0933002481389686, + "maxWaypointRelativePos": 0.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "CLB Citrus Auto", + "idealStartingState": { + "velocity": 0.5, + "rotation": -107.64200000000001 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path b/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path new file mode 100644 index 0000000..d00878d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.342, + "y": 7.37 + }, + "prevControl": null, + "nextControl": { + "x": 5.099155940801128, + "y": 7.402306101453735 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.1800082843706226, + "y": 7.37 + }, + "prevControl": { + "x": 3.5462722834950475, + "y": 7.420690225765222 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9609958506224051, + "rotationDegrees": 180.0 + } + ], + "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": 179.51226418354415 + }, + "reversed": false, + "folder": "CLB Citrus Auto", + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CLB_TrenchShoot.path b/src/main/deploy/pathplanner/paths/CLB_TrenchShoot.path index ee4c07b..483775b 100644 --- a/src/main/deploy/pathplanner/paths/CLB_TrenchShoot.path +++ b/src/main/deploy/pathplanner/paths/CLB_TrenchShoot.path @@ -61,7 +61,7 @@ "rotation": -29.999999999999996 }, "reversed": false, - "folder": "Josh", + "folder": "Misc", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/CLR_Intake.path b/src/main/deploy/pathplanner/paths/CLR_Intake.path index 6dbda4a..7cc8d8f 100644 --- a/src/main/deploy/pathplanner/paths/CLR_Intake.path +++ b/src/main/deploy/pathplanner/paths/CLR_Intake.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 8.01254981548159, - "y": 2.7933775898403677 + "y": 2.793377589840365 }, "nextControl": { "x": 7.820769955384298, diff --git a/src/main/deploy/pathplanner/paths/Copy of CLB_Intake.path b/src/main/deploy/pathplanner/paths/Copy of CLB_Intake.path index eaaab47..5ac52fb 100644 --- a/src/main/deploy/pathplanner/paths/Copy of CLB_Intake.path +++ b/src/main/deploy/pathplanner/paths/Copy of CLB_Intake.path @@ -112,7 +112,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Josh", + "folder": "Misc", "idealStartingState": { "velocity": 2.0, "rotation": -107.64200000000001 diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..8c958de --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.820870185449357, + "y": 1.2079029957203988 + }, + "prevControl": null, + "nextControl": { + "x": 6.251569186875892, + "y": 0.6515406562054202 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.979857346647646, + "y": 0.6774179743223976 + }, + "prevControl": { + "x": 6.808001727659159, + "y": 1.1486035704151556 + }, + "nextControl": { + "x": 5.229415121255349, + "y": 0.2504422253922971 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4050641940085593, + "y": 0.5350927246790309 + }, + "prevControl": { + "x": 4.039058487874465, + "y": 0.6386019971469341 + }, + "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": 90.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": 88.52172734331111 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 9f3c0c5..d8c2457 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,6 +8,8 @@ "Default NZ Auto", "Depot Auto", "Force Warmup", + "Misc", + "[Left] Double Swipe", "Testing" ], "autoFolders": [ @@ -28,7 +30,7 @@ "driveGearing": 4.71428571429, "maxDriveSpeed": 5.74, "driveMotorType": "vortex", - "driveCurrentLimit": 50.0, + "driveCurrentLimit": 80.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index cb624aa..6fde866 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 131; - public static final String GIT_SHA = "dd6a8bff3ce58acd0cf742f2c8506b4a943282f6"; - public static final String GIT_DATE = "2026-03-27 18:54:09 EDT"; + public static final int GIT_REVISION = 132; + public static final String GIT_SHA = "aa1feb136ff6fc710a89bac1e700091f00e4317e"; + public static final String GIT_DATE = "2026-03-28 14:14:15 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-28 14:08:38 EDT"; - public static final long BUILD_UNIX_TIME = 1774721318410L; + public static final String BUILD_DATE = "2026-03-28 23:01:26 EDT"; + public static final long BUILD_UNIX_TIME = 1774753286069L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2d2e283..14e4e63 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -323,7 +323,7 @@ private void configureButtonBindings() { driver .leftBumper() - .whileTrue(indexerSubsystem.runCurrentCommand()) + .whileTrue(indexerSubsystem.indexCommand()) .whileTrue(kickerSubsystem.indexCommand()) .whileTrue(agitatorSubsystem.indexCommand()) .whileTrue(pivotSubsystem.runSaltAndPepperCommand()); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 0c0efb5..c31a0bc 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -2,15 +2,15 @@ public class IndexerConstants { public static final int sparkMasterIndexerCanId = 12; - public static final int masterCurrentLimit = 50; // amps + public static final int masterCurrentLimit = 65; // amps // Temporary Values (Confirm Functionality) public static final double stowVolts = 0.0; - public static final double indexingVolts = -4; + public static final double indexingVolts = -12.0; public static final double jugglingVolts = 0.0; public static final double debuggingVolts = 0.0; - public static final double debuggingCurrent = 40; + public static final double debuggingCurrent = 60; // Used as an early warning if the Indexer is jammed public static final double highCurrentThreshold = 50; diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSpark.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSpark.java index 8181c28..ca3a6c2 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSpark.java @@ -36,6 +36,7 @@ public IndexerIOSpark() { masterSparkMaxConfig .idleMode(IdleMode.kCoast) .smartCurrentLimit(IndexerConstants.masterCurrentLimit) + .voltageCompensation(12.0) .signals .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java index ba1d4b4..f3fe969 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java @@ -24,8 +24,8 @@ public class PivotConstants { // Temporary Values (Confirm Functionality) public static final double stowAngle = 0.0; - public static final double deployedAngle = -2.06; // Not full stow prototype - public static final double halfDeployedAngle = -2.198803 / 2; // //-2.23 + public static final double deployedAngle = -2.19; // Not full stow prototype + public static final double halfDeployedAngle = deployedAngle * 0.25; // public static final double jugglingAngle = Units.degreesToRadians(0.0); public static final double debuggingAngle = Units.degreesToRadians(0.0); // Change as needed diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java index 1e15420..51d23e1 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -127,7 +127,7 @@ public Command stowCommand() { } public Command runSaltAndPepperCommand() { - return Commands.sequence(deployCommand().withTimeout(0.3), halfDeployCommand().withTimeout(0.3)) + return Commands.sequence(deployCommand().withTimeout(0.5), halfDeployCommand().withTimeout(0.5)) .repeatedly(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index c3ffae1..e29c27d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -69,7 +69,7 @@ public class SwerveConstants { public static final int backRightTurnCanId = 6; // Drive motor configuration (MAXSwerve with 14 pinion teeth and 22 spur teeth) - public static final int driveMotorCurrentLimit = 50; + public static final int driveMotorCurrentLimit = 80; // Vortex 80A, NEO 60A // Remeasure the radius in the Makerspace, Run the Wheel Characterization Auto public static final double wheelRadiusMeters = Units.inchesToMeters(1.498); @@ -97,7 +97,7 @@ public class SwerveConstants { // Turn motor configuration public static final boolean turnInverted = false; // <-- Check if Neccessary - public static final int turnMotorCurrentLimit = 20; + public static final int turnMotorCurrentLimit = 35; // 550 35A, SAFE = 20A public static final double turnMotorReduction = 9424.0 / 203.0; public static final DCMotor turnGearbox = DCMotor.getNeo550(1); @@ -115,8 +115,8 @@ public class SwerveConstants { public static final double turnPIDMaxInput = 2 * Math.PI; // Radians // PathPlanner configuration (Get Values) - public static final double robotMassKg = 74.088; - public static final double robotMOI = 6.883; + public static final double robotMassKg = 61.088; + public static final double robotMOI = 6.583; public static final double wheelCOF = 1.2; public static final RobotConfig ppConfig = new RobotConfig( diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index c3c6a5b..a784486 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -143,7 +143,7 @@ public void updateInputs(VisionIOInputs inputs) { 0.0, (int) rawSample.value[7], // tagCount rawSample.value[9], // averageTagDistance (meters) - rawSample.value[10] / 100.0, // averageTagArea: convert % to fraction + rawSample.value[10], // averageTagArea: convert % to fraction PoseObservationType.MEGATAG_2)); } From 75252cdba08d72fefb14f40a22aa30a46ecb1689 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sat, 28 Mar 2026 20:59:28 -0700 Subject: [PATCH 13/32] 3/28 - Heuristic Shot Data for Dynamic Shooting --- .../pathplanner/autos/JustConstraint.auto | 19 ++++++ .../pathplanner/paths/CLB_Intake_V2.path | 4 +- .../pathplanner/paths/JustConstraint.path | 68 +++++++++++++++++++ src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/RobotContainer.java | 5 +- .../shooter/flywheel/FlywheelConstants.java | 4 +- .../shooter/hood/HoodConstants.java | 2 +- .../shooterUtil/ShootOnTheFlyConstants.java | 47 +++++++------ 8 files changed, 128 insertions(+), 31 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/JustConstraint.auto create mode 100644 src/main/deploy/pathplanner/paths/JustConstraint.path diff --git a/src/main/deploy/pathplanner/autos/JustConstraint.auto b/src/main/deploy/pathplanner/autos/JustConstraint.auto new file mode 100644 index 0000000..7473c45 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/JustConstraint.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "JustConstraint" + } + } + ] + } + }, + "resetOdom": false, + "folder": "TestingSpartan", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path b/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path index f2551e0..a567993 100644 --- a/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path +++ b/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path @@ -69,8 +69,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 2.0933002481389686, - "maxWaypointRelativePos": 0.0, + "minWaypointRelativePos": 0.3305210918114133, + "maxWaypointRelativePos": 1.3548387096774213, "constraints": { "maxVelocity": 0.5, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/JustConstraint.path b/src/main/deploy/pathplanner/paths/JustConstraint.path new file mode 100644 index 0000000..114277e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/JustConstraint.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.959202247191011, + "y": 7.3572696629213485 + }, + "prevControl": null, + "nextControl": { + "x": 4.959202247191014, + "y": 7.3572696629213485 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.239426966292136, + "y": 7.3572696629213485 + }, + "prevControl": { + "x": 7.239426966292136, + "y": 7.3572696629213485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.1498759305210926, + "maxWaypointRelativePos": 0.5687344913151384, + "constraints": { + "maxVelocity": 0.1, + "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": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Testing", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 6fde866..baa9526 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 132; - public static final String GIT_SHA = "aa1feb136ff6fc710a89bac1e700091f00e4317e"; - public static final String GIT_DATE = "2026-03-28 14:14:15 EDT"; + public static final int GIT_REVISION = 133; + public static final String GIT_SHA = "a1e6ddbec86fddf93a820cbbb2f9895b59331194"; + public static final String GIT_DATE = "2026-03-28 23:02:02 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-28 23:01:26 EDT"; - public static final long BUILD_UNIX_TIME = 1774753286069L; + public static final String BUILD_DATE = "2026-03-28 23:59:04 EDT"; + public static final long BUILD_UNIX_TIME = 1774756744335L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 14e4e63..cd83c90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -355,8 +355,9 @@ private void configureButtonBindings() { swerveSubsystem) .ignoringDisable(true)); - driver.povUp().onTrue(hoodSubsystem.runDebuggingUpCommand()); - driver.povDown().onTrue(hoodSubsystem.runDebuggingDownCommand()); + driver.povUp().whileTrue(hoodSubsystem.runDebuggingUpCommand()); + driver.povDown().whileTrue(hoodSubsystem.runDebuggingDownCommand()); + driver.povRight().whileTrue(flywheelSubsystem.runDebuggingVelocityCommand()); // ------- Operator Controls -------- \\ diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java index 64c7032..0f66099 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java @@ -102,9 +102,9 @@ public class FlywheelConstants { // ------- GOAL CONSTANTS -------- \\ - public static final double debuggingVelocity = Units.rotationsPerMinuteToRadiansPerSecond(3300); + public static final double debuggingVelocity = Units.rotationsPerMinuteToRadiansPerSecond(3200); - public static final double layupVelocity = Units.rotationsPerMinuteToRadiansPerSecond(3000); + public static final double layupVelocity = Units.rotationsPerMinuteToRadiansPerSecond(2500); // Testing Voltage Values public static final double kDebuggingVoltage = 10; // Volts diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java index ae0eb28..8cb464c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java @@ -49,7 +49,7 @@ public class HoodConstants { // ------- GOAL CONSTANTS -------- \\ - public static final double layupAngle = Units.degreesToRadians(41.55); // 41.55 @ 1.5m + public static final double layupAngle = Units.degreesToRadians(41.9); // 41.55 @ 1.5m public static final double debuggingAngleDown = Units.degreesToRadians(5.0); // Change as needed public static final double debuggingAngleUp = Units.degreesToRadians(42); // Change as needed diff --git a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java index 9ca87db..ea8bcc9 100644 --- a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java +++ b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java @@ -52,33 +52,42 @@ public class ShootOnTheFlyConstants { // Key: Distance (meters), Value: Shooter Speed (RPM) // TO-DO: Combine Interpolators using Units.rotationsPerMinuteToRadiansPerSecond() // DO NOT USE THIS - FLYWHEEL_RPM_INTERPOLATOR.put(1.0, 3400.0); // Touching Hub //3400 - FLYWHEEL_RPM_INTERPOLATOR.put(1.5, 3400.0); // 3400 - FLYWHEEL_RPM_INTERPOLATOR.put(2.0, 3400.0); // 3400 - FLYWHEEL_RPM_INTERPOLATOR.put(2.5, 3650.0); // 3650 - FLYWHEEL_RPM_INTERPOLATOR.put(3.0, 3900.0); // 3900 - FLYWHEEL_RPM_INTERPOLATOR.put(3.5, 3970.0); // 3970 - FLYWHEEL_RPM_INTERPOLATOR.put(4.0, 4050.0); // 4050 - FLYWHEEL_RPM_INTERPOLATOR.put(4.5, 4225.0); // 4225 + // FLYWHEEL_RPM_INTERPOLATOR.put(1.0, 3400.0); // Touching Hub //3400 + // FLYWHEEL_RPM_INTERPOLATOR.put(1.5, 3400.0); // 3400 + // FLYWHEEL_RPM_INTERPOLATOR.put(2.0, 3400.0); // 3400 + // FLYWHEEL_RPM_INTERPOLATOR.put(2.5, 3650.0); // 3650 + // FLYWHEEL_RPM_INTERPOLATOR.put(3.0, 3900.0); // 3900 + // FLYWHEEL_RPM_INTERPOLATOR.put(3.5, 3970.0); // 3970 + // FLYWHEEL_RPM_INTERPOLATOR.put(4.0, 4050.0); // 4050 + // FLYWHEEL_RPM_INTERPOLATOR.put(4.5, 4225.0); // 4225 // Key: Distance (meters), Value: Shooter Velocity (rad/sec) // USE THIS, BUT FIX FIRST // Touching Hub - FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.0, Units.rotationsPerMinuteToRadiansPerSecond(3000)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.5, Units.rotationsPerMinuteToRadiansPerSecond(3300)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(3.0, Units.rotationsPerMinuteToRadiansPerSecond(3300)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(3.5, Units.rotationsPerMinuteToRadiansPerSecond(3300)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.0, Units.rotationsPerMinuteToRadiansPerSecond(3300)); - // FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.5, Units.rotationsPerMinuteToRadiansPerSecond(3400)); // + FLYWHEEL_VELOCITY_INTERPOLATOR.put(1.5, Units.rotationsPerMinuteToRadiansPerSecond(2500)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.0, Units.rotationsPerMinuteToRadiansPerSecond(2800)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.5, Units.rotationsPerMinuteToRadiansPerSecond(2900)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(3.0, Units.rotationsPerMinuteToRadiansPerSecond(3000)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(3.5, Units.rotationsPerMinuteToRadiansPerSecond(3000)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.0, Units.rotationsPerMinuteToRadiansPerSecond(3000)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.5, Units.rotationsPerMinuteToRadiansPerSecond(3100)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(5.0, Units.rotationsPerMinuteToRadiansPerSecond(3200)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(5.5, Units.rotationsPerMinuteToRadiansPerSecond(3200)); + // Check // Key: Distance (meters), Value: Hood Angle (DEGREES) // Touching Hub, Using Distance 2d - HOOD_DEGREES_INTERPOLATOR.put(2.0, 41.55); - HOOD_DEGREES_INTERPOLATOR.put(2.5, 44.7); - HOOD_DEGREES_INTERPOLATOR.put(3.0, 41.17); - HOOD_DEGREES_INTERPOLATOR.put(3.5, 30.4); - HOOD_DEGREES_INTERPOLATOR.put(4.0, 25.85); + HOOD_DEGREES_INTERPOLATOR.put(1.5, 41.98); + HOOD_DEGREES_INTERPOLATOR.put(2.0, 39.14); + HOOD_DEGREES_INTERPOLATOR.put(2.5, 41.90); + HOOD_DEGREES_INTERPOLATOR.put(3.0, 41.90); + HOOD_DEGREES_INTERPOLATOR.put(3.5, 38.87); + HOOD_DEGREES_INTERPOLATOR.put(4.0, 32.27); + HOOD_DEGREES_INTERPOLATOR.put(4.5, 27.50); + HOOD_DEGREES_INTERPOLATOR.put(5.0, 24.0); + HOOD_DEGREES_INTERPOLATOR.put(5.5, 16.0); + // HOOD_DEGREES_INTERPOLATOR.put(4.0, 31.31); // Check } } From 093db29d44065eb91fdfabd00eea1986ad8570f8 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sun, 29 Mar 2026 16:04:49 -0700 Subject: [PATCH 14/32] 3/29 - Solid Single Auto, Working in the Ferrying Shot --- .../autos/[Left Swiper Visual].auto | 37 +++ .../autos/[Left] Double Swipe.auto | 272 ++++++++++++++++++ ...ew Path.path => [Left Swipe] Entry 1.path} | 51 ++-- .../paths/[Left Swipe] Intake 1.path | 89 ++++++ .../paths/[Left Swipe] Reentry 1.path | 63 ++++ .../paths/[Left Swipe] StageShoot1.path | 105 +++++++ src/main/deploy/pathplanner/settings.json | 1 + src/main/java/frc/robot/BuildConstants.java | 10 +- .../shooterUtil/ShootOnTheFlyConstants.java | 2 +- 9 files changed, 595 insertions(+), 35 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/[Left Swiper Visual].auto create mode 100644 src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto rename src/main/deploy/pathplanner/paths/{New Path.path => [Left Swipe] Entry 1.path} (50%) create mode 100644 src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path create mode 100644 src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path create mode 100644 src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path diff --git a/src/main/deploy/pathplanner/autos/[Left Swiper Visual].auto b/src/main/deploy/pathplanner/autos/[Left Swiper Visual].auto new file mode 100644 index 0000000..52a2b16 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Left Swiper Visual].auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Entry 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Intake 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Left Swipe] StageShoot1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Reentry 1" + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Left] Double Swipe", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto new file mode 100644 index 0000000..d8d89c0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto @@ -0,0 +1,272 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleWarm" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Entry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Intake 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] StageShoot1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "flywheelLayup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Reentry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Left] Double Swipe", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path similarity index 50% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path index 8c958de..1ea697e 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path @@ -3,48 +3,41 @@ "waypoints": [ { "anchor": { - "x": 6.820870185449357, - "y": 1.2079029957203988 + "x": 3.6127078651673195, + "y": 7.3878426966292965 }, "prevControl": null, "nextControl": { - "x": 6.251569186875892, - "y": 0.6515406562054202 + "x": 4.733719101122375, + "y": 7.530516853932667 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.979857346647646, - "y": 0.6774179743223976 + "x": 7.933696629212264, + "y": 7.03115730337087 }, "prevControl": { - "x": 6.808001727659159, - "y": 1.1486035704151556 - }, - "nextControl": { - "x": 5.229415121255349, - "y": 0.2504422253922971 + "x": 7.34261797752687, + "y": 7.194213483146151 }, + "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "[Left Swipe] Entry 1 to Intake 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.30788381742738447, + "rotationDegrees": 0.0 }, { - "anchor": { - "x": 3.4050641940085593, - "y": 0.5350927246790309 - }, - "prevControl": { - "x": 4.039058487874465, - "y": 0.6386019971469341 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null + "waypointRelativePos": 0.8157676348547667, + "rotationDegrees": -102.0 } ], - "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -57,14 +50,14 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 90.0 + "velocity": 1.0, + "rotation": -102.265 }, "reversed": false, "folder": "[Left] Double Swipe", "idealStartingState": { - "velocity": 0, - "rotation": 88.52172734331111 + "velocity": 0.0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path new file mode 100644 index 0000000..352d8b8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.933696629212264, + "y": 7.03115730337087 + }, + "prevControl": null, + "nextControl": { + "x": 7.933696629212264, + "y": 6.337868868494916 + }, + "isLocked": false, + "linkedName": "[Left Swipe] Entry 1 to Intake 1" + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 4.8599776077601495 + }, + "prevControl": { + "x": 7.969051968270556, + "y": 5.107464981175588 + }, + "nextControl": { + "x": 7.892932584269662, + "y": 4.574629293153409 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.985932584269663, + "y": 6.0426292134831465 + }, + "prevControl": { + "x": 6.965550561797752, + "y": 5.573842696629214 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Swipe] Intake 1 to StageShoot1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5560165975103781, + "rotationDegrees": -102.40100556375558 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.04565756823821024, + "maxWaypointRelativePos": 0.5220843672456567, + "constraints": { + "maxVelocity": 0.75, + "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": 3.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": -102.265 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path new file mode 100644 index 0000000..fe9e0f2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.184685393258427, + "y": 7.367460674157303 + }, + "prevControl": null, + "nextControl": { + "x": 4.184685393258427, + "y": 7.367460674157303 + }, + "isLocked": false, + "linkedName": "[Left Swipe] PostShoot1 to Reentry 1" + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 7.03115730337087 + }, + "prevControl": { + "x": 7.148988764044943, + "y": 7.5101348314606735 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Swipe] Reentry 1 to Intake 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0240663900414924, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.3842323651452348, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "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": -102.265 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": -66.52706715066263 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path new file mode 100644 index 0000000..1bee1da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.985932584269663, + "y": 6.0426292134831465 + }, + "prevControl": null, + "nextControl": { + "x": 7.000023991279187, + "y": 7.047947238549122 + }, + "isLocked": false, + "linkedName": "[Left Swipe] Intake 1 to StageShoot1" + }, + { + "anchor": { + "x": 6.539180929244987, + "y": 7.367460674157303 + }, + "prevControl": { + "x": 6.8276652701283815, + "y": 7.316979209602038 + }, + "nextControl": { + "x": 6.292922834860366, + "y": 7.410553027888035 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.2228876404494375, + "y": 7.367460674157303 + }, + "prevControl": { + "x": 6.3729928437130265, + "y": 7.316116691868749 + }, + "nextControl": { + "x": 4.081494382022472, + "y": 7.418415730337078 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.184685393258427, + "y": 7.367460674157303 + }, + "prevControl": { + "x": 3.87767415730337, + "y": 7.347078651685393 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.953526970954383, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.11017369727049342, + "maxWaypointRelativePos": 1.759801488833753, + "constraints": { + "maxVelocity": 2.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": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 2.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index d8c2457..e9bcb84 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -16,6 +16,7 @@ "Citrus Auto", "Default NZ Auto", "Force Warmup", + "[Left] Double Swipe", "TestingSpartan" ], "defaultMaxVel": 3.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index baa9526..52a0ad4 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 133; - public static final String GIT_SHA = "a1e6ddbec86fddf93a820cbbb2f9895b59331194"; - public static final String GIT_DATE = "2026-03-28 23:02:02 EDT"; + public static final int GIT_REVISION = 134; + public static final String GIT_SHA = "75252cdba08d72fefb14f40a22aa30a46ecb1689"; + public static final String GIT_DATE = "2026-03-28 23:59:28 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-28 23:59:04 EDT"; - public static final long BUILD_UNIX_TIME = 1774756744335L; + public static final String BUILD_DATE = "2026-03-29 19:03:28 EDT"; + public static final long BUILD_UNIX_TIME = 1774825408406L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java index ea8bcc9..6ebd478 100644 --- a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java +++ b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java @@ -62,7 +62,7 @@ public class ShootOnTheFlyConstants { // FLYWHEEL_RPM_INTERPOLATOR.put(4.5, 4225.0); // 4225 // Key: Distance (meters), Value: Shooter Velocity (rad/sec) - // USE THIS, BUT FIX FIRST + // USE THIS, BUT FIX FIRST\] // Touching Hub FLYWHEEL_VELOCITY_INTERPOLATOR.put(1.5, Units.rotationsPerMinuteToRadiansPerSecond(2500)); FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.0, Units.rotationsPerMinuteToRadiansPerSecond(2800)); From 1b0cbabd533569ba79f35161becdbddcbf487c2b Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sun, 29 Mar 2026 16:18:06 -0700 Subject: [PATCH 15/32] 3/29 - Passing Data put into same InterpolatingTreeMap because I am lazy. --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../subsystems/shooter/flywheel/FlywheelConstants.java | 2 +- .../robot/util/shooterUtil/ShootOnTheFlyConstants.java | 6 ++++++ 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 52a0ad4..9e898e4 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 134; - public static final String GIT_SHA = "75252cdba08d72fefb14f40a22aa30a46ecb1689"; - public static final String GIT_DATE = "2026-03-28 23:59:28 EDT"; + public static final int GIT_REVISION = 135; + public static final String GIT_SHA = "093db29d44065eb91fdfabd00eea1986ad8570f8"; + public static final String GIT_DATE = "2026-03-29 19:04:49 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-29 19:03:28 EDT"; - public static final long BUILD_UNIX_TIME = 1774825408406L; + public static final String BUILD_DATE = "2026-03-29 19:12:54 EDT"; + public static final long BUILD_UNIX_TIME = 1774825974705L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java index 0f66099..9a7c6ef 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java @@ -102,7 +102,7 @@ public class FlywheelConstants { // ------- GOAL CONSTANTS -------- \\ - public static final double debuggingVelocity = Units.rotationsPerMinuteToRadiansPerSecond(3200); + public static final double debuggingVelocity = Units.rotationsPerMinuteToRadiansPerSecond(3500); public static final double layupVelocity = Units.rotationsPerMinuteToRadiansPerSecond(2500); diff --git a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java index 6ebd478..c49806b 100644 --- a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java +++ b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java @@ -73,6 +73,9 @@ public class ShootOnTheFlyConstants { FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.5, Units.rotationsPerMinuteToRadiansPerSecond(3100)); FLYWHEEL_VELOCITY_INTERPOLATOR.put(5.0, Units.rotationsPerMinuteToRadiansPerSecond(3200)); FLYWHEEL_VELOCITY_INTERPOLATOR.put(5.5, Units.rotationsPerMinuteToRadiansPerSecond(3200)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(6.2, Units.rotationsPerMinuteToRadiansPerSecond(3400)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(7.0, Units.rotationsPerMinuteToRadiansPerSecond(3500)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(8.0, Units.rotationsPerMinuteToRadiansPerSecond(3500)); // Check @@ -87,6 +90,9 @@ public class ShootOnTheFlyConstants { HOOD_DEGREES_INTERPOLATOR.put(4.5, 27.50); HOOD_DEGREES_INTERPOLATOR.put(5.0, 24.0); HOOD_DEGREES_INTERPOLATOR.put(5.5, 16.0); + HOOD_DEGREES_INTERPOLATOR.put(6.2, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(7.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(8.0, 5.0); // HOOD_DEGREES_INTERPOLATOR.put(4.0, 31.31); // Check } From 0d2dc4b5818a0a1be65b842b29cdeb2f87343dec Mon Sep 17 00:00:00 2001 From: Shaguins Date: Mon, 30 Mar 2026 15:39:15 -0700 Subject: [PATCH 16/32] 3.30.26 Match & shift timers for Drive team to practice with. I will clean it up later. --- src/main/java/frc/robot/BuildConstants.java | 10 ++-- src/main/java/frc/robot/Robot.java | 60 +++++++++++++++------ 2 files changed, 49 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 9e898e4..3b5b497 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 135; - public static final String GIT_SHA = "093db29d44065eb91fdfabd00eea1986ad8570f8"; - public static final String GIT_DATE = "2026-03-29 19:04:49 EDT"; + public static final int GIT_REVISION = 136; + public static final String GIT_SHA = "1b0cbabd533569ba79f35161becdbddcbf487c2b"; + public static final String GIT_DATE = "2026-03-29 19:18:06 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-29 19:12:54 EDT"; - public static final long BUILD_UNIX_TIME = 1774825974705L; + public static final String BUILD_DATE = "2026-03-30 18:34:52 EDT"; + public static final long BUILD_UNIX_TIME = 1774910092273L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a9e6ca8..c5005f8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,12 +13,12 @@ package frc.robot; -import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.Mode; import frc.robot.subsystems.vision.VisionConstants; import frc.robot.util.FullSubsystem; @@ -44,6 +44,9 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; private Timer m_gcTimer = new Timer(); + private Timer m_matchTimer = new Timer(); + private Timer m_shiftTimer = new Timer(); + private boolean shiftBoolean = false; // Flag to ensure we only apply the alliance offset once per DS connection @AutoLogOutput(key = "Robot/HasInitializedAlliancePose") @@ -150,12 +153,6 @@ public void disabledInit() { } else { hasInitializedAlliancePose = false; } - - // Handle PathPlanner States Initialization by Running an Auto Routine - // Should not actually drive, only warmups up the PathPlanner library - // See: https://www.chiefdelphi.com/t/pathplanner-initial-lag-after-deploying-code/455068/7 - Command forcedAutoInitCommand = new PathPlannerAuto("Force Warmup").ignoringDisable(true); - CommandScheduler.getInstance().schedule(forcedAutoInitCommand); } /** This function is called periodically when disabled. */ @@ -178,13 +175,9 @@ public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); - // 3847: Time Delay to prevent Auto from firing with a delay - Command delaggerWaitCommand = new WaitCommand(0.01); - // schedule the autonomous command (example) - if (autonomousCommand != null) { - CommandScheduler.getInstance().schedule(delaggerWaitCommand.andThen(autonomousCommand)); - } + // SequentialCommandGroup autoCommand = new SequentialCommandGroup(listOfCommands); + } /** This function is called periodically during autonomous. */ @@ -200,18 +193,53 @@ public void teleopInit() { // this line or comment it out. if (autonomousCommand != null) { autonomousCommand.cancel(); - autonomousCommand = null; } // Handle edge case where alliance data isn't recieved before enabled if (!hasInitializedAlliancePose) { robotContainer.applyAlliancePoseOffset(); } + m_matchTimer.reset(); + m_shiftTimer.stop(); + m_shiftTimer.reset(); + m_matchTimer.start(); } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + + if (!DriverStation.getGameSpecificMessage().isEmpty()) { + SmartDashboard.putBoolean("Game MSG Recieved", true); + if (!m_shiftTimer.isRunning()) { + shiftBoolean = + !(((DriverStation.getGameSpecificMessage().charAt(0) == 'B') + && (DriverStation.getAlliance().get() == Alliance.Blue)) + || ((DriverStation.getGameSpecificMessage().charAt(0) == 'R') + && (DriverStation.getAlliance().get() == Alliance.Red))); + } + } else { + SmartDashboard.putBoolean("Game MSG Recieved", false); + } + if (m_matchTimer.hasElapsed(10)) { + if (!m_shiftTimer.isRunning() && !m_matchTimer.hasElapsed(110)) { + m_shiftTimer.start(); + } else if (m_shiftTimer.advanceIfElapsed(25)) { + shiftBoolean = !shiftBoolean; + } else if (m_matchTimer.hasElapsed(110)) { + m_shiftTimer.stop(); + SmartDashboard.putBoolean("EndGame", true); + shiftBoolean = true; + } else SmartDashboard.putBoolean("EndGame", false); + + } else { + shiftBoolean = true; + } + + SmartDashboard.putBoolean("SHOOT", shiftBoolean); + SmartDashboard.putNumber("Match Timer", 140 - m_matchTimer.get()); + SmartDashboard.putNumber("Shift Timer", 25 - m_shiftTimer.get()); + } /** This function is called once when test mode is enabled. */ @Override From 82d1291c3a0e7b80e8bbd6f4b01571f83edb0f92 Mon Sep 17 00:00:00 2001 From: Shaguins Date: Thu, 2 Apr 2026 16:24:47 -0700 Subject: [PATCH 17/32] 4.2.26 Driver wanted different manual Hood and added back nulling the auton after use --- src/main/java/frc/robot/Robot.java | 18 +++++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c5005f8..51bef7e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.Mode; import frc.robot.subsystems.vision.VisionConstants; import frc.robot.util.FullSubsystem; @@ -33,6 +34,8 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.littletonrobotics.urcl.URCL; +import com.pathplanner.lib.commands.PathPlannerAuto; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -153,6 +156,12 @@ public void disabledInit() { } else { hasInitializedAlliancePose = false; } + + // Handle PathPlanner States Initialization by Running an Auto Routine + // Should not actually drive, only warmups up the PathPlanner library + // See: https://www.chiefdelphi.com/t/pathplanner-initial-lag-after-deploying-code/455068/7 + Command forcedAutoInitCommand = new PathPlannerAuto("Force Warmup").ignoringDisable(true); + CommandScheduler.getInstance().schedule(forcedAutoInitCommand); } /** This function is called periodically when disabled. */ @@ -176,7 +185,13 @@ public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) - // SequentialCommandGroup autoCommand = new SequentialCommandGroup(listOfCommands); + // 3847: Time Delay to prevent Auto from firing with a delay + Command delaggerWaitCommand = new WaitCommand(0.01); + + // schedule the autonomous command (example) + if (autonomousCommand != null) { + CommandScheduler.getInstance().schedule(delaggerWaitCommand.andThen(autonomousCommand)); + } } @@ -193,6 +208,7 @@ public void teleopInit() { // this line or comment it out. if (autonomousCommand != null) { autonomousCommand.cancel(); + autonomousCommand = null; } // Handle edge case where alliance data isn't recieved before enabled diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd83c90..e41fe10 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -355,9 +355,9 @@ private void configureButtonBindings() { swerveSubsystem) .ignoringDisable(true)); - driver.povUp().whileTrue(hoodSubsystem.runDebuggingUpCommand()); - driver.povDown().whileTrue(hoodSubsystem.runDebuggingDownCommand()); driver.povRight().whileTrue(flywheelSubsystem.runDebuggingVelocityCommand()); + driver.leftTrigger().whileTrue(hoodSubsystem.runDebuggingVoltageUpCommand()); + driver.rightTrigger().whileTrue(hoodSubsystem.runDebuggingDownCommand()); // ------- Operator Controls -------- \\ From 4283c5b58b60ba89ae2fbb5b7f0789e4eb650762 Mon Sep 17 00:00:00 2001 From: Shaguins Date: Thu, 2 Apr 2026 22:59:16 -0700 Subject: [PATCH 18/32] 4.2.26 Fixed Driver Triggers to Properly used Voltage cmd & Moved Hood to Hub Due to Drive Team & Made Hood commands That are to be tested. --- src/main/java/frc/robot/RobotContainer.java | 9 ++++----- .../frc/robot/subsystems/shooter/hood/HoodConstants.java | 2 ++ .../frc/robot/subsystems/shooter/hood/HoodSubsystem.java | 7 +++++++ 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e41fe10..8d1b395 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -335,7 +335,8 @@ private void configureButtonBindings() { swerveSubsystem, () -> -driver.getLeftY(), () -> -driver.getLeftX(), - () -> shotCalculator.getCorrectTargetRotation())); + () -> shotCalculator.getCorrectTargetRotation())) + .whileTrue(hoodSubsystem.dynamicUpdatedShootCommand(() -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))); driver .b() @@ -356,8 +357,9 @@ private void configureButtonBindings() { .ignoringDisable(true)); driver.povRight().whileTrue(flywheelSubsystem.runDebuggingVelocityCommand()); + driver.leftTrigger().whileTrue(hoodSubsystem.runDebuggingVoltageUpCommand()); - driver.rightTrigger().whileTrue(hoodSubsystem.runDebuggingDownCommand()); + driver.rightTrigger().whileTrue(hoodSubsystem.runDebuggingVoltageDownCommand()); // ------- Operator Controls -------- \\ @@ -373,9 +375,6 @@ private void configureButtonBindings() { operator .y() - .whileTrue( - hoodSubsystem.dynamicUpdatedShootCommand( - () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) .whileTrue( flywheelSubsystem.dynamicUpdatedShootCommand( () -> shotCalculator.getCorrectTargetVelocity())); diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java index 8cb464c..3655d95 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java @@ -58,4 +58,6 @@ public class HoodConstants { public static final double debuggingVoltageUP = -1.0; public static final double debuggingVoltageDOWN = 1.0; + + public static final double trenchAngle = Units.degreesToRadians(38.84); } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java index 2839501..c5ffc1a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java @@ -147,6 +147,13 @@ public boolean isDrawingHighCurrent() { // vision public Command dynamicUpdatedShootCommand(DoubleSupplier positionRad) { return run(() -> runAngular(positionRad.getAsDouble())).withName("Hood Shoot"); + //To test but this will run the Trench angle once interupptted + //startEnd(() -> runAngular(positionRad.getAsDouble()), () -> runAngular(HoodConstants.trenchAngle)).withName("Hood Shoot"); + } + + //This would be used if we want a while false + public Command trenchHoodCommand(){ + return run(() -> runAngular(HoodConstants.trenchAngle)).withName("Hood Trench"); } // ----------------------------------LAYUP COMMANDS------------------------------// From 3d71e87bfd9e5235a2e4f2ca20530efc6ac3a736 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Fri, 3 Apr 2026 18:41:44 -0700 Subject: [PATCH 19/32] 4/3 - Commiting Auto Changes and Shot Compensation at Day 1 of QM's CACAC --- .../autos/Static Shoot Preload.auto | 14 ++++++++++---- .../pathplanner/paths/CLB_StageShoot1_V2.path | 2 +- .../paths/[Left Swipe] Intake 1.path | 2 +- .../paths/[Left Swipe] StageShoot1.path | 2 +- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 4 +--- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++++++-- .../subsystems/shooter/hood/HoodSubsystem.java | 18 ++++++++++++++---- .../subsystems/swerve/SwerveConstants.java | 4 ++-- 10 files changed, 50 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto b/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto index 9154942..ce260be 100644 --- a/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto +++ b/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto @@ -16,6 +16,12 @@ "pathName": "CenterBackout" } }, + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, { "type": "race", "data": { @@ -29,13 +35,13 @@ { "type": "named", "data": { - "name": "flywheelLayup" + "name": "flywheelDynamic" } }, { "type": "named", "data": { - "name": "hoodLayup" + "name": "hoodDynamic" } } ] @@ -48,13 +54,13 @@ { "type": "named", "data": { - "name": "flywheelLayup" + "name": "flywheelDynamic" } }, { "type": "named", "data": { - "name": "hoodLayup" + "name": "hoodDynamic" } }, { diff --git a/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path b/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path index d00878d..b1c516c 100644 --- a/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path +++ b/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 179.51226418354415 + "rotation": 179.51226418354418 }, "reversed": false, "folder": "CLB Citrus Auto", diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path index 352d8b8..e371d56 100644 --- a/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 7.969051968270556, - "y": 5.107464981175588 + "y": 5.107464981175589 }, "nextControl": { "x": 7.892932584269662, diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path index 1bee1da..a62ea14 100644 --- a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path @@ -24,7 +24,7 @@ "y": 7.316979209602038 }, "nextControl": { - "x": 6.292922834860366, + "x": 6.292922834860365, "y": 7.410553027888035 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index e9bcb84..f265d75 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -31,7 +31,7 @@ "driveGearing": 4.71428571429, "maxDriveSpeed": 5.74, "driveMotorType": "vortex", - "driveCurrentLimit": 80.0, + "driveCurrentLimit": 60.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 3b5b497..c3ad8b1 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 136; - public static final String GIT_SHA = "1b0cbabd533569ba79f35161becdbddcbf487c2b"; - public static final String GIT_DATE = "2026-03-29 19:18:06 EDT"; + public static final int GIT_REVISION = 139; + public static final String GIT_SHA = "4283c5b58b60ba89ae2fbb5b7f0789e4eb650762"; + public static final String GIT_DATE = "2026-04-03 01:59:16 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-03-30 18:34:52 EDT"; - public static final long BUILD_UNIX_TIME = 1774910092273L; + public static final String BUILD_DATE = "2026-04-03 21:40:52 EDT"; + public static final long BUILD_UNIX_TIME = 1775266852321L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 51bef7e..e2f05f5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ package frc.robot; +import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; @@ -34,8 +35,6 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.littletonrobotics.urcl.URCL; -import com.pathplanner.lib.commands.PathPlannerAuto; - /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -192,7 +191,6 @@ public void autonomousInit() { if (autonomousCommand != null) { CommandScheduler.getInstance().schedule(delaggerWaitCommand.andThen(autonomousCommand)); } - } /** This function is called periodically during autonomous. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8d1b395..5894e0a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -336,7 +336,10 @@ private void configureButtonBindings() { () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> shotCalculator.getCorrectTargetRotation())) - .whileTrue(hoodSubsystem.dynamicUpdatedShootCommand(() -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))); + .whileTrue( + hoodSubsystem.dynamicUpdatedShootCommandWithLinearCompensation( + () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) + .whileFalse(hoodSubsystem.trenchHoodCommand()); driver .b() @@ -356,8 +359,17 @@ private void configureButtonBindings() { swerveSubsystem) .ignoringDisable(true)); + driver.povUp().onTrue(Commands.runOnce(() -> hoodSubsystem.shotCompensation--, hoodSubsystem)); + driver + .povDown() + .onTrue(Commands.runOnce(() -> hoodSubsystem.shotCompensation++, hoodSubsystem)); + + driver + .povLeft() + .onTrue(Commands.runOnce(() -> hoodSubsystem.shotCompensation = 0, hoodSubsystem)); + driver.povRight().whileTrue(flywheelSubsystem.runDebuggingVelocityCommand()); - + driver.leftTrigger().whileTrue(hoodSubsystem.runDebuggingVoltageUpCommand()); driver.rightTrigger().whileTrue(hoodSubsystem.runDebuggingVoltageDownCommand()); diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java index c5ffc1a..a96a525 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java @@ -2,6 +2,7 @@ import static frc.robot.subsystems.shooter.hood.HoodConstants.highCurrentThreshold; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; @@ -20,6 +21,8 @@ public class HoodSubsystem extends FullSubsystem { private Alert masterDisconnected; + @AutoLogOutput public int shotCompensation = 0; + /* * Defines all possible states for the hood. Each goal state has a DoubleSupplier arguement that updates from Robot State. Note: Only input static choices. */ @@ -147,12 +150,19 @@ public boolean isDrawingHighCurrent() { // vision public Command dynamicUpdatedShootCommand(DoubleSupplier positionRad) { return run(() -> runAngular(positionRad.getAsDouble())).withName("Hood Shoot"); - //To test but this will run the Trench angle once interupptted - //startEnd(() -> runAngular(positionRad.getAsDouble()), () -> runAngular(HoodConstants.trenchAngle)).withName("Hood Shoot"); + // To test but this will run the Trench angle once interupptted + // startEnd(() -> runAngular(positionRad.getAsDouble()), () -> + // runAngular(HoodConstants.trenchAngle)).withName("Hood Shoot"); + } + + public Command dynamicUpdatedShootCommandWithLinearCompensation(DoubleSupplier positionRad) { + return run(() -> + runAngular(positionRad.getAsDouble() + Units.degreesToRadians(shotCompensation))) + .withName("Hood Shoot With Angular Compensation"); } - //This would be used if we want a while false - public Command trenchHoodCommand(){ + // This would be used if we want a while false + public Command trenchHoodCommand() { return run(() -> runAngular(HoodConstants.trenchAngle)).withName("Hood Trench"); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index e29c27d..dfca39d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -69,7 +69,7 @@ public class SwerveConstants { public static final int backRightTurnCanId = 6; // Drive motor configuration (MAXSwerve with 14 pinion teeth and 22 spur teeth) - public static final int driveMotorCurrentLimit = 80; // Vortex 80A, NEO 60A + public static final int driveMotorCurrentLimit = 60; // Vortex 80A, NEO 60A // Remeasure the radius in the Makerspace, Run the Wheel Characterization Auto public static final double wheelRadiusMeters = Units.inchesToMeters(1.498); @@ -97,7 +97,7 @@ public class SwerveConstants { // Turn motor configuration public static final boolean turnInverted = false; // <-- Check if Neccessary - public static final int turnMotorCurrentLimit = 35; // 550 35A, SAFE = 20A + public static final int turnMotorCurrentLimit = 20; // 550 35A, SAFE = 20A public static final double turnMotorReduction = 9424.0 / 203.0; public static final DCMotor turnGearbox = DCMotor.getNeo550(1); From 48c4ee13ed208a8c80fe42cc093bd4bec816d96d Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sat, 4 Apr 2026 07:10:36 -0700 Subject: [PATCH 20/32] 4/4 - Right Autos to Showcase Capabilities --- .../autos/[Right] Double Swipe.auto | 272 ++++++++++++++++++ .../paths/[Right Swipe] Entry 1.path | 63 ++++ .../paths/[Right Swipe] Intake 1.path | 89 ++++++ .../paths/[Right Swipe] Reentry 1.path | 63 ++++ .../paths/[Right Swipe] StageShoot1.path | 105 +++++++ src/main/deploy/pathplanner/settings.json | 10 +- src/main/java/frc/robot/BuildConstants.java | 10 +- 7 files changed, 603 insertions(+), 9 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto create mode 100644 src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path create mode 100644 src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path create mode 100644 src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path create mode 100644 src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path diff --git a/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto new file mode 100644 index 0000000..3c0f946 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto @@ -0,0 +1,272 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleWarm" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Entry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Intake 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] StageShoot1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "flywheelLayup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Reentry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Right] Double Swipe", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path new file mode 100644 index 0000000..8228c05 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6127078651673195, + "y": 0.6811573033707043 + }, + "prevControl": null, + "nextControl": { + "x": 4.733719101122375, + "y": 0.5384831460673336 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 1.0378426966291308 + }, + "prevControl": { + "x": 7.34261797752687, + "y": 0.8747865168538498 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Swipe] Entry 1 to Intake 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.30788381742738447, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 0.8157676348547667, + "rotationDegrees": 102.0 + } + ], + "constraintZones": [], + "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": 102.265 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 0.0, + "rotation": -0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path new file mode 100644 index 0000000..e194d47 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.933696629212264, + "y": 1.0378426966291308 + }, + "prevControl": null, + "nextControl": { + "x": 7.933696629212264, + "y": 1.7311311315050846 + }, + "isLocked": false, + "linkedName": "[Right Swipe] Entry 1 to Intake 1" + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 3.2090223922398513 + }, + "prevControl": { + "x": 7.969051968270556, + "y": 2.961535018824412 + }, + "nextControl": { + "x": 7.892932584269662, + "y": 3.494370706846592 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.985932584269663, + "y": 2.0263707865168543 + }, + "prevControl": { + "x": 6.965550561797752, + "y": 2.495157303370787 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Swipe] Intake 1 to StageShoot1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5560165975103781, + "rotationDegrees": 102.40100556375558 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.04565756823821024, + "maxWaypointRelativePos": 0.5220843672456567, + "constraints": { + "maxVelocity": 0.75, + "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": 3.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": 102.265 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path new file mode 100644 index 0000000..6a0e0b0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.184685393258427, + "y": 0.7015393258426981 + }, + "prevControl": null, + "nextControl": { + "x": 4.184685393258427, + "y": 0.7015393258426981 + }, + "isLocked": false, + "linkedName": "[Right Swipe] PostShoot1 to Reentry 1" + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 1.0378426966291308 + }, + "prevControl": { + "x": 7.148988764044943, + "y": 0.5588651685393274 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Swipe] Reentry 1 to Intake 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0240663900414924, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 0.3842323651452348, + "rotationDegrees": -0.0 + } + ], + "constraintZones": [], + "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": 102.265 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": 66.52706715066263 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path new file mode 100644 index 0000000..f522478 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.985932584269663, + "y": 2.0263707865168543 + }, + "prevControl": null, + "nextControl": { + "x": 7.000023991279187, + "y": 1.0210527614508784 + }, + "isLocked": false, + "linkedName": "[Right Swipe] Intake 1 to StageShoot1" + }, + { + "anchor": { + "x": 6.539180929244987, + "y": 0.7015393258426981 + }, + "prevControl": { + "x": 6.8276652701283815, + "y": 0.752020790397963 + }, + "nextControl": { + "x": 6.292922834860365, + "y": 0.6584469721119657 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.2228876404494375, + "y": 0.7015393258426981 + }, + "prevControl": { + "x": 6.3729928437130265, + "y": 0.7528833081312518 + }, + "nextControl": { + "x": 4.081494382022472, + "y": 0.6505842696629226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.184685393258427, + "y": 0.7015393258426981 + }, + "prevControl": { + "x": 3.87767415730337, + "y": 0.7219213483146074 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.953526970954383, + "rotationDegrees": -180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.11017369727049342, + "maxWaypointRelativePos": 1.759801488833753, + "constraints": { + "maxVelocity": 2.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": 0, + "rotation": -180.0 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 2.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f265d75..e607171 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,15 +9,17 @@ "Depot Auto", "Force Warmup", "Misc", - "[Left] Double Swipe", - "Testing" + "[Right] Double Swipe", + "Testing", + "[Left] Double Swipe" ], "autoFolders": [ "Citrus Auto", "Default NZ Auto", "Force Warmup", - "[Left] Double Swipe", - "TestingSpartan" + "[Right] Double Swipe", + "TestingSpartan", + "[Left] Double Swipe" ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c3ad8b1..051c356 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 139; - public static final String GIT_SHA = "4283c5b58b60ba89ae2fbb5b7f0789e4eb650762"; - public static final String GIT_DATE = "2026-04-03 01:59:16 EDT"; + public static final int GIT_REVISION = 140; + public static final String GIT_SHA = "3d71e87bfd9e5235a2e4f2ca20530efc6ac3a736"; + public static final String GIT_DATE = "2026-04-03 21:41:44 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-03 21:40:52 EDT"; - public static final long BUILD_UNIX_TIME = 1775266852321L; + public static final String BUILD_DATE = "2026-04-04 10:09:22 EDT"; + public static final long BUILD_UNIX_TIME = 1775311762065L; public static final int DIRTY = 1; private BuildConstants() {} From 4fb583c31e5753e31de87fbdbbd55916f17e73f9 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sat, 4 Apr 2026 12:52:37 -0700 Subject: [PATCH 21/32] 4/4 - Add Heursitic Constants to compensate for Short Miss Scenario, Values Chosen based on median Driver overrides --- .../autos/[Left] Double Swipe.auto | 39 +------------------ .../autos/[Right] Double Swipe.auto | 39 +------------------ src/main/java/frc/robot/BuildConstants.java | 10 ++--- src/main/java/frc/robot/RobotContainer.java | 8 ++-- .../intake/pivot/PivotConstants.java | 2 +- .../subsystems/shooter/ShotCalculator.java | 8 +++- .../shooterUtil/ShootOnTheFlyConstants.java | 3 ++ 7 files changed, 22 insertions(+), 87 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto index d8d89c0..31c9b49 100644 --- a/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto +++ b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto @@ -184,7 +184,7 @@ { "type": "wait", "data": { - "waitTime": 4.0 + "waitTime": 10.0 } }, { @@ -225,43 +225,6 @@ } ] } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "[Left Swipe] Reentry 1" - } - }, - { - "type": "named", - "data": { - "name": "pivotDown" - } - }, - { - "type": "named", - "data": { - "name": "rollerIntake" - } - }, - { - "type": "named", - "data": { - "name": "agitatorIntake" - } - }, - { - "type": "named", - "data": { - "name": "hoodLayup" - } - } - ] - } } ] } diff --git a/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto index 3c0f946..389c8b5 100644 --- a/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto +++ b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto @@ -184,7 +184,7 @@ { "type": "wait", "data": { - "waitTime": 4.0 + "waitTime": 10.0 } }, { @@ -225,43 +225,6 @@ } ] } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "[Right Swipe] Reentry 1" - } - }, - { - "type": "named", - "data": { - "name": "pivotDown" - } - }, - { - "type": "named", - "data": { - "name": "rollerIntake" - } - }, - { - "type": "named", - "data": { - "name": "agitatorIntake" - } - }, - { - "type": "named", - "data": { - "name": "hoodLayup" - } - } - ] - } } ] } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 051c356..033d0d3 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 140; - public static final String GIT_SHA = "3d71e87bfd9e5235a2e4f2ca20530efc6ac3a736"; - public static final String GIT_DATE = "2026-04-03 21:41:44 EDT"; + public static final int GIT_REVISION = 141; + public static final String GIT_SHA = "48c4ee13ed208a8c80fe42cc093bd4bec816d96d"; + public static final String GIT_DATE = "2026-04-04 10:10:36 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-04 10:09:22 EDT"; - public static final long BUILD_UNIX_TIME = 1775311762065L; + public static final String BUILD_DATE = "2026-04-04 15:48:38 EDT"; + public static final long BUILD_UNIX_TIME = 1775332118565L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5894e0a..d1787d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -326,7 +326,8 @@ private void configureButtonBindings() { .whileTrue(indexerSubsystem.indexCommand()) .whileTrue(kickerSubsystem.indexCommand()) .whileTrue(agitatorSubsystem.indexCommand()) - .whileTrue(pivotSubsystem.runSaltAndPepperCommand()); + .whileTrue(pivotSubsystem.runSaltAndPepperCommand()) + .whileTrue(rollerSubsystem.intakeCommand()); driver .rightBumper() @@ -386,12 +387,13 @@ private void configureButtonBindings() { operator.b().whileTrue(rollerSubsystem.runUnjamCommand()); operator - .y() + .rightTrigger() .whileTrue( flywheelSubsystem.dynamicUpdatedShootCommand( () -> shotCalculator.getCorrectTargetVelocity())); - operator.a().onTrue(flywheelSubsystem.toggleWarm()); // Shifted from DPad Left + // Disabled toggleWarm on Operator because of fat-keying during intaking + // operator.a().onTrue(flywheelSubsystem.toggleWarm()); // Shifted from DPad Left operator .povUp() diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java index f3fe969..cced2e4 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java @@ -25,7 +25,7 @@ public class PivotConstants { // Temporary Values (Confirm Functionality) public static final double stowAngle = 0.0; public static final double deployedAngle = -2.19; // Not full stow prototype - public static final double halfDeployedAngle = deployedAngle * 0.25; // + public static final double halfDeployedAngle = deployedAngle * 0.4; // was 0.25 public static final double jugglingAngle = Units.degreesToRadians(0.0); public static final double debuggingAngle = Units.degreesToRadians(0.0); // Change as needed diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 3076607..e249f0e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -194,13 +194,17 @@ public void periodic() { fieldToTargetAngle = correctedTargetXYCoords.minus(shooterXYCoords).getAngle(); // Functionally equivalent to atan2(dy, dx) but avoids the manual subtraction + easier to read - distanceToTarget2D = shooterXYCoords.getDistance(correctedTargetXYCoords); + distanceToTarget2D = + shooterXYCoords.getDistance(correctedTargetXYCoords) + + ShootOnTheFlyConstants.shortMissFlywheelCalibration; distanceToTarget3D = shooterPose3d.getTranslation().getDistance(correctedTargetPose3d.getTranslation()); targetSpeedRadPerSec = ShootOnTheFlyConstants.FLYWHEEL_VELOCITY_INTERPOLATOR.get(distanceToTarget2D); - targetAngleDeg = ShootOnTheFlyConstants.HOOD_DEGREES_INTERPOLATOR.get(distanceToTarget2D); + targetAngleDeg = + ShootOnTheFlyConstants.HOOD_DEGREES_INTERPOLATOR.get(distanceToTarget2D) + + ShootOnTheFlyConstants.shortMissHoodCalibration; angularError = fieldToTargetAngle.minus(robotPose.getRotation()); diff --git a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java index c49806b..d2507c3 100644 --- a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java +++ b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java @@ -13,6 +13,9 @@ public class ShootOnTheFlyConstants { public static final double shooterHeightOffset = 0.5; // Meters, height of shooter from ground + public static final double shortMissFlywheelCalibration = 0.3; // meters + public static final double shortMissHoodCalibration = -3; // degrees + // ------- Transform Constants -------- \\ public static final Transform3d SHOOTER_TRANSFORM_CENTER = new Transform3d(-0.24, 0, 0.5, Rotation3d.kZero); From 37072613607b3e20683addc0292f6636e44bc374 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sun, 5 Apr 2026 13:36:35 -0700 Subject: [PATCH 22/32] 4/5 - CACAC Elimination Bracket Auto Routine and Shoot Compensation Edits based on manual Driver compensation --- .../deploy/pathplanner/autos/[Left] Double Swipe.auto | 8 +++++++- .../deploy/pathplanner/autos/[Right] Double Swipe.auto | 8 +++++++- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../robot/util/shooterUtil/ShootOnTheFlyConstants.java | 2 +- 4 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto index 31c9b49..6d1afef 100644 --- a/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto +++ b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto @@ -184,7 +184,7 @@ { "type": "wait", "data": { - "waitTime": 10.0 + "waitTime": 12.0 } }, { @@ -222,6 +222,12 @@ "data": { "name": "indexerIndex" } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto index 389c8b5..7b608c2 100644 --- a/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto +++ b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto @@ -184,7 +184,7 @@ { "type": "wait", "data": { - "waitTime": 10.0 + "waitTime": 12.0 } }, { @@ -222,6 +222,12 @@ "data": { "name": "indexerIndex" } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } } ] } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 033d0d3..24076b5 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 141; - public static final String GIT_SHA = "48c4ee13ed208a8c80fe42cc093bd4bec816d96d"; - public static final String GIT_DATE = "2026-04-04 10:10:36 EDT"; + public static final int GIT_REVISION = 142; + public static final String GIT_SHA = "4fb583c31e5753e31de87fbdbbd55916f17e73f9"; + public static final String GIT_DATE = "2026-04-04 15:52:37 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-04 15:48:38 EDT"; - public static final long BUILD_UNIX_TIME = 1775332118565L; + public static final String BUILD_DATE = "2026-04-05 16:36:01 EDT"; + public static final long BUILD_UNIX_TIME = 1775421361257L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java index d2507c3..d432de8 100644 --- a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java +++ b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java @@ -14,7 +14,7 @@ public class ShootOnTheFlyConstants { public static final double shooterHeightOffset = 0.5; // Meters, height of shooter from ground public static final double shortMissFlywheelCalibration = 0.3; // meters - public static final double shortMissHoodCalibration = -3; // degrees + public static final double shortMissHoodCalibration = -5; // degrees // ------- Transform Constants -------- \\ public static final Transform3d SHOOTER_TRANSFORM_CENTER = From ed5c4acb30be119ccf1ebee35dda84bab265c54b Mon Sep 17 00:00:00 2001 From: alexding2022-hub Date: Mon, 6 Apr 2026 23:03:10 -0700 Subject: [PATCH 23/32] Change from runCurrent to index for indexerSubsystem --- 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 d1787d7..c4a715b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -237,7 +237,7 @@ public RobotContainer() { // ------- Indexer Auto NamedCommands -------- \\ - NamedCommands.registerCommand("indexerIndex", indexerSubsystem.runCurrentCommand()); + NamedCommands.registerCommand("indexerIndex", indexerSubsystem.indexCommand()); // ------- Kicker Auto NamedCommands -------- \\ From 64e7662ec404a7e9fb9d84c2b4dac7fad49866c5 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Tue, 7 Apr 2026 18:54:52 -0700 Subject: [PATCH 24/32] 4/7 - Lowered CurrentLimits for indexer + aggitator, added shootOnTheFly constants for cross-field passing --- src/main/java/frc/robot/BuildConstants.java | 10 +++--- src/main/java/frc/robot/RobotContainer.java | 6 ++-- .../robot/commands/CrossTheBumpCommand.java | 35 +++++++++++++++++++ .../agitator/AgitatorConstants.java | 2 +- .../subsystems/indexer/IndexerConstants.java | 2 +- .../shooterUtil/ShootOnTheFlyConstants.java | 18 +++++++++- 6 files changed, 62 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/commands/CrossTheBumpCommand.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 24076b5..d898fda 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 142; - public static final String GIT_SHA = "4fb583c31e5753e31de87fbdbbd55916f17e73f9"; - public static final String GIT_DATE = "2026-04-04 15:52:37 EDT"; + public static final int GIT_REVISION = 144; + public static final String GIT_SHA = "ed5c4acb30be119ccf1ebee35dda84bab265c54b"; + public static final String GIT_DATE = "2026-04-07 02:03:10 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-05 16:36:01 EDT"; - public static final long BUILD_UNIX_TIME = 1775421361257L; + public static final String BUILD_DATE = "2026-04-07 21:53:30 EDT"; + public static final long BUILD_UNIX_TIME = 1775613210250L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c4a715b..7ca403b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -339,8 +339,8 @@ private void configureButtonBindings() { () -> shotCalculator.getCorrectTargetRotation())) .whileTrue( hoodSubsystem.dynamicUpdatedShootCommandWithLinearCompensation( - () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) - .whileFalse(hoodSubsystem.trenchHoodCommand()); + () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))); + // .whileFalse(hoodSubsystem.trenchHoodCommand()); driver .b() @@ -393,7 +393,7 @@ private void configureButtonBindings() { () -> shotCalculator.getCorrectTargetVelocity())); // Disabled toggleWarm on Operator because of fat-keying during intaking - // operator.a().onTrue(flywheelSubsystem.toggleWarm()); // Shifted from DPad Left + operator.a().onTrue(flywheelSubsystem.toggleWarm()); // Shifted from DPad Left operator .povUp() diff --git a/src/main/java/frc/robot/commands/CrossTheBumpCommand.java b/src/main/java/frc/robot/commands/CrossTheBumpCommand.java new file mode 100644 index 0000000..15a04a5 --- /dev/null +++ b/src/main/java/frc/robot/commands/CrossTheBumpCommand.java @@ -0,0 +1,35 @@ +// package frc.robot.commands; + +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.subsystems.swerve.SwerveSubsystem; + +// public class CrossTheBumpCommand extends Command{ +// // Directional Enums +// public enum CrossingDirection { +// TO_NEUTRAL, TO_ALLIANCE +// } + +// // State Machine Tracking Enum +// private enum BumpState { +// APPROACHING, +// CLIMBING, +// CRESTING, +// DESCENDING, +// LEVELING, +// COMPLETED, +// TIMED_OUT +// } + +// public static final class BumpConstants { + +// public static double approachSpeedMs = 1.0; +// public static double climbingSpeedMs = 0.6; +// public static double levelingSpeedMs = 0.4; + +// public static double pitchClimbingThreshold = Units.degreesToRadians(3.0); +// } + +// private final SwerveSubsystem drive; +// private final CrossingDirection direction; +// } diff --git a/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java b/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java index 2c5d68a..c4cb742 100644 --- a/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java +++ b/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java @@ -2,7 +2,7 @@ public class AgitatorConstants { public static final int sparkMasterAgitatorCanId = 11; - public static final int masterCurrentLimit = 50; // amps + public static final int masterCurrentLimit = 25; // amps // Temporary Values (Confirm Functionality) public static final double indexingVolts = -6.0; diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index c31a0bc..598add1 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -2,7 +2,7 @@ public class IndexerConstants { public static final int sparkMasterIndexerCanId = 12; - public static final int masterCurrentLimit = 65; // amps + public static final int masterCurrentLimit = 50; // amps // Temporary Values (Confirm Functionality) public static final double stowVolts = 0.0; diff --git a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java index d432de8..3281a97 100644 --- a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java +++ b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java @@ -78,7 +78,15 @@ public class ShootOnTheFlyConstants { FLYWHEEL_VELOCITY_INTERPOLATOR.put(5.5, Units.rotationsPerMinuteToRadiansPerSecond(3200)); FLYWHEEL_VELOCITY_INTERPOLATOR.put(6.2, Units.rotationsPerMinuteToRadiansPerSecond(3400)); FLYWHEEL_VELOCITY_INTERPOLATOR.put(7.0, Units.rotationsPerMinuteToRadiansPerSecond(3500)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(8.0, Units.rotationsPerMinuteToRadiansPerSecond(3500)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(8.0, Units.rotationsPerMinuteToRadiansPerSecond(3600)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(9.0, Units.rotationsPerMinuteToRadiansPerSecond(3800)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(10.0, Units.rotationsPerMinuteToRadiansPerSecond(4000)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(11.0, Units.rotationsPerMinuteToRadiansPerSecond(4200)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(12.0, Units.rotationsPerMinuteToRadiansPerSecond(4500)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(13.0, Units.rotationsPerMinuteToRadiansPerSecond(4700)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(14.0, Units.rotationsPerMinuteToRadiansPerSecond(4950)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(15.0, Units.rotationsPerMinuteToRadiansPerSecond(5200)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(16.0, Units.rotationsPerMinuteToRadiansPerSecond(5450)); // Check @@ -96,6 +104,14 @@ public class ShootOnTheFlyConstants { HOOD_DEGREES_INTERPOLATOR.put(6.2, 5.0); HOOD_DEGREES_INTERPOLATOR.put(7.0, 5.0); HOOD_DEGREES_INTERPOLATOR.put(8.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(9.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(10.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(11.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(12.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(13.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(14.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(15.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(16.0, 5.0); // HOOD_DEGREES_INTERPOLATOR.put(4.0, 31.31); // Check } From 8d9e3aa9932718860d719c3ab54a812de027498d Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Tue, 7 Apr 2026 19:47:31 -0700 Subject: [PATCH 25/32] Tuned hublock PID --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/commands/DriveCommands.java | 2 +- .../robot/subsystems/agitator/AgitatorConstants.java | 2 +- .../robot/subsystems/intake/pivot/PivotConstants.java | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index d898fda..fe00cf6 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 144; - public static final String GIT_SHA = "ed5c4acb30be119ccf1ebee35dda84bab265c54b"; - public static final String GIT_DATE = "2026-04-07 02:03:10 EDT"; + public static final int GIT_REVISION = 145; + public static final String GIT_SHA = "64e7662ec404a7e9fb9d84c2b4dac7fad49866c5"; + public static final String GIT_DATE = "2026-04-07 21:54:52 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-07 21:53:30 EDT"; - public static final long BUILD_UNIX_TIME = 1775613210250L; + public static final String BUILD_DATE = "2026-04-07 22:45:53 EDT"; + public static final long BUILD_UNIX_TIME = 1775616353276L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index d00f2a7..5404f35 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -133,7 +133,7 @@ public static Command joystickDriveAtAngle( Supplier rotationSupplier) { // Create PID controller - PIDController angleController = new PIDController(10, 0.0, 0.2); // Broken atm + PIDController angleController = new PIDController(10, 0.0, 0.80); //Previous Values P = 10, D = 0.75 angleController.enableContinuousInput(-Math.PI, Math.PI); // Construct command diff --git a/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java b/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java index c4cb742..fa25344 100644 --- a/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java +++ b/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java @@ -2,7 +2,7 @@ public class AgitatorConstants { public static final int sparkMasterAgitatorCanId = 11; - public static final int masterCurrentLimit = 25; // amps + public static final int masterCurrentLimit = 15; // amps // Temporary Values (Confirm Functionality) public static final double indexingVolts = -6.0; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java index cced2e4..5e76d8e 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java @@ -25,7 +25,7 @@ public class PivotConstants { // Temporary Values (Confirm Functionality) public static final double stowAngle = 0.0; public static final double deployedAngle = -2.19; // Not full stow prototype - public static final double halfDeployedAngle = deployedAngle * 0.4; // was 0.25 + public static final double halfDeployedAngle = deployedAngle * 0.2; // was 0.25 public static final double jugglingAngle = Units.degreesToRadians(0.0); public static final double debuggingAngle = Units.degreesToRadians(0.0); // Change as needed From 51f45ecbeae242781b913401e97374d51ef248a1 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Wed, 8 Apr 2026 08:27:51 -0700 Subject: [PATCH 26/32] 4/8 - Re-Addex Trench to Right Bumper after Rm'ing it for Interpolating Data Collection --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/DriveCommands.java | 3 ++- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index fe00cf6..f788199 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 145; - public static final String GIT_SHA = "64e7662ec404a7e9fb9d84c2b4dac7fad49866c5"; - public static final String GIT_DATE = "2026-04-07 21:54:52 EDT"; + public static final int GIT_REVISION = 146; + public static final String GIT_SHA = "8d9e3aa9932718860d719c3ab54a812de027498d"; + public static final String GIT_DATE = "2026-04-07 22:47:31 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-07 22:45:53 EDT"; - public static final long BUILD_UNIX_TIME = 1775616353276L; + public static final String BUILD_DATE = "2026-04-08 11:27:11 EDT"; + public static final long BUILD_UNIX_TIME = 1775662031531L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7ca403b..d7517b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -339,8 +339,8 @@ private void configureButtonBindings() { () -> shotCalculator.getCorrectTargetRotation())) .whileTrue( hoodSubsystem.dynamicUpdatedShootCommandWithLinearCompensation( - () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))); - // .whileFalse(hoodSubsystem.trenchHoodCommand()); + () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) + .whileFalse(hoodSubsystem.trenchHoodCommand()); driver .b() diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 5404f35..2d2e28b 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -133,7 +133,8 @@ public static Command joystickDriveAtAngle( Supplier rotationSupplier) { // Create PID controller - PIDController angleController = new PIDController(10, 0.0, 0.80); //Previous Values P = 10, D = 0.75 + PIDController angleController = + new PIDController(10, 0.0, 0.80); // Previous Values P = 10, D = 0.75 angleController.enableContinuousInput(-Math.PI, Math.PI); // Construct command From afc00acc4dd3429ea4119e9e596f526a220ce475 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Wed, 8 Apr 2026 11:41:24 -0700 Subject: [PATCH 27/32] 4/8 - Added CrossBump Auto Rewrite for testing a more robust way to cross/detect the bump using Pigeon 2.0 pitch position and rates. Struct is State-based and implemented using WPILib Command architecture. --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 9 + .../frc/robot/commands/CrossBumpCommand.java | 325 ++++++++++++++++++ .../robot/commands/CrossTheBumpCommand.java | 35 -- .../frc/robot/subsystems/swerve/GyroIO.java | 2 + .../subsystems/swerve/GyroIOPigeon2.java | 12 +- .../subsystems/swerve/SwerveSubsystem.java | 10 + 7 files changed, 362 insertions(+), 41 deletions(-) create mode 100644 src/main/java/frc/robot/commands/CrossBumpCommand.java delete mode 100644 src/main/java/frc/robot/commands/CrossTheBumpCommand.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index f788199..c30262c 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 146; - public static final String GIT_SHA = "8d9e3aa9932718860d719c3ab54a812de027498d"; - public static final String GIT_DATE = "2026-04-07 22:47:31 EDT"; + public static final int GIT_REVISION = 147; + public static final String GIT_SHA = "51f45ecbeae242781b913401e97374d51ef248a1"; + public static final String GIT_DATE = "2026-04-08 11:27:51 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-08 11:27:11 EDT"; - public static final long BUILD_UNIX_TIME = 1775662031531L; + public static final String BUILD_DATE = "2026-04-08 14:39:38 EDT"; + public static final long BUILD_UNIX_TIME = 1775673578356L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d7517b8..db1b8ea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.CrossBumpCommand; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.agitator.AgitatorIO; import frc.robot.subsystems.agitator.AgitatorIOSim; @@ -270,6 +271,14 @@ public RobotContainer() { () -> -driver.getLeftX(), () -> shotCalculator.getCorrectTargetRotation())); + NamedCommands.registerCommand( + "crossBumpNeutralZone", + new CrossBumpCommand(swerveSubsystem, CrossBumpCommand.CrossDirection.TO_NEUTRAL)); + + NamedCommands.registerCommand( + "crossBumpAllianceZone", + new CrossBumpCommand(swerveSubsystem, CrossBumpCommand.CrossDirection.TO_ALLIANCE)); + // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); diff --git a/src/main/java/frc/robot/commands/CrossBumpCommand.java b/src/main/java/frc/robot/commands/CrossBumpCommand.java new file mode 100644 index 0000000..7559be6 --- /dev/null +++ b/src/main/java/frc/robot/commands/CrossBumpCommand.java @@ -0,0 +1,325 @@ +package frc.robot.commands; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class CrossBumpCommand extends Command { + + // ----------------------------------Direction Enum------------------------------// + public enum CrossDirection { + // from alliance zone toward the neutral zone. + TO_NEUTRAL, + + // from neutral zone toward the alliance zone. + TO_ALLIANCE + } + + // ----------------------------------Internal State Machine------------------------------// + + private enum BumpState { + + // Driving at approach speed before any pitch is detected. + // Exits when pitch exceeds PITCH_ENTRY_THRESHOLD_RAD. + APPROACHING, + + // Exits when pitch velocity crosses zero (peak detected) + CLIMBING, + + // Exits when pitch drops below -PITCH_ENTRY_THRESHOLD_RAD (descent confirmed) + // or if peak pitch was small (robot straddled >:| the upward ramp). + CRESTING, + + // Continue at BUMP_SPEED_MS. + // Exits when pitch rises above -PITCH_FLAT_THRESHOLD_RAD for SETTLE_DEBOUNCE_SECONDS (all + // wheels back on flat ground). + DESCENDING, + + // Command ends after SETTLE_DEBOUNCE_SECONDS of flat pitch. + LEVELING, + + // isFinished() returns true. + COMPLETE, + + /** + * Safety state: bump was not detected within APPROACH_TIMEOUT_SECONDS. Robot may have stopped, + * missed the bump, or negligently our pitch sign is inverted. isFinished() returns true to + * prevent blocking the Command scheduler since we have a addRequirements call. + */ + TIMED_OUT + } + + // ----------------------------------Internal Constants ------------------------------// + + public static final class BumpConstants { + + // Start at 1.0 m/s/low value and tune up if the robot stalls on the ramp edge. + public static double APPROACH_SPEED_MS = 1.0; + + public static double BUMP_SPEED_MS = 0.6; + + // Speed during the LEVELING phase (all wheels back on flat ground) in m/s. + public static double SETTLE_SPEED_MS = 0.4; + + /** + * Pitch threshold in radians to detect ramp entry (APPROACHING to CLIMBING). 3 degrees = 0.052 + * rad. Too low: false positives. Too high: robot is already significantly on the ramp before + * detecting it. + */ + public static double PITCH_ENTRY_THRESHOLD_RAD = Units.degreesToRadians(3.0); + + /** + * Minimum peak pitch to confirm a real bump. 5 degrees = 0.087 rad. Prevents false cresting + * detection on flat ground. The bump's actual peak is 8.35 deg (AI found) This threshold should + * be below 8.35 deg to catch cases if the robot crosses near the ramp edge. + */ + public static double PITCH_PEAK_MIN_RAD = Units.degreesToRadians(5.0); + + /** Use something small like 1.5 degrees */ + public static double PITCH_FLAT_THRESHOLD_RAD = Units.degreesToRadians(1.5); + + // How long in [seconds] our pitch must remain below PITCH_FLAT_THRESHOLD_RAD before the command + // ends. + public static double SETTLE_DEBOUNCE_SECONDS = 0.15; + + public static double APPROACH_TIMEOUT_SECONDS = 4.0; + + // 6.0 seconds is already very generous + public static double TOTAL_TIMEOUT_SECONDS = 8.0; + + // When pitch velocity drops below this value (near zero or negative), + // the robot is at or past the peak. + public static double PITCH_PEAK_VELOCITY_THRESHOLD_RAD_PER_SEC = Units.degreesToRadians(0.5); + } + + // ----------------------------------Begin Here------------------------------// + + private final SwerveSubsystem drive; + private final CrossDirection direction; + + // +1 for TO_NEUTRAL, -1 for TO_ALLIANCE. + private final double driveSign; + + private BumpState state = BumpState.APPROACHING; + + // Peak pitch magnitude observed during the CLIMBING phase (radians). + private double peakPitchRad = 0.0; + + // Timer started when the command begins, used for both timeouts. + private final Timer totalTimer = new Timer(); + + /** Timer started when APPROACHING begins, used for approach timeout. */ + private final Timer approachTimer = new Timer(); + + // I'm sure there are better ways to do this but I have familiairty with debouncers + private final Debouncer flatDebouncer = + new Debouncer(BumpConstants.SETTLE_DEBOUNCE_SECONDS, DebounceType.kRising); + + // Signing of this command is decided in construction time (not part of state machine) + public CrossBumpCommand(SwerveSubsystem drive, CrossDirection direction) { + this.drive = drive; + this.direction = direction; + this.driveSign = (direction == CrossDirection.TO_NEUTRAL) ? 1.0 : -1.0; + addRequirements(drive); + } + + @Override + public void initialize() { + state = BumpState.APPROACHING; + peakPitchRad = 0.0; + + totalTimer.reset(); + totalTimer.start(); + approachTimer.reset(); + approachTimer.start(); + + // Reset debouncer to ensure it starts in the not-debounced state + flatDebouncer.calculate(false); + + Logger.recordOutput("CrossBump/Direction", direction.name()); + Logger.recordOutput("CrossBump/State", state.name()); + } + + // The use of transitionTo() makes the logic much easier to read and ensures consistent logging on + // state changes. + // NOTE: Use this is as a gold standard template + @Override + public void execute() { + double pitch = drive.getPitchPositionRad(); + double pitchRate = drive.getPitchVelocityRadPerSec(); + double absPitch = Math.abs(pitch); + + // total timeout for safety + if (totalTimer.hasElapsed(BumpConstants.TOTAL_TIMEOUT_SECONDS)) { + transitionTo(BumpState.TIMED_OUT); + drive.stop(); + logState(pitch, pitchRate, 0.0); + return; + } + + double speedMs; + + switch (state) { + case APPROACHING: + speedMs = BumpConstants.APPROACH_SPEED_MS; + + // Approach timeout for bump not reached + if (approachTimer.hasElapsed(BumpConstants.APPROACH_TIMEOUT_SECONDS)) { + transitionTo(BumpState.TIMED_OUT); + drive.stop(); + break; + } + + // Bump detected: pitch is rising and exceeds entry threshold. + // Important: Also check that the pitch sign matches the direction of travel: + // Might be better: just check |pitch| > threshold regardless of sign + if (absPitch > BumpConstants.PITCH_ENTRY_THRESHOLD_RAD) { + peakPitchRad = absPitch; // peak tracker + transitionTo(BumpState.CLIMBING); + } + break; + + case CLIMBING: + speedMs = BumpConstants.BUMP_SPEED_MS; + + // Track the peak pitch so we can confirm our crossing was a real bump + if (absPitch > peakPitchRad) { + peakPitchRad = absPitch; + } + + // Apex detection: pitch velocity has crossed zero (going from positive + // to zero/negative means the tilt is decreasing). + // Require peakPitchRad > minimum to filter out noise on flat ground. + boolean peakReached = + pitchRate < BumpConstants.PITCH_PEAK_VELOCITY_THRESHOLD_RAD_PER_SEC + && peakPitchRad > BumpConstants.PITCH_PEAK_MIN_RAD; + + if (peakReached) { + transitionTo(BumpState.CRESTING); + } + break; + + case CRESTING: + speedMs = BumpConstants.BUMP_SPEED_MS; + + // Descent confirmation: pitch has gone negative enough on the other + // side of the bump. + if (pitch < -BumpConstants.PITCH_ENTRY_THRESHOLD_RAD) { + transitionTo(BumpState.DESCENDING); + } + + // Edge case: if the bump detection was very small (robot barely pitched) and + // pitch has already returned near flat, skip straight to LEVELING. + if (absPitch < BumpConstants.PITCH_FLAT_THRESHOLD_RAD + && peakPitchRad > BumpConstants.PITCH_PEAK_MIN_RAD) { + transitionTo(BumpState.LEVELING); + } + break; + + case DESCENDING: + speedMs = BumpConstants.BUMP_SPEED_MS; + + // Rear wheels have cleared the ramp when pitch returns near flat. + // Debouncer prevents early transition due to mid-ramp bounce. + boolean nowFlat = absPitch < BumpConstants.PITCH_FLAT_THRESHOLD_RAD; + if (flatDebouncer.calculate(nowFlat)) { + transitionTo(BumpState.LEVELING); + } + break; + + case LEVELING: + // Slow down slightly to confirm swerve stability before stopping. + speedMs = BumpConstants.SETTLE_SPEED_MS; + + // The debouncer already fired to get us here, so just run for one + // more SETTLE_DEBOUNCE_SECONDS worth of time for a smooth finish. + // Use the flat debouncer again and reset it explicitly to restart the settle timer). + // Simpler way would be to just stop after a tiny delay timer. + // We transition to COMPLETE here and let WPI isFinished() end the command. + transitionTo(BumpState.COMPLETE); + break; + + // Falling cases are intended here, do not make mods without checking with Kaden first + case COMPLETE: + case TIMED_OUT: + default: + drive.stop(); + logState(pitch, pitchRate, 0.0); + return; + } + + // ----------------------------------Swerve Drive Commands------------------------------// + // NOTE: This is assuming the robot should be aligned before calling this command. + // THE USER MUST GET THIS RIGHT + // If the robot is not aligned with the bump, add a heading controller here in a future + // improvement. + double vxFieldRelative = driveSign * speedMs; + + // Was originally going to use robotRelative speeds + ChassisSpeeds fieldRelativeSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + new ChassisSpeeds(vxFieldRelative, 0.0, 0.0), drive.getRotation()); + + drive.runVelocity(fieldRelativeSpeeds); + logState(pitch, pitchRate, vxFieldRelative); + } + + @Override + public boolean isFinished() { + return state == BumpState.COMPLETE || state == BumpState.TIMED_OUT; + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + + if (interrupted) { + Logger.recordOutput("CrossBump/State", "INTERRUPTED"); + } else { + Logger.recordOutput("CrossBump/State", state.name()); + } + + Logger.recordOutput("CrossBump/ElapsedSeconds", totalTimer.get()); + totalTimer.stop(); + approachTimer.stop(); + } + + // ----------------------------------Internal Helper Methods------------------------------// + + private void transitionTo(BumpState next) { + Logger.recordOutput("CrossBump/PreviousState", state.name()); + state = next; + Logger.recordOutput("CrossBump/State", state.name()); + + // Reset the flat debouncer whenever we leave DESCENDING so it starts + // fresh if somehow re-entered. + // If this triggers, honestly, ggs. + if (next == BumpState.CRESTING || next == BumpState.APPROACHING) { + flatDebouncer.calculate(false); + } + } + + /** NEW: Template logging format we should be using from now on */ + private void logState(double pitch, double pitchRate, double vx) { + Logger.recordOutput("CrossBump/State", state.name()); + Logger.recordOutput("CrossBump/PitchRad", pitch); + Logger.recordOutput("CrossBump/PitchDeg", Math.toDegrees(pitch)); + Logger.recordOutput("CrossBump/PitchRateRadPerSec", pitchRate); + Logger.recordOutput("CrossBump/PeakPitchDeg", Math.toDegrees(peakPitchRad)); + Logger.recordOutput("CrossBump/VxFieldRelative", vx); + Logger.recordOutput("CrossBump/ElapsedSeconds", totalTimer.get()); + } + + // ----------------------------------Public AutoLogOutput Methods------------------------------// + + @AutoLogOutput(key = "CrossBump/ActiveState") + public String getStateName() { + return state.name(); + } +} diff --git a/src/main/java/frc/robot/commands/CrossTheBumpCommand.java b/src/main/java/frc/robot/commands/CrossTheBumpCommand.java deleted file mode 100644 index 15a04a5..0000000 --- a/src/main/java/frc/robot/commands/CrossTheBumpCommand.java +++ /dev/null @@ -1,35 +0,0 @@ -// package frc.robot.commands; - -// import edu.wpi.first.math.util.Units; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.swerve.SwerveSubsystem; - -// public class CrossTheBumpCommand extends Command{ -// // Directional Enums -// public enum CrossingDirection { -// TO_NEUTRAL, TO_ALLIANCE -// } - -// // State Machine Tracking Enum -// private enum BumpState { -// APPROACHING, -// CLIMBING, -// CRESTING, -// DESCENDING, -// LEVELING, -// COMPLETED, -// TIMED_OUT -// } - -// public static final class BumpConstants { - -// public static double approachSpeedMs = 1.0; -// public static double climbingSpeedMs = 0.6; -// public static double levelingSpeedMs = 0.4; - -// public static double pitchClimbingThreshold = Units.degreesToRadians(3.0); -// } - -// private final SwerveSubsystem drive; -// private final CrossingDirection direction; -// } diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/GyroIO.java index 4798f5c..8876874 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIO.java @@ -22,6 +22,8 @@ public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = Rotation2d.kZero; public double yawVelocityRadPerSec = 0.0; + public double pitchPositionRad = 0.0; + public double pitchVelocityRadPerSec = 0.0; public double[] odometryYawTimestamps = new double[] {}; public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java index e8797d6..d5ca6dc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java @@ -31,6 +31,8 @@ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2(pigeonCanId); private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal pitch = pigeon.getPitch(); + private final StatusSignal pitchVelocity = pigeon.getAngularVelocityYWorld(); private final PrimitiveDoubleQueue yawPositionQueue; private final PrimitiveDoubleQueue yawTimestampQueue; private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); @@ -42,7 +44,9 @@ public GyroIOPigeon2() { pigeon.getConfigurator().apply(gyroTrimConfigs); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(odometryFrequency); - yawVelocity.setUpdateFrequency(50.0); + pitch.setUpdateFrequency(odometryFrequency); + yawVelocity.setUpdateFrequency(odometryFrequency); + pitchVelocity.setUpdateFrequency(odometryFrequency); pigeon.optimizeBusUtilization(); yawTimestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); @@ -55,9 +59,15 @@ public GyroIOPigeon2() { @Override public void updateInputs(GyroIOInputs inputs) { inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + // Values from [Pigeon 2.0] are in degrees and degrees per second, so convert to radians and + // radians per second + inputs.pitchPositionRad = Units.degreesToRadians(pitch.getValueAsDouble()); // invert if needed + inputs.pitchVelocityRadPerSec = Units.degreesToRadians(pitchVelocity.getValueAsDouble()); + inputs.odometryYawTimestamps = yawTimestampQueue.toArray(); double[] rawYaw = yawPositionQueue.toArray(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index d2e50da..1f70b84 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -350,11 +350,21 @@ public Rotation2d getRotation() { return getPose().getRotation(); } + /** Returns the current pitch position in radians. */ + public double getPitchPositionRad() { + return gyroInputs.pitchPositionRad; + } + /** Returns the current yaw velocity rate */ public double getYawVelocityRate() { return rawGyroYawRate; } + /** Returns the current pitch velocity in radians per second. */ + public double getPitchVelocityRadPerSec() { + return gyroInputs.pitchVelocityRadPerSec; + } + /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); From a9d622ecfcf68b75914cfaebc631ec990afa3b33 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Fri, 10 Apr 2026 07:59:48 -0700 Subject: [PATCH 28/32] 4/10 - Pre-Practice Matches at Day One of CANCMP --- .../pathplanner/autos/[Right] Bump.auto | 19 ++++++++ src/main/deploy/pathplanner/settings.json | 12 +++-- src/main/java/frc/robot/BuildConstants.java | 10 ++--- .../frc/robot/commands/CrossBumpCommand.java | 45 +++++++++++++------ .../intake/roller/RollerConstants.java | 2 +- .../subsystems/swerve/GyroIOPigeon2.java | 11 +++-- .../subsystems/swerve/SwerveConstants.java | 2 +- .../subsystems/swerve/SwerveSubsystem.java | 5 ++- 8 files changed, 78 insertions(+), 28 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/[Right] Bump.auto diff --git a/src/main/deploy/pathplanner/autos/[Right] Bump.auto b/src/main/deploy/pathplanner/autos/[Right] Bump.auto new file mode 100644 index 0000000..723eebf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Right] Bump.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "crossBumpAllianceZone" + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Right] Bump", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index e607171..b3d4b78 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,17 +9,21 @@ "Depot Auto", "Force Warmup", "Misc", - "[Right] Double Swipe", + "[Right] Bump", "Testing", - "[Left] Double Swipe" + "[Left] Bump", + "[Left] Double Swipe", + "[Right] Double Swipe" ], "autoFolders": [ "Citrus Auto", "Default NZ Auto", "Force Warmup", - "[Right] Double Swipe", + "[Right] Bump", "TestingSpartan", - "[Left] Double Swipe" + "[Left] Bump", + "[Left] Double Swipe", + "[Right] Double Swipe" ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c30262c..e238a79 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 147; - public static final String GIT_SHA = "51f45ecbeae242781b913401e97374d51ef248a1"; - public static final String GIT_DATE = "2026-04-08 11:27:51 EDT"; + public static final int GIT_REVISION = 148; + public static final String GIT_SHA = "afc00acc4dd3429ea4119e9e596f526a220ce475"; + public static final String GIT_DATE = "2026-04-08 14:41:24 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-08 14:39:38 EDT"; - public static final long BUILD_UNIX_TIME = 1775673578356L; + public static final String BUILD_DATE = "2026-04-10 10:56:44 EDT"; + public static final long BUILD_UNIX_TIME = 1775833004144L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/commands/CrossBumpCommand.java b/src/main/java/frc/robot/commands/CrossBumpCommand.java index 7559be6..6c0c879 100644 --- a/src/main/java/frc/robot/commands/CrossBumpCommand.java +++ b/src/main/java/frc/robot/commands/CrossBumpCommand.java @@ -4,9 +4,12 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.swerve.SwerveSubsystem; +import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -60,9 +63,9 @@ private enum BumpState { public static final class BumpConstants { // Start at 1.0 m/s/low value and tune up if the robot stalls on the ramp edge. - public static double APPROACH_SPEED_MS = 1.0; + public static double APPROACH_SPEED_MS = 4.0; - public static double BUMP_SPEED_MS = 0.6; + public static double BUMP_SPEED_MS = 5.0; // Speed during the LEVELING phase (all wheels back on flat ground) in m/s. public static double SETTLE_SPEED_MS = 0.4; @@ -104,7 +107,8 @@ public static final class BumpConstants { private final CrossDirection direction; // +1 for TO_NEUTRAL, -1 for TO_ALLIANCE. - private final double driveSign; + private double driveSign; + private double pitchSign; private BumpState state = BumpState.APPROACHING; @@ -125,7 +129,7 @@ public static final class BumpConstants { public CrossBumpCommand(SwerveSubsystem drive, CrossDirection direction) { this.drive = drive; this.direction = direction; - this.driveSign = (direction == CrossDirection.TO_NEUTRAL) ? 1.0 : -1.0; + // Don't add in the drive sign early into construction, FMS not ready addRequirements(drive); } @@ -142,6 +146,17 @@ public void initialize() { // Reset debouncer to ensure it starts in the not-debounced state flatDebouncer.calculate(false); + Optional alliance = DriverStation.getAlliance(); + boolean isRed = alliance.isPresent() && alliance.get() == Alliance.Red; + + double allianceMultiplier = isRed ? -1.0 : 1.0; + double directionMultiplier = (direction == CrossDirection.TO_NEUTRAL) ? 1.0 : -1.0; + + // Pre-Calculate driveSign to prevent double signing logic in run-time + this.driveSign = directionMultiplier * allianceMultiplier; + + Logger.recordOutput("CrossBump/CalculatedDriveSign", driveSign); + Logger.recordOutput("CrossBump/CalculatedPitchSign", pitchSign); Logger.recordOutput("CrossBump/Direction", direction.name()); Logger.recordOutput("CrossBump/State", state.name()); } @@ -151,8 +166,12 @@ public void initialize() { // NOTE: Use this is as a gold standard template @Override public void execute() { - double pitch = drive.getPitchPositionRad(); - double pitchRate = drive.getPitchVelocityRadPerSec(); + // Raw data from SwerveSubsystem + double rawPitch = drive.getPitchPositionRad(); + double rawPitchRate = drive.getPitchVelocityRadPerSec(); + + double pitch = rawPitch * driveSign; + double pitchRate = rawPitchRate * driveSign; double absPitch = Math.abs(pitch); // total timeout for safety @@ -179,7 +198,7 @@ public void execute() { // Bump detected: pitch is rising and exceeds entry threshold. // Important: Also check that the pitch sign matches the direction of travel: // Might be better: just check |pitch| > threshold regardless of sign - if (absPitch > BumpConstants.PITCH_ENTRY_THRESHOLD_RAD) { + if (pitch > BumpConstants.PITCH_ENTRY_THRESHOLD_RAD) { peakPitchRad = absPitch; // peak tracker transitionTo(BumpState.CLIMBING); } @@ -210,16 +229,16 @@ public void execute() { // Descent confirmation: pitch has gone negative enough on the other // side of the bump. - if (pitch < -BumpConstants.PITCH_ENTRY_THRESHOLD_RAD) { + if (absPitch < -BumpConstants.PITCH_ENTRY_THRESHOLD_RAD) { transitionTo(BumpState.DESCENDING); } // Edge case: if the bump detection was very small (robot barely pitched) and // pitch has already returned near flat, skip straight to LEVELING. - if (absPitch < BumpConstants.PITCH_FLAT_THRESHOLD_RAD - && peakPitchRad > BumpConstants.PITCH_PEAK_MIN_RAD) { - transitionTo(BumpState.LEVELING); - } + // if (absPitch < BumpConstants.PITCH_FLAT_THRESHOLD_RAD + // && peakPitchRad > BumpConstants.PITCH_PEAK_MIN_RAD) { + // transitionTo(BumpState.LEVELING); + // } break; case DESCENDING: @@ -308,7 +327,7 @@ private void transitionTo(BumpState next) { /** NEW: Template logging format we should be using from now on */ private void logState(double pitch, double pitchRate, double vx) { Logger.recordOutput("CrossBump/State", state.name()); - Logger.recordOutput("CrossBump/PitchRad", pitch); + Logger.recordOutput("CrossBump/PitchRad", drive.getPitchPositionRad()); Logger.recordOutput("CrossBump/PitchDeg", Math.toDegrees(pitch)); Logger.recordOutput("CrossBump/PitchRateRadPerSec", pitchRate); Logger.recordOutput("CrossBump/PeakPitchDeg", Math.toDegrees(peakPitchRad)); diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java index 3cd387b..74177c4 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java @@ -2,7 +2,7 @@ public class RollerConstants { public static final int sparkMasterRollerCanId = 10; - public static final int masterCurrentLimit = 70; // amps + public static final int masterCurrentLimit = 50; // amps // Temporary Values (Confirm Functionality) public static final double stowVolts = 0.0; diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java index d5ca6dc..a5d200d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java @@ -31,11 +31,11 @@ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2(pigeonCanId); private final StatusSignal yaw = pigeon.getYaw(); - private final StatusSignal pitch = pigeon.getPitch(); - private final StatusSignal pitchVelocity = pigeon.getAngularVelocityYWorld(); + private final StatusSignal pitch = pigeon.getRoll(); private final PrimitiveDoubleQueue yawPositionQueue; private final PrimitiveDoubleQueue yawTimestampQueue; private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final StatusSignal pitchVelocity = pigeon.getAngularVelocityXWorld(); private final GyroTrimConfigs gyroTrimConfigs = new GyroTrimConfigs().withGyroScalarZ(-2.04); public GyroIOPigeon2() { @@ -43,11 +43,15 @@ public GyroIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().apply(gyroTrimConfigs); pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(odometryFrequency); pitch.setUpdateFrequency(odometryFrequency); + yawVelocity.setUpdateFrequency(odometryFrequency); pitchVelocity.setUpdateFrequency(odometryFrequency); + pigeon.optimizeBusUtilization(); + yawTimestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); var yawClone = yaw.clone(); // Status signals are not thread-safe @@ -58,7 +62,8 @@ public GyroIOPigeon2() { @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.connected = + BaseStatusSignal.refreshAll(yaw, yawVelocity, pitch, pitchVelocity).equals(StatusCode.OK); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index dfca39d..ca3bc63 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -69,7 +69,7 @@ public class SwerveConstants { public static final int backRightTurnCanId = 6; // Drive motor configuration (MAXSwerve with 14 pinion teeth and 22 spur teeth) - public static final int driveMotorCurrentLimit = 60; // Vortex 80A, NEO 60A + public static final int driveMotorCurrentLimit = 50; // Vortex 80A, NEO 60A // Remeasure the radius in the Makerspace, Run the Wheel Characterization Auto public static final double wheelRadiusMeters = Units.inchesToMeters(1.498); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 1f70b84..86648ab 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -72,6 +72,7 @@ public class SwerveSubsystem extends FullSubsystem { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); private Rotation2d rawGyroRotation = new Rotation2d(); private double rawGyroYawRate = 0.0; + private double rawGyroPitch = 0.0; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { new SwerveModulePosition(), @@ -210,6 +211,7 @@ public void periodic() { // Use the real gyro angle rawGyroRotation = gyroInputs.odometryYawPositions[i]; rawGyroYawRate = gyroInputs.yawVelocityRadPerSec; + rawGyroPitch = gyroInputs.pitchPositionRad; } else { // Use the angle delta from the kinematics and module deltas Twist2d twist = kinematics.toTwist2d(moduleDeltas); @@ -350,9 +352,10 @@ public Rotation2d getRotation() { return getPose().getRotation(); } + @AutoLogOutput(key = "Odometry/PitchRad") /** Returns the current pitch position in radians. */ public double getPitchPositionRad() { - return gyroInputs.pitchPositionRad; + return rawGyroPitch; } /** Returns the current yaw velocity rate */ From 9091fa7023eca46228c989cc1cc4bbe544b59d5a Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sat, 11 Apr 2026 08:40:16 -0700 Subject: [PATCH 29/32] 4/11 - Pre-Qualification Day 2 of CANCMP, Edit of Autos for Current Limit and Blocking Bump Bots --- .../autos/[Left] Bump Disrupt.auto | 63 +++++++++++++++ .../autos/[Right] Bump Disrupt.auto | 63 +++++++++++++++ .../paths/Left Bump Disrupt Block.path | 79 +++++++++++++++++++ .../paths/Left Bump Disrupt Entry.path | 54 +++++++++++++ .../paths/Right Bump Disrupt Block.path | 75 ++++++++++++++++++ .../paths/Right Bump Disrupt Entry.path | 54 +++++++++++++ src/main/deploy/pathplanner/settings.json | 10 ++- src/main/java/frc/robot/BuildConstants.java | 10 +-- 8 files changed, 400 insertions(+), 8 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto create mode 100644 src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto create mode 100644 src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path create mode 100644 src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path create mode 100644 src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path create mode 100644 src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path diff --git a/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto b/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto new file mode 100644 index 0000000..e776dcf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleWarm" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Left Bump Disrupt Entry" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "pivotUp" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Bump Disrupt Block" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Left] Bump Disrupt", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto b/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto new file mode 100644 index 0000000..65bc66e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleWarm" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bump Disrupt Entry" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "pivotUp" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bump Disrupt Block" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Right] Bump Disrupt", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path new file mode 100644 index 0000000..d7650e5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.264507845935014, + "y": 7.276134094155207 + }, + "prevControl": null, + "nextControl": { + "x": 7.0373344200118195, + "y": 7.290847159455852 + }, + "isLocked": false, + "linkedName": "[Left Bump Disrupt] Entry" + }, + { + "anchor": { + "x": 8.153552068474243, + "y": 6.616262482172326 + }, + "prevControl": { + "x": 8.048357809895414, + "y": 6.928791486051591 + }, + "nextControl": { + "x": 8.245845591456037, + "y": 6.342061178872171 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.153552068474243, + "y": 5.225609814878692 + }, + "prevControl": { + "x": 8.153552068474243, + "y": 6.0411559832069495 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.164179104477597, + "rotationDegrees": -124.509 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -124.509 + }, + "reversed": false, + "folder": "[Left] Bump Disrupt", + "idealStartingState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path new file mode 100644 index 0000000..06ff7f1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.655090117679368, + "y": 7.434229990952901 + }, + "prevControl": null, + "nextControl": { + "x": 4.65509011767937, + "y": 7.434229990952901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.264507845935014, + "y": 7.276134094155207 + }, + "prevControl": { + "x": 5.289926685119949, + "y": 7.453350886478522 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Bump Disrupt] Entry" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "reversed": false, + "folder": "[Left] Bump Disrupt", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path new file mode 100644 index 0000000..f7c381c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.009974856443805, + "y": 0.6719662921348311 + }, + "prevControl": null, + "nextControl": { + "x": 7.02796968517205, + "y": 0.8661313326451534 + }, + "isLocked": false, + "linkedName": "[Right Bump Disrupt] Entry" + }, + { + "anchor": { + "x": 7.840887140551429, + "y": 1.3579805493452808 + }, + "prevControl": { + "x": 7.547923178609416, + "y": 0.8548467886187774 + }, + "nextControl": { + "x": 8.155124140109843, + "y": 1.8976484398912565 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.134630422747337, + "y": 2.9974779383457086 + }, + "prevControl": { + "x": 8.134630422747337, + "y": 1.9974779383457086 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.24647302904564, + "rotationDegrees": 124.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 124.0 + }, + "reversed": false, + "folder": "[Right] Bump Disrupt", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path new file mode 100644 index 0000000..6fa836d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.639534881514021, + "y": 0.6719662921348311 + }, + "prevControl": null, + "nextControl": { + "x": 4.639534881514023, + "y": 0.6719662921348311 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.009974856443805, + "y": 0.6719662921348311 + }, + "prevControl": { + "x": 5.009974856443805, + "y": 0.6719662921348311 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Bump Disrupt] Entry" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "reversed": false, + "folder": "[Right] Bump Disrupt", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index b3d4b78..52b4bd1 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,20 +9,24 @@ "Depot Auto", "Force Warmup", "Misc", - "[Right] Bump", + "[Right] Bump Disrupt", "Testing", "[Left] Bump", + "[Left] Bump Disrupt", "[Left] Double Swipe", + "[Right] Bump", "[Right] Double Swipe" ], "autoFolders": [ "Citrus Auto", "Default NZ Auto", "Force Warmup", - "[Right] Bump", + "[Right] Bump Disrupt", "TestingSpartan", "[Left] Bump", + "[Left] Bump Disrupt", "[Left] Double Swipe", + "[Right] Bump", "[Right] Double Swipe" ], "defaultMaxVel": 3.0, @@ -37,7 +41,7 @@ "driveGearing": 4.71428571429, "maxDriveSpeed": 5.74, "driveMotorType": "vortex", - "driveCurrentLimit": 60.0, + "driveCurrentLimit": 50.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e238a79..8b697eb 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 148; - public static final String GIT_SHA = "afc00acc4dd3429ea4119e9e596f526a220ce475"; - public static final String GIT_DATE = "2026-04-08 14:41:24 EDT"; + public static final int GIT_REVISION = 155; + public static final String GIT_SHA = "a58272399f9b7802e172bf9ca774834b0f785298"; + public static final String GIT_DATE = "2026-04-10 10:59:51 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-10 10:56:44 EDT"; - public static final long BUILD_UNIX_TIME = 1775833004144L; + public static final String BUILD_DATE = "2026-04-11 11:38:59 EDT"; + public static final long BUILD_UNIX_TIME = 1775921939856L; public static final int DIRTY = 1; private BuildConstants() {} From 1f1b86453a772c0cf0701378a0303097f45dcaba Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sat, 11 Apr 2026 17:07:41 -0700 Subject: [PATCH 30/32] 4/11 - Defend Citrus Commit, Max Current Limits to Drive, Theory-Crafted Left Double Swipe Auto Routine --- .../autos/Static Shoot Preload.auto | 12 -- .../autos/[Left] Bump Disrupt.auto | 6 - .../autos/[Left] Double Swipe.auto | 169 +++++++++++++++++- .../autos/[Right] Bump Disrupt.auto | 6 - .../paths/Left Bump Disrupt Block.path | 6 +- .../paths/Left Bump Disrupt Entry.path | 8 +- .../paths/Right Bump Disrupt Block.path | 10 +- .../paths/Right Bump Disrupt Entry.path | 10 +- .../paths/[Left Swipe] Entry 1.path | 6 +- .../paths/[Left Swipe] Intake 1.path | 18 +- .../paths/[Left Swipe] Intake 2.path | 68 +++++++ .../paths/[Left Swipe] Reentry 1.path | 20 +-- .../paths/[Left Swipe] StageShoot1.path | 6 +- .../paths/[Left Swipe] StageShoot2.path | 54 ++++++ .../paths/[Right Swipe] Intake 1.path | 4 +- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 74 ++++---- .../subsystems/swerve/SwerveConstants.java | 4 +- 19 files changed, 376 insertions(+), 117 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/[Left Swipe] Intake 2.path create mode 100644 src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot2.path diff --git a/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto b/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto index ce260be..f1e48c7 100644 --- a/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto +++ b/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto @@ -80,18 +80,6 @@ "data": { "name": "kickerIndex" } - }, - { - "type": "named", - "data": { - "name": "agitatorIndex" - } - }, - { - "type": "named", - "data": { - "name": "pivotShake" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto b/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto index e776dcf..cfe2522 100644 --- a/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto +++ b/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "toggleWarm" - } - }, { "type": "race", "data": { diff --git a/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto index 6d1afef..263fa32 100644 --- a/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto +++ b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto @@ -17,7 +17,7 @@ { "type": "wait", "data": { - "waitTime": 0.75 + "waitTime": 0.5 } }, { @@ -128,7 +128,7 @@ { "type": "named", "data": { - "name": "flywheelLayup" + "name": "flywheelDynamic" } } ] @@ -147,7 +147,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.5 } }, { @@ -184,7 +184,7 @@ { "type": "wait", "data": { - "waitTime": 12.0 + "waitTime": 4.0 } }, { @@ -231,6 +231,167 @@ } ] } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Reentry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Intake 2" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] StageShoot2" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + } + ] + } } ] } diff --git a/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto b/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto index 65bc66e..099198e 100644 --- a/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto +++ b/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "toggleWarm" - } - }, { "type": "race", "data": { diff --git a/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path index d7650e5..34c841d 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path @@ -63,16 +63,16 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { - "velocity": 0, + "velocity": 2.0, "rotation": -124.509 }, "reversed": false, "folder": "[Left] Bump Disrupt", "idealStartingState": { - "velocity": 3.5, + "velocity": 4.0, "rotation": 0.0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path index 06ff7f1..a2f64d5 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path @@ -33,12 +33,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 3.5, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path index f7c381c..1695217 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path @@ -54,12 +54,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 2.0, @@ -68,8 +68,8 @@ "reversed": false, "folder": "[Right] Bump Disrupt", "idealStartingState": { - "velocity": 0, + "velocity": 4.0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path index 6fa836d..65fe0ac 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 5.009974856443805, - "y": 0.6719662921348311 + "y": 0.6719662921348312 }, "nextControl": null, "isLocked": false, @@ -33,12 +33,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, - "unlimited": false + "unlimited": true }, "goalEndState": { "velocity": 3.5, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path index 1ea697e..d8ceeea 100644 --- a/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path @@ -42,8 +42,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -59,5 +59,5 @@ "velocity": 0.0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path index e371d56..ea52a5b 100644 --- a/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path @@ -17,15 +17,15 @@ { "anchor": { "x": 7.933696629212264, - "y": 4.8599776077601495 + "y": 5.062089807762963 }, "prevControl": { "x": 7.969051968270556, - "y": 5.107464981175589 + "y": 5.309577181178402 }, "nextControl": { "x": 7.892932584269662, - "y": 4.574629293153409 + "y": 4.776741493156222 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 6.0426292134831465 }, "prevControl": { - "x": 6.965550561797752, - "y": 5.573842696629214 + "x": 6.985932584269663, + "y": 4.554741573033708 }, "nextControl": null, "isLocked": false, @@ -54,7 +54,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0.04565756823821024, - "maxWaypointRelativePos": 0.5220843672456567, + "maxWaypointRelativePos": 0.4267990074441744, "constraints": { "maxVelocity": 0.75, "maxAcceleration": 3.0, @@ -68,8 +68,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -85,5 +85,5 @@ "velocity": 0, "rotation": -102.265 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 2.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 2.path new file mode 100644 index 0000000..c77db16 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.812685393258427, + "y": 7.061730337078652 + }, + "prevControl": null, + "nextControl": { + "x": 6.802494382022473, + "y": 6.144539325842698 + }, + "isLocked": false, + "linkedName": "[Left Swipe] Reentry 1 to Intake 2" + }, + { + "anchor": { + "x": 5.922158156517307, + "y": 5.567559408383813 + }, + "prevControl": { + "x": 6.5082850713280145, + "y": 5.576717641427731 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Swipe] Intake2 to StageShoot2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.2, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 3.5, + "rotation": -102.265 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path index fe9e0f2..8575490 100644 --- a/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.184685393258427, - "y": 7.367460674157303 + "x": 3.4323011519061213, + "y": 7.333015992865022 }, "isLocked": false, "linkedName": "[Left Swipe] PostShoot1 to Reentry 1" }, { "anchor": { - "x": 7.933696629212264, - "y": 7.03115730337087 + "x": 6.812685393258427, + "y": 7.061730337078652 }, "prevControl": { - "x": 7.148988764044943, - "y": 7.5101348314606735 + "x": 4.871526943777191, + "y": 7.505709086836602 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "rotationDegrees": 0.0 }, { - "waypointRelativePos": 0.3842323651452348, + "waypointRelativePos": 0.6315352697095409, "rotationDegrees": 0.0 } ], @@ -42,8 +42,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -59,5 +59,5 @@ "velocity": 0, "rotation": -66.52706715066263 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path index a62ea14..9ba03d5 100644 --- a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path @@ -84,8 +84,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -101,5 +101,5 @@ "velocity": 2.0, "rotation": 90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot2.path b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot2.path new file mode 100644 index 0000000..73206ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.922158156517307, + "y": 5.567559408383813 + }, + "prevControl": null, + "nextControl": { + "x": 4.560471910112359, + "y": 5.543269662921348 + }, + "isLocked": false, + "linkedName": "[Left Swipe] Intake2 to StageShoot2" + }, + { + "anchor": { + "x": 2.991056179775281, + "y": 5.567559408383813 + }, + "prevControl": { + "x": 4.580853932584269, + "y": 5.553460674157304 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 4.5, + "rotation": 180.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 3.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path index e194d47..74c913c 100644 --- a/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path @@ -36,8 +36,8 @@ "y": 2.0263707865168543 }, "prevControl": { - "x": 6.965550561797752, - "y": 2.495157303370787 + "x": 6.9912097503691495, + "y": 3.3666185534132946 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 52b4bd1..5776a3c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -41,7 +41,7 @@ "driveGearing": 4.71428571429, "maxDriveSpeed": 5.74, "driveMotorType": "vortex", - "driveCurrentLimit": 50.0, + "driveCurrentLimit": 80.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8b697eb..e75bff0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 155; - public static final String GIT_SHA = "a58272399f9b7802e172bf9ca774834b0f785298"; - public static final String GIT_DATE = "2026-04-10 10:59:51 EDT"; + public static final int GIT_REVISION = 156; + public static final String GIT_SHA = "9091fa7023eca46228c989cc1cc4bbe544b59d5a"; + public static final String GIT_DATE = "2026-04-11 11:40:16 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-11 11:38:59 EDT"; - public static final long BUILD_UNIX_TIME = 1775921939856L; + public static final String BUILD_DATE = "2026-04-11 20:05:00 EDT"; + public static final long BUILD_UNIX_TIME = 1775952300169L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index db1b8ea..4a95534 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -330,31 +330,31 @@ private void configureButtonBindings() { () -> -driver.getLeftX(), () -> -driver.getRightX())); - driver - .leftBumper() - .whileTrue(indexerSubsystem.indexCommand()) - .whileTrue(kickerSubsystem.indexCommand()) - .whileTrue(agitatorSubsystem.indexCommand()) - .whileTrue(pivotSubsystem.runSaltAndPepperCommand()) - .whileTrue(rollerSubsystem.intakeCommand()); - - driver - .rightBumper() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - swerveSubsystem, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - () -> shotCalculator.getCorrectTargetRotation())) - .whileTrue( - hoodSubsystem.dynamicUpdatedShootCommandWithLinearCompensation( - () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) - .whileFalse(hoodSubsystem.trenchHoodCommand()); - - driver - .b() - .whileTrue(flywheelSubsystem.juggleCommand()) - .whileTrue(hoodSubsystem.juggleCommand()); + // driver + // .leftBumper() + // .whileTrue(indexerSubsystem.indexCommand()) + // .whileTrue(kickerSubsystem.indexCommand()) + // .whileTrue(agitatorSubsystem.indexCommand()) + // .whileTrue(pivotSubsystem.runSaltAndPepperCommand()) + // .whileTrue(rollerSubsystem.intakeCommand()); + + // driver + // .rightBumper() + // .whileTrue( + // DriveCommands.joystickDriveAtAngle( + // swerveSubsystem, + // () -> -driver.getLeftY(), + // () -> -driver.getLeftX(), + // () -> shotCalculator.getCorrectTargetRotation())) + // .whileTrue( + // hoodSubsystem.dynamicUpdatedShootCommandWithLinearCompensation( + // () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) + // .whileFalse(hoodSubsystem.trenchHoodCommand()); + + // driver + // .b() + // .whileTrue(flywheelSubsystem.juggleCommand()) + // .whileTrue(hoodSubsystem.juggleCommand()); // Reset gyro to 0° when B button is pressed driver @@ -385,21 +385,21 @@ private void configureButtonBindings() { // ------- Operator Controls -------- \\ - operator.leftBumper().onTrue(pivotSubsystem.deployCommand()); - operator.rightBumper().onTrue(pivotSubsystem.stowCommand()); + // operator.leftBumper().onTrue(pivotSubsystem.deployCommand()); + // operator.rightBumper().onTrue(pivotSubsystem.stowCommand()); - operator - .x() - .whileTrue(rollerSubsystem.intakeCommand()) - .whileTrue(agitatorSubsystem.intakeCommand()); + // operator + // .x() + // .whileTrue(rollerSubsystem.intakeCommand()) + // .whileTrue(agitatorSubsystem.intakeCommand()); - operator.b().whileTrue(rollerSubsystem.runUnjamCommand()); + // operator.b().whileTrue(rollerSubsystem.runUnjamCommand()); - operator - .rightTrigger() - .whileTrue( - flywheelSubsystem.dynamicUpdatedShootCommand( - () -> shotCalculator.getCorrectTargetVelocity())); + // operator + // .rightTrigger() + // .whileTrue( + // flywheelSubsystem.dynamicUpdatedShootCommand( + // () -> shotCalculator.getCorrectTargetVelocity())); // Disabled toggleWarm on Operator because of fat-keying during intaking operator.a().onTrue(flywheelSubsystem.toggleWarm()); // Shifted from DPad Left diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index ca3bc63..e29c27d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -69,7 +69,7 @@ public class SwerveConstants { public static final int backRightTurnCanId = 6; // Drive motor configuration (MAXSwerve with 14 pinion teeth and 22 spur teeth) - public static final int driveMotorCurrentLimit = 50; // Vortex 80A, NEO 60A + public static final int driveMotorCurrentLimit = 80; // Vortex 80A, NEO 60A // Remeasure the radius in the Makerspace, Run the Wheel Characterization Auto public static final double wheelRadiusMeters = Units.inchesToMeters(1.498); @@ -97,7 +97,7 @@ public class SwerveConstants { // Turn motor configuration public static final boolean turnInverted = false; // <-- Check if Neccessary - public static final int turnMotorCurrentLimit = 20; // 550 35A, SAFE = 20A + public static final int turnMotorCurrentLimit = 35; // 550 35A, SAFE = 20A public static final double turnMotorReduction = 9424.0 / 203.0; public static final DCMotor turnGearbox = DCMotor.getNeo550(1); From b6c923628e233616407657cfb9223cd528923c30 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Sun, 12 Apr 2026 22:23:41 -0700 Subject: [PATCH 31/32] 4/12 - Post-CANCMP Commit, Double Right Swipes, Reverted Swerve Edits --- .../autos/Static Shoot Preload.auto | 12 ++ .../[Right] Double Swipe Visualizer.auto | 49 +++++ .../autos/[Right] Double Swipe.auto | 201 +++++++++++++++++- .../paths/Right Bump Disrupt Entry.path | 2 +- .../paths/[Left Swipe] StageShoot1.path | 4 +- .../paths/[Right Swipe] Entry 1.path | 6 +- .../paths/[Right Swipe] Intake 1.path | 20 +- .../paths/[Right Swipe] Intake 2.path | 84 ++++++++ .../paths/[Right Swipe] Reentry 1.path | 16 +- .../paths/[Right Swipe] StageShoot1.path | 34 +-- .../paths/[Right Swipe] StageShoot2.path | 75 +++++++ src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 74 +++---- .../subsystems/swerve/SwerveConstants.java | 4 +- 15 files changed, 505 insertions(+), 88 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/[Right] Double Swipe Visualizer.auto create mode 100644 src/main/deploy/pathplanner/paths/[Right Swipe] Intake 2.path create mode 100644 src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot2.path diff --git a/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto b/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto index f1e48c7..ce260be 100644 --- a/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto +++ b/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto @@ -80,6 +80,18 @@ "data": { "name": "kickerIndex" } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/[Right] Double Swipe Visualizer.auto b/src/main/deploy/pathplanner/autos/[Right] Double Swipe Visualizer.auto new file mode 100644 index 0000000..43fcb1f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Right] Double Swipe Visualizer.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Entry 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Intake 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] StageShoot1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Reentry 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Intake 2" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] StageShoot2" + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Right] Double Swipe", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto index 7b608c2..524cd71 100644 --- a/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto +++ b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto @@ -147,7 +147,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.5 } }, { @@ -184,7 +184,7 @@ { "type": "wait", "data": { - "waitTime": 12.0 + "waitTime": 3.25 } }, { @@ -231,6 +231,203 @@ } ] } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Reentry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Intake 2" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] StageShoot2" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.5 + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + } + ] + } } ] } diff --git a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path index 65fe0ac..8020582 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path @@ -41,7 +41,7 @@ "unlimited": true }, "goalEndState": { - "velocity": 3.5, + "velocity": 4.5, "rotation": 0.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path index 9ba03d5..35d4473 100644 --- a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path @@ -69,8 +69,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.11017369727049342, - "maxWaypointRelativePos": 1.759801488833753, + "minWaypointRelativePos": 0.11, + "maxWaypointRelativePos": 1.76, "constraints": { "maxVelocity": 2.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path index 8228c05..091927d 100644 --- a/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path @@ -42,8 +42,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -59,5 +59,5 @@ "velocity": 0.0, "rotation": -0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path index 74c913c..34f2507 100644 --- a/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path @@ -17,15 +17,15 @@ { "anchor": { "x": 7.933696629212264, - "y": 3.2090223922398513 + "y": 2.9975487986925713 }, "prevControl": { "x": 7.969051968270556, - "y": 2.961535018824412 + "y": 2.750061425277132 }, "nextControl": { "x": 7.892932584269662, - "y": 3.494370706846592 + "y": 3.282897113299312 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 2.0263707865168543 }, "prevControl": { - "x": 6.9912097503691495, - "y": 3.3666185534132946 + "x": 6.840959608233403, + "y": 3.3887350357286063 }, "nextControl": null, "isLocked": false, @@ -54,9 +54,9 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0.04565756823821024, - "maxWaypointRelativePos": 0.5220843672456567, + "maxWaypointRelativePos": 0.3712158808933023, "constraints": { - "maxVelocity": 0.75, + "maxVelocity": 1.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -68,8 +68,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -85,5 +85,5 @@ "velocity": 0, "rotation": 102.265 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 2.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 2.path new file mode 100644 index 0000000..fcbfbc9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 2.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.281471910117554, + "y": 1.0694157303404221 + }, + "prevControl": null, + "nextControl": { + "x": 7.3516932933484975, + "y": 2.0031704948881686 + }, + "isLocked": false, + "linkedName": "[Right Swipe] Reentry 1 to Intake 2" + }, + { + "anchor": { + "x": 6.924786516853931, + "y": 3.0260898876404485 + }, + "prevControl": { + "x": 7.639184049069801, + "y": 2.8367736612095276 + }, + "nextControl": { + "x": 6.683127875040819, + "y": 3.090129725530952 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.099314606746768, + "y": 1.8643146067449163 + }, + "prevControl": { + "x": 6.048760951336342, + "y": 3.005068379793869 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Swipe] Intake2 to StageShoot2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.36327543424318565, + "maxWaypointRelativePos": 0.6531017369727078, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 2.0, + "rotation": 102.265 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path index 6a0e0b0..efd2a95 100644 --- a/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.933696629212264, - "y": 1.0378426966291308 + "x": 7.281471910117554, + "y": 1.0694157303404221 }, "prevControl": { - "x": 7.148988764044943, - "y": 0.5588651685393274 + "x": 6.496764044950233, + "y": 0.5904382022506188 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "rotationDegrees": -0.0 }, { - "waypointRelativePos": 0.3842323651452348, + "waypointRelativePos": 0.4373443983402455, "rotationDegrees": -0.0 } ], @@ -42,8 +42,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -59,5 +59,5 @@ "velocity": 0, "rotation": 66.52706715066263 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path index f522478..ef87df6 100644 --- a/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 6.539180929244987, - "y": 0.7015393258426981 + "x": 6.516881888575901, + "y": 0.5990589998344193 }, "prevControl": { - "x": 6.8276652701283815, - "y": 0.752020790397963 + "x": 6.805366229459295, + "y": 0.6495404643896843 }, "nextControl": { - "x": 6.292922834860365, - "y": 0.6584469721119657 + "x": 6.270623794191279, + "y": 0.555966646103687 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.2228876404494375, - "y": 0.7015393258426981 + "x": 5.193517213729849, + "y": 0.5990589998344193 }, "prevControl": { - "x": 6.3729928437130265, - "y": 0.7528833081312518 + "x": 6.344711612736678, + "y": 0.6104449028086686 }, "nextControl": { - "x": 4.081494382022472, - "y": 0.6505842696629226 + "x": 4.943529440613317, + "y": 0.5965864925568396 }, "isLocked": false, "linkedName": null @@ -69,8 +69,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.11017369727049342, - "maxWaypointRelativePos": 1.759801488833753, + "minWaypointRelativePos": 0.2709677419354811, + "maxWaypointRelativePos": 0.7533498759305253, "constraints": { "maxVelocity": 2.0, "maxAcceleration": 3.0, @@ -84,8 +84,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -101,5 +101,5 @@ "velocity": 2.0, "rotation": -90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot2.path b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot2.path new file mode 100644 index 0000000..246a498 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot2.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.099314606746768, + "y": 1.8643146067449163 + }, + "prevControl": null, + "nextControl": { + "x": 6.099864155797287, + "y": 1.2693087830908234 + }, + "isLocked": false, + "linkedName": "[Right Swipe] Intake2 to StageShoot2" + }, + { + "anchor": { + "x": 5.894926982172234, + "y": 0.5588599145239721 + }, + "prevControl": { + "x": 6.243320177334823, + "y": 0.5793536318864778 + }, + "nextControl": { + "x": 5.0219236310432755, + "y": 0.5075067762222673 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.92333796460896, + "y": 0.6749909795781683 + }, + "prevControl": { + "x": 3.7264351950132233, + "y": 0.6813754391366251 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8481327800829823, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 5776a3c..52b4bd1 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -41,7 +41,7 @@ "driveGearing": 4.71428571429, "maxDriveSpeed": 5.74, "driveMotorType": "vortex", - "driveCurrentLimit": 80.0, + "driveCurrentLimit": 50.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e75bff0..780aff6 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 156; - public static final String GIT_SHA = "9091fa7023eca46228c989cc1cc4bbe544b59d5a"; - public static final String GIT_DATE = "2026-04-11 11:40:16 EDT"; + public static final int GIT_REVISION = 157; + public static final String GIT_SHA = "1f1b86453a772c0cf0701378a0303097f45dcaba"; + public static final String GIT_DATE = "2026-04-11 20:07:41 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-11 20:05:00 EDT"; - public static final long BUILD_UNIX_TIME = 1775952300169L; + public static final String BUILD_DATE = "2026-04-13 01:23:22 EDT"; + public static final long BUILD_UNIX_TIME = 1776057802371L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4a95534..db1b8ea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -330,31 +330,31 @@ private void configureButtonBindings() { () -> -driver.getLeftX(), () -> -driver.getRightX())); - // driver - // .leftBumper() - // .whileTrue(indexerSubsystem.indexCommand()) - // .whileTrue(kickerSubsystem.indexCommand()) - // .whileTrue(agitatorSubsystem.indexCommand()) - // .whileTrue(pivotSubsystem.runSaltAndPepperCommand()) - // .whileTrue(rollerSubsystem.intakeCommand()); - - // driver - // .rightBumper() - // .whileTrue( - // DriveCommands.joystickDriveAtAngle( - // swerveSubsystem, - // () -> -driver.getLeftY(), - // () -> -driver.getLeftX(), - // () -> shotCalculator.getCorrectTargetRotation())) - // .whileTrue( - // hoodSubsystem.dynamicUpdatedShootCommandWithLinearCompensation( - // () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) - // .whileFalse(hoodSubsystem.trenchHoodCommand()); - - // driver - // .b() - // .whileTrue(flywheelSubsystem.juggleCommand()) - // .whileTrue(hoodSubsystem.juggleCommand()); + driver + .leftBumper() + .whileTrue(indexerSubsystem.indexCommand()) + .whileTrue(kickerSubsystem.indexCommand()) + .whileTrue(agitatorSubsystem.indexCommand()) + .whileTrue(pivotSubsystem.runSaltAndPepperCommand()) + .whileTrue(rollerSubsystem.intakeCommand()); + + driver + .rightBumper() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + swerveSubsystem, + () -> -driver.getLeftY(), + () -> -driver.getLeftX(), + () -> shotCalculator.getCorrectTargetRotation())) + .whileTrue( + hoodSubsystem.dynamicUpdatedShootCommandWithLinearCompensation( + () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) + .whileFalse(hoodSubsystem.trenchHoodCommand()); + + driver + .b() + .whileTrue(flywheelSubsystem.juggleCommand()) + .whileTrue(hoodSubsystem.juggleCommand()); // Reset gyro to 0° when B button is pressed driver @@ -385,21 +385,21 @@ private void configureButtonBindings() { // ------- Operator Controls -------- \\ - // operator.leftBumper().onTrue(pivotSubsystem.deployCommand()); - // operator.rightBumper().onTrue(pivotSubsystem.stowCommand()); + operator.leftBumper().onTrue(pivotSubsystem.deployCommand()); + operator.rightBumper().onTrue(pivotSubsystem.stowCommand()); - // operator - // .x() - // .whileTrue(rollerSubsystem.intakeCommand()) - // .whileTrue(agitatorSubsystem.intakeCommand()); + operator + .x() + .whileTrue(rollerSubsystem.intakeCommand()) + .whileTrue(agitatorSubsystem.intakeCommand()); - // operator.b().whileTrue(rollerSubsystem.runUnjamCommand()); + operator.b().whileTrue(rollerSubsystem.runUnjamCommand()); - // operator - // .rightTrigger() - // .whileTrue( - // flywheelSubsystem.dynamicUpdatedShootCommand( - // () -> shotCalculator.getCorrectTargetVelocity())); + operator + .rightTrigger() + .whileTrue( + flywheelSubsystem.dynamicUpdatedShootCommand( + () -> shotCalculator.getCorrectTargetVelocity())); // Disabled toggleWarm on Operator because of fat-keying during intaking operator.a().onTrue(flywheelSubsystem.toggleWarm()); // Shifted from DPad Left diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index e29c27d..ca3bc63 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -69,7 +69,7 @@ public class SwerveConstants { public static final int backRightTurnCanId = 6; // Drive motor configuration (MAXSwerve with 14 pinion teeth and 22 spur teeth) - public static final int driveMotorCurrentLimit = 80; // Vortex 80A, NEO 60A + public static final int driveMotorCurrentLimit = 50; // Vortex 80A, NEO 60A // Remeasure the radius in the Makerspace, Run the Wheel Characterization Auto public static final double wheelRadiusMeters = Units.inchesToMeters(1.498); @@ -97,7 +97,7 @@ public class SwerveConstants { // Turn motor configuration public static final boolean turnInverted = false; // <-- Check if Neccessary - public static final int turnMotorCurrentLimit = 35; // 550 35A, SAFE = 20A + public static final int turnMotorCurrentLimit = 20; // 550 35A, SAFE = 20A public static final double turnMotorReduction = 9424.0 / 203.0; public static final DCMotor turnGearbox = DCMotor.getNeo550(1); From e413af7449fdad55ac0e10d898068431ee3887f9 Mon Sep 17 00:00:00 2001 From: Kaden Chow Date: Mon, 13 Apr 2026 20:36:09 -0700 Subject: [PATCH 32/32] 4/13 - Testing Slack's Github Intergration to push all commits --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 780aff6..4fd9673 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 157; - public static final String GIT_SHA = "1f1b86453a772c0cf0701378a0303097f45dcaba"; - public static final String GIT_DATE = "2026-04-11 20:07:41 EDT"; + public static final int GIT_REVISION = 158; + public static final String GIT_SHA = "b6c923628e233616407657cfb9223cd528923c30"; + public static final String GIT_DATE = "2026-04-13 01:23:41 EDT"; public static final String GIT_BRANCH = "test/2026/FullRobotV3"; - public static final String BUILD_DATE = "2026-04-13 01:23:22 EDT"; - public static final long BUILD_UNIX_TIME = 1776057802371L; + public static final String BUILD_DATE = "2026-04-13 23:35:41 EDT"; + public static final long BUILD_UNIX_TIME = 1776137741238L; public static final int DIRTY = 1; private BuildConstants() {}