/// <summary>
/// This function will move the robot while also stopping if there are obstacles and adjusting
/// course if it's heading towards a wall
/// </summary>
/// <param name="speed">Speed robot will move</param>
/// <param name="distMM">Distance in millimeters</param>
/// <returns></returns>
public async Task ContinuousMove(int speed, int distMM, CancellationToken token)
{
if (!Roomba.IsEnabled)
{
Debug.WriteLine("Roomba is not enabled! Cancelling move...");
return;
}
lastStopType = StopReason.Success;
int distMoved = 0;
do
{
// Cancel the entire move if we collided with something or canceled everything
if (token != null && token.IsCancellationRequested)
{
return;
}
if (lastStopType == StopReason.Collision || lastStopType == StopReason.Cancellation)
{
return;
}
switch (Analysis.LastRecommendation)
{
case LidarRecommendation.Move:
Debug.WriteLine("ContinuousMove: Moving...");
// Calculate distance left to move
distMM -= distMoved;
// Move the robot
OnControllerMessageReceived($"Moving {distMM} mm...");
await Roomba.MoveDistanceEncoders(speed, distMM);
// Determine how far the robot moved
distMoved = (int)Roomba.AverageDistanceTraveled;
break;
case LidarRecommendation.AdjustObstacle:
Debug.WriteLine("ContinuousMove: Adjusting...");
// It doesn't matter what the actual adjust angle is, since the robot will
// stop rotating once we get the recommendation to move
if (Analysis.LastAdjustAngle < 0)
{
Roomba.SetSpeed(DEFAULT_ROTATE_SPEED, 0);
}
else
{
Roomba.SetSpeed(0, DEFAULT_ROTATE_SPEED);
}
break;
}
await Task.Delay(100);
}while (distMoved < distMM);
}
