State machine working

State machine and trajectory generator working. More tests needed.