Debugging of basic commands; added new position and route commands for debugging (and moveto arm ops)