%grupa1.m
% Definisanje kinematickih i dinamickih parametara robota
puma560;
%p560=p560(1:3,:);

% definisanje vremenskih parametara
T=3;          %ukupno vreme
dt=0.01;      %vremenski korak
tap=0.2;      %deo vremena za ubrzanje/usporenje
t=[0:dt:T]';  %vektor vremena

% definisanje trajektorije
r0=[0.4,0.1,0.2];   %pocetni polozaj
rf=[0.2,0.20,-0.25];   %krajnji polozaj

disp('Generisanje trajektorije');
[r,rd,rdd]=trajlin(r0,rf,T,dt,tap);

% Svodenje na dimenziju zadatka
% Prostorno kretanje dim=3
M=[1 1 1 0 0 0];  % Matrica vaznosti prostornih koordinata
r=r(:,1:3);
rd=rd(:,1:3);
rdd=rdd(:,1:3);

% definisanje pocetnih uslova u unutrasnjim koordinatama
disp('ikine');
%[pi/6 pi/6 -pi/12,0,pi/2,0]
q0=ikine(p560,transl(r0),[pi/6 pi/6 -pi/12,0,pi/2,0],M);
qd0=[0 0 0 0 0 0];

% inverzna kinematika
Ke=5000;  % konstanta povratne sprege
disp('Inverzna kinematika');
[q,qd,qdd]=ikinddf(p560,r,rd,rdd,q0,qd0,dt,Ke,M);

% Direktna kinematika za odredivanje greske
disp('Direktna kinematika');
FX=fkine(p560,q);

% Prikazivanje gresaka
figure(1);
plot(t,r(:,1)-FX(:,13));
ylabel('X_zad-X_ostv');

figure(2);
plot(t,r(:,2)-FX(:,14));
ylabel('Y_zad-Y_ostv');

figure(3);
plot(t,r(:,3)-FX(:,15));
ylabel('Z_zad-Z_ostv');

% Prikazivanje trajektorije
figure(4);
plot3(FX(:,13),FX(:,14),FX(:,15));

% Dinamika manipulatora

% Dinamicki parametri modela motora
%        R   Kem   Kme     J       N      B    U
motdyn=[1.1 0.034 0.034 0.00132 62.6111 0.0001 20
        1.1 0.034 0.034 0.00132 107.815 0.0001 20
        1.1 0.034 0.034 0.00132 53.7063 0.0001 20
        1.1 0.034 0.034 0.00132 76.0364 0.0001 20
        1.1 0.034 0.034 0.00132 71.9230 0.0001 20
        1.1 0.034 0.034 0.00132 76.6860 0.0001 20];

% Odredivanje maximalne inercije na trajektoriji
disp('Odredivanje max. inercije');
Md=[];
for i=1:numrows(q),
    Mi=inertia(p560,q(i,:));
    mvec=diag(Mi);
    Md=[Md;mvec'];
end;

J=max(Md);                   % Maximalni moment inercije
w=[157 157 157 157 157 157]; % Strukturne rez. frekv.

% Formule podesavanja PD
Pgain=(J.*(w.^2)).*(motdyn(:,1)')./(motdyn(:,5)')./(motdyn(:,2)')/4
Dgain=2*sqrt(Pgain)

% Standardna procedura za poziv funkcije fdynact
qt=[t q qd];
global Pgain Dgain qt;

% Simulacija
disp('Direktna dinamika');
[tsim,q_r,qd_r]=fdynact(p560,motdyn,0,T,'pdctl',q0,zeros(1,6));

% Standardno uskladivanje dimenzija vektora
q_dmd=interp1 (qt(:,1),qt(:,2:7),tsim);
qd_dmd=interp1 (qt(:,1),qt(:,8:13),tsim);
x_dmd=interp1 (t,r(:,1),tsim);
y_dmd=interp1 (t,r(:,2),tsim);
z_dmd=interp1 (t,r(:,3),tsim);

% Izracunavanje greske u unutrasnjim koordinatama
e_din=q_dmd-q_r;
ed_din=qd_dmd-qd_r;

% Izracunavanje upravljackog napona PD regulatora
u1s=e_din * diag(Pgain) + ed_din * diag(Dgain);

F=fkine(p560,q_r);
gr_x=x_dmd-F(:,13);
gr_y=y_dmd-F(:,14);
gr_z=z_dmd-F(:,15);

figure(5);
subplot(3,1,1);
plot(tsim,gr_x);
ylabel('Greska po X');
subplot(3,1,2);
plot(tsim,gr_y);
ylabel('Greska po Y');
subplot(3,1,3);
plot(tsim,gr_z);
ylabel('Greska po Z');
xlabel('Vreme');

save grupa1.mat


