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