/// <summary>
/// Sets engine's power.
/// </summary>
/// <param name="controllerCommand">The controller command.</param>
private void SetEnginesPower(IControllerStateCommand controllerCommand)
{
var processor = new TwoThumbsControllerProcessor();
processor.CalculateEnginesPower(controllerCommand, out int leftEnginePower, out int rightEnginePower);
var engineCommand = new RobotEnginesPowerCommand(leftEnginePower, rightEnginePower);
CommandManager.Instance.BroadcastData(this, new List <ICommand> {
engineCommand
});
}
/// <summary>
/// Calculates robot's engines power.
/// </summary>
/// <param name="controllerCommand">The controller command.</param>
/// <param name="leftEnginePower">The left engine power.</param>
/// <param name="rightEnginePower">The right engine power.</param>
public void CalculateEnginesPower(IControllerStateCommand controllerCommand, out int leftEnginePower, out int rightEnginePower)
{
var powerL = Math.Abs(GetY(controllerCommand));
var directionL = Math.Sign(GetY(controllerCommand));
var powerR = Math.Abs(GetY(controllerCommand));
var directionR = Math.Sign(GetY(controllerCommand));
var turn = GetX(controllerCommand);
powerR -= (turn > 0 ? turn : 0);
powerR = powerR < 0 ? 0 : powerR;
powerL -= (turn < 0 ? -turn : 0);
powerL = powerL < 0 ? 0 : powerL;
if (!(powerL > 0 || powerR > 0))
{
powerL = powerR = Math.Abs(turn);
directionL = Math.Sign(turn);
directionR = Math.Sign(-turn);
}
leftEnginePower = powerL * directionL;
rightEnginePower = powerR * directionR;
}
/// <summary>
/// Gets the Y axis power.
/// </summary>
/// <param name="controllerCommand">The controller command.</param>
/// <returns>
/// Y axis power (-100; 100).
/// </returns>
protected override int GetY(IControllerStateCommand controllerCommand)
{
return(controllerCommand.LeftThumb.Y);
}
/// <summary>
/// Gets the X axis power.
/// </summary>
/// <param name="controllerCommand">The controller command.</param>
/// <returns>
/// X axis power (-100; 100).
/// </returns>
protected override int GetX(IControllerStateCommand controllerCommand)
{
return(controllerCommand.RightThumb.X);
}
/// <summary> /// Gets the Y axis power. /// </summary> /// <param name="controllerCommand">The controller command.</param> /// <returns>Y axis power (-100; 100).</returns> protected abstract int GetY(IControllerStateCommand controllerCommand);