% V2DIN.M vezba iz robotike u kokoj se razmatra dinamika manipulatora sa 2 SS. u vertikalnoj ravni

m1=10;
m2=5;
m3=5;

l1=0.5;
l2=1;
l3=1;

R=[1.35 1.35 1.35];
Kem=[0.1 0.1 0.1];
Kme=Kem;
N=[35 35 35];
Jm=[122e-6 122e-6 122e-6];
B=[0 0 0];
U=[24 24 24];

%           alfa   a     theta    d     sigma  m     rx      ry      rz       Ixx          Iyy         Izz       Ixy  Iyz  Ixz   Jm      G     B      Tc+   Tc-
veljko_dh=[ pi/2   0       0      l1      0    m1    0      -l1/2    0    m1*l1*l1/12       0       m1*l1*l1/12   0    0    0    Jm(1)   N(1)  B(1)   0     0
 	     0     l2      0      0       0    m2   -l2/2    0       0         0       m2*l2*l2/12  m2*l2*l2/12   0    0    0    Jm(2)   N(2)  B(2)   0     0
	     0     l3      0      0       0    m3   -l3/2    0       0         0       m3*l3*l3/12  m3*l3*l3/12   0    0    0    Jm(3)   N(3)  B(3)   0     0];

q0=[0 90 -90]*pi/180;
qk=[-90 60 -120]*pi/180;
tk=5;
proc=0.2;
dtnom=1e-3;

tc=proc*tk;
qct=[qk-q0]/(2*tk*(1-proc));
qctt=qct/tc;

dt=tc/round(tc/dtnom);
for t=0:dt:tc,
  q=[q0(1)+qctt(1)*t*t/2, q0(2)+qctt(2)*t*t/2, q0(3)+qctt(3)*t*t/2];
end
q_tc=q(numrows(q),:);

dt=(tk-2*tc)/round((tk-2*tc)/dtnom);
for t=tc+dt:dt:tf-tc,
  q=[q_tc(1)+qct(1)*(t-tc), q_tc(2)+qct(2)*(t-tc), q_tc(3)+qct(3)*(t-tc)];
end
q_tk_tc=q(numrows(q),:);

dt=tc/round(tc/dtnom);
for t=tk-tc+dt:dt:tk,
  q=[q_tk_tc(1)+qct(1)*(t-tk+tc)-qctt(1)*(t-tk+tc)^2/2, q_tk_tc(2)+qct(2)*(t-tk+tc)-qctt(2)*(t-tk+tc)^2/2, q_tk_tc(3)+qct(3)*(t-tk+tc)-qctt(3)*(t-tk+tc)^2/2];
end

figure(1);
plot(t,q(:,1),'y', t,qd(:,1),'r', t,qdd(:,1)/4,'c');
xlabel('t [s]');
legend('q1 [rad]','qd1 [rad/s]','qdd1/4 [rad/s^2]');
whitebg('k')

figure(2);
plot(t,q(:,2),'y', t,qd(:,2),'r', t,qdd(:,2)/4,'c');
xlabel('t [s]');
legend('q2 [rad]','qd2 [rad/s]','qdd2/4 [rad/s^2]');

figure(3);
plot(t,q(:,3),'y', t,qd(:,3),'r', t,qdd(:,3)/4,'c');
xlabel('t [s]');
legend('q3 [rad]','qd3 [rad/s]','qdd3/4 [rad/s^2]');

disp('Pritisni razmaknicu za nastavak');
pause;

FX=fkine(veljko_dh,q);

figure(4);
plot3(FX(:,13),FX(:,14),FX(:,15));
hold;
plotbot(veljko_dh,q);	% kretanje robota po zadatoj trajektoriji
 
%             Rr    Kem    Kme     Jm     N    B    U
veljko_act=[R(1)  Kem(1) Kme(1)  Jm(1)  N(1)  0   U(1)
             R(2)  Kem(2) Kme(2)  Jm(2)  N(2)  0   U(2)
             R(3)  Kem(3) Kme(3)  Jm(3)  N(3)  0   U(3)];

%********************
% inverzna dinamika *
%********************
disp('INVERZNA DINAMIKA');
un=idynact(veljko_dh,veljko_act,q,qd,qdd);

figure(4);
plot(t,un(:,1),'y', t,un(:,2),'r', t,un(:,2),'b');
xlabel('t [s]');
legend('U1nom [V]','U2nom [V]', 'U3nom [V]');

disp('Pritisni razmaknicu za nastavak');
pause;

%********************
% direktna dinamika *
%********************
qd0=[0 0 0];
t111=t';
taunom=[t111, un]; 
qt=[t111, q, qd, qdd];
Pgain=[0 0 0];
Dgain=[0 0 0];
tf=tk;
qd0=qd(1,:);
global taunom qt Pgain Dgain ctlname;
disp('DIREKTNA DINAMIKA');
[tsim, q_os, qd_os]= fdynact(veljko_dh, veljko_act, 0, tk, 'nomctl', q0, qd0); % integracija modela robota za pokret
                                                                       % koji traje od 0 do tf, sa pocetnom
                                                                       % pozicijom q0 i pocetnom brzinom qd0
                                                                       % q_os i qd_os jesu ostvarene pozicije
                                                                       % i brzine, za vremenski vektor tsim
disp('generisanje q_osint i qd_osint');
q_os=interp1(tsim, q_os, t111); % interpolacija ostvarenih pozicija radi uskladjivanja sa t111
qd_os=interp1(tsim, qd_os, t111); % interpolacija ostvarenih brzina radi uskladjivanja sa t111

figure(5);
plot(t,q_os(:,1),'y', t,qd_os(:,1),'r');
xlabel('t [s]');
legend('q1_os [rad]','qd1_os [rad/s]');
whitebg('k')

figure(6);
plot(t,q_os(:,2),'y', t,qd_os(:,2),'r');
xlabel('t [s]');
legend('q2_os [rad]','qd2_os [rad/s]');

figure(7);
plot(t,q_os(:,3),'y', t,qd_os(:,3),'r');
xlabel('t [s]');
legend('q3_os [rad]','qd3_os [rad/s]');

figure(8);
plot(t,q_os-q);
xlabel('t [s]');
legend('q_os-q [rad]');
whitebg('k')

figure(9);
plot(t,qd_os-qd);
xlabel('t [s]');
legend('qd_os-qd [rad/s]');
whitebg('k')

disp('Pritisni razmaknicu za nastavak');
pause;

%********************************
% animacija ostvarenog kretanja *
%********************************
figure(10);
plotbot(veljko_act,q_os);	% kretanje robota po zadatoj trajektoriji
