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;
}
