private async Task CheckHang(CancellationToken cancellationToken)
{
if (!SpeedSensor.IsDriving)
{
var carMoveCommand = new CarMoveCommand
{
Speed = 0
};
_motorController.MoveCar(null, carMoveCommand);
Driving = null;
await AudioPlayerController.PlayAndWaitAsync(AudioName.AutomatischesFahrenFesthaengen).WithCancellation(cancellationToken);
await TurnBackward(1000, cancellationToken);
await TurnLeft(700, cancellationToken, false);
carMoveCommand = new CarMoveCommand
{
Speed = 0
};
_motorController.MoveCar(null, carMoveCommand);
}
}
private async Task TurnLeft(int milliseconds, CancellationToken cancellationToken, bool checkHang)
{
var carMoveCommand = new CarMoveCommand
{
Speed = 1,
LeftCircle = true
};
_motorController.MoveCar(null, carMoveCommand);
await Task.Delay(TimeSpan.FromMilliseconds(milliseconds), cancellationToken);
if (checkHang)
{
await CheckHang(cancellationToken);
}
carMoveCommand = new CarMoveCommand
{
Speed = 0
};
_motorController.MoveCar(null, carMoveCommand);
Driving = null;
}
private async Task TurnFull(CancellationToken cancellationToken)
{
var carMoveCommand = new CarMoveCommand
{
Speed = 1,
LeftCircle = true
};
_motorController.MoveCar(null, carMoveCommand);
await Task.Delay(TimeSpan.FromMilliseconds(1250), cancellationToken);
await CheckHang(cancellationToken);
carMoveCommand = new CarMoveCommand
{
Speed = 0
};
_motorController.MoveCar(null, carMoveCommand);
Driving = null;
}
private async Task TurnBackward(int milliseconds, CancellationToken cancellationToken)
{
var carMoveCommand = new CarMoveCommand
{
Speed = SPEED,
ForwardBackward = true
};
_motorController.MoveCar(null, carMoveCommand);
await Task.Delay(TimeSpan.FromMilliseconds(milliseconds), cancellationToken);
await CheckHang(cancellationToken);
carMoveCommand = new CarMoveCommand
{
Speed = 0
};
_motorController.MoveCar(null, carMoveCommand);
Driving = null;
}
public void MoveCar(CarControlCommand carControlCommand,
CarMoveCommand carMoveCommandParam)
{
if (_isStopped)
{
return;
}
//Speed from 0 to 255
var dcMotorMaxSpeed = 255;
CarMoveCommand carMoveCommand = null;
if (carMoveCommandParam != null)
{
carMoveCommand = carMoveCommandParam;
}
else if (carControlCommand != null)
{
carMoveCommand = new CarMoveCommand(carControlCommand);
}
if (_lastCarMoveCommand != null &&
_lastCarMoveCommand.ForwardBackward == carMoveCommand.ForwardBackward &&
_lastCarMoveCommand.LeftCircle == carMoveCommand.LeftCircle &&
_lastCarMoveCommand.RightCircle == carMoveCommand.RightCircle &&
_lastCarMoveCommand.RightLeft == carMoveCommand.RightLeft &&
_lastCarMoveCommand.Speed == carMoveCommand.Speed)
{
return;
}
_lastCarMoveCommand = carMoveCommand;
_automaticSpeakController.CarMoveCommand = carMoveCommand;
var motorLeft1 = GetMotor(3);
var motorLeft2 = GetMotor(4);
var motorRight1 = GetMotor(1);
var motorRight2 = GetMotor(2);
if (carMoveCommand.Speed == 0)
{
motorLeft1.SetSpeed(0);
motorLeft2.SetSpeed(0);
motorRight1.SetSpeed(0);
motorRight2.SetSpeed(0);
motorLeft1.Run(MotorAction.RELEASE);
motorLeft2.Run(MotorAction.RELEASE);
motorRight1.Run(MotorAction.RELEASE);
motorRight2.Run(MotorAction.RELEASE);
}
else if (carMoveCommand.RightCircle)
{
var carSpeedFull = (int)Math.Round(carMoveCommand.Speed * dcMotorMaxSpeed, 0);
if (carMoveCommand.ForwardBackward)
{
motorLeft1.Run(MotorAction.FORWARD);
motorLeft2.Run(MotorAction.FORWARD);
motorRight1.Run(MotorAction.BACKWARD);
motorRight2.Run(MotorAction.BACKWARD);
motorLeft1.SetSpeed(carSpeedFull);
motorLeft2.SetSpeed(carSpeedFull);
motorRight1.SetSpeed(carSpeedFull);
motorRight2.SetSpeed(carSpeedFull);
}
else
{
motorLeft1.Run(MotorAction.BACKWARD);
motorLeft2.Run(MotorAction.BACKWARD);
motorRight1.Run(MotorAction.FORWARD);
motorRight2.Run(MotorAction.FORWARD);
motorLeft1.SetSpeed(carSpeedFull);
motorLeft2.SetSpeed(carSpeedFull);
motorRight1.SetSpeed(carSpeedFull);
motorRight2.SetSpeed(carSpeedFull);
}
}
else if (carMoveCommand.LeftCircle)
{
var carSpeedFull = (int)Math.Round(carMoveCommand.Speed * dcMotorMaxSpeed, 0);
if (carMoveCommand.ForwardBackward)
{
motorLeft1.Run(MotorAction.BACKWARD);
motorLeft2.Run(MotorAction.BACKWARD);
motorRight1.Run(MotorAction.FORWARD);
motorRight2.Run(MotorAction.FORWARD);
motorLeft1.SetSpeed(carSpeedFull);
motorLeft2.SetSpeed(carSpeedFull);
motorRight1.SetSpeed(carSpeedFull);
motorRight2.SetSpeed(carSpeedFull);
}
else
{
motorLeft1.Run(MotorAction.FORWARD);
motorLeft2.Run(MotorAction.FORWARD);
motorRight1.Run(MotorAction.BACKWARD);
motorRight2.Run(MotorAction.BACKWARD);
motorLeft1.SetSpeed(carSpeedFull);
motorLeft2.SetSpeed(carSpeedFull);
motorRight1.SetSpeed(carSpeedFull);
motorRight2.SetSpeed(carSpeedFull);
}
}
//Left
else if (carMoveCommand.RightLeft < 0)
{
var carSpeedFull = (int)Math.Round(carMoveCommand.Speed * dcMotorMaxSpeed, 0);
var carSpeedSlow = (int)Math.Round(carMoveCommand.Speed * (1 - Math.Abs(carMoveCommand.RightLeft)) * dcMotorMaxSpeed, 0);
if (carMoveCommand.ForwardBackward)
{
motorLeft1.Run(MotorAction.FORWARD);
motorLeft2.Run(MotorAction.FORWARD);
motorRight1.Run(MotorAction.FORWARD);
motorRight2.Run(MotorAction.FORWARD);
}
else
{
motorLeft1.Run(MotorAction.BACKWARD);
motorLeft2.Run(MotorAction.BACKWARD);
motorRight1.Run(MotorAction.BACKWARD);
motorRight2.Run(MotorAction.BACKWARD);
}
motorLeft1.SetSpeed(carSpeedSlow);
motorLeft2.SetSpeed(carSpeedSlow);
motorRight1.SetSpeed(carSpeedFull);
motorRight2.SetSpeed(carSpeedFull);
}
//Right
else if (carMoveCommand.RightLeft > 0)
{
var carSpeedFull = (int)Math.Round(carMoveCommand.Speed * dcMotorMaxSpeed, 0);
var carSpeedSlow = (int)Math.Round(carMoveCommand.Speed * (1 - Math.Abs(carMoveCommand.RightLeft)) * dcMotorMaxSpeed, 0);
if (carMoveCommand.ForwardBackward)
{
motorLeft1.Run(MotorAction.FORWARD);
motorLeft2.Run(MotorAction.FORWARD);
motorRight1.Run(MotorAction.FORWARD);
motorRight2.Run(MotorAction.FORWARD);
}
else
{
motorLeft1.Run(MotorAction.BACKWARD);
motorLeft2.Run(MotorAction.BACKWARD);
motorRight1.Run(MotorAction.BACKWARD);
motorRight2.Run(MotorAction.BACKWARD);
}
motorLeft1.SetSpeed(carSpeedFull);
motorLeft2.SetSpeed(carSpeedFull);
motorRight1.SetSpeed(carSpeedSlow);
motorRight2.SetSpeed(carSpeedSlow);
}
else if (carMoveCommand.RightLeft == 0)
{
var carSpeedFull = (int)Math.Round(carMoveCommand.Speed * dcMotorMaxSpeed, 0);
if (carMoveCommand.ForwardBackward)
{
motorLeft1.Run(MotorAction.FORWARD);
motorLeft2.Run(MotorAction.FORWARD);
motorRight1.Run(MotorAction.FORWARD);
motorRight2.Run(MotorAction.FORWARD);
}
else
{
motorLeft1.Run(MotorAction.BACKWARD);
motorLeft2.Run(MotorAction.BACKWARD);
motorRight1.Run(MotorAction.BACKWARD);
motorRight2.Run(MotorAction.BACKWARD);
}
motorLeft1.SetSpeed(carSpeedFull);
motorLeft2.SetSpeed(carSpeedFull);
motorRight1.SetSpeed(carSpeedFull);
motorRight2.SetSpeed(carSpeedFull);
}
}
private async Task <FreeDirection> GetFreeDirection(CancellationToken cancellationToken)
{
if (!_isForward)
{
_servoController.PwmController.SetPwm(2, 0, FRONT_MIDDLE_HORIZONTAL);
await Task.Delay(1000, cancellationToken);
}
var currentDistance = await _distanceMeasurementSensor.ReadDistanceInCm(3);
if (currentDistance > MIN_DETECTION_CM)
{
_isForward = true;
return(FreeDirection.Forward);
}
_isForward = false;
var carMoveCommand = new CarMoveCommand
{
Speed = 0
};
_motorController.MoveCar(null, carMoveCommand);
Driving = null;
//Left
_servoController.PwmController.SetPwm(2, 0, FRONT_LEFT_HORIZONTAL);
await Task.Delay(500, cancellationToken);
currentDistance = await _distanceMeasurementSensor.ReadDistanceInCm(3);
if (currentDistance > MIN_DETECTION_CM)
{
return(FreeDirection.Left);
}
//LeftMiddle
_servoController.PwmController.SetPwm(2, 0, FRONT_LEFT_MIDDLE_HORIZONTAL);
await Task.Delay(250, cancellationToken);
currentDistance = await _distanceMeasurementSensor.ReadDistanceInCm(3);
if (currentDistance > MIN_DETECTION_CM)
{
return(FreeDirection.LeftMiddle);
}
//RightMiddle
_servoController.PwmController.SetPwm(2, 0, FRONT_RIGHT_MIDDLE_HORIZONTAL);
await Task.Delay(250, cancellationToken);
currentDistance = await _distanceMeasurementSensor.ReadDistanceInCm(3);
if (currentDistance > MIN_DETECTION_CM)
{
return(FreeDirection.RightMiddle);
}
//Right
_servoController.PwmController.SetPwm(2, 0, FRONT_RIGHT_HORIZONTAL);
await Task.Delay(250, cancellationToken);
currentDistance = await _distanceMeasurementSensor.ReadDistanceInCm(3);
if (currentDistance > MIN_DETECTION_CM)
{
return(FreeDirection.Right);
}
return(FreeDirection.None);
}
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;
}
