Cleans up state transitions, ups rotational velocity on spin state, adds goal state