Add Simulation Support, Diagnostic Testing Framework, Auto Routines, and Code Cleanup - #23
Open
jmmaloney3 wants to merge 126 commits into
Open
jmmaloney3 wants to merge 126 commits into
jmmaloney3 wants to merge 126 commits into
Conversation
…th orbit tuning constans later.
- Add SimulationManager class to simulate robot motion and sensor updates. - Add simulation lifecycle methods (simulationInit, simulationPeriodic) to Robot. - Add simulation support getters to SwerveSubsystem and SwerveModule. - Update .gitignore to ignore ctre_sim/ directory. All simulation code is optional and clearly separated from production code.
* Test Reject on Compile Error (#7) * test reject merge to main * test reject merge with compile error * fix compile error - pull request test * Shooter Subsystem * Shooter Subsystem * Kinda buggy light subsystem + Blinking lights uhh nothing really else * Constant motor speed * Slight organization * X config + More optimized code * Lights When Shooting + Shooting Lights + Light Control * ID Switch * Controll Changes and Feeder Addition * Minor Feeder Bot Changes * Moving IDS to constant * !!!(NEEDS TESTING)!!! Robot motor code updated maybe * Attempted fixes (not tested) * Configured the Motors * removed line 21 * made a boolean public * working shooter and feeder code shooter is a toggle feeder works when held * Remove lightSubsystem.java - moved to light-subsystem branch --------- Co-authored-by: AlexHuie <ahuie26@moundsparkacademy.org> Co-authored-by: Zachray <145231265+ZachrayL@users.noreply.github.com> Co-authored-by: jmmaloney3 <jmmaloney3@gmail.com> * Auto align (#9) * Update LimelightHelpers.java * AutoAlign coded with placeholder distance * Coded auto align, also commented for most of them. Need correction with orbit tuning constans later. * Add PID controller for correct radius & feedforward with angular velocity * Fixed the swervemodules --------- Co-authored-by: Seqi <gavin2noah@gmail.com> * Pathplanner lib and basic auto structure (no commands) * errors but commit so other people can get code * Intake Toggle Code (#8) * intake code does not work yet * updated intake code changed control to toggle w/ x button on hems controller roborio connection problem can build but can't deploy * updated pid value for test board * toggle intake code * started intake arm code doesn't work yet * added intake arm code needs PID tuning and testing * arm code with setpoints moves to the setpoints but oscillates around them nees tuning * updated PID values (in progress) * Changed intake button to two separate ones, A for lowering the intake arm, Y for raising the intake arm on helms controller * adjust intake * updated controls * fixed intake arm (working), the motor is really strong but it's fine --------- Co-authored-by: Nonochen0104 <xchen26@moundsparkacademy.org> * shooter subsystem (#10) * Shooter Subsystem * Kinda buggy light subsystem + Blinking lights uhh nothing really else * Constant motor speed * Slight organization * X config + More optimized code * Lights When Shooting + Shooting Lights + Light Control * ID Switch * Controll Changes and Feeder Addition * Minor Feeder Bot Changes * Moving IDS to constant * !!!(NEEDS TESTING)!!! Robot motor code updated maybe * Attempted fixes (not tested) * Configured the Motors * removed line 21 * made a boolean public * working shooter and feeder code shooter is a toggle feeder works when held * Remove lightSubsystem.java - moved to light-subsystem branch * Added hood and controllers for all shooter related parts * Fixed all the shooter parts, everything is working (shooter, feeder, hood) * Deleted repeated helms controller definition --------- Co-authored-by: FireheadReal <jwrich28@moundsparkacademy.org> Co-authored-by: AlexHuie <ahuie26@moundsparkacademy.org> Co-authored-by: jmmaloney3 <jmmaloney3@gmail.com> Co-authored-by: Nonochen0104 <xchen26@moundsparkacademy.org> Co-authored-by: Gavin <gavin2noah@gmail.com> * Fix compile error in startAutoAt() method - Complete the incomplete if statement - Implement proper odometry reset with alliance-aware coordinate flipping - Add missing Translation2d import - Method now properly resets robot pose to starting position for autonomous * fix compile error by completing startAutoAt method * Theoretically working auto (probably not) * partial autobuilder configuration code * finished autoBuilder config needs testing and review * changed build.gradle * Added pathplaner folder with eight lemon path * added depot auto(s) * add leave auto --------- Co-authored-by: jmmaloney3 <jmmaloney3@users.noreply.github.com> Co-authored-by: FireheadReal <jwrich28@moundsparkacademy.org> Co-authored-by: AlexHuie <ahuie26@moundsparkacademy.org> Co-authored-by: Zachray <145231265+ZachrayL@users.noreply.github.com> Co-authored-by: jmmaloney3 <jmmaloney3@gmail.com> Co-authored-by: Seqi <gavin2noah@gmail.com>
This reverts commit 24fa677.
This reverts commit a6b3ecb.
Resolved conflicts: - Constants.java: Use INTAKE_ID=63 and INTAKE_ARM_ID=64 from jmm-sim-support - SwerveSubsystem.java: Keep both drift test code and simulation support methods
…leConfig Problem: In autonomous simulation, the robot teleports to the path starting point (correct) but then sits still. Root cause: ModuleConfig.maxDriveVelocityMPS was set to 3.0 m/s (the teleop software speed limit) instead of the motor's physical free speed (~5.35 m/s). At 3.0 m/s the NEO Vortex draws 94.6A, exceeding the 40A current limit. PathPlanner's trajectory generator clamped both max-speed and stall current to 40A, producing zero available torque, zero acceleration, and totalTime=0s. Fix: Derive maxDriveVelocityMPS from DCMotor.getNeoVortex(1).freeSpeedRadPerSec through the gear reduction and wheel radius (~5.35 m/s). The path file maxVelocity constraint still limits planned speed to 3.0 m/s. Add TrajectoryGenerationTest with programmatic test paths (no dependency on path files) that reproduces the bug with the old config and verifies the fix. Set DEBUG=true for verbose output. See jmm-auto-mode-debug branch for full analysis (docs/auto-sim-log-analysis.md).
…c (not checked into git) gradle.init setting
Multi-trial simulator diagnostic test that reproduces issue 8.1/8.2: - Phase 1: Resets pose and observes heading corruption from odometry.resetPosition() receiving desired heading as gyro baseline instead of actual pigeon reading - Phase 2: Drives a closed-loop L-shaped path to quantify impact of corruption on robot motion - 10 trials with per-trial statistics and summary table - Registered in DiagnosticTestRegistry as ODOMETRY_RESET
Starts in front of the hub, backup and then shoot. After shooting, drives to the depot for intake then drives closer to the hub to shoot again.
Start infront of the trench, move to shoot then drive to the depot for intake, drive back to shoot again.
This reverts commit ff3a1b7.
Kept (auto-related):
- CenterToDepotAuto.java — new center-to-depot autonomous routine
- TrenchToDepotAuto.java — new trench-to-depot autonomous routine
- AutoMode enum: added None, TrenchToDepotAuto, CenterToDepotAuto
- AutoConstants: refactored static{} to lazy initDashboard() with guard
- AutoAlign: orbit radius tuned to 2.4384m (8 ft)
- RobotContainer: new auto imports, switch cases, initDashboard() call,
slow-mode multiplier 0.7 → 0.85
- Constants: inner classes changed to static final (FieldConstants,
ShooterConstants, IntakeConstants, CANdleConstants)
Not merged (sim-related — conflicts with SimulationManager approach):
- RobotSimulation.java (Nono's sim class)
- SwerveSubsystem sim fields/methods (simulationReset, simulationUpdate,
lastCommandedSpeeds, simYawDegrees, simWheelPositionsMeters)
- SwerveSubsystem.periodic() if(!isSimulation()) guard
- Robot.java sim hooks (RobotSimulation instantiation and delegation)
- ctre_sim/ binary files (gitignored)
- FORCE_BLUE_ALLIANCE constant
Added:
- docs/sim-alternatives.md — comparison of SimulationManager vs
RobotSimulation approaches with external FRC team research
jmmaloney3
changed the title
When the robot transitions to disabled, modules retain their last commanded velocity. Zero out module speeds on the enabled→disabled transition to stop SimulationManager from integrating stale values.
Replace if/else fallback with MathUtil.clamp(dt, 0.0, 0.05). Zero dt now produces zero motion instead of a phantom 20ms step. Upper bound reduced from 1.0s to 50ms to limit pose jumps during GC pauses or debugger breakpoints.
jmmaloney3
commented
Feb 26, 2026
| private final StructArrayPublisher<SwerveModuleState> desiredSwerveDataPublisher = NetworkTableInstance.getDefault() | ||
| .getStructArrayTopic("Desired Swerve States", SwerveModuleState.struct).publish(); | ||
| private final StructPublisher<Pose2d> robotPose = NetworkTableInstance.getDefault() | ||
| .getStructTopic("Robot Pose", Pose2d.struct).publish(); |
Contributor
Author
There was a problem hiding this comment.
This is required for AdvantageScope compatibility. Otherwise, robot "spins" in simulation mode.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
4 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Add Simulation Support, Diagnostic Testing Framework, Auto Routines, and Code Cleanup
Summary
This PR combines work from multiple feature branches (
jmm-sim-support,jmm-test-support,jmm-clean-up) and merges autonomous routines from theNonoAutobranch. It adds comprehensive simulation support, a reusable diagnostic testing framework with five diagnostic tests, autonomous path-following routines, a PathPlanner trajectory generation fix, and general code cleanup. All simulation and test code is optional and clearly separated from production code.WPILib Upgrade
Upgraded to WPILib version
2026.2.1.Simulation Support
New Files
frc.robot.sim.SimulationManager: Core simulation logic — simulates robot motion, updates encoder and gyro readings, and drives Field2d visualizationModified Files
Robot.java: AddedsimulationInit()andsimulationPeriodic()lifecycle hooks; joystick connection warnings suppressed in sim mode via user-specific~/.gradle/init.gradlesystem property (sim.silenceJoystick)SwerveModule.java: StoredesiredStatefor simulation access; refactoredpointInDirection()to route throughsetDesiredState()for simulation consistency; added simulation getters (getCanCoderDevice(),getDriveEncoder(),getAngleEncoder(),getDesiredState())SwerveSubsystem.java: Added simulation support methods; refactored to read desired states from modules instead of storing them redundantly; addedStructPublisher<Pose2d>to fix spinning robot visualization in AdvantageScopeRobotContainer.java: AddedgetSwerveSubsystem()accessor for simulation and test accessBenefits
Diagnostic Testing Framework
New Framework (
frc.lib.testpackage)DiagnosticTest: Common interface for all diagnostic tests with optional parameter initialization and result reportingDiagnosticTestRegistry: Enum-based registry with 5 tests (swerve angle drift, swerve alignment, swerve straight line, LED state, odometry reset)DiagnosticTestManager: Manages test lifecycle, SmartDashboard UI (dropdown selection, start button, status display), parameter initialization, and test execution via CommandSchedulerTestDashboard: Helper class that eliminates prefix constants and provides type-safe SmartDashboard methods for test parameters and resultsTest Implementations (
frc.robot.testpackage)SwerveAngleDriftTestCommand: Tests encoder drift in swerve module angle motors — performs N cycles of angle movements, records encoder measurements, calculates drift statisticsSwerveAlignmentTestCommand: Commands all 4 modules to the same angle(s) and measures per-module accuracy, cross-module spread, and calibration error (integrated vs absolute encoder accounting for angleOffset)SwerveStraightLineTestCommand: Drives straight with known ChassisSpeeds (bypassing joystick), records per-module angles/velocities and gyro heading drift, supports open-loop vs closed-loop comparisonLedStateTestCommand: Tests CandleSubsystem LED states independently — sets selected LED state for specified duration, then automatically turns LEDs offOdometryResetTestCommand: Multi-trial test for odometry pose reset behavior — resets pose with different headings, observes pigeon latency, then drives an L-shaped closed-loop path to quantify impact on robot motionIntegration Points
Robot.java: IntegratedDiagnosticTestManagerintestInit(),testPeriodic(), andtestExit()methodsRobotContainer.java: AddedgetCandleSubsystem()accessor for test accessSwerveSubsystem.java: AddedgetModule()method for individual module accessSwerveModule.java: AddedgetAngleOffset()getter for calibration checksUnit Tests
DiagnosticTestValidationTest: Validates framework behavior (parameter validation, cycle result creation, drift calculation)Usage
DiagnosticTests/TestSelectordropdown in SmartDashboardDiagnosticTests/[TestName]/Parameters/DiagnosticTests/Start-Cancel Testto executeDiagnosticTests/TestStatusand view results inDiagnosticTests/[TestName]/Results/DiagnosticTests/Start-Cancel Testwhile running to cancelArchitecture Benefits
frc.lib.test) has no production dependencies; test code (frc.robot.test) depends only on framework and subsystems; production code has zero test/simulation dependenciesDiagnosticTestand adding an enum value toDiagnosticTestRegistryNonoAuto Auto Routines Merge
Merged autonomous routine work from the
NonoAutobranch, keeping auto-related features and excluding the alternative simulation framework (seedocs/sim-alternatives.mdfor a comparison of approaches).New Files
frc.robot.Auto.CenterToDepotAuto: Autonomous routine — center starting position to depotfrc.robot.Auto.TrenchToDepotAuto: Autonomous routine — trench starting position to depotModified Files
Constants.java: AddedNone,TrenchToDepotAuto,CenterToDepotAutotoAutoModeenum; refactored dashboard initialization to lazyinitDashboard()with guard; inner classes changed tostatic finalRobotContainer.java: Added switch cases for new auto modes; slow-mode multiplier 0.7 → 0.85AutoAlign.java: Orbit radius tuned from 2.22m to 2.4384m (8 ft)Not Merged (sim-related — conflicts with SimulationManager approach)
RobotSimulation.java,SwerveSubsystemsim fields/methods,Robot.javasim hooks,FORCE_BLUE_ALLIANCEconstant,ctre_sim/binary filesPathPlanner Trajectory Generation Fix
Problem
In autonomous simulation, the robot teleports to the path starting point (correct) but then sits still. PathPlanner's
FollowPathCommandgenerates a trajectory withtotalTime=0.0sand terminates immediately.Root Cause
ModuleConfig.maxDriveVelocityMPSwas set toSwerveConstants.maxSpeed(3.0 m/s) — the teleop software speed limit. PathPlanner uses this value to model motor physics during trajectory generation. At 3.0 m/s, the NEO Vortex motor draws 94.6A at 12V, which exceeds the configured 40A current limit. PathPlanner clamps both the max-speed current and the stall current to 40A, producing identical torque values — resulting in zero available torque for acceleration, zero velocity at every trajectory state, andtotalTime=0.0s.Fix
Constants.java: AddedSwerveConstants.maxDriveVelocityMPS, derived from the motor's physical free speed through the gearbox (DCMotor.getNeoVortex(1).withReduction(driveGearRatio).freeSpeedRadPerSec * wheelRadius≈ 5.35 m/s). ChangedMODULE_CONFIGto use this physical max speed instead ofmaxSpeed. The path filemaxVelocityconstraint (3.0 m/s) still limits actual planned speed during path following.Unit Tests
TrajectoryGenerationTest(src/test/java/frc/robot/auto/): 7 JUnit tests that validate trajectory generation using programmatic test paths (straight line, S-curve) — no dependency on path files. Tests include config sanity checks, trajectory generation for multiple path variants (original, flipped, mirrored), and a root cause regression test that reproduces the zero-torque bug with the old config value and verifies the fix with the corrected value. SetDEBUG=truefor verbose trajectory output.Analysis
Full root cause analysis with motor physics explanation available on the
jmm-auto-mode-debugbranch (docs/auto-sim-log-analysis.md).Code Cleanup
ctre_sim/directory and add to.gitignoreto prevent re-addingPhoenix5-frc2026-latest.json) — all code uses Phoenix 6build.gradle— only needed for Phoenix 5RobotContainer.java(GenericHID, duplicateRunCommand,JoystickButton,POVButton)INTAKE_IDandINTAKE_ARM_IDwere colliding withSHOOTER_IDandHOOD_ID, causing simulator crash. Updated to unique IDs (63, 64)CandleSubsystem— RenamedChangeColor,ChangeState,ClearAnimations,LightConfigto camelCase (changeColor,changeState,clearAnimations,lightConfig)SwerveModule.java,Robot.java,RobotContainer.java, andSwerveSubsystem.javaSendableChooserUtilfor enum-based chooser creationDocumentation
docs/simulation-test-architecture.md— Simulation and test architecture overviewdocs/diagnostic-testing-framework-design.md— Testing framework designdocs/test-dashboard-design.md— TestDashboard helper designdocs/led-status-architecture.md— LED status architecturedocs/architecture-patterns-comparison.md— Architecture patterns comparisondocs/debugging-methodology.md— Debugging methodologydocs/robotstate-networktables-hybrid.md— RobotState/NetworkTables hybrid approachdocs/2024-26-comparison-report.md— 2024 vs 2026 codebase comparison reportdocs/2024-auto-mode.md— 2024 auto mode referencedocs/sensors-and-pose-estimation.md— Sensors and pose estimation overviewdocs/sim-alternatives.md— Comparison of SimulationManager vs RobotSimulation approachesdocs/code-improvements.md— Added Section 18:RobotStateclass recommendation (references FRC 6328 and FRC 254)docs/auto-sim-analysis/— Autonomous simulation analysis:auto-sim-analysis.md— Root cause analysis of PathPlanner trajectory generation failureauto-sim-log-analysis.md— Detailed cycle-by-cycle log analysis and remaining issuesauto-sim-log-analysis-prompt.md— Prompt used to generate the log analysisauto-sim-instrument-prompt.md— Prompt used to guide autonomous simulation instrumentationFiles Changed (46 files, +16,382 / -243)
New Files (30)
docs/— 12 documentation files +docs/auto-sim-analysis/(4 analysis documents)src/main/java/frc/lib/—SendableChooserUtil.java+ 4 test framework filessrc/main/java/frc/robot/sim/SimulationManager.javasrc/main/java/frc/robot/test/— 5 diagnostic test commandssrc/main/java/frc/robot/Auto/—CenterToDepotAuto.java,TrenchToDepotAuto.javasrc/test/java/frc/lib/test/DiagnosticTestValidationTest.javasrc/test/java/frc/robot/auto/TrajectoryGenerationTest.javaModified Files (10)
Robot.java,RobotContainer.java,SwerveModule.java,SwerveSubsystem.java,CandleSubsystem.java,Constants.java,AutoAlign.java,build.gradle,code-improvements.md,.gitignoreRemoved Files (6)
ctre_sim/— 5 simulator data filesvendordeps/Phoenix5-frc2026-latest.jsonTesting