Fixed state machine code to follow robotc symantics