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 0000000..f76dd23 Binary files /dev/null and b/.gradle-user-home/caches/8.11/file-changes/last-build.bin differ 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 0000000..710272c Binary files /dev/null and b/.gradle-user-home/caches/8.11/fileContent/fileContent.lock differ 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 0000000..afdb949 Binary files /dev/null and b/.gradle-user-home/caches/8.11/fileHashes/fileHashes.bin differ diff --git a/.gradle-user-home/caches/8.11/fileHashes/fileHashes.lock b/.gradle-user-home/caches/8.11/fileHashes/fileHashes.lock new file mode 100644 index 0000000..84ef27a Binary files /dev/null and b/.gradle-user-home/caches/8.11/fileHashes/fileHashes.lock differ 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 0000000..f76dd23 Binary files /dev/null and b/.gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata/metadata.bin differ 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 0000000..8f7305c Binary files /dev/null and b/.gradle-user-home/caches/8.11/groovy-dsl/ca920892c4b5e0d312869e8a7707af09/metadata.bin differ 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 0000000..bdc955b Binary files /dev/null and b/.gradle-user-home/caches/8.11/groovy-dsl/ca920892c4b5e0d312869e8a7707af09/metadata/metadata.bin differ 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 0000000..f76dd23 Binary files /dev/null and b/.gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata/metadata.bin differ 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 0000000..82e1092 Binary files /dev/null and b/.gradle-user-home/caches/8.11/md-rule/md-rule.lock differ 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 0000000..92e2522 Binary files /dev/null and b/.gradle-user-home/caches/8.11/md-supplier/md-supplier.lock differ 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 0000000..35a0387 Binary files /dev/null and b/.gradle-user-home/caches/jars-9/0a726dd67844590711bc1f8ea66e9286/settings.lock.lock differ 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 0000000..35a0387 Binary files /dev/null and b/.gradle-user-home/caches/jars-9/468103ed59db743d56a33d562f8368c4/cp_settings.lock.lock differ 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 0000000..35a0387 Binary files /dev/null and b/.gradle-user-home/caches/jars-9/fa2a3dce8357f878d1a7f11fc39e3bc6/cp_proj.lock.lock differ 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 0000000..5e5dcef Binary files /dev/null and b/.gradle-user-home/caches/jars-9/jars-9.lock differ 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 0000000..14ebc42 Binary files /dev/null and b/.gradle-user-home/caches/journal-1/file-access.bin differ diff --git a/.gradle-user-home/caches/journal-1/file-access.properties b/.gradle-user-home/caches/journal-1/file-access.properties new file mode 100644 index 0000000..99248cb --- /dev/null +++ b/.gradle-user-home/caches/journal-1/file-access.properties @@ -0,0 +1,2 @@ +#Wed Mar 25 22:19:18 EDT 2026 +inceptionTimestamp=1774491558414 diff --git a/.gradle-user-home/caches/journal-1/journal-1.lock b/.gradle-user-home/caches/journal-1/journal-1.lock new file mode 100644 index 0000000..0a07514 Binary files /dev/null and b/.gradle-user-home/caches/journal-1/journal-1.lock differ diff --git a/.gradle-user-home/caches/modules-2/metadata-2.107/module-metadata.bin b/.gradle-user-home/caches/modules-2/metadata-2.107/module-metadata.bin new file mode 100644 index 0000000..7c2e6cd Binary files /dev/null and b/.gradle-user-home/caches/modules-2/metadata-2.107/module-metadata.bin differ diff --git a/.gradle-user-home/caches/modules-2/metadata-2.107/resource-at-url.bin b/.gradle-user-home/caches/modules-2/metadata-2.107/resource-at-url.bin new file mode 100644 index 0000000..7c2e6cd Binary files /dev/null and b/.gradle-user-home/caches/modules-2/metadata-2.107/resource-at-url.bin differ diff --git a/.gradle-user-home/caches/modules-2/modules-2.lock b/.gradle-user-home/caches/modules-2/modules-2.lock new file mode 100644 index 0000000..ed132ec Binary files /dev/null and b/.gradle-user-home/caches/modules-2/modules-2.lock differ diff --git a/.gradle-user-home/daemon/8.11/registry.bin b/.gradle-user-home/daemon/8.11/registry.bin new file mode 100644 index 0000000..079afe1 Binary files /dev/null and b/.gradle-user-home/daemon/8.11/registry.bin differ diff --git a/.gradle-user-home/daemon/8.11/registry.bin.lock b/.gradle-user-home/daemon/8.11/registry.bin.lock new file mode 100644 index 0000000..e64b505 Binary files /dev/null and b/.gradle-user-home/daemon/8.11/registry.bin.lock differ 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/.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/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/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/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..6a76dcb 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -15,10 +15,11 @@ 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; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -116,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(), @@ -188,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); @@ -239,6 +242,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/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/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; - } } 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..fab5f76 --- /dev/null +++ b/src/main/java/frc/robot/sim/MapleSimManager.java @@ -0,0 +1,160 @@ +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 int hubHitCount = 0; + + 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(); + } + + /** 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); + 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")); + Logger.recordOutput("MapleSim/HubHitCount", hubHitCount); + } +} 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/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 e95ba7a..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; @@ -47,6 +48,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; @@ -223,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(); @@ -239,6 +257,7 @@ enum RotationRangeFrame { private final SwerveModuleGeneralConfig moduleGeneralConfig; private final SwerveDrivetrainConfig drivetrainConfig; + private final RobotStateConfig robotStateConfig; private SwerveDriveKinematics kinematics; private final DashboardMotorControlLoopConfigurator driveControlLoopConfigurator; @@ -264,6 +283,7 @@ private SwerveDrive() { ); drivetrainConfig = swerveConfig.drivetrain; moduleGeneralConfig = swerveConfig.moduleGeneral; + robotStateConfig = ConfigLoader.load("robotState", RobotStateConfig.class); if (useSimulation) { modules = new ModuleIO[] { @@ -272,7 +292,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() {}, @@ -460,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()); } } @@ -497,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(); @@ -506,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. */ 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 704af76..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; @@ -28,8 +29,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( @@ -47,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) { @@ -183,7 +188,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 +201,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 +227,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 +337,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 +371,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/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/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)); + } +} 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