Robot autonomous code for Team 4537 RoboRoos' 2021 FRC season, the COVID-era "@ Home" challenges where teams ran their robots through scored autonomous tasks on home practice fields. The robot ran on a working swerve drive: the so-called holy grail of FRC drivetrains, and I wrote the autonomous side on top: the path-following commands, sensor integration, and the path library that drove our Galactic Search, AutoNav, and barrel-racing entries.
Infinite Recharge At Home was the FRC 2021 challenge format, instead of head-to-head matches, teams scored their robots solo on a set of autonomous tasks: Galactic Search (read four Power Cells from one of four hidden A/B Red/Blue layouts and collect them in order), AutoNav (Barrel Racing / Slalom / Bounce timed-route trials), and a Power Port shooting challenge. All judged from video.
I worked on this with a small team of programmers, my contribution was the autonomous side: the path-following commands, the path library, and the integration with our custom CAN sensor pods.
Between a working swerve drive (rare) and a tightly tuned autonomous suite running on top of it, our results blew most of the competition out of the water. We had a very good season.
The thing this whole project rode on, mechanically and programmatically, was that we had a working swerve drive. In FRC this is a big deal, it's widely treated as the holy grail of drivetrains. Every wheel is independently steerable and independently driven, so the chassis can translate in any direction, rotate on the spot, or do both simultaneously without changing which way it's "facing." It's the closest you can get to ideal omnidirectional motion on a four-wheeled robot.
It's also why most teams never run it. Each module is two motors, an absolute angle sensor, and a stack of synchronisation logic; the math (forward/inverse kinematics, field-relative chassis speeds, per-module azimuth offsets) needs to be right or the robot does something unpredictable instead of just being slower than a tank drive. The vast majority of FRC teams stick with tank or mecanum because swerve is mechanically and functionally complex enough that it's easy to spend a season fighting the drivetrain instead of competing with it.
Ours actually worked, four independently steerable, independently driven wheel modules sat underneath a field-relative kinematics layer with a gyro for heading. The mechanical and low-level driver work was the team's; what I built on top assumed it.
The drivebase exposed a single "go this fast in X, this fast in Y, this fast in rotation" entry point, and that's what every autonomous command below talks to. positionX, DriveByTime, the DriveToDestination family, the trajectory follower, they all only really work because that input means the robot can do exactly that, without first having to face the direction it wants to move.
Three short clips of the robot running an autonomous routine on our practice field.
Everything built on WPILib's command-based framework. Each autonomous command is a standalone CommandBase subclass that drives the swerve drivebase using one of our sensors as feedback. They were designed to be composable into SequentialCommandGroups so a routine could be assembled in one place.
HolonomicDriveController: separate X/Y PID controllers and a profiled rotation PID, sampled against a generated Trajectory. Mostly the team's experiment with the official trajectory toolchain.We ran two custom sensor packages on the CAN bus, each on its own device ID, behind dirt-simple Java wrappers that just unpacked a 16-bit packet into typed fields:
update() to keep the totals in field-relative coordinates. Powers DriveByDistance and the entire DriveToDestination family. Streamed to SmartDashboard as a number-array for live debugging.Ldist, Rdist, and a signed angle result: the difference between them, used directly for perpendicular wall alignment. Drives positionX.Every scored route was hand-tuned as a list of XY waypoints in millimetres, indexed by an enum in AutonomousConstants. The DriveToDestination4 walker would drive the robot through them in order, the actual trick was finding a sequence of points where the robot's natural arc through each waypoint produced a smooth path matching the field markings.
The Galactic Search variant was the interesting one, the rules forbade telling the robot which of the four ball layouts you'd be facing, so we had a vision pipeline running a BallTrackingServer NetworkTable that exposed the size and X position of the nearest ball pre-match. The autonomous routine would read those values, pattern-match to one of the four layouts, and pick the matching path. (The detection switch was commented out in the final commit while we were tuning paths, but the four named paths are all there in source.)
The naming gives away the iteration: DriveToDestination, then 2, 3, 4. Each version solved the previous one's failure mode rather than refactoring in place, partly because we needed the older versions to keep working for the routes that already used them.
oddPod.getY() until the robot has actually moved the requested distance. Removed the clock-vs-physics drift problem.signum on the acceleration.int[][] of waypoints, computes a unit-vector toward the current target so the robot always faces straight down its trajectory, and advances to the next waypoint once within one robot radius. This is what every AutonomousConstants path runs through.HolonomicDriveController. Smoother in theory but added a lot of toolchain weight; the hand-tuned waypoint approach was less elegant but easier for us to debug from the side of the field.