Added initial control loop for robot