private IEnumerator doPark() { //Enable automatic control control.autoParking = true; //Setup sensors sensors = control.sensors; DistanceSensor backPointingRight = sensors[0]; DistanceSensor backMiddlePointingBack1 = sensors[2]; DistanceSensor backMiddlePointingBack2 = sensors[3]; DistanceSensor frontRightAngular = sensors[1]; //Setup other variables float lastDistance1 = 0; float lastDistance2 = 0; float backSensorOffset = 0.1f; float angle = 0; //Drive back without turning control.command = new ControlCar.CommandSet(-1, 0, false); while (backPointingRight.getDistance() != -1) { yield return(null); // until back end of car is aligned with obstacle 1 } StartCoroutine(breaK()); while (doingSubRoutine) { yield return(null); } StartCoroutine(turnRight()); while (doingSubRoutine) { yield return(null); } //Drive back while turning right control.command = new ControlCar.CommandSet(-1, 1, false); while (backMiddlePointingBack1.getDistance() == -1) { yield return(null); //until obstacle two is visible; } while (backMiddlePointingBack1.getDistance() != -1) //and then until it isn't visible again, while keeping track of the last measured value { lastDistance1 = backMiddlePointingBack1.getDistance(); lastDistance2 = backMiddlePointingBack2.getDistance(); yield return(null); } // Calculate angle between alignent of obstacle2(and supposedly object1) and the alignment of the car angle = Mathf.Atan((lastDistance1 - lastDistance2) / backSensorOffset) * Mathf.Rad2Deg; //Set the rotation of the angluar distance sensor to the difference in alignment frontRightAngular.transform.localEulerAngles = new Vector3(0, 180 + angle, 0); StartCoroutine(breaK()); while (doingSubRoutine) { yield return(null); } StartCoroutine(turnMiddle()); while (doingSubRoutine) { yield return(null); } //Drive back without turning control.command = new ControlCar.CommandSet(-1, 0, false); while (frontRightAngular.getDistance() == -1) { yield return(null); //Until front right corner of car clears obstacle 1 } StartCoroutine(breaK()); while (doingSubRoutine) { yield return(null); } StartCoroutine(turnLeft()); while (doingSubRoutine) { yield return(null); } //Drive back while turning left control.command = new ControlCar.CommandSet(-1, -1, false); // until car is aligned with object 2 while (Mathf.Atan((backMiddlePointingBack1.getDistance() - backMiddlePointingBack2.getDistance()) / backSensorOffset) * Mathf.Rad2Deg > 0 || backMiddlePointingBack1.getDistance() == -1) { yield return(null); } ; StartCoroutine(breaK()); while (doingSubRoutine) { yield return(null); } StartCoroutine(turnMiddle()); while (doingSubRoutine) { yield return(null); } //Done - Give back control control.autoParking = false; }