Lemonlight

Scoring on the reef is a two-axis problem that has to close in about a second and a half. The robot has to be at the right place relative to an AprilTag, and the elevator has to be at the right one of four heights, and neither can wait for the other. Get the order wrong and the carriage swings into the reef; run the elevator while a coral is still crossing the intake and it jams against the frame.

The elevator is also the part of the robot with the most stored energy and the shortest distance to a broken mechanism. Driving it open-loop means every height is the driver's thumb and the top of its travel is whatever stops it.

The elevator is a Motion Magic position loop, not a speed. Two Krakens run as a leader-follower pair under Phoenix 6 MotionMagicVoltage, profiled at 40 rps cruise, 72.5 rps² acceleration and a jerk limit, with position gains on slot 0 and a static feedforward term for the friction it has to break out of. Eight named setpoints — ground, L2, L3, L4, the algae presets and a default height chosen to clear the reef without blocking the front camera — are addressed by name rather than by number, so the same height means the same thing in teleop and in autonomous. "At target" is a half-rotation tolerance read off a value cached in readPeriodicInputs, not a live motor query, because the callers are on other threads.

Three interlocks, each guarding a different failure. The move is refused outright while the intake LaserCAN reads under 100 mm — a coral in the throat means the elevator does not go anywhere. A mSafeStop flag is raised on every enable, so a robot powered on mid-cycle holds position rather than driving to a target left over from the last match. A bank of magnetic switches on DIO 0–4 backs the encoder at ground, L2, L3, L4 and the algae position, and was live at competition — an encoder that has drifted still reports a plausible number, and these are the sensors that do not. Stator and supply current are both limited to 40 A, which is what turns a jam into a stall instead of a fire.

Autonomous routines are a string. AutoStringAuto parses a small language into nested actions: bare tokens name field points and generate the trajectory between them, brackets run actions in parallel, parentheses in series, braces repeat, and CIntake, CShoot, Elevator 4 and Snap LEFTBAR are the verbs. A new routine is a new string, which means it can be changed between matches by someone who is not editing Java.

Approaches are generated at runtime, not drawn in advance. SnapToTag takes an offset name — left bar, right bar, centre, coral station, hang — looks up the nearest matching AprilTag from the field layout, and builds a PathPlanner path from the robot's current pose to that offset, with its own constraints zone for the final approach. The same action is bound to the driver's bumpers in teleop and dropped into autonomous strings unchanged.

Faster swerve, deliberately. The drivetrain is SDS MK4i on Kraken X60s at the L3 drive ratio — the fast option of the three — with the module constants pulled from a shared table rather than transcribed. Pose comes from a wheel tracker fused with a multi-camera vision manager that runs candidate poses past an acceptor before they are allowed to move the estimate.