private async void StartInternal(CancellationToken cancellationToken)
{
try
{
cancellationToken.ThrowIfCancellationRequested();
_servoController.PwmController.SetPwm(2, 0, FRONT_MIDDLE_HORIZONTAL);
_servoController.PwmController.SetPwm(3, 0, MIDDLE_VERTICAL);
await Task.Delay(TimeSpan.FromMilliseconds(1500), cancellationToken);
_isForward = true;
while (!_isStopping)
{
await Task.Delay(TimeSpan.FromMilliseconds(25), cancellationToken);
CarMoveCommand carMoveCommand = null;
var freeDirection = await GetFreeDirection(cancellationToken);
if (freeDirection == FreeDirection.Forward)
{
if (_isStopping)
{
break;
}
if (Driving.HasValue && DateTime.Now >= Driving.Value.AddMilliseconds(700))
{
await CheckHang(cancellationToken);
}
carMoveCommand = new CarMoveCommand
{
Speed = SPEED
};
_motorController.MoveCar(null, carMoveCommand);
Driving = DateTime.Now;
}
else
{
if (freeDirection == FreeDirection.Left)
{
if (_isStopping)
{
break;
}
await TurnLeft(700, cancellationToken, true);
}
else if (freeDirection == FreeDirection.LeftMiddle)
{
if (_isStopping)
{
break;
}
await TurnLeft(350, cancellationToken, false);
}
else if (freeDirection == FreeDirection.Right)
{
if (_isStopping)
{
break;
}
await TurnRight(700, cancellationToken, true);
}
else if (freeDirection == FreeDirection.RightMiddle)
{
if (_isStopping)
{
break;
}
await TurnRight(350, cancellationToken, false);
}
else if (freeDirection == FreeDirection.None)
{
if (_isStopping)
{
break;
}
await TurnFull(cancellationToken);
}
}
}
}
catch (OperationCanceledException) { }
var carMoveCommandEnd = new CarMoveCommand
{
Speed = 0
};
_motorController.MoveCar(null, carMoveCommandEnd);
Driving = null;
_servoController.PwmController.SetPwm(2, 0, FRONT_LEFT_HORIZONTAL);
_servoController.PwmController.SetPwm(3, 0, 182);
_isStopped = true;
}
