First version of custom controller to control ANY robot part 1