public void SetRobot(Robot r)
{
this.robot = r;
status = DriverStatus.Done;
rangeFinder = robot.GetSensorByName(rangeFinderName) as Robot.SensorRangeFinder;
leftWhell = robot.GetPartByName(leftWheelName);
rightWheel = robot.GetPartByName(rightWheelName);
direction = new Vector(0, 0);
updateDirection();
commandQueue = new List<Command>();
}