Case Study 02 · Robotics & Control Systems · 2023 – Present

FRC 1506 Robot Control & Physics Simulation

250 Hz rigid-body physics simulation, CTRE Phoenix 6 swerve odometry, and multi-camera AprilTag pose fusion.

Visit Live Platform View Source on GitHub

Role & Responsibilities

Lead Programmer & Simulation Architect

Primary Stack

Java 17, WPILib, CTRE Phoenix 6, MapleSim (dyn4j), Limelight MegaTag2, PathPlanner

Key Engineering Metric

250 Hz (4ms) physics loop decoupling software validation from robot hardware

Status & Verification

Competition Proven

250 Hz 4ms Physics Loop

< 2.5 cm 15s Odometry Drift

4.5 m/s Swerve Speed

8 Routines Pre-Build Validated

Executive Overview

The Problem

Mechanical assembly timelines limit physical track testing for high-speed swerve odometry, autonomous routines, and dynamic targeting.

The Architecture

MapleSim desktop simulation running dyn4j physics at 250 Hz, fusing CTRE swerve odometry with dual Limelight vision in an Extended Kalman Filter.

Engineering Impact

Decoupled software validation from hardware, enabling 8 autonomous routines and shoot-on-the-move targeting to be tuned pre-delivery.

[ PathPlanner Autonomous Commands ] ──► [ Swerve Kinematics & Desaturation ]
                                                    │
                 ┌──────────────────────────────────┴──────────────────────────────────┐
                 ▼                                                                     ▼
       [ Physical Robot Hardware ]                                           [ MapleSim dyn4j Sim ]
       - CTRE Kraken/Falcon Swerve                                           - 250 Hz (4ms) Physics Loop
       - Dual Limelight MegaTag2 Vision                                      - Synthetic AprilTag Pipeline
                 │                                                                     │
                 └──────────────────────────────────┬──────────────────────────────────┘
                                                    ▼
                               [ Extended Kalman Filter (EKF) ]
                                                    │
                                                    ▼
                               [ Shoot-on-the-Move Ballistic Math ]

Technical Architecture

1. MapleSim: 250 Hz dyn4j Physics Simulation

Simulates chassis inertia (55 kg) and 4 swerve contact patches to evaluate pathing routines on desktop CPUs:

MapleSim Swerve Physics Simulation Loop Java

```java public class MapleSwerveSimulation { private final World physicsWorld = new World(); private final Body robotBody = new Body(); private final SwerveModuleSimulation[] modules = new SwerveModuleSimulation[4]; public MapleSwerveSimulation(RobotConfig config) { physicsWorld.setGravity(World.ZERO_GRAVITY); robotBody.setMass(MassType.NORMAL.create(55.0, 3.8)); for (int i = 0; i < 4; i++) { modules[i] = new SwerveModuleSimulation(config.getModuleOffset(i)); } physicsWorld.addBody(robotBody); } public void updatePhysics(SwerveModuleState[] states) { for (int i = 0; i < 4; i++) { Vector2 force = modules[i].computeFrictionForce(states[i], robotBody.getLinearVelocity(), robotBody.getAngularVelocity()); robotBody.applyForce(force, modules[i].getPositionOnRobot()); } physicsWorld.step(1, 0.004); // 250 Hz Loop } } ```

2. Multi-Sensor Pose Fusion (Limelight MegaTag2 + EKF)

Fuses 250 Hz wheel odometry with dual optical Limelight coprocessors, scaling covariance dynamically based on target distance:

Dynamic Optical Covariance Pose Estimator Java

```java public void addVisionMeasurement(LimelightResults visionUpdate) { if (!visionUpdate.hasValidTargets()) return; Pose2d estimatedPose = visionUpdate.getBotPose2d(); double timestamp = Timer.getFPGATimestamp() - visionUpdate.getLatencySeconds(); double tagDistance = visionUpdate.getAvgTagDistance(); // Scale measurement standard deviations based on optical distance double xyStdDev = 0.03 * Math.pow(tagDistance, 1.8); double thetaStdDev = 0.05 * Math.pow(tagDistance, 1.5); poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev)); poseEstimator.addVisionMeasurement(estimatedPose, timestamp); } ```

Key Takeaways & Verification

Sim Determinism: 250 Hz physics loop executes in < 0.8ms compute time per frame, validating 8 auto routines pre-hardware.
Odometry Precision: Multi-camera vision fusion reduced cumulative autonomous path drift to < 2.5 cm over 15-second cycles.