public CarMoveCommand(CarControlCommand carControlCommand) { if (carControlCommand.SpeedControlForward || carControlCommand.SpeedControlBackward) { Speed = 1; if (carControlCommand.SpeedControlForward) { ForwardBackward = true; } } if (carControlCommand.DirectionControlLeft) { Speed = carControlCommand.SpeedControlLeftRight; ForwardBackward = true; LeftCircle = true; } else if (carControlCommand.DirectionControlRight) { Speed = carControlCommand.SpeedControlLeftRight; ForwardBackward = true; RightCircle = true; } }
public void MoveServo(CarControlCommand carControlCommand) { if (_isStopped) { return; } if (carControlCommand.DirectionControlUp) { if (_servoMinValue == _servoCameraVerticalValue) { return; } _servoCameraVerticalValue -= carControlCommand.DirectionControlUpDownStepSpeed; if (_servoCameraVerticalValue < _servoMinValue) { _servoCameraVerticalValue = _servoMinValue; } PwmController.SetPwm(1, 0, _servoCameraVerticalValue); } else if (carControlCommand.DirectionControlDown) { if (_servoMaxValue == _servoCameraVerticalValue) { return; } _servoCameraVerticalValue += carControlCommand.DirectionControlUpDownStepSpeed; if (_servoCameraVerticalValue > _servoMaxValue) { _servoCameraVerticalValue = _servoMaxValue; } PwmController.SetPwm(1, 0, _servoCameraVerticalValue); } }
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); } }