Controll Loop For robot, Must be run in V-rep.