%Zmienne w czasie
syms alpha beta l2
 
%Stałe w czasie
h = 0.5; l1 = 0.5;
 
%Współrzędne punktu Q
xSQ = (l1+l2)*sin(beta)*cos(alpha);
ySQ = (l1+l2)*sin(beta)*sin(alpha);
zSQ = h + (l1+l2)*cos(beta);
 
 
[alpha1, beta1, l21]=solve(xSQ==-0.3, ySQ==-0.9, zSQ==0.5, [alpha beta l2])
[alpha2, beta2, l22]=solve(xSQ==0.7, ySQ==-0.8, zSQ==1, [alpha beta l2])
 
as=(3*10^(1/2))/10 - 1/2
%Parametry
alpha1 = -1.2490;
beta1 = 1.5708;
l21 = 0.4487;
 
alpha2 = -0.85196632717327211381430754011385; 
beta2 = 1.1311402603991679040897483740452;
l22 = 0.67473401244707305869685063659647;
 
%Rysowanie
X1 = [0, 0, l1*sin(beta1)*cos(alpha1), (l1+l21)*sin(beta1)*cos(alpha1)];
Y1 = [0, 0, l1*sin(beta1)*sin(alpha1), (l1+l21)*sin(beta1)*sin(alpha1)];
Z1 = [0, h, h + l1*cos(beta1), h + (l1+l21)*cos(beta1)];
 
X2 = [0, 0, l1*sin(beta2)*cos(alpha2), (l1+l22)*sin(beta2)*cos(alpha2)];
Y2 = [0, 0, l1*sin(beta2)*sin(alpha2), (l1+l22)*sin(beta2)*sin(alpha2)];
Z2 = [0, h, h + l1*cos(beta2), h + (l1+l22)*cos(beta2)];
 
figure(1)
plot3(X1,Y1,Z1)
hold on
plot3(X1,Y1,Z1, 'go')
hold on
 
plot3(X2,Y2,Z2)
hold on
plot3(X2,Y2,Z2, 'ro')
hold off
 
% „Sterowanie”
% Obliczenia różnicy pomiędzy sterowanymi parametrami w początkowym i końcowym położeniu manipulatora.
S_alpha=alpha2-alpha1;
S_beta=beta2-beta1;
S_l2=l22-l21;
 
% Przyjęte maksymalne prędkości silników
V_ang=0.1;
V_lin=0.1;
 
% Określenie kierunku obrotu silników przy maksymalnych prędkościach
Vd_alpha=sign(S_alpha)*V_ang;
Vd_beta=sign(S_beta)*V_ang;
Vd_l2=sign(S_l2)*V_lin;
 
% Parametry czasowe
t0=0; % start
tk=100; % koniec
t=t0:0.1:tk;
tz=5; % czas rozpędzania i hamowania silników
 
% Czas potrzebny do wykonania zadania
T_alpha=(S_alpha/Vd_alpha)-tz;
T_beta=(S_beta/Vd_beta)-tz;
T_l2=(S_l2/Vd_l2)-tz;
 
% Praca silników - prędkości
V_alpha=(Vd_alpha/tz).*(heaviside(t-t0).*(t-t0)-heaviside(t-t0-tz).*(t-t0-tz)-heaviside(t-t0-tz-T_alpha).*(t-t0-tz-T_alpha)+heaviside(t-t0-2*tz-T_alpha).*(t-t0-2*tz-T_alpha));
V_beta=(Vd_beta/tz).*(heaviside(t-t0).*(t-t0)-heaviside(t-t0-tz).*(t-t0-tz)-heaviside(t-t0-tz-T_beta).*(t-t0-tz-T_beta)+heaviside(t-t0-2*tz-T_beta).*(t-t0-2*tz-T_beta));
V_l2=(Vd_l2/tz).*(heaviside(t-t0).*(t-t0)-heaviside(t-t0-tz).*(t-t0-tz)-heaviside(t-t0-tz-T_l2).*(t-t0-tz-T_l2)+heaviside(t-t0-2*tz-T_l2).*(t-t0-2*tz-T_l2));
 
% Przyspieszenia
A_alpha=(Vd_alpha/tz).*(heaviside(t-t0)-heaviside(t-t0-tz)-heaviside(t-t0-tz-T_alpha)+heaviside(t-t0-2*tz-T_alpha));
A_beta=(Vd_beta/tz).*(heaviside(t-t0)-heaviside(t-t0-tz)-heaviside(t-t0-tz-T_beta)+heaviside(t-t0-2*tz-T_beta));
A_l2=(Vd_l2/tz).*(heaviside(t-t0)-heaviside(t-t0-tz)-heaviside(t-t0-tz-T_l2)+heaviside(t-t0-2*tz-T_l2));
 
% Przemieszczenia
Q_alpha=(Vd_alpha/(2*tz)).*(heaviside(t-t0).*((t-t0).^2)-heaviside(t-t0-tz).*((-t+t0+tz).^2)-heaviside(t-t0-tz-T_alpha).*((-t+t0+tz+T_alpha)).^2+heaviside(t-t0-2*tz-T_alpha).*((-t+t0+2*tz+T_alpha).^2));
Q_beta=(Vd_beta/(2*tz)).*(heaviside(t-t0).*((t-t0).^2)-heaviside(t-t0-tz).*((-t+t0+tz).^2)-heaviside(t-t0-tz-T_beta).*((-t+t0+tz+T_beta)).^2+heaviside(t-t0-2*tz-T_beta).*((-t+t0+2*tz+T_beta).^2));
Q_l2=(Vd_l2/(2*tz)).*(heaviside(t-t0).*((t-t0).^2)-heaviside(t-t0-tz).*((-t+t0+tz).^2)-heaviside(t-t0-tz-T_l2).*((-t+t0+tz+T_l2)).^2+heaviside(t-t0-2*tz-T_l2).*((-t+t0+2*tz+T_l2).^2));
 
%Rysowanie - cz2
figure(2)
plot(t,V_alpha)
hold on
plot(t,V_beta)
plot(t,V_l2)
hold off
 
figure(3)
plot(t,A_alpha)
hold on
plot(t,A_beta)
plot(t,A_l2)
hold off
 
figure(4)
plot(t,Q_alpha)
hold on
plot(t,Q_beta)
plot(t,Q_l2)
hold off
 
%Rysowanie - animacja
 
for i = 1:length(Q_alpha)
    alphaTemp = alpha1 + Q_alpha(i);
    betaTemp = beta1 + Q_beta(i);
    l2Temp = l21 + Q_l2(i);
 
    X = [0, 0, l1*sin(betaTemp)*cos(alphaTemp), (l1+l2Temp)*sin(betaTemp)*cos(alphaTemp)];
    Y = [0, 0, l1*sin(betaTemp)*sin(alphaTemp), (l1+l2Temp)*sin(betaTemp)*sin(alphaTemp)];
    Z = [0, h, h + l1*cos(betaTemp), h + (l1+l2Temp)*cos(betaTemp)];
 
    figure(5)
    plot3(X,Y,Z)
    xlabel('X')
    ylabel('Y')
    zlabel('Z')
    grid on
    hold on
    plot3(X,Y,Z, 'go')
    xlim([-1 1])
    ylim([-1 1])
    zlim([-0.5 2])
    view(45,45)
    hold off
 
end