RotateDegrees / DriveDistance

IEnumerator<ITask> Algoritmo()
{
yield return Arbiter.Receive(false, TimeoutPort(600*TOTALROBOS), delegate(DateTime t) { });

LogInfo(LogGroups.Console, "Algoritmo DHM");

Random rnd = new Random();

// Move
yield return Arbiter.Receive(false,
StartMove(1),
delegate(bool result) { });

// wait
yield return Arbiter.Receive(false, TimeoutPort(10000), delegate(DateTime t) { });

while (true)
{
// Turn
yield return Arbiter.Receive(false,
StartTurn(-90),
delegate(bool result) { });

// wait
yield return Arbiter.Receive(false, TimeoutPort(2000), delegate(DateTime t) { });

// Move
yield return Arbiter.Receive(false,
StartMove(rnd.NextDouble() / 2 + 0.5),
delegate(bool result) { });

// wait
yield return Arbiter.Receive(false, TimeoutPort(10000), delegate(DateTime t) { });
}

yield break;

}

Port<bool> StartTurn(double angulo)
{
Port<bool> result = new Port<bool>();
// start a turn
SpawnIterator<Port<bool>, double>(result, angulo, Gira);
return result;
}

Port<bool> StartMove(double distancia)
{
Port<bool> result = new Port<bool>();
// start movement
SpawnIterator<Port<bool>, double>(result, distancia, Anda);
return result;
}

IEnumerator<ITask> Gira(Port<bool> done, double angulo)
{
LogInfo(LogGroups.Console, string.Format("Girando angulo {0}°", angulo));

drive.RotateDegreesRequest request = new drive.RotateDegreesRequest();
request.Degrees = angulo;
request.Power = 0.2;//fator de escala para velocidade linear de 10cm/s

yield return
Arbiter.Choice(
_drivePort.RotateDegrees(request),
delegate(DefaultUpdateResponseType rsp) {done.Post(true); },
delegate(W3C.Soap.Fault failure)
{
LogError("Erro ao girar rob?");
});

yield break;
}

IEnumerator<ITask> Anda(Port<bool> done, double distancia)
{
LogInfo(LogGroups.Console, string.Format("Andando distancia {0}m", distancia));

drive.DriveDistanceRequest request = new drive.DriveDistanceRequest();
request.Distance = distancia;
request.Power = 0.2;//fator de escala para velocidade linear de 10cm/s

yield return
Arbiter.Choice(
_drivePort.DriveDistance(request),
delegate(DefaultUpdateResponseType rsp) { done.Post(true); },
delegate(W3C.Soap.Fault failure)
{
LogError("Erro ao movimentar rob?");
done.Post(false);
});

yield break;
}