Added to manual movement state machine. Removed non-compiling function block.