The algorithm can be modelised by the following chart :


flow-chart

It can be described as following :
  1. The robot is at the starting position. It moves forward, turns right and moves forward while checking the distance from the fence.

  2. The robot turns left and moves towards the box while checking the distance. It then stops and starts the picking up system.

  3. The scissor lift goes down, the clamp opens, tries to pick a ball and then to life it up. While there is no ball detected by the color sensor, the picking mechanism continues trying.

  4. Now that it picked a ball and is still facing the source cube, the robot has one of two paths to go :