converted voronoi control law to waypoint control, and created a seperate script for it