Robot Motion Control functions

/ Added function to take Robot Motion exclusive control
/ Minor changes to code structure