%% Przykładowe zadanie - projekt  z zakresu rozwiązywania układów równań
% Przygotował dr inż. Robert JAKUBOWSKI, Politechnika Rzeszowska, KSiSL
% Wizualizacja pracy manipulatora na płaszczyźnie x y
%
% Powyżej przedstawiono manipulator dwuramienny o ramionach a1 i a2. Zdefinujmy ruch manipulatora w węzłach B i C względem osi x i y. W tym celu wykorzystamy kąty które tworzą ramiona, przy czym theta1 jest kątem mierzonym względem osi x a theta 2 jest mierzony względem osi ramienia 1. Do rozwiązania zadania zdefiniujemy kąty theta i ramiona jako zmienne symboliczne

syms a1 a2 theta1 theta2

% Położenie węzła B w układzie współrzędnych x i y mozna zdefiniować

xB=a1*cos(theta1)
yB=a1*sin(theta1)

% Położenie węzła C można zapisać względem A w następujący

xC=a2*cos(theta1+theta2)+xB
yC=a2*sin(theta1+theta2)+yB

% Zdefiniujmy długości ramion odpowiednio a1=5 i a2 =3 i określmy funkcje zależne tylko od kąta

fxB(theta1)=subs(xB,a1,5)
fyB(theta1)=subs(yB,a1,5)
fxC(theta1,theta2)=subs(xC,[a1 a2],[5 3])
fyC(theta1,theta2)=subs(yC,[a1 a2],[5 3])

% Narysujmy przemieszczanie się manipulatora punktu B i C gdy theta1=[-90 -45 0 45 90] a theta2 ma pełny obrót (kąty wyrażamy w mierze łukowej)

theta1_1=[-pi/2 -pi/4 0 pi/4 pi/2];
theta2_1=[-pi/2:pi/36:pi/2];
%  wykres połóżenia punktu b
plot(fxB(theta1_1),fyB(theta1_1),'Color','r','Marker',"o","LineStyle","none");
hold on
% wykres połóżenia punktu C
for i=1:length(theta1_1)
    fplot(fxC(theta1_1(i),theta2),fyC(theta1_1(i),theta2))
end
axis equal

grid on
hold off

%
% Wyzmaczmy wartość położenia końcówki manipulatora (punktu B) dla wybranych wartości kąta theta1 i thetta1
% W tym celu wprowadzimy nowe zmienne theta1_2, theta2_2, którym przypiszmy konkretne wartości kątowe i po przeliczeniu na miarę łukową wprowadzimy do zadania

%zaadajemy wartość w stopniach
theta1_2=50;
theta2_2=30;
%przeliczamy na miarę łukową
theta1_2=theta1_2*pi/180;
theta2_2=theta2_2*pi/180;
% podstawiamy do funkcji okreslajacych pozycję x y punktu c w miejsce kątów
% i wyliczamy
[Pos1]=[fxC(theta1_2,theta2_2),fyC(theta1_2,theta2_2)]
Pos1_val=double(Pos1)

%
% Rozwiiążmy zadanie odwrotne: znajdżmy kąty jakie może mieć manipulator aby znajdował się w okreśłonej pozycji
% Wprowadźmy zmienne okreśłjące pozycję (jeden z punktó z obszarów zakreślonych przez koła)

Pos_x=5
Pos_y=6

% Rozwiążmy układ równań

[theta1_roz,theta2_roz]=solve(fxC==Pos_x,fyC==Pos_y)

% Przeliczmy to na miarę kątową

theta1_val=double(theta1_roz)*180/pi
theta2_val=double(theta2_roz)*180/pi

% Narysujmy proponowane rozwiązania:

%Policzmy pozycję położenia węzłów B i C
Bx=double(fxB(theta1_roz))
By=double(fyB(theta1_roz))
Cx=double(fxC(theta1_roz,theta2_roz))
Cy=double(fyC(theta1_roz,theta2_roz))
%Narysujmy obie belki
% Wyznaczmy początek jako punkt zerowy
pocz=zeros(size(Bx));
X_poz=[pocz Bx Cx];
Y_poz=[pocz,By,Cy];
plot(X_poz',Y_poz','-o')
grid on
axis equal

%
% Rozwiązanie z ograniczeniami
% Wprowadźmy do rozwiązania ograniczenie, co do wartości kąta theta1. Załóżmy, że będzie on się mógł zawierać w przedziale od 30 do 90 st. Sprawdźmy jakie będzie rozwiązanie w takim przypadku.

%Wprowadzimy ograniczenie na zmienną theta1
assume(theta1>30*pi/180)
assumeAlso(theta1<pi/2)

% Rozwiążmy ponownie układ równań

[theta1_roz,theta2_roz]=solve(fxC==Pos_x,fyC==Pos_y)

% Przeliczmy to na miarę kątową

theta1_val=double(theta1_roz)*180/pi
theta2_val=double(theta2_roz)*180/pi

% Narysujmy wyliczone rozwiązania:

%Policzmy pozycję położenia węzłów B i C
Bx=double(fxB(theta1_roz))
By=double(fyB(theta1_roz))
Cx=double(fxC(theta1_roz,theta2_roz))
Cy=double(fyC(theta1_roz,theta2_roz))
%Narysujmy obie belki
% Wyznaczmy początek jako punkt zerowy
pocz=zeros(size(Bx));
X_poz=[pocz Bx Cx];
Y_poz=[pocz,By,Cy];
plot(X_poz',Y_poz','-o')
grid on
axis equal
assume(theta1,'clear')

%
%% ZADANIA do zrealizowania
% 1. Powyższy przykład rozwiąż, gdy wskaznym punktem będzie (7, 2)




%
% 2.  Wstaw ograniczenie na theta1 jak w poprzednim zadaniu i na theta2 żeby ograniczyć możliwość wychylania belki a2 do +-30 st.  Pokaż jakie wtedy będzie rozwiązanie dla położenia ramion manipulatora x=5,y=6. Rozwiązanie przedstaw graficznie





%
% 3. Wyznacz położenie punktu C dla theta1=120 st i theta 2=-45 st. Rozwiązanie przedstaw graficznie



%