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