Feature/state machine

Implemented the point_robot_at_target function, grab is still unfinished