clc
clear all



 L1=input('Podaj wartosc ramienia 1: ');
 L2=input('Podaj wartosc ramienia 2: ');
 L3=input('Podaj wartosc ramienia 3: ');
 L4=input('Podaj wartosc ramienia 4: ');
 L5=input('Podaj wartosc ramienia 5: ');


 J1=input('Podaj wartosc obrotu J1: ');
 J2=input('Podaj wartosc obrotu J2: ');
 J3=input('Podaj wartosc obrotu J3: ');
 J4=input('Podaj wartosc obrotu J4: ');
 J5=input('Podaj wartosc obrotu J5: ');

Tabela=[J1 L1 0 -90;J2 0 L2 0;-90 0 0 90;J3 -L3 0 -90;J4 0 0 90;0 -L4 0 0;J5 -L5 0 0];

for i=1:7

    if(i==1)
    Rz=[cos(Tabela(i,1)) -sin(Tabela(i,1)) 0 0;sin(Tabela(i,1)) cos(Tabela(i,1)) 0 0;0 0 1 0;0 0 0 1];
    Tz=[1 0 0 0;0 1 0 0;0 0 1 Tabela(i,2);0 0 0 1];
    Tx=[1 0 0 Tabela(i,3);0 1 0 0;0 0 1 0;0 0 0 1];
    Rx=[1 0 0 0;0 cos(Tabela(i,4)) sin(Tabela(i,4)) 0;0 -sin(Tabela(i,4)) cos(Tabela(i,4)) 0;0 0 0 1];
    A1=Rz*Tz*Tx*Rx;

    elseif(i==2)
    Rz=[cos(Tabela(i,1)) -sin(Tabela(i,1)) 0 0;sin(Tabela(i,1)) cos(Tabela(i,1)) 0 0;0 0 1 0;0 0 0 1];
    Tz=[1 0 0 0;0 1 0 0;0 0 1 Tabela(i,2);0 0 0 1];
    Tx=[1 0 0 Tabela(i,3);0 1 0 0;0 0 1 0;0 0 0 1];
    Rx=[1 0 0 0;0 cos(Tabela(i,4)) sin(Tabela(i,4)) 0;0 -sin(Tabela(i,4)) cos(Tabela(i,4)) 0;0 0 0 1];
    A2=Rz*Tz*Tx*Rx;

    elseif(i==3)
    Rz=[cos(Tabela(i,1)) -sin(Tabela(i,1)) 0 0;sin(Tabela(i,1)) cos(Tabela(i,1)) 0 0;0 0 1 0;0 0 0 1];
    Tz=[1 0 0 0;0 1 0 0;0 0 1 Tabela(i,2);0 0 0 1];
    Tx=[1 0 0 Tabela(i,3);0 1 0 0;0 0 1 0;0 0 0 1];
    Rx=[1 0 0 0;0 cos(Tabela(i,4)) sin(Tabela(i,4)) 0;0 -sin(Tabela(i,4)) cos(Tabela(i,4)) 0;0 0 0 1];
    A3=Rz*Tz*Tx*Rx;

    elseif(i==4)
    Rz=[cos(Tabela(i,1)) -sin(Tabela(i,1)) 0 0;sin(Tabela(i,1)) cos(Tabela(i,1)) 0 0;0 0 1 0;0 0 0 1];
    Tz=[1 0 0 0;0 1 0 0;0 0 1 Tabela(i,2);0 0 0 1];
    Tx=[1 0 0 Tabela(i,3);0 1 0 0;0 0 1 0;0 0 0 1];
    Rx=[1 0 0 0;0 cos(Tabela(i,4)) sin(Tabela(i,4)) 0;0 -sin(Tabela(i,4)) cos(Tabela(i,4)) 0;0 0 0 1];
    A4=Rz*Tz*Tx*Rx;

    elseif(i==5)
    Rz=[cos(Tabela(i,1)) -sin(Tabela(i,1)) 0 0;sin(Tabela(i,1)) cos(Tabela(i,1)) 0 0;0 0 1 0;0 0 0 1];
    Tz=[1 0 0 0;0 1 0 0;0 0 1 Tabela(i,2);0 0 0 1];
    Tx=[1 0 0 Tabela(i,3);0 1 0 0;0 0 1 0;0 0 0 1];
    Rx=[1 0 0 0;0 cos(Tabela(i,4)) sin(Tabela(i,4)) 0;0 -sin(Tabela(i,4)) cos(Tabela(i,4)) 0;0 0 0 1];
    A5=Rz*Tz*Tx*Rx;

    elseif(i==6)
    Rz=[cos(Tabela(i,1)) -sin(Tabela(i,1)) 0 0;sin(Tabela(i,1)) cos(Tabela(i,1)) 0 0;0 0 1 0;0 0 0 1];
    Tz=[1 0 0 0;0 1 0 0;0 0 1 Tabela(i,2);0 0 0 1];
    Tx=[1 0 0 Tabela(i,3);0 1 0 0;0 0 1 0;0 0 0 1];
    Rx=[1 0 0 0;0 cos(Tabela(i,4)) sin(Tabela(i,4)) 0;0 -sin(Tabela(i,4)) cos(Tabela(i,4)) 0;0 0 0 1];
    A6=Rz*Tz*Tx*Rx;

    elseif(i==7)
    Rz=[cos(Tabela(i,1)) -sin(Tabela(i,1)) 0 0;sin(Tabela(i,1)) cos(Tabela(i,1)) 0 0;0 0 1 0;0 0 0 1];
    Tz=[1 0 0 0;0 1 0 0;0 0 1 Tabela(i,2);0 0 0 1];
    Tx=[1 0 0 Tabela(i,3);0 1 0 0;0 0 1 0;0 0 0 1];
    Rx=[1 0 0 0;0 cos(Tabela(i,4)) sin(Tabela(i,4)) 0;0 -sin(Tabela(i,4)) cos(Tabela(i,4)) 0;0 0 0 1];
    A7=Rz*Tz*Tx*Rx;

    end


end

Wynik=A1*A2*A3*A4*A5*A6*A7