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

function v2din(Tseg)

q0=[30 -60]*pi/180;
q1=[80 -30]*pi/180;

t=[0:0.001:Tseg];

[q,qd,qdd]=jtraj(q0,q1,t);

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]');

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

R1=1.35;
Kem1=0.1;
Kme1=Kem1;
N1=35;
l1=0.42;
m1=3.312;
J1=122e-6;

R2=8;
Kem2=48/(38*69);
Kme2=Kem2;
N2=69;
l2=0.28;
m2=0.42;
J2=1.1261e-6;

%           alfa   a     theta    d    sigma  m     rx      ry      rz       Ixx     Iyy            Izz           Ixy  Iyz  Ixz  Jm   G     B   Tc+   Tc-
ee2rob=[     0     l1      0      0      0    m1   -l1/2    0        0       0       m1*l1*l1/12    m1*l1*l1/12   0    0    0    J1   N1    0   0     0
 	     0     l2      0      0      0    m2   -l2/2    0        0       0       m2*l2*l2/12    m2*l2*l2/12   0    0    0    J2   N2    0   0     0];


%       Rr Kem  Kme   Jm  N     B  U
ee2acd=[R1 Kem1 Kme1  J1  N1    0  24
        R2 Kem2 Kme2  J2  N2    0  24];

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

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

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

%********************
% direktna dinamika *
%********************
t111=t';
taunom=[t111, un]; 
qt=[t111, q, qd, qdd];
Pgain=[0 0];
Dgain=[0 0];
tf=Tseg;
qd0=qd(1,:);
global taunom qt Pgain Dgain ctlname;
disp('DIREKTNA DINAMIKA');
[tsim, q_os, qd_os]= fdynact(ee2rob, ee2acd, 0, tf, '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(4);
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(5);
plot(t,q_os(:,2),'y', t,qd_os(:,2),'r');
xlabel('t [s]');
legend('q2_os [rad]','qd2_os [rad/s]');

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

figure(7);
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(8);
plotbot(ee2rob,q_os);	% kretanje robota po zadatoj trajektoriji
