Wait for lift (or peg, not sure what is the best term)
Align to lift
Return to wait
Arm gear
Place gear
Return to ready
Disarm gear
Idle
Robot remains in "Wait for lift" until the ultrasonic sensor detects the lift (debounced) then transitions to "Align to lift". In "Align to lift" the rail moves to the position of the detected lift, but the state may transition "Return to wait" if the lift is no longer available. When the robot is in "Align to lift state" a boolean input will trigger transition to "Arm Gear" state. In "Return to wait", the carriage returns to the near side of the rail, then transitions to "Wait for lift". In "Arm gear" the gear gate is lowered and the state immediately transitions to "Place gear". In place gear, the gear pusher pistons are extended and the state transitions to "Return to ready" when the place boolean input is deactivated and the pistons have completed motion. In "Return to Ready", the gear pusher pistons retract and the state transitions to "Disarm gear" as soon as the pistons retract. In "Disarm gear", the gear gate is returned to hold position and the gear carriage returns to the far side then the state transitions to "Idle". "Idle" should do nothing.
One boolean input should trigger a transition from "Idle" to "Wait for lift". This is a "start" trigger that does nothing to any other states.
Another boolean input should trigger an immediate transition to "Return to ready" from states 1-5 and do nothing in states 6-8. This is a "reset" button and should have precedence over all other inputs.
The following states need to be implemented:
Robot remains in "Wait for lift" until the ultrasonic sensor detects the lift (debounced) then transitions to "Align to lift". In "Align to lift" the rail moves to the position of the detected lift, but the state may transition "Return to wait" if the lift is no longer available. When the robot is in "Align to lift state" a boolean input will trigger transition to "Arm Gear" state. In "Return to wait", the carriage returns to the near side of the rail, then transitions to "Wait for lift". In "Arm gear" the gear gate is lowered and the state immediately transitions to "Place gear". In place gear, the gear pusher pistons are extended and the state transitions to "Return to ready" when the place boolean input is deactivated and the pistons have completed motion. In "Return to Ready", the gear pusher pistons retract and the state transitions to "Disarm gear" as soon as the pistons retract. In "Disarm gear", the gear gate is returned to hold position and the gear carriage returns to the far side then the state transitions to "Idle". "Idle" should do nothing.
One boolean input should trigger a transition from "Idle" to "Wait for lift". This is a "start" trigger that does nothing to any other states. Another boolean input should trigger an immediate transition to "Return to ready" from states 1-5 and do nothing in states 6-8. This is a "reset" button and should have precedence over all other inputs.