/// <summary>
/// Otočí kolo nohy ve směru pohybu
/// </summary>
/// <param name="direction">směr ve stupních 0 až 359</param>
/// <param name="motorZK">motor pro otáčení nohy</param>
/// <param name="motorR">motor pro otáčení kola nohy</param>
/// <param name="angleCorection">korekce úhlu nohy...</param>
/// <returns>true pokud obrátit směr pohybu kola</returns>
private bool rotateWheelForMove(int direction, IMotor motorZK, IMotor motorR, IMotor motorP, int angleCorection)
{
bool reverseWheelSpeed = false;
int kartezLegAngle = MathLibrary.changeScale(motorZK.angle, motorZK.minAngle, motorZK.maxAngle, 0, 90);
if (angleCorection < 0)
{
if (direction - (angleCorection - kartezLegAngle) < 0)
{
reverseWheelSpeed = !reverseWheelSpeed;
}
}
else
{
if (direction - (angleCorection - kartezLegAngle) > 0)
{
reverseWheelSpeed = !reverseWheelSpeed;
}
}
int kartezWheelAngle = 180 - 90 - (180 - direction - kartezLegAngle);
kartezWheelAngle = kartezWheelAngle % 180;
if (kartezWheelAngle < 0)
{
kartezWheelAngle += 180;
reverseWheelSpeed = !reverseWheelSpeed;
}
int wheelAngle = kartezWheelAngle;
if (Math.Abs(wheelAngle - motorR.angle) > ZKTolerance)
{
motorP.disable();
}
motorR.moveToAngle(wheelAngle);
return reverseWheelSpeed;
}
