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