From 9ab6e79944ef7408ec021dd942b0b30c2436a695 Mon Sep 17 00:00:00 2001 From: Sleepy197 Date: Sun, 15 Mar 2026 14:29:15 -0400 Subject: [PATCH 1/4] Update SwerveDrivetrainConfig.java --- .../frc/robot/configs/SwerveDrivetrainConfig.java | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/main/java/frc/robot/configs/SwerveDrivetrainConfig.java b/src/main/java/frc/robot/configs/SwerveDrivetrainConfig.java index ff657ea..96b4fcc 100644 --- a/src/main/java/frc/robot/configs/SwerveDrivetrainConfig.java +++ b/src/main/java/frc/robot/configs/SwerveDrivetrainConfig.java @@ -1,6 +1,5 @@ package frc.robot.configs; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Translation2d; public class SwerveDrivetrainConfig { @@ -21,12 +20,6 @@ public class SwerveDrivetrainConfig { public double rotationCompensationCoefficient; - //TODO: these are usless constants remove translationControllerKP ... - public double translationControllerKP; - public double translationControllerKI; - public double translationControllerKD; - // ^^ - public double translationToleranceMeters; public double translationVelocityToleranceMeters; @@ -63,11 +56,4 @@ public Translation2d getBackLeftPositionMeters() { public Translation2d getBackRightPositionMeters() { return new Translation2d(backRightX, backRightY); } - - public PIDController getTranslationController() { - //TODO: these are usless constants remove - PIDController controller = new PIDController(translationControllerKP, translationControllerKI, translationControllerKD); - controller.setTolerance(translationToleranceMeters, translationVelocityToleranceMeters); - return controller; - } } From f5002f1b614cef1e6f8c89c2c78add942e5b8b05 Mon Sep 17 00:00:00 2001 From: Sleepy197 Date: Mon, 23 Mar 2026 20:59:48 -0400 Subject: [PATCH 2/4] Add vision sampling guard and precompute blockers Precompute mirrored rectangular blockers in Superstructure to avoid rebuilding geometry every cycle and move latency compensation default initialization into the constructor. Introduce a sampling budget for vision I/O: add MAX_OBSERVATION_SAMPLES_PER_UPDATE and getLatestSampleStartIndex to VisionIO, cap processed samples in VisionIOLimelight and VisionIOPhotonVision, and track/report droppedObservationCount. Refactor detailed-pose logging gating into isDetailedPoseLoggingEnabled helpers and wire up dropped-observation logging in Vision. Add unit tests (VisionPerformanceGuardTest) for the sample-dropping logic and detailed logging behavior. --- .../gradle-8.11-bin.zip.lck | 0 .../gradle-8.11-bin.zip.part | 0 .../frc/robot/subsystems/Superstructure.java | 26 ++++++++--------- .../frc/robot/subsystems/vision/Vision.java | 19 ++++++++++-- .../frc/robot/subsystems/vision/VisionIO.java | 7 +++++ .../subsystems/vision/VisionIOLimelight.java | 24 ++++++++------- .../vision/VisionIOPhotonVision.java | 14 ++++++--- .../vision/VisionPerformanceGuardTest.java | 29 +++++++++++++++++++ 8 files changed, 88 insertions(+), 31 deletions(-) create mode 100644 .gradle-user-home/permwrapper/dists/gradle-8.11-bin/c4te04g51qsyw1bxcb929u7br/gradle-8.11-bin.zip.lck create mode 100644 .gradle-user-home/permwrapper/dists/gradle-8.11-bin/c4te04g51qsyw1bxcb929u7br/gradle-8.11-bin.zip.part create mode 100644 src/test/java/frc/robot/subsystems/vision/VisionPerformanceGuardTest.java diff --git a/.gradle-user-home/permwrapper/dists/gradle-8.11-bin/c4te04g51qsyw1bxcb929u7br/gradle-8.11-bin.zip.lck b/.gradle-user-home/permwrapper/dists/gradle-8.11-bin/c4te04g51qsyw1bxcb929u7br/gradle-8.11-bin.zip.lck new file mode 100644 index 0000000..e69de29 diff --git a/.gradle-user-home/permwrapper/dists/gradle-8.11-bin/c4te04g51qsyw1bxcb929u7br/gradle-8.11-bin.zip.part b/.gradle-user-home/permwrapper/dists/gradle-8.11-bin/c4te04g51qsyw1bxcb929u7br/gradle-8.11-bin.zip.part new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index e6822f6..c2f0a8a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -42,6 +42,14 @@ public class Superstructure extends SubsystemBase { private static final double TARGET_HEIGHT_REACH_EPSILON_METERS = 5e-4; + // Precompute mirrored blockers once so shot calculations do not rebuild geometry every 20 ms. + private static final RectangleZone[] HUB_LINE_OF_SIGHT_BLOCKERS = + buildMirroredRectangularBlockers(ZoneConstants.Tower.EXCLUSION); + private static final RectangleZone[] PASS_LINE_OF_SIGHT_BLOCKERS = + buildMirroredRectangularBlockers( + ZoneConstants.Hub.EXCLUSION, + ZoneConstants.Tower.EXCLUSION + ); private static Superstructure instance; public static Superstructure getInstance() { @@ -235,6 +243,7 @@ record TargetCandidate( private Superstructure() { config = ConfigLoader.load("superstructure", SuperstructureConfig.class); bumpSnapAngle = Rotation2d.fromDegrees(config.bumpSnapAngleDegrees); + latencyCompensationSeconds.setDefault(config.latencyCompensationSeconds); // Set up suppliers for the shooter - these provide dynamic setpoints based on shot calculation shooter.setHoodAngleSupplier(this::getTargetHoodAngle); @@ -245,8 +254,6 @@ private Superstructure() { @Override public void periodic() { - latencyCompensationSeconds.setDefault(config.latencyCompensationSeconds); - // Calculate raw shot data once per cycle, then apply close-shot guard for mechanism setpoints. cachedShotComputationContext = buildShotComputationContext(); mostRecentShotData = calculateShotData(cachedShotComputationContext); @@ -889,23 +896,18 @@ private ShotComputationContext buildShotComputationContext() { boolean passingTarget = zoneResolvedTarget.isPassTarget(); Translation2d shooterPosition = shooterKinematics.shooterPosition().toTranslation2d(); Translation2d targetPosition = targetLocation.toTranslation2d(); - RectangleZone[] hubLineOfSightBlockers = buildMirroredRectangularBlockers(ZoneConstants.Tower.EXCLUSION); - RectangleZone[] passLineOfSightBlockers = buildMirroredRectangularBlockers( - ZoneConstants.Hub.EXCLUSION, - ZoneConstants.Tower.EXCLUSION - ); boolean lineOfSightClear = passingTarget ? hasClearLineOfSightWithRectangularBlockers( shooterPosition, targetPosition, config.passHubBlockerRadiusMeters, - passLineOfSightBlockers + PASS_LINE_OF_SIGHT_BLOCKERS ) : hasClearLineOfSightWithRectangularBlockers( shooterPosition, targetPosition, config.passHubBlockerRadiusMeters, - hubLineOfSightBlockers + HUB_LINE_OF_SIGHT_BLOCKERS ); currentTargetState = zoneResolvedTarget; @@ -1048,10 +1050,6 @@ public Optional getClosestLineOfSightAlliancePassTarget() { robotState.getFieldRelativeSpeeds() ); Translation2d shooterPosition = shooterKinematics.shooterPosition().toTranslation2d(); - RectangleZone[] passLineOfSightBlockers = buildMirroredRectangularBlockers( - ZoneConstants.Hub.EXCLUSION, - ZoneConstants.Tower.EXCLUSION - ); ArrayList candidates = new ArrayList<>(); for (TargetState targetState : TargetState.values()) { @@ -1063,7 +1061,7 @@ public Optional getClosestLineOfSightAlliancePassTarget() { shooterPosition, targetPosition, config.passHubBlockerRadiusMeters, - passLineOfSightBlockers + PASS_LINE_OF_SIGHT_BLOCKERS ); candidates.add(new TargetCandidate(targetState, targetPosition, lineOfSightClear)); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 704af76..8ae0e85 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -28,8 +28,6 @@ public class Vision extends SubsystemBase { private static final double DETAILED_POSE_LOG_PERIOD_SECONDS = 0.1; private static final double IMU_MODE_REASSERT_PERIOD_SECONDS = 1.0; - private static final boolean ENABLE_DETAILED_POSE_LOGGING = true; - // Constants.currentMode == Constants.Mode.REPLAY || Constants.VERBOSE_LOGGING_ENABLED; private static Vision instance = null; private static final VisionConfig config = ConfigLoader.load( @@ -183,7 +181,7 @@ public void periodic() { // Initialize logging values boolean shouldLogDetailedPoseArrays = - ENABLE_DETAILED_POSE_LOGGING + isDetailedPoseLoggingEnabled() && currentTime - lastDetailedPoseLogTimestampSeconds >= DETAILED_POSE_LOG_PERIOD_SECONDS; if (shouldLogDetailedPoseArrays) { lastDetailedPoseLogTimestampSeconds = currentTime; @@ -196,6 +194,7 @@ public void periodic() { int totalRawMegatag1Count = 0; int totalRawMegatag2Count = 0; int totalRawObservationCount = 0; + int totalDroppedObservationCount = 0; // Loop over cameras for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { @@ -221,6 +220,7 @@ public void periodic() { totalRawMegatag1Count += inputs[cameraIndex].rawMegatag1ObservationCount; totalRawMegatag2Count += inputs[cameraIndex].rawMegatag2ObservationCount; totalRawObservationCount += inputs[cameraIndex].rawObservationCount; + totalDroppedObservationCount += inputs[cameraIndex].droppedObservationCount; // Loop over pose observations for (var observation : inputs[cameraIndex].robotPoseObservations) { @@ -330,6 +330,10 @@ public void periodic() { cameraTimingPrefix + "/RawObservationCount", inputs[cameraIndex].rawObservationCount ); + Logger.recordOutput( + cameraTimingPrefix + "/DroppedObservationCount", + inputs[cameraIndex].droppedObservationCount + ); if (shouldLogDetailedPoseArrays) { Logger.recordOutput(cameraTimingPrefix + "/TagPoses", inputs[cameraIndex].tagPoses); Logger.recordOutput( @@ -360,6 +364,15 @@ public void periodic() { Logger.recordOutput("Vision/Summary/RawMegatag1ObservationCount", totalRawMegatag1Count); Logger.recordOutput("Vision/Summary/RawMegatag2ObservationCount", totalRawMegatag2Count); Logger.recordOutput("Vision/Summary/RawObservationCount", totalRawObservationCount); + Logger.recordOutput("Vision/Summary/DroppedObservationCount", totalDroppedObservationCount); + } + + static boolean isDetailedPoseLoggingEnabled(Constants.Mode mode, boolean verboseLoggingEnabled) { + return mode == Constants.Mode.REPLAY || verboseLoggingEnabled; + } + + private static boolean isDetailedPoseLoggingEnabled() { + return isDetailedPoseLoggingEnabled(Constants.currentMode, Constants.VERBOSE_LOGGING_ENABLED); } private void writeImuMode(int imuMode, double currentTime) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index dc78764..e5468a2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -5,6 +5,8 @@ import org.littletonrobotics.junction.AutoLog; public interface VisionIO { + int MAX_OBSERVATION_SAMPLES_PER_UPDATE = 5; + @AutoLog public static class VisionIOInputs { public boolean connected = false; @@ -15,6 +17,7 @@ public static class VisionIOInputs { public int rawMegatag1ObservationCount = 0; public int rawMegatag2ObservationCount = 0; public int rawObservationCount = 0; + public int droppedObservationCount = 0; public TargetObservation latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); } @@ -41,4 +44,8 @@ public default boolean publishRobotOrientation() { } public default void updateInputs(VisionIOInputs inputs) {} + + static int getLatestSampleStartIndex(int availableSampleCount) { + return Math.max(0, availableSampleCount - MAX_OBSERVATION_SAMPLES_PER_UPDATE); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 66bc5a5..62804ea 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -85,32 +85,36 @@ public void updateInputs(VisionIOInputs inputs) { Rotation2d.fromDegrees(txSubscriber.get()), Rotation2d.fromDegrees(tySubscriber.get())); // Read new pose observations from NetworkTables + TimestampedDoubleArray[] megatag1Samples = megatag1Subscriber.readQueue(); + TimestampedDoubleArray[] megatag2Samples = megatag2Subscriber.readQueue(); Set tagIds = new HashSet<>(); - List poseObservations = new ArrayList<>(); - int rawMegatag1ObservationCount = 0; - int rawMegatag2ObservationCount = 0; - for (TimestampedDoubleArray rawSample : megatag1Subscriber.readQueue()) { + List poseObservations = new ArrayList<>( + Math.min(megatag1Samples.length + megatag2Samples.length, MAX_OBSERVATION_SAMPLES_PER_UPDATE * 2)); + int megatag1StartIndex = VisionIO.getLatestSampleStartIndex(megatag1Samples.length); + int megatag2StartIndex = VisionIO.getLatestSampleStartIndex(megatag2Samples.length); + inputs.rawMegatag1ObservationCount = megatag1Samples.length; + inputs.rawMegatag2ObservationCount = megatag2Samples.length; + inputs.rawObservationCount = megatag1Samples.length + megatag2Samples.length; + inputs.droppedObservationCount = megatag1StartIndex + megatag2StartIndex; + for (int sampleIndex = megatag1StartIndex; sampleIndex < megatag1Samples.length; sampleIndex++) { + TimestampedDoubleArray rawSample = megatag1Samples[sampleIndex]; collectTagIds(tagIds, rawSample.value); Optional poseObservation = parsePoseObservation(rawSample, PoseObservationType.MEGATAG_1); if (poseObservation.isPresent()) { poseObservations.add(poseObservation.get()); - rawMegatag1ObservationCount++; } } - for (TimestampedDoubleArray rawSample : megatag2Subscriber.readQueue()) { + for (int sampleIndex = megatag2StartIndex; sampleIndex < megatag2Samples.length; sampleIndex++) { + TimestampedDoubleArray rawSample = megatag2Samples[sampleIndex]; collectTagIds(tagIds, rawSample.value); Optional poseObservation = parsePoseObservation(rawSample, PoseObservationType.MEGATAG_2); if (poseObservation.isPresent()) { poseObservations.add(poseObservation.get()); - rawMegatag2ObservationCount++; } } inputs.robotPoseObservations = poseObservations.toArray(new PoseObservation[0]); - inputs.rawMegatag1ObservationCount = rawMegatag1ObservationCount; - inputs.rawMegatag2ObservationCount = rawMegatag2ObservationCount; - inputs.rawObservationCount = poseObservations.size(); inputs.tagIds = new int[tagIds.size()]; int i = 0; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 99d58cb..91aa5b4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -6,7 +6,6 @@ import java.util.ArrayList; import java.util.HashSet; -import java.util.LinkedList; import java.util.List; import java.util.Optional; import java.util.Set; @@ -34,11 +33,19 @@ public void updateInputs(VisionIOInputs inputs) { inputs.connected = camera.isConnected(); Optional aprilTagLayout = VisionConstants.getAprilTagLayout(); + var unreadResults = camera.getAllUnreadResults(); + int resultStartIndex = VisionIO.getLatestSampleStartIndex(unreadResults.size()); + inputs.rawMegatag1ObservationCount = 0; + inputs.rawMegatag2ObservationCount = 0; + inputs.rawObservationCount = unreadResults.size(); + inputs.droppedObservationCount = resultStartIndex; // Read new camera observations Set tagIds = new HashSet<>(); - List poseObservations = new LinkedList<>(); - for (var result : camera.getAllUnreadResults()) { + List poseObservations = + new ArrayList<>(Math.min(unreadResults.size(), MAX_OBSERVATION_SAMPLES_PER_UPDATE)); + for (int resultIndex = resultStartIndex; resultIndex < unreadResults.size(); resultIndex++) { + var result = unreadResults.get(resultIndex); // Update latest target observation if (result.hasTargets()) { inputs.latestTargetObservation = @@ -113,7 +120,6 @@ public void updateInputs(VisionIOInputs inputs) { for (int i = 0; i < poseObservations.size(); i++) { inputs.robotPoseObservations[i] = poseObservations.get(i); } - inputs.rawObservationCount = poseObservations.size(); // Save tag IDs to inputs objects inputs.tagIds = new int[tagIds.size()]; diff --git a/src/test/java/frc/robot/subsystems/vision/VisionPerformanceGuardTest.java b/src/test/java/frc/robot/subsystems/vision/VisionPerformanceGuardTest.java new file mode 100644 index 0000000..2ae6371 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/vision/VisionPerformanceGuardTest.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.vision; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import frc.robot.constants.Constants; +import org.junit.jupiter.api.Test; + +class VisionPerformanceGuardTest { + @Test + void keepsAllSamplesWhenQueueIsWithinBudget() { + assertEquals(0, VisionIO.getLatestSampleStartIndex(0)); + assertEquals(0, VisionIO.getLatestSampleStartIndex(VisionIO.MAX_OBSERVATION_SAMPLES_PER_UPDATE)); + } + + @Test + void dropsOldestSamplesWhenQueueBacklogExceedsBudget() { + assertEquals(3, VisionIO.getLatestSampleStartIndex(VisionIO.MAX_OBSERVATION_SAMPLES_PER_UPDATE + 3)); + assertEquals(15, VisionIO.getLatestSampleStartIndex(20)); + } + + @Test + void detailedPoseLoggingOnlyRunsInReplayOrVerboseModes() { + assertFalse(Vision.isDetailedPoseLoggingEnabled(Constants.Mode.COMP, false)); + assertTrue(Vision.isDetailedPoseLoggingEnabled(Constants.Mode.REPLAY, false)); + assertTrue(Vision.isDetailedPoseLoggingEnabled(Constants.Mode.COMP, true)); + } +} From 9bc1c76a8197ad20de0286434a069d65214d73f9 Mon Sep 17 00:00:00 2001 From: Sleepy197 Date: Wed, 25 Mar 2026 23:14:26 -0400 Subject: [PATCH 3/4] Integrate Maple simulation for swerve & vision Add a shared Maple simulation backend and wire it into drivetrain, modules, gyro, and vision systems. Introduces MapleSimManager to host a SimulatedArena and SwerveDriveSimulation, a new GyroIOSim, and a simulation-backed ModuleIOSim that uses Maple simulated motor controllers and modules. Robot lifecycle and RobotState.resetPose now synchronize simulation state; Vision is configured to use PhotonVision sim instances tied to MapleSimManager when simulation is enabled. Also add vendordeps metadata for maple-sim and a small update to VisionIOPhotonVisionSim to throttle sim updates. (Gradle cache/daemon files shown in the diff are environment artifacts and not functionally relevant.) --- .../8.11/dependencies-accessors/gc.properties | 0 .../caches/8.11/file-changes/last-build.bin | Bin 0 -> 1 bytes .../caches/8.11/fileContent/fileContent.lock | Bin 0 -> 39 bytes .../caches/8.11/fileHashes/fileHashes.bin | Bin 0 -> 19497 bytes .../caches/8.11/fileHashes/fileHashes.lock | Bin 0 -> 39 bytes .../metadata.bin | 1 + .../metadata/metadata.bin | Bin 0 -> 1 bytes .../metadata.bin | Bin 0 -> 159 bytes .../metadata/metadata.bin | Bin 0 -> 2 bytes .../metadata.bin | 1 + .../metadata/metadata.bin | Bin 0 -> 1 bytes .../caches/8.11/md-rule/md-rule.lock | Bin 0 -> 17 bytes .../caches/8.11/md-supplier/md-supplier.lock | Bin 0 -> 17 bytes .gradle-user-home/caches/CACHEDIR.TAG | 4 + .../settings.lock.lock | Bin 0 -> 2 bytes .../settings.receipt | 0 .../cp_settings.lock.lock | Bin 0 -> 2 bytes .../cp_settings.receipt | 0 .../cp_proj.lock.lock | Bin 0 -> 2 bytes .../cp_proj.receipt | 0 .gradle-user-home/caches/jars-9/jars-9.lock | Bin 0 -> 39 bytes .../caches/journal-1/file-access.bin | Bin 0 -> 18647 bytes .../caches/journal-1/file-access.properties | 2 + .../caches/journal-1/journal-1.lock | Bin 0 -> 39 bytes .../metadata-2.107/module-metadata.bin | Bin 0 -> 18497 bytes .../metadata-2.107/resource-at-url.bin | Bin 0 -> 18497 bytes .../caches/modules-2/modules-2.lock | Bin 0 -> 39 bytes .gradle-user-home/daemon/8.11/registry.bin | Bin 0 -> 437 bytes .../daemon/8.11/registry.bin.lock | Bin 0 -> 17 bytes .gradle-user-home/daemon/CACHEDIR.TAG | 4 + .../native-platform-file-events.dll.lock | 1 + .../windows-amd64/native-platform.dll.lock | 1 + .../8.11/release-features.rendered | 0 simgui-ds.json | 5 + src/main/java/frc/robot/Robot.java | 12 + src/main/java/frc/robot/RobotState.java | 6 + .../constants/vision/VisionConstants.java | 5 - .../java/frc/robot/sim/MapleSimManager.java | 150 +++++++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 3 +- .../subsystems/swerve/gyro/GyroIOSim.java | 28 +++ .../subsystems/swerve/module/ModuleIOSim.java | 206 +++++++++--------- .../frc/robot/subsystems/vision/Vision.java | 9 +- .../vision/VisionIOPhotonVisionSim.java | 9 +- vendordeps/maple-sim-0.4.0-beta.json | 26 +++ 44 files changed, 360 insertions(+), 113 deletions(-) create mode 100644 .gradle-user-home/caches/8.11/dependencies-accessors/gc.properties create mode 100644 .gradle-user-home/caches/8.11/file-changes/last-build.bin create mode 100644 .gradle-user-home/caches/8.11/fileContent/fileContent.lock create mode 100644 .gradle-user-home/caches/8.11/fileHashes/fileHashes.bin create mode 100644 .gradle-user-home/caches/8.11/fileHashes/fileHashes.lock create mode 100644 .gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata.bin create mode 100644 .gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata/metadata.bin create mode 100644 .gradle-user-home/caches/8.11/groovy-dsl/ca920892c4b5e0d312869e8a7707af09/metadata.bin create mode 100644 .gradle-user-home/caches/8.11/groovy-dsl/ca920892c4b5e0d312869e8a7707af09/metadata/metadata.bin create mode 100644 .gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata.bin create mode 100644 .gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata/metadata.bin create mode 100644 .gradle-user-home/caches/8.11/md-rule/md-rule.lock create mode 100644 .gradle-user-home/caches/8.11/md-supplier/md-supplier.lock create mode 100644 .gradle-user-home/caches/CACHEDIR.TAG create mode 100644 .gradle-user-home/caches/jars-9/0a726dd67844590711bc1f8ea66e9286/settings.lock.lock create mode 100644 .gradle-user-home/caches/jars-9/0a726dd67844590711bc1f8ea66e9286/settings.receipt create mode 100644 .gradle-user-home/caches/jars-9/468103ed59db743d56a33d562f8368c4/cp_settings.lock.lock create mode 100644 .gradle-user-home/caches/jars-9/468103ed59db743d56a33d562f8368c4/cp_settings.receipt create mode 100644 .gradle-user-home/caches/jars-9/fa2a3dce8357f878d1a7f11fc39e3bc6/cp_proj.lock.lock create mode 100644 .gradle-user-home/caches/jars-9/fa2a3dce8357f878d1a7f11fc39e3bc6/cp_proj.receipt create mode 100644 .gradle-user-home/caches/jars-9/jars-9.lock create mode 100644 .gradle-user-home/caches/journal-1/file-access.bin create mode 100644 .gradle-user-home/caches/journal-1/file-access.properties create mode 100644 .gradle-user-home/caches/journal-1/journal-1.lock create mode 100644 .gradle-user-home/caches/modules-2/metadata-2.107/module-metadata.bin create mode 100644 .gradle-user-home/caches/modules-2/metadata-2.107/resource-at-url.bin create mode 100644 .gradle-user-home/caches/modules-2/modules-2.lock create mode 100644 .gradle-user-home/daemon/8.11/registry.bin create mode 100644 .gradle-user-home/daemon/8.11/registry.bin.lock create mode 100644 .gradle-user-home/daemon/CACHEDIR.TAG create mode 100644 .gradle-user-home/native/100fb08df4bc3b14c8652ba06237920a3bd2aa13389f12d3474272988ae205f9/windows-amd64/native-platform-file-events.dll.lock create mode 100644 .gradle-user-home/native/c067742578af261105cb4f569cf0c3c89f3d7b1fecec35dd04571415982c5e48/windows-amd64/native-platform.dll.lock create mode 100644 .gradle-user-home/notifications/8.11/release-features.rendered create mode 100644 src/main/java/frc/robot/sim/MapleSimManager.java create mode 100644 src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java create mode 100644 vendordeps/maple-sim-0.4.0-beta.json diff --git a/.gradle-user-home/caches/8.11/dependencies-accessors/gc.properties b/.gradle-user-home/caches/8.11/dependencies-accessors/gc.properties new file mode 100644 index 0000000..e69de29 diff --git a/.gradle-user-home/caches/8.11/file-changes/last-build.bin b/.gradle-user-home/caches/8.11/file-changes/last-build.bin new file mode 100644 index 0000000000000000000000000000000000000000..f76dd238ade08917e6712764a16a22005a50573d GIT binary patch literal 1 IcmZPo000310RR91 literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/8.11/fileContent/fileContent.lock b/.gradle-user-home/caches/8.11/fileContent/fileContent.lock new file mode 100644 index 0000000000000000000000000000000000000000..710272c3050207a92309e8b61fc48db93ccb194e GIT binary patch literal 39 pcmZP;WXztgy<@c`0~9bbFx)nLHupD|!MX5u238|8BU2*=1^~2r2|xe< literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/8.11/fileHashes/fileHashes.bin b/.gradle-user-home/caches/8.11/fileHashes/fileHashes.bin new file mode 100644 index 0000000000000000000000000000000000000000..afdb94917932ce5b9a61c58482c09e61e9a9009f GIT binary patch literal 19497 zcmeI3`%hC>0LL!_wN$ai)lyj;8XXD<*4U;%7Y7@0JbVFyY>Sk}Xd})CBT_9wI~*BU zkZD5{4Jk}~nT^}H19Zrc+v13CmW*Hh{_11Gv%zF0^Xuc=8=bn7;`JB`9 zx#zdulfrQd=_^{ew`J_xCR#86126ysFaQHE00S@p126ysFaQHE00S@p126ysFaQJZ zi-8<|A!4!^#k3E64T+KCl%x2D?0ZdEe5X~rk~~hcv;Pl%ii`*!s@5q<<<8~bUH63jc9-^7v%IrIACPlgvzGQ>$#SpSH2=J8 zNg&Bni&-9YMm4k|xJ_O^c2~Mdo^gTYpXN3%(w1uIx-x7mPcL@2cga;h({WrZpQngAacNWiZ92{% z%NL|gh}V2RdWhzFmS@F?^Ui(P(?RldC(8}tu7>dJ*t0Y@u>9Mqk#QBusEu@-=`7!% zT%6@he%?*;G%d^7o#O%q$TQS100S@p126ysFaQHE00S@p126ysFaQHE00S@p126ys zFaQHE00S@p126ysFaQHE00S@p126ysFaQHE00S@p1Mj+lnfwBx_-mr?lFlhLW=?ye z)Ex<{c^LoKiF*DE^2RBik~XqR>F@kK?!Uy6WQQ_a-jH5-LAP*tmP$_v`t85GKuC1k z2BIx}c}eT9S(cv3-s_-*hs+@<6cV0QwXK^wLcYE|qbU38p}DgOVGJTf@F^j2@PW3j z=vJO}eT&t&^~;wPbPjs5feR24cM}I3Tb^b9GWB0hws=-s93}X>uZVN`%5UXT*YlXn zOzVbmiE9*w0&@!`V&4Fq)OR|;kND`38Xq~Vs{RafUuac+l`Vu4MnZ_RLSoCw Zy1E2cpILXfz4ntG|Jo*Q11_%&kW?;B&X!)&a&Vo{*gAA-jW=5t)3=9DFTnhOB literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata.bin b/.gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata.bin new file mode 100644 index 0000000..419d978 --- /dev/null +++ b/.gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata.bin @@ -0,0 +1 @@ +5cotl4u46bgtxgg5b27ftytpuyX-ʏy+.instrumentedOutput[\Q8)8UmetadataDir)Ce9=*s@propertyUpgradeReportOutputyޜ3&; \ No newline at end of file diff --git a/.gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata/metadata.bin b/.gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata/metadata.bin new file mode 100644 index 0000000000000000000000000000000000000000..f76dd238ade08917e6712764a16a22005a50573d GIT binary patch literal 1 IcmZPo000310RR91 literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/8.11/groovy-dsl/ca920892c4b5e0d312869e8a7707af09/metadata.bin b/.gradle-user-home/caches/8.11/groovy-dsl/ca920892c4b5e0d312869e8a7707af09/metadata.bin new file mode 100644 index 0000000000000000000000000000000000000000..8f7305cad4a46a52516f770fe9b40a257e952b0e GIT binary patch literal 159 zcmbPjnw(#fV^V5jmXuynk)CdvWMrOJQdv?^S}9<@Nz*-e_tO%FjGsmyKi8DjGEdIT zD=sN2%}vcKNlo!D1*$4x6gZG%UbuZ_R8dLHtkemq?WH}rsU?Xii6x0HnMI5Omsj^O z_0Ll_@n`><%Jtz!=bVC~{DRb?lFHD6^rFO+)S%RY{Gt+=S(~;Tt9)>8j`4i8e`ily H_gn=4&qPJd literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/8.11/groovy-dsl/ca920892c4b5e0d312869e8a7707af09/metadata/metadata.bin b/.gradle-user-home/caches/8.11/groovy-dsl/ca920892c4b5e0d312869e8a7707af09/metadata/metadata.bin new file mode 100644 index 0000000000000000000000000000000000000000..bdc955b7b2e610ad5a72302b139a2e6cb325519a GIT binary patch literal 2 JcmZQz1ONa700IC2 literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata.bin b/.gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata.bin new file mode 100644 index 0000000..2c96bf6 --- /dev/null +++ b/.gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata.bin @@ -0,0 +1 @@ +5cotl4u46bgtxgg5b27ftytpuy~G`*:* -instrumentedOutput̑ӣzI)t-j5metadataDir)Ce9=*s@propertyUpgradeReportOutputyޜ3&; \ No newline at end of file diff --git a/.gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata/metadata.bin b/.gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata/metadata.bin new file mode 100644 index 0000000000000000000000000000000000000000..f76dd238ade08917e6712764a16a22005a50573d GIT binary patch literal 1 IcmZPo000310RR91 literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/8.11/md-rule/md-rule.lock b/.gradle-user-home/caches/8.11/md-rule/md-rule.lock new file mode 100644 index 0000000000000000000000000000000000000000..82e1092badd96308dc9dd7a1e6aa4d5ef1be75fc GIT binary patch literal 17 TcmZQB5S(mMdFqim0~7!ND}Do* literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/8.11/md-supplier/md-supplier.lock b/.gradle-user-home/caches/8.11/md-supplier/md-supplier.lock new file mode 100644 index 0000000000000000000000000000000000000000..92e25221325943f217662071ddbb577b7884b164 GIT binary patch literal 17 TcmZSHWc*=YT+UT_1}FdkH=G1j literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/CACHEDIR.TAG b/.gradle-user-home/caches/CACHEDIR.TAG new file mode 100644 index 0000000..c8907d7 --- /dev/null +++ b/.gradle-user-home/caches/CACHEDIR.TAG @@ -0,0 +1,4 @@ +Signature: 8a477f597d28d172789f06886806bc55 +# This file is a cache directory tag created by Gradle. +# For information about cache directory tags, see: +# https://bford.info/cachedir/ \ No newline at end of file diff --git a/.gradle-user-home/caches/jars-9/0a726dd67844590711bc1f8ea66e9286/settings.lock.lock b/.gradle-user-home/caches/jars-9/0a726dd67844590711bc1f8ea66e9286/settings.lock.lock new file mode 100644 index 0000000000000000000000000000000000000000..35a038769b15c0935bb3cd038f5cc1de7579f128 GIT binary patch literal 2 JcmZQ%0000400IC2 literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/jars-9/0a726dd67844590711bc1f8ea66e9286/settings.receipt b/.gradle-user-home/caches/jars-9/0a726dd67844590711bc1f8ea66e9286/settings.receipt new file mode 100644 index 0000000..e69de29 diff --git a/.gradle-user-home/caches/jars-9/468103ed59db743d56a33d562f8368c4/cp_settings.lock.lock b/.gradle-user-home/caches/jars-9/468103ed59db743d56a33d562f8368c4/cp_settings.lock.lock new file mode 100644 index 0000000000000000000000000000000000000000..35a038769b15c0935bb3cd038f5cc1de7579f128 GIT binary patch literal 2 JcmZQ%0000400IC2 literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/jars-9/468103ed59db743d56a33d562f8368c4/cp_settings.receipt b/.gradle-user-home/caches/jars-9/468103ed59db743d56a33d562f8368c4/cp_settings.receipt new file mode 100644 index 0000000..e69de29 diff --git a/.gradle-user-home/caches/jars-9/fa2a3dce8357f878d1a7f11fc39e3bc6/cp_proj.lock.lock b/.gradle-user-home/caches/jars-9/fa2a3dce8357f878d1a7f11fc39e3bc6/cp_proj.lock.lock new file mode 100644 index 0000000000000000000000000000000000000000..35a038769b15c0935bb3cd038f5cc1de7579f128 GIT binary patch literal 2 JcmZQ%0000400IC2 literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/jars-9/fa2a3dce8357f878d1a7f11fc39e3bc6/cp_proj.receipt b/.gradle-user-home/caches/jars-9/fa2a3dce8357f878d1a7f11fc39e3bc6/cp_proj.receipt new file mode 100644 index 0000000..e69de29 diff --git a/.gradle-user-home/caches/jars-9/jars-9.lock b/.gradle-user-home/caches/jars-9/jars-9.lock new file mode 100644 index 0000000000000000000000000000000000000000..5e5dcefc5a03746151b609e94db90dd238952eb8 GIT binary patch literal 39 qcmZQBd)_;J;+4Gb3{b$#z;N4e>*pDPt!}ILF|Zn$8JQX}FaQ7$#tbO{ literal 0 HcmV?d00001 diff --git a/.gradle-user-home/caches/journal-1/file-access.bin b/.gradle-user-home/caches/journal-1/file-access.bin new file mode 100644 index 0000000000000000000000000000000000000000..14ebc4208a8568fbf7f47de042a4a7d1da534f1c GIT binary patch literal 18647 zcmeI(F-ikL7zWUZ1Q8KTVIYl-h-m^w1g!-NOA&0uDhE&@MbJX5Z1ezH1QAW4MVb_L zT3C1m5!4G<AV7cs0RjXF5FkK+ z009C72oNAZfB*pk|3zRsxyWRCnM!SJI83)fs81!s_Y5S>E{D^WW^&_WTBjpmYzF%U$Is6~hvgcOUgcRPuzx4XyPt}!+Ww&D*E z1O+Rx^FP@54{U5LEUdNAyC@FK%ws;@ym0`)aq=4vkGE#4cXj#t{dj-w0K9!=jnoHE ze{%|9nDH8^UbbUjnkDeQn5^=16m$nDG#aO^=a%Es=7DQmgEx)Y=yg+&!PAXb8qh zt5vD9<2o9aL@dCjIKgV6T$C`GwdOaGMKvpLUpQLp}6iQB=34IjFMWWDHmv~Ox&yPHeu|q6f)JZtbV!Z literal 0 HcmV?d00001 diff --git a/.gradle-user-home/daemon/CACHEDIR.TAG b/.gradle-user-home/daemon/CACHEDIR.TAG new file mode 100644 index 0000000..c8907d7 --- /dev/null +++ b/.gradle-user-home/daemon/CACHEDIR.TAG @@ -0,0 +1,4 @@ +Signature: 8a477f597d28d172789f06886806bc55 +# This file is a cache directory tag created by Gradle. +# For information about cache directory tags, see: +# https://bford.info/cachedir/ \ No newline at end of file diff --git a/.gradle-user-home/native/100fb08df4bc3b14c8652ba06237920a3bd2aa13389f12d3474272988ae205f9/windows-amd64/native-platform-file-events.dll.lock b/.gradle-user-home/native/100fb08df4bc3b14c8652ba06237920a3bd2aa13389f12d3474272988ae205f9/windows-amd64/native-platform-file-events.dll.lock new file mode 100644 index 0000000..6b2aaa7 --- /dev/null +++ b/.gradle-user-home/native/100fb08df4bc3b14c8652ba06237920a3bd2aa13389f12d3474272988ae205f9/windows-amd64/native-platform-file-events.dll.lock @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/.gradle-user-home/native/c067742578af261105cb4f569cf0c3c89f3d7b1fecec35dd04571415982c5e48/windows-amd64/native-platform.dll.lock b/.gradle-user-home/native/c067742578af261105cb4f569cf0c3c89f3d7b1fecec35dd04571415982c5e48/windows-amd64/native-platform.dll.lock new file mode 100644 index 0000000..6b2aaa7 --- /dev/null +++ b/.gradle-user-home/native/c067742578af261105cb4f569cf0c3c89f3d7b1fecec35dd04571415982c5e48/windows-amd64/native-platform.dll.lock @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/.gradle-user-home/notifications/8.11/release-features.rendered b/.gradle-user-home/notifications/8.11/release-features.rendered new file mode 100644 index 0000000..e69de29 diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c10b969..f0ca931 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,7 @@ import frc.robot.constants.FieldConstants; import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.util.PhoenixUtil; +import frc.robot.sim.MapleSimManager; /** * The VM is configured to automatically run this class, and to call the @@ -258,10 +259,21 @@ public void testPeriodic() { /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { + if (shouldUseMapleSimulation()) { + MapleSimManager.getInstance().resetFieldForAuto(); + } } /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { + if (shouldUseMapleSimulation()) { + MapleSimManager.getInstance().simulationPeriodic(); + } + } + + private boolean shouldUseMapleSimulation() { + return Constants.shouldUseSimulation(Constants.SimOnlySubsystems.SWERVE) + || Constants.shouldUseSimulation(Constants.SimOnlySubsystems.VISION); } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 502a564..f9c8391 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -15,7 +15,9 @@ import frc.robot.configs.RobotStateConfig; import frc.robot.configs.SwerveConfig; import frc.robot.configs.SwerveDrivetrainConfig; +import frc.robot.constants.Constants; import frc.robot.lib.util.ConfigLoader; +import frc.robot.sim.MapleSimManager; import frc.robot.subsystems.swerve.SwerveDrive; import java.util.NoSuchElementException; @@ -239,6 +241,10 @@ public void addVisionObservation(VisionObservation observation) { */ public void resetPose(Pose2d initialPose) { SwerveDrive.getInstance().resetGyro(initialPose.getRotation()); + if (Constants.shouldUseSimulation(Constants.SimOnlySubsystems.SWERVE) + && Constants.currentMode != Constants.Mode.REPLAY) { + MapleSimManager.getInstance().resetRobotPose(initialPose); + } swerveDrivePoseEstimator.resetPosition(initialPose.getRotation(), lastWheelPositions, initialPose); accumulatedYawRad = initialPose.getRotation().getRadians(); lastWrappedYawRad = initialPose.getRotation().getRadians(); diff --git a/src/main/java/frc/robot/constants/vision/VisionConstants.java b/src/main/java/frc/robot/constants/vision/VisionConstants.java index 9bdce16..df0e424 100644 --- a/src/main/java/frc/robot/constants/vision/VisionConstants.java +++ b/src/main/java/frc/robot/constants/vision/VisionConstants.java @@ -9,7 +9,6 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.constants.Constants; public class VisionConstants { // AprilTag layout @@ -70,10 +69,6 @@ private static AprilTagFieldLayout loadAprilTagLayout() { } public static synchronized Optional getAprilTagLayout() { - if (!Constants.VERBOSE_LOGGING_ENABLED) { - return Optional.empty(); - } - if (aprilTagLayout == null) { aprilTagLayout = loadAprilTagLayout(); } diff --git a/src/main/java/frc/robot/sim/MapleSimManager.java b/src/main/java/frc/robot/sim/MapleSimManager.java new file mode 100644 index 0000000..4c76d03 --- /dev/null +++ b/src/main/java/frc/robot/sim/MapleSimManager.java @@ -0,0 +1,150 @@ +package frc.robot.sim; + +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Kilograms; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.configs.SwerveConfig; +import frc.robot.configs.SwerveDrivetrainConfig; +import frc.robot.configs.SwerveModuleGeneralConfig; +import frc.robot.constants.Constants; +import frc.robot.lib.util.ConfigLoader; + +/** Shared Maple simulation world for drivetrain, odometry, and vision. */ +public final class MapleSimManager { + private static final double ROBOT_MASS_KG = 52.0; + private static final double BUMPER_LENGTH_METERS = 0.86; + private static final double BUMPER_WIDTH_METERS = 0.86; + private static final double DRIVE_FRICTION_VOLTS = 0.1; + private static final double STEER_FRICTION_VOLTS = 0.2; + private static final double STEER_MOI_KG_M2 = 0.03; + + private static MapleSimManager instance; + + public static MapleSimManager getInstance() { + if (instance == null) { + instance = new MapleSimManager(); + } + return instance; + } + + private final SimulatedArena arena; + private final SwerveDriveSimulation driveSimulation; + + private MapleSimManager() { + SwerveConfig swerveConfig = ConfigLoader.load( + "swerve", + ConfigLoader.getModeFolder(Constants.SimOnlySubsystems.SWERVE), + SwerveConfig.class + ); + driveSimulation = new SwerveDriveSimulation( + createDriveTrainSimulationConfig(swerveConfig.drivetrain, swerveConfig.moduleGeneral), + new Pose2d() + ); + + arena = SimulatedArena.getInstance(); + arena.addDriveTrainSimulation(driveSimulation); + arena.resetFieldForAuto(); + } + + private static DriveTrainSimulationConfig createDriveTrainSimulationConfig( + SwerveDrivetrainConfig drivetrainConfig, + SwerveModuleGeneralConfig moduleGeneralConfig + ) { + Translation2d[] moduleTranslations = new Translation2d[] { + drivetrainConfig.getFrontLeftPositionMeters(), + drivetrainConfig.getFrontRightPositionMeters(), + drivetrainConfig.getBackLeftPositionMeters(), + drivetrainConfig.getBackRightPositionMeters() + }; + + SwerveModuleSimulationConfig moduleSimulationConfig = new SwerveModuleSimulationConfig( + DCMotor.getKrakenX60Foc(1), + DCMotor.getKrakenX60Foc(1), + moduleGeneralConfig.driveMotorToOutputShaftRatio, + moduleGeneralConfig.steerMotorToOutputShaftRatio, + Volts.of(DRIVE_FRICTION_VOLTS), + Volts.of(STEER_FRICTION_VOLTS), + Meters.of(moduleGeneralConfig.driveWheelRadiusMeters), + KilogramSquareMeters.of(STEER_MOI_KG_M2), + COTS.WHEELS.DEFAULT_NEOPRENE_TREAD.cof + ); + + return DriveTrainSimulationConfig.Default() + .withRobotMass(Kilograms.of(ROBOT_MASS_KG)) + .withBumperSize(Meters.of(BUMPER_LENGTH_METERS), Meters.of(BUMPER_WIDTH_METERS)) + .withCustomModuleTranslations(moduleTranslations) + .withGyro(COTS.ofPigeon2()) + .withSwerveModule(moduleSimulationConfig); + } + + public SimulatedArena getArena() { + return arena; + } + + public SwerveDriveSimulation getDriveSimulation() { + return driveSimulation; + } + + public SwerveModuleSimulation getModuleSimulation(int moduleIndex) { + return driveSimulation.getModules()[moduleIndex]; + } + + public GyroSimulation getGyroSimulation() { + return driveSimulation.getGyroSimulation(); + } + + public Pose2d getActualPose() { + return driveSimulation.getSimulatedDriveTrainPose(); + } + + public void resetRobotPose(Pose2d pose) { + driveSimulation.setSimulationWorldPose(pose); + driveSimulation.setRobotSpeeds(new ChassisSpeeds()); + driveSimulation.getGyroSimulation().setRotation(pose.getRotation()); + } + + public void resetFieldForAuto() { + arena.resetFieldForAuto(); + } + + public double[] getOdometryTimestampsSeconds() { + int subTicks = SimulatedArena.getSimulationSubTicksIn1Period(); + double dtSeconds = SimulatedArena.getSimulationDt().in(Seconds); + double nowSeconds = Timer.getTimestamp(); + double[] timestampsSeconds = new double[subTicks]; + double periodStartSeconds = nowSeconds - (dtSeconds * subTicks); + + for (int i = 0; i < subTicks; i++) { + timestampsSeconds[i] = periodStartSeconds + (dtSeconds * (i + 1)); + } + + return timestampsSeconds; + } + + public void simulationPeriodic() { + arena.simulationPeriodic(); + Logger.recordOutput("MapleSim/ActualRobotPose", getActualPose()); + Logger.recordOutput( + "MapleSim/ActualRobotSpeedsRobotRelative", + driveSimulation.getDriveTrainSimulatedChassisSpeedsRobotRelative() + ); + Logger.recordOutput("FieldSimulation/FuelPoses", arena.getGamePiecesArrayByType("Fuel")); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index e95ba7a..dfd8256 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -47,6 +47,7 @@ import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.BLine.Path; import frc.robot.subsystems.swerve.gyro.GyroIO; +import frc.robot.subsystems.swerve.gyro.GyroIOSim; import frc.robot.subsystems.swerve.gyro.GyroIOInputsAutoLogged; import frc.robot.subsystems.swerve.gyro.GyroIOPigeon2; import frc.robot.subsystems.swerve.module.ModuleIO; @@ -272,7 +273,7 @@ private SwerveDrive() { new ModuleIOSim(moduleGeneralConfig, 2), new ModuleIOSim(moduleGeneralConfig, 3) }; - gyroIO = new GyroIO() {}; + gyroIO = new GyroIOSim(); } else if (Constants.currentMode == Constants.Mode.REPLAY) { modules = new ModuleIO[] { new ModuleIO() {}, diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java new file mode 100644 index 0000000..f9c4d24 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.swerve.gyro; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import frc.robot.sim.MapleSimManager; + +public class GyroIOSim implements GyroIO { + private final MapleSimManager mapleSim = MapleSimManager.getInstance(); + + @Override + public void updateInputs(GyroIOInputs inputs) { + Rotation2d gyroReading = mapleSim.getGyroSimulation().getGyroReading(); + + inputs.isConnected = true; + inputs.gyroOrientation = new Rotation3d(0.0, 0.0, gyroReading.getRadians()); + inputs.yawVelocityRadPerSec = + mapleSim.getGyroSimulation().getMeasuredAngularVelocity().in(RadiansPerSecond); + inputs.odometryTimestampsSeconds = mapleSim.getOdometryTimestampsSeconds(); + inputs.odometryYawPositions = mapleSim.getGyroSimulation().getCachedGyroReadings().clone(); + } + + @Override + public void resetGyro(Rotation2d yaw) { + mapleSim.getGyroSimulation().setRotation(yaw); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java index 66844e6..6a5e4db 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java @@ -1,163 +1,160 @@ package frc.robot.subsystems.swerve.module; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.configs.SwerveModuleGeneralConfig; import frc.robot.lib.util.DashboardMotorControlLoopConfigurator.MotorControlLoopConfig; +import frc.robot.sim.MapleSimManager; public class ModuleIOSim implements ModuleIO { - private final DCMotor driveMotorModel = DCMotor.getKrakenX60Foc(1); - private final DCMotor turnMotorModel = DCMotor.getKrakenX60Foc(1); - - private final DCMotorSim driveSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(driveMotorModel, (2.8087 * 0.0194 * 0.0485614385) / 6.12, 6.12), // J = (kA_linear * Kt * r) / G - driveMotorModel - ); - private final DCMotorSim steerSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(turnMotorModel, 0.00015, 21.428), // magic number because steer is not important - turnMotorModel - ); + private final MapleSimManager mapleSim = MapleSimManager.getInstance(); + private final SwerveModuleGeneralConfig config; + private final SwerveModuleSimulation moduleSimulation; + private final SimulatedMotorController.GenericMotorController driveMotorController; + private final SimulatedMotorController.GenericMotorController steerMotorController; + private PIDController driveFeedback; + private PIDController steerFeedback; + private SimpleMotorFeedforward driveFeedforward; - private PIDController driveFeedback = new PIDController(0, 0.0, 0.0); - private PIDController steerFeedback = new PIDController(25, 0.0, 0.0); - - private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 2.44, 0.1); - - private boolean isSteerClosedLoop = true; - private boolean isDriveClosedLoop = true; private boolean isDriveEStopped = false; private boolean isSteerEStopped = false; - private SwerveModuleState lastDesiredState = new SwerveModuleState(); - - private double lastTimeInputs = Timer.getTimestamp(); - - private final int moduleID; public ModuleIOSim(SwerveModuleGeneralConfig config, int moduleID) { - this.moduleID = moduleID; + this.config = config; + this.moduleSimulation = mapleSim.getModuleSimulation(moduleID); + this.driveMotorController = moduleSimulation.useGenericMotorControllerForDrive() + .withCurrentLimit(Amps.of(config.driveStatorCurrentLimit)); + this.steerMotorController = moduleSimulation.useGenericControllerForSteer() + .withCurrentLimit(Amps.of(config.steerStatorCurrentLimit)); + + driveFeedback = new PIDController(config.driveKP, config.driveKI, config.driveKD); + steerFeedback = new PIDController(config.steerKP, config.steerKI, config.steerKD); steerFeedback.enableContinuousInput(-Math.PI, Math.PI); + driveFeedforward = new SimpleMotorFeedforward(config.driveKS, config.driveKV, config.driveKA); } @Override public void updateInputs(ModuleIOInputs inputs) { - double dt = Timer.getTimestamp() - lastTimeInputs; - lastTimeInputs = Timer.getTimestamp(); - - if (isDriveEStopped) { - driveSim.setInputVoltage(0); - } else if (isDriveClosedLoop) { - driveSim.setInputVoltage( - MathUtil.clamp( - driveFeedforward.calculate(lastDesiredState.speedMetersPerSecond) + - driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec() * 0.0485614385), // wheel radius in meters - -12, - 12 - ) - ); - } - - if (isSteerEStopped) { - steerSim.setInputVoltage(0); - } else if (isSteerClosedLoop) { - steerSim.setInputVoltage( - MathUtil.clamp( - steerFeedback.calculate(steerSim.getAngularPositionRad()), - -12, - 12 - ) - ); - } - - steerSim.update(dt); - driveSim.update(dt); + double wheelPositionRadians = moduleSimulation.getDriveWheelFinalPosition().in(Radians); + double wheelVelocityRadiansPerSecond = moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond); + Rotation2d steerPosition = moduleSimulation.getSteerAbsoluteFacing(); inputs.driveMotorConnected = true; inputs.steerMotorConnected = true; inputs.steerEncoderConnected = true; - - inputs.drivePositionMeters = driveSim.getAngularPositionRad() * 0.0485614385; // wheel radius in meters - inputs.driveVelocityMetersPerSec = driveSim.getAngularVelocityRadPerSec() * 0.0485614385; - inputs.driveAppliedVolts = driveSim.getInputVoltage(); - inputs.steerPosition = new Rotation2d(steerSim.getAngularPosition()); - inputs.steerVelocityRadPerSec = steerSim.getAngularVelocityRadPerSec(); - inputs.steerAppliedVolts = steerSim.getInputVoltage(); + inputs.drivePositionMeters = wheelPositionRadians * config.driveWheelRadiusMeters; + inputs.driveVelocityMetersPerSec = wheelVelocityRadiansPerSecond * config.driveWheelRadiusMeters; + inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVoltage().in(Volts); - inputs.steerEncoderAbsolutePosition = inputs.steerPosition; - inputs.steerEncoderPosition = inputs.steerPosition; + inputs.steerPosition = steerPosition; + inputs.steerVelocityRadPerSec = moduleSimulation.getSteerAbsoluteEncoderSpeed().in(RadiansPerSecond); + inputs.steerAppliedVolts = moduleSimulation.getSteerMotorAppliedVoltage().in(Volts); - inputs.driveTorqueCurrent = driveSim.getCurrentDrawAmps(); - inputs.steerTorqueCurrent = steerSim.getCurrentDrawAmps(); + inputs.steerEncoderAbsolutePosition = steerPosition; + inputs.steerEncoderPosition = steerPosition; - inputs.odometryTimestampsSeconds = new double[] {Timer.getTimestamp()}; - inputs.odometryDrivePositionsMeters = new double[] {inputs.drivePositionMeters}; - inputs.odometrySteerPositions = new Rotation2d[] {inputs.steerPosition}; + inputs.driveTorqueCurrent = moduleSimulation.getDriveMotorStatorCurrent().in(Amps); + inputs.driveTemperatureFahrenheit = 0.0; + inputs.steerTorqueCurrent = moduleSimulation.getSteerMotorStatorCurrent().in(Amps); + inputs.steerTemperatureFahrenheit = 0.0; + + inputs.odometryTimestampsSeconds = mapleSim.getOdometryTimestampsSeconds(); + inputs.odometryDrivePositionsMeters = convertWheelPositionCacheToMeters(); + inputs.odometrySteerPositions = moduleSimulation.getCachedSteerAbsolutePositions().clone(); + } + + private double[] convertWheelPositionCacheToMeters() { + var cachedWheelPositions = moduleSimulation.getCachedDriveWheelFinalPositions(); + double[] drivePositionsMeters = new double[cachedWheelPositions.length]; + for (int i = 0; i < cachedWheelPositions.length; i++) { + drivePositionsMeters[i] = cachedWheelPositions[i].in(Radians) * config.driveWheelRadiusMeters; + } + return drivePositionsMeters; } @Override public void setState(SwerveModuleState state) { + double compensatedVelocityMetersPerSecond = + state.speedMetersPerSecond * state.angle.minus(moduleSimulation.getSteerAbsoluteFacing()).getCos(); + requestDriveVelocity(compensatedVelocityMetersPerSecond); + requestSteerPosition(state.angle); + } + + @Override + public void setSteerTorqueCurrentFOC(double torqueCurrentFOC, double driveVelocityMetersPerSec) { + requestDriveVelocity(driveVelocityMetersPerSec); + requestSteerVoltage(torqueCurrentFOC); + } + + @Override + public void setDriveTorqueCurrentFOC(double torqueCurrentFOC, Rotation2d steerAngle) { + requestDriveVoltage(torqueCurrentFOC); + requestSteerPosition(steerAngle); + } + + private void requestDriveVelocity(double setpointMetersPerSecond) { if (isDriveEStopped) { - driveSim.setInputVoltage(0); - isDriveClosedLoop = false; - } else { - driveFeedback.setSetpoint(state.speedMetersPerSecond); - isDriveClosedLoop = true; + requestDriveVoltage(0.0); + return; } + double measuredMetersPerSecond = + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond) * config.driveWheelRadiusMeters; + double driveVolts = driveFeedforward.calculate(setpointMetersPerSecond) + + driveFeedback.calculate(measuredMetersPerSecond, setpointMetersPerSecond); + requestDriveVoltage(driveVolts); + } + + private void requestSteerPosition(Rotation2d setpoint) { if (isSteerEStopped) { - steerSim.setInputVoltage(0); - isSteerClosedLoop = false; - } else { - steerFeedback.setSetpoint(state.angle.getRadians()); - isSteerClosedLoop = true; + requestSteerVoltage(0.0); + return; } - lastDesiredState = state; + double steerVolts = steerFeedback.calculate( + moduleSimulation.getSteerAbsoluteFacing().getRadians(), + setpoint.getRadians() + ); + requestSteerVoltage(steerVolts); } - @Override - public void setSteerTorqueCurrentFOC(double torqueCurrentFOC, double driveVelocityMetersPerSec) { - // In sim, treat torqueCurrentFOC as voltage for simplicity - steerSim.setInputVoltage(isSteerEStopped ? 0 : torqueCurrentFOC); - - isDriveClosedLoop = false; - isSteerClosedLoop = !isSteerEStopped; + private void requestDriveVoltage(double volts) { + driveMotorController.requestVoltage(Volts.of(MathUtil.clamp(volts, -12.0, 12.0))); } - @Override - public void setDriveTorqueCurrentFOC(double torqueCurrentFOC, Rotation2d steerAngle) { - // In sim, treat torqueCurrentFOC as voltage for simplicity - driveSim.setInputVoltage(isDriveEStopped ? 0 : torqueCurrentFOC); - - isDriveClosedLoop = !isDriveEStopped; - isSteerClosedLoop = true; + private void requestSteerVoltage(double volts) { + steerMotorController.requestVoltage(Volts.of(MathUtil.clamp(volts, -12.0, 12.0))); } @Override public void configureDriveControlLoop(MotorControlLoopConfig config) { - // No-op in sim + driveFeedback = new PIDController(config.kP(), config.kI(), config.kD()); + driveFeedforward = new SimpleMotorFeedforward(config.kS(), config.kV(), config.kA()); } @Override public void configureSteerControlLoop(MotorControlLoopConfig config) { - // No-op in sim + steerFeedback = new PIDController(config.kP(), config.kI(), config.kD()); + steerFeedback.enableContinuousInput(-Math.PI, Math.PI); } @Override public void enableDriveEStop() { isDriveEStopped = true; - driveSim.setInputVoltage(0); - isDriveClosedLoop = false; + requestDriveVoltage(0.0); } @Override @@ -168,8 +165,7 @@ public void disableDriveEStop() { @Override public void enableSteerEStop() { isSteerEStopped = true; - steerSim.setInputVoltage(0); - isSteerClosedLoop = false; + requestSteerVoltage(0.0); } @Override diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 8ae0e85..8af42de 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -16,6 +16,7 @@ import frc.robot.constants.Constants; import frc.robot.lib.util.ConfigLoader; import frc.robot.lib.util.LimelightHelpers; +import frc.robot.sim.MapleSimManager; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.ArrayList; @@ -45,7 +46,13 @@ public static Vision getInstance() { config.cameras != null ? config.cameras : List.of(); if (useSimulation) { for (VisionConfig.CameraConfig camera : cameraConfigs) { - ioList.add(new VisionIO() {}); + ioList.add( + new VisionIOPhotonVisionSim( + camera.name, + camera.getRobotToCamera(), + MapleSimManager.getInstance()::getActualPose + ) + ); } } else { for (VisionConfig.CameraConfig camera : cameraConfigs) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index ae1a07e..fa2ef96 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import org.photonvision.simulation.PhotonCameraSim; @@ -11,7 +12,9 @@ /** IO implementation for physics sim using PhotonVision simulator. */ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { + private static final double SIM_UPDATE_GUARD_SECONDS = 0.01; private static VisionSystemSim visionSim; + private static double lastVisionSimUpdateTimestampSeconds = Double.NEGATIVE_INFINITY; private final Supplier poseSupplier; private final PhotonCameraSim cameraSim; @@ -44,7 +47,11 @@ public VisionIOPhotonVisionSim( @Override public void updateInputs(VisionIOInputs inputs) { - visionSim.update(poseSupplier.get()); + double timestampSeconds = Timer.getTimestamp(); + if (timestampSeconds - lastVisionSimUpdateTimestampSeconds > SIM_UPDATE_GUARD_SECONDS) { + visionSim.update(poseSupplier.get()); + lastVisionSimUpdateTimestampSeconds = timestampSeconds; + } super.updateInputs(inputs); } } diff --git a/vendordeps/maple-sim-0.4.0-beta.json b/vendordeps/maple-sim-0.4.0-beta.json new file mode 100644 index 0000000..d9bf4ab --- /dev/null +++ b/vendordeps/maple-sim-0.4.0-beta.json @@ -0,0 +1,26 @@ +{ + "fileName": "maple-sim-0.4.0-beta.json", + "name": "maplesim", + "version": "0.4.0-beta", + "frcYear": "2026", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.4.0-beta" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file From 023c68f431c0fab688907465f9f48c9c2f1bff3d Mon Sep 17 00:00:00 2001 From: Sleepy197 Date: Tue, 31 Mar 2026 16:25:36 -0400 Subject: [PATCH 4/4] Sim: add MapleSim projectiles and odometry guards Add MapleSim integrations and robustness fixes across simulation and drivetrain code. - Add new autonomous path file: Middle_to_depot.json. - RobotState: defend against non-finite odometry/vision inputs to avoid corrupting pose buffer and logging rejection correctly. - VisualizeShot: launch a RebuiltFuelOnFly projectile into MapleSim for physics-based visualization and register hit/trajectory callbacks. - MapleSimManager: track hub hit count, expose logger output, and print counts for projectile hits. - IntakeIOSim: integrate IntakeSimulation to spawn/consume MapleSim game pieces, control intake start/stop logic, and expose simulation metrics. - Shooter: guard against non-finite angle/velocity inputs in setters to prevent invalid actuator commands. - SwerveDrive: add filtered odometry buffers, rejection of tilted odometry samples (based on max tilt config), continuity handling for wheel travel while tilted, helper routines for odometry updates, and additional logging of filtered states. These changes improve simulation fidelity (game pieces and scoring) and harden the robot code against bad sensor inputs and tilt-induced odometry errors. --- .../deploy/autos/paths/Middle_to_depot.json | 115 +++++++++++++++++ src/main/java/frc/robot/RobotState.java | 25 ++-- src/main/java/frc/robot/VisualizeShot.java | 109 +++++++++++++--- .../java/frc/robot/sim/MapleSimManager.java | 10 ++ .../robot/subsystems/intake/IntakeIOSim.java | 73 +++++++++++ .../frc/robot/subsystems/shooter/Shooter.java | 16 +++ .../robot/subsystems/swerve/SwerveDrive.java | 116 ++++++++++++++---- 7 files changed, 413 insertions(+), 51 deletions(-) create mode 100644 src/main/deploy/autos/paths/Middle_to_depot.json diff --git a/src/main/deploy/autos/paths/Middle_to_depot.json b/src/main/deploy/autos/paths/Middle_to_depot.json new file mode 100644 index 0000000..0005cd2 --- /dev/null +++ b/src/main/deploy/autos/paths/Middle_to_depot.json @@ -0,0 +1,115 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 13.361426533523543, + "y_meters": 2.6083523537803153, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": 3.141592653589793, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "intake" + }, + { + "type": "rotation", + "rotation_radians": 0.0, + "t_ratio": 0.46635262064706745, + "profiled_rotation": true + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 15.087902995720405, + "y_meters": 2.4117459175884877, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "deploy" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 15.829598984635563, + "y_meters": 2.399, + "intermediate_handoff_radius_meters": 0.1 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 14.757243618343884, + "y_meters": 1.7609543509272472, + "intermediate_handoff_radius_meters": 0.25 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 15.83, + "y_meters": 1.6514122681883032, + "intermediate_handoff_radius_meters": 0.1 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 14.319480741797435, + "y_meters": 2.5342906939501617, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 1.65, + "start_ordinal": 0, + "end_ordinal": 1 + }, + { + "value": 0.75, + "start_ordinal": 2, + "end_ordinal": 2 + }, + { + "value": 1.35, + "start_ordinal": 3, + "end_ordinal": 3 + }, + { + "value": 0.75, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 1.25, + "start_ordinal": 5, + "end_ordinal": 5 + } + ] + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index f9c8391..6a76dcb 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -20,7 +20,6 @@ import frc.robot.sim.MapleSimManager; import frc.robot.subsystems.swerve.SwerveDrive; -import java.util.NoSuchElementException; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -118,6 +117,10 @@ private RobotState() { /** Add odometry observation */ public void addOdometryObservation(OdometryObservation observation) { + if (!Double.isFinite(observation.timestampsSeconds()) || !Double.isFinite(observation.yawVelocityRadPerSec())) { + return; + } + observation = new OdometryObservation( observation.timestampsSeconds(), observation.isGyroConnected(), @@ -190,19 +193,17 @@ public void addOdometryObservation(OdometryObservation observation) { } public void addVisionObservation(VisionObservation observation) { + if (!Double.isFinite(observation.timestampSeconds()) + || !Double.isFinite(observation.visionRobotPoseMeters().getX()) + || !Double.isFinite(observation.visionRobotPoseMeters().getY())) { + return; + } + boolean shouldLogObservation = shouldLogVisionObservation(); // If measurement is old enough to be outside the pose buffer's timespan, skip. - try { - if (poseBuffer.getInternalBuffer().lastKey() - robotStateConfig.poseBufferSizeSeconds > observation.timestampSeconds()) { - incrementVisionRejectedCount(); - if (shouldLogObservation) { - logVisionObservationMetrics(null, observation.timestampSeconds(), false); - } - return; - } - } - - catch (NoSuchElementException ex) { + var internalBuffer = poseBuffer.getInternalBuffer(); + if (internalBuffer.isEmpty() + || internalBuffer.lastKey() - robotStateConfig.poseBufferSizeSeconds > observation.timestampSeconds()) { incrementVisionRejectedCount(); if (shouldLogObservation) { logVisionObservationMetrics(null, observation.timestampSeconds(), false); diff --git a/src/main/java/frc/robot/VisualizeShot.java b/src/main/java/frc/robot/VisualizeShot.java index 8105650..f1923fb 100644 --- a/src/main/java/frc/robot/VisualizeShot.java +++ b/src/main/java/frc/robot/VisualizeShot.java @@ -1,5 +1,11 @@ package frc.robot; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose3d; @@ -12,6 +18,7 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; import frc.robot.lib.util.ballistics.ProjectileVisualizer; +import frc.robot.sim.MapleSimManager; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.shooter.Shooter; @@ -28,17 +35,17 @@ private void visualize(double launchExitVelocityMetersPerSec) { if (Constants.currentMode != Constants.Mode.SIM) { return; } - + // Convert robot pose from field-relative Pose2d to Pose3d Pose3d robotPose3d = new Pose3d(RobotState.getInstance().getEstimatedPose()); Rotation2d robotHeading = RobotState.getInstance().getEstimatedPose().getRotation(); ChassisSpeeds fieldRelativeSpeeds = RobotState.getInstance().getFieldRelativeSpeeds(); - + // Transform shooter relative pose to field coordinates Pose3d shooterPose = robotPose3d.plus( - new Transform3d(new Pose3d(), Shooter.getInstance().getShooterRelativePose()) - ); - Translation2d shooterOffsetFromRobotCenter = Shooter.getInstance().getShooterRelativePose().getTranslation().toTranslation2d(); + new Transform3d(new Pose3d(), Shooter.getInstance().getShooterRelativePose())); + Translation2d shooterOffsetFromRobotCenter = Shooter.getInstance().getShooterRelativePose().getTranslation() + .toTranslation2d(); // Apply hood angle and turret rotation as local rotations // This needs to be done by creating a proper rotation that combines: @@ -48,21 +55,22 @@ private void visualize(double launchExitVelocityMetersPerSec) { Rotation3d currentRotation = shooterPose.getRotation(); double hoodAngleRadians = Shooter.getInstance().getHoodAngleRotations() * (2 * Math.PI); double turretAngleRadians = Shooter.getInstance().getTurretAngleRotations() * (2 * Math.PI); - - // Create new rotation: keep roll (X) at 0, add pitch (Y) from hood, combine yaw (Z) from robot + turret + + // Create new rotation: keep roll (X) at 0, add pitch (Y) from hood, combine yaw + // (Z) from robot + turret Rotation3d newRotation = new Rotation3d( - 0, // roll (X) - hoodAngleRadians, // pitch (Y) from hood - currentRotation.getZ() + turretAngleRadians // yaw (Z) from robot rotation + turret rotation + 0, // roll (X) + hoodAngleRadians, // pitch (Y) from hood + currentRotation.getZ() + turretAngleRadians // yaw (Z) from robot rotation + turret rotation ); - + shooterPose = new Pose3d(shooterPose.getTranslation(), newRotation); double flywheelRPS = Shooter.getInstance().getFlywheelVelocityRotationsPerSec(); double exitVelocity = launchExitVelocityMetersPerSec; Superstructure superstructure = Superstructure.getInstance(); Translation3d targetLocation = superstructure.getCurrentFieldTargetLocation(); - + // Include tangential velocity from robot rotation (omega cross r) double omega = fieldRelativeSpeeds.omegaRadiansPerSecond; double dx = shooterOffsetFromRobotCenter.getX(); @@ -99,11 +107,76 @@ private void visualize(double launchExitVelocityMetersPerSec) { } ProjectileVisualizer.addProjectile( - shooterVxField, - shooterVyField, - exitVelocity, - shooterPose, - targetLocation.getZ() - ); + shooterVxField, + shooterVyField, + exitVelocity, + shooterPose, + targetLocation.getZ()); + + // Launch a MapleSim RebuiltFuelOnFly projectile for physics-based simulation + launchMapleSimProjectile(shooterPose, exitVelocity, hoodAngleRadians, turretAngleRadians, fieldRelativeSpeeds); + } + + /** + * Creates and launches a RebuiltFuelOnFly projectile via MapleSim's + * SimulatedArena. + * This provides maple-sim's built-in projectile physics with hub target + * detection. + */ + private void launchMapleSimProjectile( + Pose3d shooterPose, + double exitVelocityMps, + double hoodAngleRadians, + double turretAngleRadians, + ChassisSpeeds fieldRelativeSpeeds) { + + MapleSimManager mapleSimManager = MapleSimManager.getInstance(); + + // Get the actual robot pose from the MapleSim drive simulation for consistency + var driveSimulation = mapleSimManager.getDriveSimulation(); + + // The shooter offset from robot center (in robot-relative frame) + Translation2d shooterOffset = Shooter.getInstance() + .getShooterRelativePose() + .getTranslation() + .toTranslation2d(); + + // The facing direction combines robot heading + turret rotation + Rotation2d launchDirection = driveSimulation.getSimulatedDriveTrainPose().getRotation() + .plus(Rotation2d.fromRadians(turretAngleRadians)); + + // Initial height of the projectile (Z of the shooter pose) + double initialHeightMeters = shooterPose.getZ(); + + // Hood angle is the pitch (elevation) angle of the launch + double launchAngleDegrees = Math.toDegrees(hoodAngleRadians); + + // Create the projectile + RebuiltFuelOnFly fuelOnFly = new RebuiltFuelOnFly( + driveSimulation.getSimulatedDriveTrainPose().getTranslation(), + shooterOffset, + driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(), + launchDirection, + Meters.of(initialHeightMeters), + MetersPerSecond.of(exitVelocityMps), + Degrees.of(launchAngleDegrees)); + + // Configure trajectory visualization via AdvantageScope + fuelOnFly.withProjectileTrajectoryDisplayCallBack( + (poses) -> Logger.recordOutput( + "MapleSim/FuelProjectileSuccessful", + poses.toArray(Pose3d[]::new)), + (poses) -> Logger.recordOutput( + "MapleSim/FuelProjectileMissed", + poses.toArray(Pose3d[]::new))); + + // Register callback for when this projectile hits the Hub target + fuelOnFly.withHitTargetCallBack(() -> mapleSimManager.incrementHubHitCount()); + + // Enable the projectile to become a field game piece on touchdown + fuelOnFly.enableBecomesGamePieceOnFieldAfterTouchGround(); + + // Add the projectile to the simulated arena + SimulatedArena.getInstance().addGamePieceProjectile(fuelOnFly); } } diff --git a/src/main/java/frc/robot/sim/MapleSimManager.java b/src/main/java/frc/robot/sim/MapleSimManager.java index 4c76d03..fab5f76 100644 --- a/src/main/java/frc/robot/sim/MapleSimManager.java +++ b/src/main/java/frc/robot/sim/MapleSimManager.java @@ -35,6 +35,8 @@ public final class MapleSimManager { private static final double STEER_FRICTION_VOLTS = 0.2; private static final double STEER_MOI_KG_M2 = 0.03; + private int hubHitCount = 0; + private static MapleSimManager instance; public static MapleSimManager getInstance() { @@ -124,6 +126,13 @@ public void resetFieldForAuto() { arena.resetFieldForAuto(); } + /** Called by projectile hit callbacks to track Hub scoring. */ + public void incrementHubHitCount() { + hubHitCount++; + Logger.recordOutput("MapleSim/HubHitCount", hubHitCount); + System.out.println("[MapleSim] FUEL hits HUB! Total: " + hubHitCount); + } + public double[] getOdometryTimestampsSeconds() { int subTicks = SimulatedArena.getSimulationSubTicksIn1Period(); double dtSeconds = SimulatedArena.getSimulationDt().in(Seconds); @@ -146,5 +155,6 @@ public void simulationPeriodic() { driveSimulation.getDriveTrainSimulatedChassisSpeedsRobotRelative() ); Logger.recordOutput("FieldSimulation/FuelPoses", arena.getGamePiecesArrayByType("Fuel")); + Logger.recordOutput("MapleSim/HubHitCount", hubHitCount); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 15f4633..6f187c5 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -1,5 +1,10 @@ package frc.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Meters; + +import org.ironmaple.simulation.IntakeSimulation; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -11,8 +16,17 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.configs.IntakeConfig; import frc.robot.lib.util.DashboardMotorControlLoopConfigurator.MotorControlLoopConfig; +import frc.robot.sim.MapleSimManager; public class IntakeIOSim implements IntakeIO { + private static final String MAPLE_GAME_PIECE_TYPE = "Fuel"; + private static final double MAPLE_INTAKE_WIDTH_METERS = 0.70; + private static final double MAPLE_INTAKE_EXTENSION_METERS = 0.20; + private static final int MAPLE_INTAKE_CAPACITY = 1; + private static final double MIN_INTAKE_SPEED_RPS = 1.0; + private static final double MIN_INTAKE_VOLTAGE = 0.1; + private static final double DEPLOYED_PIVOT_MARGIN_ROTATIONS = 0.01; + private final DCMotor rollerMotorModel = DCMotor.getKrakenX60Foc(1); private final DCMotor pivotMotorModel = DCMotor.getKrakenX60Foc(1); @@ -37,9 +51,12 @@ public class IntakeIOSim implements IntakeIO { private double desiredRollerVelocityRotationsPerSec = 0; private boolean isRollerEStopped = false; private boolean isPivotEStopped = false; + private double commandedRollerVoltage = 0.0; + private boolean mapleIntakeRunning = false; private double lastTimeInputs = Timer.getTimestamp(); private final IntakeConfig config; + private final IntakeSimulation intakeSimulation; public IntakeIOSim(IntakeConfig config) { this.config = config; @@ -56,6 +73,15 @@ public IntakeIOSim(IntakeConfig config) { ); pivotSim.setState(config.pivotStartingAngleRotations * 2 * Math.PI, 0); + intakeSimulation = IntakeSimulation.OverTheBumperIntake( + MAPLE_GAME_PIECE_TYPE, + MapleSimManager.getInstance().getDriveSimulation(), + Meters.of(MAPLE_INTAKE_WIDTH_METERS), + Meters.of(MAPLE_INTAKE_EXTENSION_METERS), + IntakeSimulation.IntakeSide.FRONT, + MAPLE_INTAKE_CAPACITY + ); + intakeSimulation.stopIntake(); } @Override @@ -90,6 +116,7 @@ public void updateInputs(IntakeIOInputs inputs) { rollerSim.update(dt); pivotSim.update(dt); + updateMapleIntakeSimulation(); inputs.rollerMotorConnected = true; inputs.rollerVelocityRotationsPerSec = rollerSim.getAngularVelocityRadPerSec() / (2 * Math.PI); @@ -103,6 +130,9 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.pivotAppliedVolts = pivotSim.getInputVoltage(); inputs.pivotTorqueCurrent = pivotSim.getCurrentDrawAmps(); inputs.pivotTemperatureFahrenheit = 70.0; + + Logger.recordOutput("Intake/Sim/MapleIntakeRunning", mapleIntakeRunning); + Logger.recordOutput("Intake/Sim/MapleGamePiecesInIntake", intakeSimulation.getGamePiecesAmount()); } @Override @@ -119,6 +149,7 @@ public void setVelocity(double velocityRotationsPerSec) { @Override public void setVoltage(double voltage) { + commandedRollerVoltage = voltage; rollerSim.setInputVoltage(isRollerEStopped ? 0 : voltage); isRollerClosedLoop = false; } @@ -158,6 +189,7 @@ public void configurePivotControlLoop(MotorControlLoopConfig config) { @Override public void enableRollerEStop() { isRollerEStopped = true; + commandedRollerVoltage = 0; rollerSim.setInputVoltage(0); isRollerClosedLoop = false; } @@ -178,4 +210,45 @@ public void enablePivotEStop() { public void disablePivotEStop() { isPivotEStopped = false; } + + private void updateMapleIntakeSimulation() { + boolean shouldRun = shouldRunMapleIntake(); + if (shouldRun && !mapleIntakeRunning) { + intakeSimulation.startIntake(); + mapleIntakeRunning = true; + } else if (!shouldRun && mapleIntakeRunning) { + intakeSimulation.stopIntake(); + mapleIntakeRunning = false; + } + + if (isOuttaking()) { + intakeSimulation.obtainGamePieceFromIntake(); + } + } + + private boolean shouldRunMapleIntake() { + if (isRollerEStopped || isPivotEStopped) { + return false; + } + + boolean pivotDeployed = + pivotSim.getAngularPositionRotations() > config.pivotUpAngleRotations + DEPLOYED_PIVOT_MARGIN_ROTATIONS; + if (!pivotDeployed) { + return false; + } + + return isRollerClosedLoop + ? desiredRollerVelocityRotationsPerSec > MIN_INTAKE_SPEED_RPS + : commandedRollerVoltage > MIN_INTAKE_VOLTAGE; + } + + private boolean isOuttaking() { + if (isRollerEStopped) { + return false; + } + + return isRollerClosedLoop + ? desiredRollerVelocityRotationsPerSec < -MIN_INTAKE_SPEED_RPS + : commandedRollerVoltage < -MIN_INTAKE_VOLTAGE; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b412a96..c24b4ca 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -242,6 +242,10 @@ public void setFlywheelSetpoint(FlywheelSetpoint setpoint) { // Direct setters for mechanism control (exposed for tuning/overrides in RobotContainer) public void setHoodAngle(Rotation2d angle) { + if (!Double.isFinite(angle.getDegrees())) { + return; + } + double clampedAngleDeg = MathUtil.clamp( angle.getDegrees(), config.hoodMinAngleDegrees, @@ -270,6 +274,10 @@ private void setTurretAngle( double requestedVelocityRotPerSec, TurretReferenceFrame referenceFrame ) { + if (!Double.isFinite(angle.getDegrees())) { + return; + } + Rotation2d robotYaw = RobotState.getInstance().getEstimatedPose().getRotation(); double robotYawVelocityRotPerSec = RobotState.getInstance().getYawVelocityRadPerSec() / (2.0 * Math.PI); double minDeg = config.turretMinAngleDeg; @@ -324,6 +332,10 @@ private void setTurretAngle( turretProfileSetpoint.velocity ); + if (!Double.isFinite(turretProfileSetpoint.position) || !Double.isFinite(turretProfileSetpoint.velocity)) { + return; + } + shooterIO.setTurretAngle(turretProfileSetpoint.position, turretProfileSetpoint.velocity); } @@ -492,6 +504,10 @@ enum TurretReferenceFrame { } public void setShotVelocity(double velocityRotationsPerSec) { + if (!Double.isFinite(velocityRotationsPerSec)) { + velocityRotationsPerSec = 0.0; + } + flywheelSetpointRPS = velocityRotationsPerSec; Logger.recordOutput("Shooter/shotVelocitySetpointRotationsPerSec", velocityRotationsPerSec); shooterIO.setShotVelocity(velocityRotationsPerSec); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index dfd8256..9388ea2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -38,6 +38,7 @@ import frc.robot.RobotState; import frc.robot.RobotState.OdometryObservation; import frc.robot.constants.Constants; +import frc.robot.configs.RobotStateConfig; import frc.robot.configs.SwerveConfig; import frc.robot.configs.SwerveDrivetrainConfig; import frc.robot.configs.SwerveModuleGeneralConfig; @@ -224,13 +225,29 @@ enum RotationRangeFrame { new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition() - }; + }; + private final SwerveModulePosition[] filteredOdometryModulePositions = new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; private SwerveModuleState[] moduleStates = new SwerveModuleState[] { new SwerveModuleState(), new SwerveModuleState(), new SwerveModuleState(), new SwerveModuleState() }; + private final SwerveModuleState[] filteredOdometryModuleStates = new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + private final double[] lastRawOdometryDrivePositionsMeters = new double[4]; + private final double[] rejectedOdometryDriveDistanceMeters = new double[4]; + private boolean hasInitializedFilteredOdometry = false; + private boolean isRejectingTiltedOdometry = false; private ChassisSpeeds desiredRobotRelativeSpeeds = new ChassisSpeeds(); private ChassisSpeeds obtainableFieldRelativeSpeeds = new ChassisSpeeds(); @@ -240,6 +257,7 @@ enum RotationRangeFrame { private final SwerveModuleGeneralConfig moduleGeneralConfig; private final SwerveDrivetrainConfig drivetrainConfig; + private final RobotStateConfig robotStateConfig; private SwerveDriveKinematics kinematics; private final DashboardMotorControlLoopConfigurator driveControlLoopConfigurator; @@ -265,6 +283,7 @@ private SwerveDrive() { ); drivetrainConfig = swerveConfig.drivetrain; moduleGeneralConfig = swerveConfig.moduleGeneral; + robotStateConfig = ConfigLoader.load("robotState", RobotStateConfig.class); if (useSimulation) { modules = new ModuleIO[] { @@ -461,35 +480,36 @@ public void periodic() { } ArrayList updatedPoses = Constants.VERBOSE_LOGGING_ENABLED ? new ArrayList() : null; - + boolean gyroConnected = gyroInputs.isConnected; + double angleToFloorDegrees = getAngleToFloorDegrees(gyroInputs.gyroOrientation); + isRejectingTiltedOdometry = + gyroConnected && angleToFloorDegrees > robotStateConfig.maxTiltAngleDegrees; + Logger.recordOutput("SwerveDrive/odometry/angleToFloorDegrees", angleToFloorDegrees); + Logger.recordOutput("SwerveDrive/odometry/isRejectingTiltedOdometry", isRejectingTiltedOdometry); + + RobotState robotState = RobotState.getInstance(); double[] odometryTimestampsSeconds = moduleInputs[0].odometryTimestampsSeconds; for (int i = 0; i < odometryTimestampsSeconds.length; i++) { - for (int j = 0; j < 4; j++) { - modulePositions[j] = new SwerveModulePosition( - moduleInputs[j].odometryDrivePositionsMeters[i], - moduleInputs[j].odometrySteerPositions[i] - ); - } - - RobotState.getInstance().addOdometryObservation( + Rotation3d odometryGyroOrientation = getOdometryGyroOrientation(i); + boolean shouldRejectOdometrySample = + gyroConnected && getAngleToFloorDegrees(odometryGyroOrientation) > robotStateConfig.maxTiltAngleDegrees; + updateOdometryObservation(i, shouldRejectOdometrySample); + + robotState.addOdometryObservation( new OdometryObservation( odometryTimestampsSeconds[i], - gyroInputs.isConnected, - modulePositions, - moduleStates, - gyroInputs.isConnected ? - new Rotation3d( - gyroInputs.gyroOrientation.getX(), - gyroInputs.gyroOrientation.getY(), - gyroInputs.odometryYawPositions[i].getRadians() - ) : + gyroConnected, + filteredOdometryModulePositions, + filteredOdometryModuleStates, + gyroConnected ? + odometryGyroOrientation : new Rotation3d(), - gyroInputs.isConnected ? gyroInputs.yawVelocityRadPerSec : 0 + gyroConnected ? gyroInputs.yawVelocityRadPerSec : 0 ) ); if (updatedPoses != null) { - updatedPoses.add(RobotState.getInstance().getEstimatedPose()); + updatedPoses.add(robotState.getEstimatedPose()); } } @@ -498,6 +518,9 @@ public void periodic() { } Logger.recordOutput("SwerveDrive/measuredModuleStates", moduleStates); Logger.recordOutput("SwerveDrive/measuredModulePositions", modulePositions); + Logger.recordOutput("SwerveDrive/filteredOdometryModuleStates", filteredOdometryModuleStates); + Logger.recordOutput("SwerveDrive/filteredOdometryModulePositions", filteredOdometryModulePositions); + Logger.recordOutput("SwerveDrive/rejectedOdometryDriveDistanceMeters", rejectedOdometryDriveDistanceMeters); // FSM processing handleStateTransitions(); @@ -507,6 +530,57 @@ public void periodic() { Logger.recordOutput("SwerveDrive/CurrentCommand", this.getCurrentCommand() == null ? "" : this.getCurrentCommand().toString()); } + private void updateOdometryObservation(int odometrySampleIndex, boolean shouldRejectTiltedOdometry) { + for (int moduleIndex = 0; moduleIndex < moduleInputs.length; moduleIndex++) { + double rawDrivePositionMeters = moduleInputs[moduleIndex].odometryDrivePositionsMeters[odometrySampleIndex]; + Rotation2d steerPosition = moduleInputs[moduleIndex].odometrySteerPositions[odometrySampleIndex]; + + modulePositions[moduleIndex] = new SwerveModulePosition(rawDrivePositionMeters, steerPosition); + + if (!hasInitializedFilteredOdometry) { + lastRawOdometryDrivePositionsMeters[moduleIndex] = rawDrivePositionMeters; + } else if (shouldRejectTiltedOdometry) { + // Keep odometry continuous by absorbing wheel travel collected while the robot is tilted. + rejectedOdometryDriveDistanceMeters[moduleIndex] += + rawDrivePositionMeters - lastRawOdometryDrivePositionsMeters[moduleIndex]; + } + + filteredOdometryModulePositions[moduleIndex] = new SwerveModulePosition( + rawDrivePositionMeters - rejectedOdometryDriveDistanceMeters[moduleIndex], + steerPosition + ); + filteredOdometryModuleStates[moduleIndex] = new SwerveModuleState( + shouldRejectTiltedOdometry ? 0.0 : moduleInputs[moduleIndex].driveVelocityMetersPerSec, + steerPosition + ); + lastRawOdometryDrivePositionsMeters[moduleIndex] = rawDrivePositionMeters; + } + + hasInitializedFilteredOdometry = true; + } + + private Rotation3d getOdometryGyroOrientation(int odometrySampleIndex) { + if (!gyroInputs.isConnected) { + return new Rotation3d(); + } + + double yawRadians = gyroInputs.gyroOrientation.getZ(); + if (gyroInputs.odometryYawPositions.length > odometrySampleIndex) { + yawRadians = gyroInputs.odometryYawPositions[odometrySampleIndex].getRadians(); + } + + return new Rotation3d( + gyroInputs.gyroOrientation.getX(), + gyroInputs.gyroOrientation.getY(), + yawRadians + ); + } + + private double getAngleToFloorDegrees(Rotation3d orientation) { + double cosine = Math.cos(orientation.getX()) * Math.cos(orientation.getY()); + return Math.toDegrees(Math.acos(MathUtil.clamp(cosine, -1.0, 1.0))); + } + /** * Determines the next measured state based on the desired state. */