Code that implements a convoy system for lego mindstorm robots