// Listing 7.10. Mechanizm tłumaczący PUMT — odwrotna kinematyka mechanicznego ramienia nr 1


/*Rama projektowa Softbot
Nazwa: ServoMotor
Części:
Sekcja silników:
Jeden serwomotor
    
Akcje:
Znając długość ramienia robota i docelowe współrzędne x, y efektora końcowego:
Krok 1.: Podnieś wartości x i y do kwadratu.
Krok 2.: Jeżeli pierwiastek kwadratowy sumy podniesionych do kwadratu wartości x i y jest mniejszy od długości ramienia,
Krok 2.1.: określ wartość arcus tangens dla punktu x, y,
Krok 2.2.: ustaw kąt położenia serwomotoru na podstawie tej wartości.

     
Zadania:
Sprawdź działanie serwomotorów, sterując położeniem ich wałów.
Koniec ramy*/

           ...
300        SquaredX = Math.pow(X,2);
301        SquaredY = Math.pow(Y,2);
302        if(ArmLength >= Math.sqrt(SquaredX + SquaredY))
303        {
304           ArmAngle = math.atan2(X,Y);
305        }
306        ...


