Introduction
In FTC, your OpMode’s loop() runs over and over—dozens of times per second. If you put a long, linear sequence of actions (drive here, wait, shoot, drive there) in that loop with blocking calls or sleeps, the robot can’t respond to the driver or to the game clock. State machines let you break behavior into discrete states and transitions: each loop you update the current state and check whether it’s time to switch. Everything stays non-blocking so the loop keeps running and other logic (e.g. path following, telemetry) can run in the same loop.
This tutorial teaches you how to design and implement your own state machine library. You’ll build three pieces: a Task (a single unit of behavior with enter/update/exit), a Sequence (ordered steps that run one after another), and a Machine (states and transitions driven by an enum). The ideas are drawn from real FTC code; we explain the “why” and give you minimal implementation patterns so you can adapt them to your team’s style and language (Java or Kotlin).
For the design process and preplanning steps (game plan → states → transitions, UML diagram, and how we use it on the robot), see the State machine tutorial. For the theory behind finite state machines, see Game Manual 0: Finite State Machines. For execution order and timing details, see How it works below.
What is an Infinite State Machine?
The Infinite State Machine (ISM) is a state machine library designed specifically for FTC robotics. It consists of three core components:
- StateMachine — The top-level controller that manages states and transitions, generic over an enum of state names
- StateSequence — An ordered list of steps that run sequentially without blocking the main loop
- StateTask — A single unit of behavior with optional enter, update, and exit hooks
Why "Infinite"?
The name "Infinite State Machine" emphasizes that the number of states and transitions is not fixed by the library—you define as many as your game plan requires. Unlike some state machine implementations that have hard limits or predefined state types, the ISM lets you create an unbounded number of states and transitions. Your state graph can grow from a simple three-state autonomous (IDLE → SHOOT → END) to a complex multi-cycle routine with dozens of states (INTAKE_1, INTAKE_2, INTAKE_3, SHOOT, GATE, etc.), all using the same building blocks.
In formal computer science terms, a "finite state machine" still has finitely many states (even if that number is large). The ISM's "infinite" name highlights its extensibility: there's no built-in limit on how many states you can add, and the library doesn't constrain what each state can do. You're free to design state machines of any complexity.
Relation to Finite State Machines
The ISM follows the same fundamental principles as finite state machines: discrete states, transitions between states based on conditions, and a clear lifecycle (enter → update → exit). For the theoretical background, see Game Manual 0: Finite State Machines. The ISM library implements these concepts in a way that's practical for FTC: non-blocking execution, integration with path following, and support for sequences of actions within a single state.
Why use it?
State machines solve a fundamental problem in FTC: how to run complex, multi-step behaviors without blocking the main loop. Benefits include:
- Non-blocking execution — The loop keeps running, so path following, telemetry, and driver input can all work simultaneously
- Clear phases — Each state represents one "thing the robot is doing," making the code readable and debuggable
- Testable behavior — Tasks and sequences can be tested independently
- Scalability — The same patterns work for simple autonomous (3 states) and complex routines (20+ states)
- Reusability — Tasks can be reused across multiple states; sequences can be shared between similar behaviors
Whether you're building a simple "wait → shoot → leave" autonomous or a complex multi-cycle routine with conditional routing, the ISM provides the same clean abstractions and non-blocking execution model.
Design: The three building blocks
Before writing code, it helps to fix the abstractions.
1. Task
A Task is one unit of behavior. It has three optional hooks: onEnter (run once when the task starts), onUpdate(dt) (run every loop with delta time in seconds), and onExit (run once when the task ends). For example: “turn the intake motor on” in onEnter and “turn it off” in onExit; or “run the shooter PID” in onUpdate. Tasks are reusable: the same “intake on” task might be used in several states. Storing callbacks (e.g. Runnable, Consumer<Double>) keeps the design flexible and testable.
2. Sequence
A Sequence is an ordered list of steps. Each step is a Task plus an optional condition and optional minimum time. The sequence runs one step at a time: when the current step’s condition is true and its minimum time has elapsed, it runs that step’s onExit, advances to the next step, and runs the next step’s onEnter. When there are no steps left, the sequence is “complete.” This gives you multi-step flows (e.g. “intake on → open gate → wait 1.5 s → close gate → intake off”) without blocking the loop—you just call sequence.update(dt) each loop until it returns true.
3. Machine
The Machine is the top-level controller. It’s generic over an enum of state names (e.g. IDLE, SHOOT, INTAKE). For each enum value you define a state: what happens on enter, update, and exit (often by attaching a Task or a Sequence). You also define transitions: “when in state X, if condition C is true and at least minTime seconds have passed, go to state Y.” Transitions are attached to the state you just defined—so we use a “last defined state” rule: after you add a state, the next transition(s) you add apply to that state until you add another state. That keeps the fluent API readable (state, then its transitions, then the next state). You call setInitial(enum) once and update() every loop; the machine runs the current state’s update and then checks transitions in order.
Using an enum gives you type safety and a fixed set of states; the fluent API (methods return this) lets you build the whole machine in one readable chain. Optional extras include “onComplete” transitions (fire only when a state finishes, e.g. a sequence completes), “transitionFrom(previousState, nextState)” so multiple states can route to the same next state, and “blocking” vs “volatile” sequences (whether you can leave a sequence state early via another transition).
How it works
Understanding the execution model helps you debug state machines and reason about timing. This section explains what happens each loop iteration and how states transition.
One update per loop
The OpMode calls fsm.update() once per loop() iteration. There's no threading or background processing—everything runs synchronously in the main loop. The machine runs the current state's update and then evaluates transitions; at most one transition fires per update call. This prevents cascading transitions and keeps behavior predictable.
Order of operations
Each time update() is called, the machine performs these steps in order:
- Initialization (first call only) — If this is the first
update()call, set the current state to the initial state (fromsetInitial()), run that state'sonEnter, and record the current time asstateEnterTime. Mark the machine as started. - Compute delta time — Calculate
dt(delta time in seconds) as the difference between the current time andlastUpdateTime. This represents how much time has passed since the last update. - Run current state's update — Call the current state's
onUpdate(dt)callback. This runs every loop while you're in that state. If the state uses a Sequence, the sequence'supdate(dt, now)is called here. - Evaluate transitions — If the current state is not a "volatile" sequence (or if it is volatile, only check non-onComplete transitions), iterate through the state's transitions in order:
- Check if the transition's
fromStatematches the previous state (or is unspecified) - Check if
timeInState >= minTime(wheretimeInState = (now - stateEnterTime) / 1e9) - Check if the transition's condition is true
- If it's an "onComplete" transition, also check that the state reports "complete" (e.g., a sequence has finished all steps)
- If all checks pass: run the current state's
onExit, setpreviousState = currentState, setcurrentStateto the next state, run the new state'sonEnter, recordstateEnterTime = now, and break (only one transition fires per update)
- Check if the transition's
- Store update time — Save the current time as
lastUpdateTimefor the next delta time calculation.
This order ensures that state behavior (onUpdate) runs before transitions are checked, and that cleanup (onExit) happens before setup (onEnter) when transitioning.
Lifecycle of a state
Each state follows a predictable lifecycle:
- Enter —
onEnterruns once when you enter the state. Use this for one-time setup: set motor power, move servos, start paths, initialize variables. - Update —
onUpdate(dt)runs every loop while you're in that state. Use this for continuous behavior: run PID controllers, update telemetry, check sensors. Thedtparameter tells you how much time has passed since the last update, useful for time-based calculations. - Exit —
onExitruns once when you leave the state (when a transition fires). Use this for cleanup: stop motors, reset servos, cancel paths, so the next state starts from a known condition.
The lifecycle is: onEnter → (loop: onUpdate → check transitions) → onExit → (next state's onEnter).
Transitions
Transitions are evaluated in the order they were added to the state. The first matching transition fires, and evaluation stops (no other transitions are checked that loop). This means order matters: if you have two transitions with overlapping conditions, put the more specific one first.
Each transition has:
- Condition — A boolean function that must return
truefor the transition to fire - Next state — The enum value to transition to
- Minimum time — The minimum seconds that must pass in the current state before this transition can fire (default: 0.0)
- Optional: onComplete — If true, only fire when the state reports "complete" (e.g., a sequence has finished all steps)
- Optional: fromState — If specified, only fire when the previous state was this enum (useful for routing: multiple states can transition to the same next state, and you route by origin)
Time in state is calculated as (now - stateEnterTime) / 1e9 (converting nanoseconds to seconds). This is compared against each transition's minTime. The minTime prevents transitions from firing too quickly (e.g., give a servo time to move, or wait for a sensor to stabilize).
Sequence internals
When a state uses a Sequence, the sequence manages its own internal state:
- Steps — An ordered list of steps, each with a Task, optional condition, and optional
minTime - Current index — Which step is currently active (starts at 0)
- Step enter time — When the current step started (for calculating time in step)
When the sequence is entered (state's onEnter), the sequence calls start(now): set the index to 0, record stepEnterTime, and run the first step's onEnter.
Each update, the sequence:
- Runs the current step's
onUpdate(dt) - Calculates
timeInStep = (now - stepEnterTime) / 1e9 - If
timeInStep >= minTimeand the step's condition is true:- Run the current step's
onExit - Increment the index
- If there are more steps: record
stepEnterTime = nowand run the next step'sonEnter - If there are no more steps: return
true(sequence is complete)
- Run the current step's
If the state is left early (e.g., a volatile sequence interrupted by another transition), the machine calls sequence.reset(now): run the current step's onExit, reset the index to 0, and record the reset time. This ensures cleanup happens even if the sequence doesn't finish.
Blocking vs volatile sequences
A blocking sequence state only evaluates transitions (including "onComplete") after the sequence's update() returns true (all steps are done). This means you can't leave the state until the sequence completes.
A volatile sequence state evaluates non-onComplete transitions every loop, even while the sequence is still running. This allows interrupting the sequence early (e.g., driver cancel) before it finishes. If a non-onComplete transition fires, the sequence is reset and the state exits.
Timing
All timing in the ISM uses consistent units:
- Delta time (
dt) — Always in seconds (e.g., 0.02 for ~50 Hz loop rate) - Minimum time (
minTime) — Always in seconds (e.g., 1.5 for "wait 1.5 seconds") - Internal timestamps — Stored in nanoseconds (using
ElapsedTime.nanoseconds()orSystem.nanoTime()) for precision
When comparing times, convert nanoseconds to seconds: (now - stateEnterTime) / 1e9. This gives you sub-millisecond precision while keeping the API simple (all user-facing times are in seconds).
The "minimum time" is always calculated as "time in current state" (for state transitions) or "time in current step" (for sequence steps). This is measured from when you entered the state/step, not from when the machine started.
Path following integration
Path-following states typically work like this:
- onEnter — Call
follower.followPath(path)to start following a path - onUpdate — Call
follower.update()every loop to update the path follower's control loop - Transition condition —
() -> !follower.isBusy()or() -> follower.isDone()to transition when the path is complete
The path follower runs in the same loop as the state machine: fsm.update() runs first, then follower.update() runs right after. This keeps motion and state synchronized—the state machine knows when paths are done, and paths update every loop just like states do.
You can combine path following with other tasks (e.g., run intake while driving) by using parallel tasks or by running both the path follower and the task in the state's onUpdate.
Implementing the Task
The Task is the smallest building block. You need to store three callbacks: one that runs once when the task is entered, one that runs every update with delta time, and one that runs once when the task is exited. In Java that’s Runnable for enter/exit and Consumer<Double> for update; in Kotlin you can use () -> Unit and (Double) -> Unit. For missing callbacks, use no-ops (empty lambdas) rather than null so you never have to null-check when invoking.
Factory methods make common cases easy: “only onEnter,” “onEnter and onExit,” “only onUpdate,” and “all three.” That way users don’t have to pass three arguments when they only need one. Below is a minimal shape: a class that holds the three callbacks and exposes them (e.g. getters), plus one static factory. You can add more overloads (e.g. create(onEnter), create(onUpdate), empty()) as needed.
public class StateTask {
private final Runnable onEnter;
private final Consumer<Double> onUpdate;
private final Runnable onExit;
public StateTask(Runnable onEnter, Consumer<Double> onUpdate, Runnable onExit) {
this.onEnter = onEnter != null ? onEnter : () -> {};
this.onUpdate = onUpdate != null ? onUpdate : dt -> {};
this.onExit = onExit != null ? onExit : () -> {};
}
public static StateTask create(Runnable onEnter, Consumer<Double> onUpdate, Runnable onExit) {
return new StateTask(onEnter, onUpdate, onExit);
}
public Runnable getOnEnter() { return onEnter; }
public Consumer<Double> getOnUpdate() { return onUpdate; }
public Runnable getOnExit() { return onExit; }
}
class StateTask(
private val onEnter: () -> Unit,
private val onUpdate: (Double) -> Unit,
private val onExit: () -> Unit
) {
fun getOnEnter(): () -> Unit = onEnter
fun getOnUpdate(): (Double) -> Unit = onUpdate
fun getOnExit(): () -> Unit = onExit
companion object {
fun create(
onEnter: () -> Unit = {},
onUpdate: (Double) -> Unit = {},
onExit: () -> Unit = {}
) = StateTask(onEnter, onUpdate, onExit)
}
}
Why each callback? onEnter is for one-time setup (motor power, servo position). onUpdate is for continuous behavior (PID, telemetry) that needs delta time. onExit is for cleanup (stop motor, reset servo) so the next state starts from a known condition.
Implementing the Sequence
A Sequence holds an ordered list of steps. Each step has a Task, an optional condition (BooleanSupplier or () -> Boolean), and an optional minimum time in seconds. When the sequence is entered, you run the first step’s onEnter and record the start time. Each update you run the current step’s onUpdate; then, if timeInStep >= minTime and the condition is true, you run the current step’s onExit, advance the index, and run the next step’s onEnter (if any). When the index is past the last step, the sequence is complete—return true so the caller (e.g. the machine) can trigger an “onComplete” transition.
You’ll need reset(now) to clear the index and optionally run the current step’s onExit when the sequence is left early (e.g. volatile transition). The machine will call reset when exiting the state and start/update when entering and each loop. Below is a minimal sketch: a list of steps, current index, and step-enter time; update advances when condition and minTime are met.
public class StateSequence {
public static class Step {
final StateTask task;
final BooleanSupplier condition;
final double minTime;
Step(StateTask task, BooleanSupplier condition, double minTime) {
this.task = task;
this.condition = condition;
this.minTime = minTime;
}
}
private final List<Step> steps = new ArrayList<>();
private int currentStep = 0;
private long stepEnterTime;
public StateSequence step(StateTask task, BooleanSupplier condition, double minTime) {
steps.add(new Step(task, condition != null ? condition : () -> true, minTime));
return this;
}
public void reset(long now) {
if (currentStep < steps.size()) {
Runnable onExit = steps.get(currentStep).task.getOnExit();
if (onExit != null) onExit.run();
}
currentStep = 0;
stepEnterTime = now;
}
void start(long now) {
stepEnterTime = now;
if (!steps.isEmpty())
steps.get(0).task.getOnEnter().run();
}
boolean update(double dt, long now) {
if (currentStep >= steps.size()) return true;
Step step = steps.get(currentStep);
step.task.getOnUpdate().accept(dt);
double timeInStep = (now - stepEnterTime) / 1e9;
if (timeInStep >= step.minTime && step.condition.getAsBoolean()) {
step.task.getOnExit().run();
currentStep++;
if (currentStep < steps.size()) {
stepEnterTime = now;
steps.get(currentStep).task.getOnEnter().run();
}
}
return currentStep >= steps.size();
}
}
class StateSequence {
data class Step(val task: StateTask, val condition: () -> Boolean, val minTime: Double)
private val steps = mutableListOf<Step>()
private var currentStep = 0
private var stepEnterTime = 0L
fun step(task: StateTask, condition: () -> Boolean = { true }, minTime: Double = 0.0): StateSequence {
steps.add(Step(task, condition, minTime))
return this
}
fun reset(now: Long) {
if (currentStep < steps.size) steps[currentStep].task.getOnExit().invoke()
currentStep = 0
stepEnterTime = now
}
fun start(now: Long) {
stepEnterTime = now
if (steps.isNotEmpty()) steps[0].task.getOnEnter().invoke()
}
fun update(dt: Double, now: Long): Boolean {
if (currentStep >= steps.size) return true
val step = steps[currentStep]
step.task.getOnUpdate().invoke(dt)
val timeInStep = (now - stepEnterTime) / 1e9
if (timeInStep >= step.minTime && step.condition()) {
step.task.getOnExit().invoke()
currentStep++
if (currentStep < steps.size) {
stepEnterTime = now
steps[currentStep].task.getOnEnter().invoke()
}
}
return currentStep >= steps.size
}
}
Why minTime and condition? minTime avoids advancing too fast (e.g. give the gate servo time to move). The condition lets you wait for sensor feedback (e.g. “beam broken”) or always advance (() -> true). Together they keep the loop non-blocking: you never sleep, you just return “not done” until the step is satisfied.
Implementing the Machine
The Machine is generic over an enum type E. You maintain a map from enum to State; each State holds enter/update/exit callbacks and a list of Transitions. A Transition has a condition, a next-state enum, a minimum time in the current state, and optionally “onComplete” (only fire when the state reports done, e.g. sequence complete) and “fromState” (only fire when the previous state was that enum—so multiple states can transition to the same next state and you route by origin).
Each loop: (1) If not started, set current state to the initial state and run its enter. (2) Compute dt from last update time. (3) Run current state’s update. (4) If the state is not a “volatile” sequence, evaluate transitions in order: if the transition’s fromState matches (or is unspecified), minTime is met, and the condition is true (and if onComplete, the state is complete), then run current state’s exit, set current to the next state, run its enter, and break. (5) Store current time for next dt.
Use a single “last defined state” reference: when you add a state, set lastDefinedState = that state; when you add a transition, append it to lastDefinedState. That way the fluent chain “.state(A, ...).transition(cond, B).state(B, ...)” correctly attaches the transition to A. Below is a minimal core: add state, add transition, setInitial, update.
public class StateMachine<E extends Enum<E>> {
private final Map<E, State> states = new HashMap<>();
private State currentState, previousState, lastDefinedState;
private E initialStateE;
private boolean hasStarted = false;
private long stateEnterTime, lastUpdateTime;
private final ElapsedTime timer = new ElapsedTime();
public StateMachine<E> state(E id, Runnable onEnter, Consumer<Double> onUpdate, Runnable onExit) {
State s = new State(id, onEnter, onUpdate, onExit);
states.put(id, s);
lastDefinedState = s;
return this;
}
public StateMachine<E> transition(BooleanSupplier condition, E next, double minTime) {
if (lastDefinedState == null) throw new IllegalStateException("Define a state first");
lastDefinedState.transitions.add(new Transition(condition, next, minTime));
return this;
}
public void setInitial(E id) {
if (!states.containsKey(id)) throw new IllegalArgumentException("Unknown state: " + id);
initialStateE = id;
}
public void update() {
long now = timer.nanoseconds();
if (!hasStarted) {
currentState = states.get(initialStateE);
stateEnterTime = now;
currentState.enter();
hasStarted = true;
}
double dt = (now - lastUpdateTime) / 1e9;
lastUpdateTime = now;
if (currentState != null) currentState.update(dt);
for (Transition t : currentState.transitions) {
double timeIn = (now - stateEnterTime) / 1e9;
if (timeIn >= t.minTime && t.condition.getAsBoolean()) {
currentState.exit();
previousState = currentState;
currentState = states.get(t.nextStateE);
stateEnterTime = now;
currentState.enter();
break;
}
}
}
}
class StateMachine<E : Enum<E>> {
private val states = mutableMapOf<E, State>()
private var currentState: State? = null
private var previousState: State? = null
private var lastDefinedState: State? = null
private var initialStateE: E? = null
private var hasStarted = false
private var stateEnterTime = 0L
private var lastUpdateTime = 0L
private val timer = ElapsedTime()
fun state(id: E, onEnter: () -> Unit, onUpdate: (Double) -> Unit, onExit: () -> Unit): StateMachine<E> {
val s = State(id, onEnter, onUpdate, onExit)
states[id] = s
lastDefinedState = s
return this
}
fun transition(condition: () -> Boolean, next: E, minTime: Double = 0.0): StateMachine<E> {
lastDefinedState?.transitions?.add(Transition(condition, next, minTime))
?: throw IllegalStateException("Define a state first")
return this
}
fun setInitial(id: E) {
require(states.containsKey(id)) { "Unknown state: $id" }
initialStateE = id
}
fun update() {
val now = timer.nanoseconds()
if (!hasStarted) {
currentState = states[initialStateE!!]
stateEnterTime = now
currentState!!.enter()
hasStarted = true
}
val dt = (now - lastUpdateTime) / 1e9
lastUpdateTime = now
currentState?.update(dt)
currentState?.transitions?.forEach { t ->
val timeIn = (now - stateEnterTime) / 1e9
if (timeIn >= t.minTime && t.condition()) {
currentState!!.exit()
previousState = currentState
currentState = states[t.nextStateE]
stateEnterTime = now
currentState!!.enter()
return@forEach
}
}
}
}
You’ll also need inner classes State (callbacks + list of Transition) and Transition (condition, nextStateE, minTime). Extend with delay(seconds, next) as transition(() -> true, next, seconds), onComplete(next) for sequence states, and transitionFrom(from, next) by storing fromStateE in the transition and skipping when previousState.stateE != fromStateE.
Using your machine in an OpMode
Build the machine once, typically in init() after hardware and paths are set up. Call setInitial(yourEnum.INITIAL) so the first update() enters that state. In loop(), call machine.update() once per iteration—and, if you use path following, call your follower’s update right after so motion and state stay in sync. Optionally log getCurrentState() and timeInState() to telemetry for debugging.
Example flow: start in IDLE (e.g. rev shooter, gate closed). After 1.5 s, go to PRELOAD: follow a path to the shooting position while running the intake task. When the path is done, go to SHOOT: run a sequence (intake on, open gate, wait, close gate, intake off). When the sequence completes, go to INTAKE_1 (or INTAKE_2/INTAKE_3 depending on where you came from—use onCompleteFrom). In INTAKE, run the intake task and after 2.5 s go to LEAVE; follow the leave path to END and stop. The exact states and transitions depend on your game; the pattern is: define states, attach transitions to the last defined state, set initial, update every loop.
@Override
public void init() {
initHardware();
buildPaths();
buildStateMachine();
fsm.setInitial(States.IDLE);
}
@Override
public void loop() {
fsm.update();
if (follower != null) follower.update();
telemetry.addData("State", fsm.getCurrentState());
telemetry.addData("Time in state", fsm.timeInState());
}
override fun init() {
initHardware()
buildPaths()
buildStateMachine()
fsm.setInitial(States.IDLE)
}
override fun loop() {
fsm.update()
follower?.update()
telemetry.addData("State", fsm.getCurrentState())
telemetry.addData("Time in state", fsm.timeInState())
}
Optional extensions
- Parallel tasks – One state that runs several tasks at once: in enter, run every task’s onEnter; in update, run every task’s onUpdate(dt); in exit, run every task’s onExit. Useful for “idle” (e.g. rev shooter + keep gate closed) without blocking.
- Path-following state – A state that calls your path follower’s
followPath(path)in enter and transitions when!follower.isBusy(). You can combine it with a task (e.g. run intake while driving) by running both in enter/update/exit. - Blocking vs volatile sequence – In a blocking sequence state, only evaluate transitions (including “onComplete”) after the sequence’s update returns true. In a volatile sequence state, evaluate non-onComplete transitions every loop so the user can interrupt (e.g. cancel) before the sequence finishes.
Tutorial stepper
Walk through the build in order: define states, implement the Task, then the Sequence, then the Machine, then wire it into your OpMode. Use the buttons below to move step by step.
Step 1: Define your states
Create an enum with one value per state. Use clear names (e.g. IDLE, PRELOAD, SHOOT, INTAKE, END) so the flow is easy to read. The machine will be generic over this enum.
public enum States {
IDLE,
PRELOAD,
SHOOT,
INTAKE,
INTAKE_1,
INTAKE_2,
LEAVE,
END
}
enum class States {
IDLE,
PRELOAD,
SHOOT,
INTAKE,
INTAKE_1,
INTAKE_2,
LEAVE,
END
}
Step 2: Implement the Task
Add a class that holds onEnter, onUpdate(dt), and onExit callbacks. Provide factory methods so you can build tasks without passing three args every time.
StateTask revShooter = StateTask.create((Double dt) ->
turret.setTargetVelocity(calculatedVelocity));
StateTask intakeOn = StateTask.create(
() -> intake.setPower(1.0),
() -> intake.setPower(0.0));
StateTask stopAll = StateTask.create(() -> { /* stop motors */ });
val revShooter = StateTask.create(onUpdate = { dt ->
turret.setTargetVelocity(calculatedVelocity)
})
val intakeOn = StateTask.create(
onEnter = { intake.setPower(1.0) },
onExit = { intake.setPower(0.0) }
)
val stopAll = StateTask.create(onEnter = { /* stop motors */ })
Step 3: Implement the Sequence
Build a sequence of steps; each step is a Task plus optional condition and minTime. When the current step is satisfied, run its onExit, advance, and run the next step's onEnter. Return true when all steps are done.
StateSequence shootSeq = new StateSequence()
.step(StateTask.create(() -> intake.setPower(1.0)))
.step(StateTask.create(() -> gate.open()), 1.5)
.step(StateTask.create(() -> gate.close()), 0.25)
.step(StateTask.create(() -> intake.setPower(0.0)));
// In the machine: blockingSequence(States.SHOOT, shootSeq)
// .onCompleteFrom(States.PRELOAD, States.INTAKE_1);
val shootSeq = StateSequence()
.step(StateTask.create(onEnter = { intake.setPower(1.0) }))
.step(StateTask.create(onEnter = { gate.open() }), 1.5)
.step(StateTask.create(onEnter = { gate.close() }), 0.25)
.step(StateTask.create(onEnter = { intake.setPower(0.0) }))
// In the machine: blockingSequence(States.SHOOT, shootSeq)
// .onCompleteFrom(States.PRELOAD, States.INTAKE_1)
Step 4: Implement the Machine
Add states with state(id, onEnter, onUpdate, onExit) or state(id, task). After each state, add its transitions. End with setInitial(States.IDLE).
fsm = new StateMachine<States>()
.state(States.IDLE, revShooter, closeGate)
.delay(1.5, States.PRELOAD)
.state(States.PRELOAD, preloadTask)
.transition(() -> !follower.isBusy(), States.SHOOT)
.blockingSequence(States.SHOOT, shootSeq)
.onCompleteFrom(States.PRELOAD, States.INTAKE_1)
.state(States.INTAKE_1, goToIntakeThenIn)
.state(States.INTAKE, intakeOn)
.transition(() -> true, States.LEAVE, 2.5)
.state(States.LEAVE, leavePath)
.state(States.END, stopAll);
fsm.setInitial(States.IDLE);
fsm = StateMachine<States>()
.state(States.IDLE, revShooter, closeGate)
.delay(1.5, States.PRELOAD)
.state(States.PRELOAD, preloadTask)
.transition({ !follower.isBusy() }, States.SHOOT)
.blockingSequence(States.SHOOT, shootSeq)
.onCompleteFrom(States.PRELOAD, States.INTAKE_1)
.state(States.INTAKE_1, goToIntakeThenIn)
.state(States.INTAKE, intakeOn)
.transition({ true }, States.LEAVE, 2.5)
.state(States.LEAVE, leavePath)
.state(States.END, stopAll)
fsm.setInitial(States.IDLE)
Step 5: Wire into your OpMode
In init(): build hardware and paths, then build the state machine and call setInitial. In loop(): call fsm.update() once, then follower.update() if you use path following. Log getCurrentState() and timeInState() to telemetry for debugging.
@Override
public void init() {
initHardware();
buildPaths();
buildStateMachine();
fsm.setInitial(States.IDLE);
}
@Override
public void loop() {
fsm.update();
if (follower != null) follower.update();
telemetry.addData("State", fsm.getCurrentState());
telemetry.addData("Time in state", fsm.timeInState());
}
override fun init() {
initHardware()
buildPaths()
buildStateMachine()
fsm.setInitial(States.IDLE)
}
override fun loop() {
fsm.update()
follower?.update()
telemetry.addData("State", fsm.getCurrentState())
telemetry.addData("Time in state", fsm.timeInState())
}
Glossary & Tips
- Infinite State Machine (ISM)
- The state machine library for FTC consisting of StateMachine, StateSequence, and StateTask. The name emphasizes extensibility—there's no built-in limit on how many states you can add. See What is an Infinite State Machine? for details.
- State
- One of the enum values in your state machine. Each state has optional onEnter, onUpdate, and onExit behavior.
- Transition
- A rule to switch from the current state to another when a condition is true and (if set) minimum time in state has elapsed. Transitions are evaluated in order; only the first matching one fires per update. See How it works for execution details.
- Lifecycle (enter/update/exit)
- The three phases of a state: onEnter runs once when entering, onUpdate runs every loop while in the state, onExit runs once when leaving. See How it works for the complete lifecycle explanation.
- Execution model
- The order of operations each loop: compute dt, run current state's update, evaluate transitions in order, fire first matching transition. See How it works for the detailed execution order.
- Blocking sequence
- A state that runs a Sequence; transitions (including onComplete) are only checked after the sequence finishes.
- Volatile sequence
- A sequence state that can be interrupted by non-onComplete transitions while the sequence is still running.
- minTime
- Minimum seconds that must pass in the current state (or current step, for sequences) before a transition is allowed to fire. Prevents transitions from firing too quickly. See How it works for timing details.
- onComplete
- A transition that fires when the state “completes” (e.g. a blocking sequence has finished all steps). Use onCompleteFrom when multiple states can transition to the same “complete” state so you can route by previous state.
Tips
- Keep tasks small and focused (e.g. one motor or one servo).
- Use
transitionFrom(from, to)when multiple states need to go to the same next state (e.g. different intake positions all going to SHOOT). - If you use path following, add a state that starts the path in enter and transitions when !follower.isBusy(); otherwise use state(...) and your own logic.