Series: FRC Technical Foundations Unit: 4 — Software and Perception Session length: ~3 hours Prerequisites: Lessons 1–6, plus basic Java (variables, methods, classes, conditionals, loops)
Here is the failure this lesson exists to prevent.
A team writes their robot code as a single large TeleOp file. It works. Then they add the elevator, and the file grows. Then the intake, with a rule that it cannot run while the elevator is up. Then autonomous. By week five the file is 1,400 lines, the elevator interlock is implemented in three different places with slightly different conditions, nobody can add a feature without breaking something else, and the team is debugging in the pit instead of practising.
This happens to almost every team once. The architecture in this lesson is how you avoid it happening twice.
WPILib’s command-based framework separates two things that rookie code mixes together:
The framework guarantees one rule that does all the work: only one command can use a given subsystem at a time. If a new command requires the elevator, any running command that required the elevator is cancelled automatically.
That single guarantee eliminates an entire category of bug. Without it, two pieces of code can command the same motor in the same loop iteration, and which one wins depends on execution order. That bug is maddening to find and it disappears entirely under command-based.
Rule 7.1 — Exactly one subsystem owns each piece of hardware, and nothing else touches it. If two classes both call elevatorMotor.set(), you have already lost.
src/main/java/frc/robot/
├── Robot.java // lifecycle; keep it nearly empty
├── RobotContainer.java // subsystems, controls, button bindings
├── Constants.java // every tunable number
├── subsystems/
│ ├── Drivetrain.java
│ ├── Elevator.java
│ └── Intake.java
└── commands/
└── ScoreSequence.java
Robot.java should contain almost nothing. The scheduler runs, and that is it. If logic accumulates there, it is a sign something is wrong.
A subsystem should expose intent, not implementation. Compare:
// Bad: caller has to know about motors, units, and safety
elevator.motor.set(0.6);
// Good: caller expresses what it wants
elevator.goToHeight(Meters.of(1.2));
The second version lets the subsystem enforce soft limits, apply feedforward, handle unit conversion, and change from a duty-cycle to a closed-loop implementation — all without any caller changing.
A reasonable subsystem skeleton:
public class Elevator extends SubsystemBase {
private final SparkMax leader = new SparkMax(Constants.Elevator.LEADER_ID, kBrushless);
private final RelativeEncoder encoder = leader.getEncoder();
public Elevator() {
// configure current limits, brake mode, conversion factors here
}
public Command goToHeight(double meters) {
return run(() -> setTargetInternal(meters))
.until(this::atTarget);
}
public double getHeight() { return encoder.getPosition(); }
@Override
public void periodic() {
// publish telemetry every loop
}
}
Two things to notice. The motor is private — nothing outside can touch it. And goToHeight returns a Command, so callers can compose it with other commands without knowing anything about elevators.
Every subsystem should have a default command: what it does when nothing else has claimed it.
For a drivetrain, the default command reads the joysticks and drives. For an elevator, the default might hold its current position. For an intake, stop.
The important consequence is that the safe state is the default, not something you have to remember to command. If a scoring command is cancelled halfway through, the subsystems fall back to their defaults automatically, and the robot ends up somewhere sane rather than with an intake still spinning.
Rule 7.2 — Define the safe state as a default command, not as cleanup code.
Software architecture and driver experience are the same topic, because the driver is a component in the control loop.
Joysticks do not read exactly zero at rest. Without a deadband, the robot creeps. Apply one:
double x = MathUtil.applyDeadband(controller.getLeftX(), 0.08);
Use the WPILib helper rather than a naive if (Math.abs(x) < 0.08) x = 0; — the naive version creates a discontinuity where output jumps from 0 to 0.08 the instant the stick passes the threshold. The helper rescales so output starts from zero smoothly.
Squaring or cubing the input while preserving sign gives fine control near centre and full power at the extremes:
double scaled = Math.copySign(x * x, x);
Almost every driver prefers this once they have tried it. Let them choose — it is a preference, not a rule.
A driver manually holding an elevator at the right height with a joystick, under match pressure, with a defender pushing them, will be slower and less consistent than a button that commands a known position. Every mechanism with discrete useful positions should have buttons for them.
Conditions the robot enforces regardless of what the driver asks:
Implement these inside the subsystem or as command conditions, never as something the driver is expected to remember.
For a holonomic drivetrain, field-relative control means pushing the stick forward moves the robot away from the driver station regardless of which way the robot is facing. It is the single largest usability improvement available on a swerve robot.
It depends entirely on the gyro being correct. A drifted or wrongly-zeroed gyro makes the robot uncontrollable in a way drivers find deeply disorienting. Always provide a re-zero button, and make sure the driver knows it exists.
[ANECDOTE SLOT] — A control mapping that seemed reasonable in the shop and fell apart under match pressure. Driver feedback stories are valuable because students consistently underrate this subsystem.
Every magic number goes in Constants.java, organised by subsystem, with units in the name:
public final class Constants {
public static final class Elevator {
public static final int LEADER_ID = 21;
public static final double MAX_HEIGHT_METERS = 1.45;
public static final double kP = 2.4;
}
}
The reason is not tidiness. It is that during a competition you will need to change a number and re-deploy in under four minutes, and hunting through logic for a hardcoded 0.6 is how teams miss matches.
Better still: use WPILib’s units library (Meters.of(1.2)) so the compiler catches unit errors. A surprising number of mechanism bugs are somebody mixing rotations with metres.
Git is not optional, and the reason is specific: you need to be able to get back to the version that worked.
A workable minimum discipline:
Rule 7.3 — The code on the robot at the end of every build session gets committed, even if it is ugly. “I’ll commit it tomorrow” is how teams lose an evening’s work.
Time: 90 minutes. Equipment: Laptops with WPILib, a training robot with at least two mechanisms.
Provide students with a deliberately bad single-file robot program — everything in teleopPeriodic(), hardcoded numbers, two places that command the same motor, no interlocks. (Write this yourself beforehand; making it realistically bad is worth the effort.)
Refactor it into command-based: subsystems that own hardware, commands that express actions, constants extracted, at least one interlock implemented properly.
Then deploy it and verify it behaves identically — refactoring means changing structure, not behaviour.
Evidence of learning: A compiling, deployed project; a subsystem interface diagram; and a short written list of the bugs the original structure made possible.
Time: 60 minutes. Equipment: A drivable robot with at least one mechanism, a marked course, a stopwatch.
Evidence of learning: Before-and-after times, a written control map, and a list of changes traced to specific driver feedback.
Time: 30 minutes. Equipment: A repository with tagged releases.
A mentor commits a subtle breaking change to main without telling anyone. Students must notice the robot misbehaving, identify that software changed, and restore the tagged working release — under a ten-minute limit, as if between matches.
Evidence of learning: A written recovery procedure short enough to fit on one pit card.
Open-loop code gets a robot moving. Lesson 8 — Feedback Control and Autonomous Motion is about making it go where you actually told it to.
