added state machine to step to different positions

need to finish state machine