Heron Robotics
Hi. We wrote code for the FTC 2025-2026 DECODE season.
Our code is, fundamentally, a new take on what's possible in the FIRST Tech Challenge. The core technology is a publisher/subscriber code architecture that allows for more modular, performant, and maintainable code. This is done through a number of key systems described below. For a high-level understanding of our cool things, check out our NorCal Regionals ENP.
Pub/Sub enables out-of-the-box multithreading, action composition, and code simplicity that is nearly impossible to achieve with traditional command-based code.
The smallest building block of our robot's control system is what we call a "Node." Nodes are entirely independent units of code that perform a specific function; they can be subsystems (see DrivetrainNode.java), mechanisms (see GateServo.java), state managers (see MechanismManager.java), or computation processors (see TrajectoryDurationPredictor.java).
Nodes are built on top of pub/sub's core philosophy of modular design, which consists of two key principles of separation: 1. Separation of Processing: Every Node runs on a separate thread. (“But wait—multithreading in FTC is hard! It's so complicated! You shouldn't mess with it!”... well, maybe you're not Heron Robotics. Womp womp. Pub/sub makes multithreading incredibly easy and out of sight. More on that later!) 2. Separation of Communication: Nodes NEVER directly reference each other. Instead, they communicate via publishing and subscribing to topics. This means that all Nodes are implementation-blind to their dependencies, which makes them very easy to hotswap or modify on the fly.
But what is Pub/Sub? What is publishing? Topics? The idea is pretty simple, so to make it more complicated I'll introduce a metaphor.
Imagine you are a radio DJ (call that DJ Heron, or whatever). You have a radio station! And, if all is well, you have listeners who tune in to your station to hear your music. In this metaphor, you are a Node, your radio wavelength is a Topic, and your listeners are Subscribers.
In practice this means that when you want to share information (like the current position of the robot, or the state of a mechanism), you "publish" that information to a Topic (which is identified with a string id, so like "robot/current/velocity" or "mechanisms/current/state"*). Any Node that is "subscribed" to that Topic will receive the information you published in one of two forms.
The first form is via @SubscribedTo annotations:
@SubscribedTo(topic = "drivetrain/set/slowmode")
public void setSlowMode(boolean yesOrNah) {
slowmode.set(yesOrNah);
}SubscribedTo annotations are a way to essentially attach callbacks: "Whenever a new value is published
to this topic, run this function with the new value as an argument." This is how most things on the robot
work! Gamepad inputs are processed by TwoControllerTeleop.java and turned into actions (like setting slowmode to true).
The second form is via LastMessagePublished annotations:
// ExampleNode.java
@LastMessagePublished(topic = "drivetrain/current/velocity")
private Vector currentVelocity;This is where the power of pub/sub's multithreading really comes through, and where it's obvious just how behind-the-scenes it is. With Java Reflections and some clever code, we can automatically update the value of field variables to take on the value of the most recently published message in a topic. From any thread.
This means that whenever this example Node refers to this.currentVelocity, it is guaranteed to be
referencing the most current velocity that the robot has measured without having to do any extra work to "fetch" that value.
Let's take a look at traditional, single-threaded, command-based FTC code. I'll use the example Auto from Pedro Pathing, which you can find on their docs here (as of March 1st 2026).
The general idea of it is to call subsystem.update() on every subsystem in a loop, and then to
compose actions by setting state variables in those subsystems. It should be obvious that this gets
out of hand very easily:
public void autonomousPathUpdate() {
switch (pathState) {
case 0:
follower.followPath(scorePreload);
setPathState(1);
break;
case 1:
/* You could check for
- Follower State: "if(!follower.isBusy()) {}"
- Time: "if(pathTimer.getElapsedTimeSeconds() > 1) {}"
- Robot Position: "if(follower.getPose().getX() > 36) {}"
*/
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */
if (!follower.isBusy()) {
/* Score Preload */
/* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */
follower.followPath(grabPickup1, true);
setPathState(2);
}
break;
case 2:
/* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the pickup1Pose's position */
if (!follower.isBusy()) {
/* Grab Sample */
}
// ... etc
}
}And this is the code for just one path! You need to keep track of a bunch of numbers—by the way, Pedro devs, have you heard of enums?—and you have to be very careful about the order of your cases.
Now try adding in a path between the first and second paths... You have to shift the index of every single path down!
All this to say: single-threaded code in FTC works, but is prehistorically primitive and a royal annoyance in the rear end to actually use.
This is where pub/sub's multithreading shines. To demonstrate, here's our actual code for our 18-ball auto (I've excluded path declarations):
// CloseZoneSoloGateCycleAuto.java
public void runOpMode(HardwareOrchestrator orchestrator) throws InterruptedException, ExecutionException {
timer.resetTimer();
orchestrator.dispatch("start", true);
orchestrator.dispatch(CLOSE_GATE, true);
orchestrator.dispatch(TopicNames.MANUAL_SHOOT_STOP, true);
// start -> shoot
orchestrator.dispatch(SHOOTING_POSITION_OVERRIDE, lastPoseOnPath(startToShootPath));
AutoCommon.minimalFollowAndAwait(follower, startToShootPath);
if (GATE_CYCLE_COUNT == 3) {
sleep(650);
}
AutoCommon.shoot0(orchestrator, SHOOTING_WAIT_TIME);
// spike two intake -> shoot
orchestrator.dispatch(SHOOTING_POSITION_OVERRIDE, lastPoseOnPath(spikeTwoToShootPath));
// shoot -> spike two intake -> shoot
AutoCommon.minimalFollowAndAwait(follower, shootToSpikeTwoPath);
if (GATE_CYCLE_COUNT == 3) {
sleep((long) CLOSE_WAIT_BEFORE_SHOOTING);
}
AutoCommon.shoot0(orchestrator, SHOOTING_WAIT_TIME);
// gate cycles
for (int i = 0; i < GATE_CYCLE_COUNT; i++) {
Pose gateIntakePose = new Pose(); // ...
PathChain gateToShootPath = null; // ...
PathChain shootToPreGatePath = null; // ...
orchestrator.dispatch(SET_INTAKE_STATE_TEMPORARY, IntakeNode.IntakeState.INTAKE);
AutoCommon.minimalFollowAndAwait(follower, shootToPreGatePath);
AutoCommon.awaitGateIntake(orchestrator, CLOSE_ZONE_GATE_INTAKE_WAIT_TIME);
AutoCommon.minimalFollowAndAwait(follower, gateToShootPath);
if (GATE_CYCLE_COUNT == 3) {
sleep(350);
}
AutoCommon.shoot0(orchestrator, SHOOTING_WAIT_TIME);
}
// spike one intake -> leave
orchestrator.dispatch(SHOOTING_POSITION_OVERRIDE, lastPoseOnPath(spikeOneToLeaveShootPath));
AutoCommon.minimalFollowAndAwait(follower, shootToSpikeOnePath);
sleep(200);
AutoCommon.shoot0(orchestrator, SHOOTING_WAIT_TIME);
}// AutoCommon.java: (more or less)
public void minimalFollowAndAwait(FollowerNode follower, PathChain path) {
follower.followPath(path);
follower.autoAwaitPathCompletion();
}
public void shoot0(Orchestrator orch, long waitTime) {
Robot.unsetTurretOverride(orchestrator);
orchestrator.dispatch(TopicNames.MANUAL_SHOOT_START, true);
if (!Robot.BREAK_BEAM_ENABLED) {
log.warn("Break beam disabled, shooting delay will be based on time only");
sleep(delayMillis);
} // ("else" case is omitted; you can find it in AutoCommon.java if you're interested)
orchestrator.dispatch(TopicNames.MANUAL_SHOOT_STOP, true);
}Since Node behaviours are written as synchronous code but run on separate threads and hold no direct references to each other, we're able to start asynchronous actions simply by publishing to a topic, while also maintaining the ability to write code in a simple, linear way. (Or wait for synchronous actions, like the beambreak logic, to complete!)
We all know that FTC hardware interactions take ~3ms per read/write operation. This means the theoretical maximum frequency at which you can read from or write to hardware is ~333Hz. In practice, it's more like 100Hz due to various factors outside of FTC teams' control. Let's go with the 100Hz number for now.
Now, let's say I have an intake. If the right bumper on my gamepad is pressed, I want to run the intake; otherwise, it should be stopped. I also want to update the LED color to reflect some pretty unimportant value, like—I don't know—the number of balls the robot is holding. (In this case the LED is just for the driver to have visual feedback. It's not too critical.)
In this scenario, I'd want...
- my gamepad to poll at 60Hz,
- my intake to only update when the bumper pressed state changes,
- and my LEDs to update at 10Hz.
In a command-based architecture, this is... pretty hard to do.
Remember that you're going to be calling .update() on your intake and LED subsystems in the same
loop, so they'll be forced to update at the same (largely uncontrollable) frequency. You could do something
like this:
public void runOpMode() {
int loopCounter = 0;
Gamepad oldGamepad = gamepad1.copy();
while (opModeIsActive()) {
loopCounter++;
// Intake updates when bumper state changes
if (gamepad1.right_bumper != oldGamepad.right_bumper) {
if (gamepad1.right_bumper) {
intake.setPower(1.0);
} else {
intake.setPower(0.0);
}
}
// update once every ten loops (so hopefully around 10Hz?)
if (loopCounter % 10 == 0) {
leds.setColor(getColorBasedOnBallCount());
}
// ... other subsystem updates
}
}Note the "hopefully around 10Hz" comment. In a command-based architecture, you have very little control over the frequency, and it's nearly impossible to guarantee that your subsystem updates will run at the frequency you want. Of course, it is possible—it's just very difficult.
Here's how we'd match fulfill these requirements with pub/sub:
// GamepadNode.java
// these methods will be called when the bumper state changes automatically—see TwoControllerTeleop.java!
@SubscribedTo(topic = "gamepad1/right_bumper/rising")
public void onRightBumperChange(boolean isPressed) {
orchestrator.dispatch("intake/set/power", 1.0);
}
@SubscribedTo(topic = "gamepad1/right_bumper/falling")
public void onRightBumperChange(boolean isPressed) {
orchestrator.dispatch("intake/set/power", 0.0);
}// IntakeNode.java
// dispatched by GamepadNode (or any node that wants to set the intake power)
@SubscribedTo(topic = "intake/set/power")
public void setIntakePower(double power) {
intake.setPower(power);
}// LEDNode.java
// this annotation is all it takes to set up a recurring action
// and ensure that the LEDs will only update ten times per second,
// regardless of what the rest of the robot is doing!
@RunPeriodically(maxFrequency = 10)
public void updateLEDColor() {
int ballCount = // ... get ball count from wherever
leds.setColor(getColorBasedOnBallCount(ballCount));
}Whoa.
This section is short. Use your brain: if the REV Control & Expansion Hubs have more than one core, but your code is single-threaded, then you're not using the full potential of your hardware. Use more threads! Pub/sub makes it easy.
I should note that hardware interactions are still single-threaded, even if they don't appear to be. This is because REV is pretty stupid and annoying and blah blah blah. The point is: with pub/sub, you are almost guaranteed to be exclusively I/O-bound instead of CPU-bound.
For context: our hood angle logic runs at ~60Hz despite being (quite literally) an O(n^3) algorithm—I couldn't be bothered to optimize it at the time, and then our season ended before I had to—while our other mechanisms run at whatever we set them to. LEDs run at 10Hz, drivetrain and turret run at 50Hz, and so on and so forth.
Um, poke around. Lots to see.
Some things I didn't get into, but which are or enable incredibly cool features:
ConstantsNode.java&./upload_constants—a Node that leverages file I/O via theadbshell to allow for on-the-fly changes to constants (such as PIDF coefficients, hardware ports, or feature flags) and dynamic evaluation of expressions...- ...for Coordinate-less Pose Definitions: in
DRIVE_CONSTANTS.env, you'll see that we define all of our auto poses as expressions; things like3t-cos(38d)*w/2are automatically evaluated as "three tiles plus 2.5 tabs* minus the cosine of 38 degrees times half the robot width." This allows us to (a) automatically "flip" poses for red and blue sides and (b) ensure that our autonomous works on any field size. - Defining hardware via ports: instead of using HardwareMap and requiring the proper robot
configuration on the Driver Station app, we just hit the
DcMotorEx motorOne = hp.motor(CONTROL_HUB, port, Direction.REVERSED)type shit. - Simulation of the robot with fake hardware, while emulating on real drive data. Heh.
WASD robot control??
Thanks to pub/sub. Might link a video of this later... but not today.
I never planned to do much documentation, but in the week leading up to our competition I realized I probably should; as such, I really focused my efforts on instructing Claude Code to maintain CLAUDE.md with a thorough explanation of the codebase. Check that out!
Otherwise:
/api: the actual implementation of pub/sub. All the multithreading and ugly stuff and whatnot. Yuck. Not too fun./simulator: a hardware-less simulation of the robot that leverages pub/sub and real drive data to simulate the robot./TeamCode: never in a million years will you believe what this contains.../web-viewer: our custom web dashboard, which enables visualizing the robot's shooting state & controlling it with WASD./kalman-tuner: vibecoded React app for tuning our Kalman filter using real drive data.
Heron Robotics
Developers:
- Boris Nezlobin, Team Captain.
- Jake (JBlitzar), Software Lead.
Other members of Heron Robotics contributed to designing, building, and wiring the robot, and to match strategy, logistics, outreach, finances, and more:
- Gavin Zhang, Founder; Hardware Lead; Lead Robot Driver.
- Arjun Jindal, Strategy Lead; Hardware.
- Preston, Outreach Lead.
- Stepan Stadnyk, Logistics; Hardware; Outreach.
- Matthew Wei, Outreach.
- Luka Balva, Outreach.
Pub/Sub was originally developed by Alec Petridis (@chop0) on Kuriosity Robotics in the FTC POWERPLAY season. You can find it at KuriosityRobotics/power-play*.
* — I did always want to fix that README... sigh.
It's been real. I'm done with FTC, and this was Heron's last season. I'm grateful for everything FTC has given me (especially the food) and taught me. I hope this codebase serves as an example of what is possible in FTC to some curious nerd some time down the line. If you have any questions, feel free to reach out to me!