An Algorithm for Supervised Driving of Cooperative Semi-Autonomous Vehicles (Extended)