Controller for robot is now working