Added simple fsm system
small GetDistance fix
Merge branch 'dev' into fsm
merge fsm branch, call it v0.9.0
merge fsm branch, call it v0.9.0
Merge pull request #9 from NicoM1/fsm

Merge FSM