%---- LABORATORIUM POMIARÓW, AUTOMATYKI I STEROWANIA ----
%-----------          Cwiczenie 1          --------------
% Obliczenia kinematyczne w generatorze trajektorii robota rownoległego

% Skrypt pomocniczy do cwiczenia
% Nalezy postepowac zgodnie z kolejnoscia sugerowanych czynnosci
% Dla lepszej czytelnosci, warto usuwac komentarze po zakonczeniu danego etapu

% W celu przetestowania kodu, istnieje mozliwosc uruchomienia pojedynczej
% sekcji za pomoca skrotu [ctrl] + [enter] (lub Run Section na pasku edytora)

% Przeprwanie pracy programu w przypadku uruchomienia nieskonczonej
% petli: [ctrl] + [c] (nalezy miec aktywne pole Command Window)

%% zdefiniowac funkcje obliczajaca macierz rotacji 
% (*) zapisana w osobnym pliku LUB
% (*) zapisana w formie funkcji inline R = @(q) (wyrazenie)

%% Zdefiniowac rownania wiezow kinematycznych FI_K
% (*) q == [fi1, fi2, alfa, beta]'
% (*) Okreslic pomocnicze wyrazenia: Omega, u, v
% (*) Przykladowe przyblizenie poczatkowe:
q = [0; 0; pi/4; 0]; % Jakiej konfiguracji odpowiada ten wektor?

L = 5;               % Dlugosc czlonu (w calach)

disp( FI_K )         % Jak interpretowac ten wynik?
%% Zdefiniowac wektor wiezow domykajacych FI_D
% (*) Zadanie odwrotne: okreslic punkt E dla danego wektora q
Eref = [5; 6];          % Przykladowa wartosc zadana
% (*) Zadanie proste: przykladowe wartosci zadane:
fi1ref = pi/10;
fi2ref = -pi/10;


FI = [Fi_K; Fi_D];
%% Zdefiniowac macierz Jacobiego
% (*) Jaki jest rozmiar macierzy?

%% Wykorzystac powyzsze zaleznosci do uzupelnienia kodu
iter_limit = 10;    eps = 1e-10;
failure = false;

while true % nieskonczona petla
    
% FI = ?
% Fq = ?
    
% to do: Rozwiazac rownanie Fq * delta_q = -FI
% (*) Nie uzywac funkcji inv()
% (*) Przetestowac dzialanie operatora "\"
disp(4 / 2)
disp(4 \ 2)

q = q + delta_q;
    
% Warunki wyjscia z petli
    if zbieznosc_osiagnieta
        break
    end
    if brak_zbieznosci
        failure = true;
        break
    end
    
end

disp(q)
%% Na podstawie powyzszego kodu utworzyc jedna z funkcji:
% function [fi1, fi2, NR_flag, q] = inv_kinematics(Ex, Ey, q0)
% function [Ex, Ey] = fwd_kinematics(fi1, fi2)

% (*) Warto rozpoczac od wykonania kopii zapasowej skryptu
% (*) Przetestowac poprawnosc dzialania
