Slipknot

Fifteen seconds of autonomous, and the scoring cycle is: shoot what you are holding, drive to a note, pick it up, drive back, shoot. Four notes means that cycle four times with no human in the loop, and every stage of it can fail silently. The note does not always get picked up. The flywheels do not always reach speed before the feeder pushes. A path that assumes the last one finished cleanly starts from the wrong pose.

A routine written as a list of timed steps handles none of that. It handles the run where everything works, and on the run where the note slips it keeps going anyway and scores nothing for the rest of the period.

One state machine, two jobs. StateMachine<S> is thirty lines: a map from a state to a function of elapsed milliseconds, returning the next state or null to stay. Three helpers build the states that come up — addTimerState for "do this for N ms then move on", addBoolState for "do this until the condition holds", addOffState for a terminal state. That is the whole abstraction, and it drives both the teleoperated shot sequence and every autonomous routine, which means the autonomous is built out of pieces that have been running all match rather than out of code that only executes in the first fifteen seconds.

The four-note autonomous is sixteen states over eight trajectories. NonGoofyCenterAuto alternates in-and-out PathPlanner paths (InN2/OutN2, InN3/OutN3, …) with shoot states, and the transitions are conditions rather than clocks: a shoot state ends when the IR beam-break says the note has left, an approach state ends when the trajectory reports it has finished sampling. The intake runs only after a per-path settling delay, tuned separately per note, so the feeder is not dragging against a note that has not seated yet.

Shot readiness is measured, not waited for. Shooter runs two TalonFX flywheel motors under Phoenix 6 VelocityVoltage with a velocity-feedforward gain, and shooterRampedUp() reports true when the mean measured wheel speed passes 90 rps. The speaker shot transitions SPIN_UP → SHOOT on that signal; the fixed 1500 ms timer version of the same transition is still in the file directly above it, commented out. That one-line change is the difference between a shot that is on time and a shot that is on schedule.

An interface, so manual and automatic are not two copies. IFS — intake, feeder, shooter — is a four-method interface with two implementations. IFSManual maps controls straight through; IFSAuto puts the same subsystems behind three state machines, one per shot type (speaker, amp, and a max-power shot), and adds the operator overrides that matter at competition: an intake override for when the beam-break is lying, and live amp-speed trim on the second controller's D-pad.

Hand-rolled PID with the two terms that were actually needed. swervedrive/PID decays the integral term instead of accumulating it without bound, and clamps output slew to a configured maximum acceleration. Every gain is readable and writable from SmartDashboard, so tuning happened between matches rather than between builds.