% 3d. dan racunskih vezbi iz Tehnicke robotike u zimskom semestru skolske 1998/99
% 3. 12. 1998.

%*************
% Zadatak 3. *
%*************

% Parametri segmenata i motora E2 manipulatora

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

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

% Generisanje trajektorije
disp('Generisanje trajektorije')

tf=1;
dt=10e-3;

t=[0:dt:tf];

q0=[30 -60]*pi/180

qf=[90 -30]*pi/180

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

figure(1);
plot(t,q);
xlabel('Vreme [s]')
ylabel('Uglovi [rad]')
legend('q1', 'q2')

figure(2);
plot(t,qd);
xlabel('Vreme [s]')
ylabel('Brzine [rad/s]')
legend('qt1', 'qt2')

figure(3);
plot(t,qdd);
xlabel('Vreme [s]')
ylabel('Ubrzanja [rad/s^2]')
legend('qtt1', 'qtt2')

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;

% Matrica kinematickih i dinamickih parametara E2R manipulatora

%           alfa   a     theta    d    sigma  m     rx      ry      rz       Ixx     Iyy            Izz           Ixy  Iyz  Ixz  Jm   G     B   Tc+   Tc-
dh_ee2r=[     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];

xy=fkine(dh_ee2r,q);

disp('Animacija kretanja');

figure(4)
plot(xy(:,13),xy(:,14),'r');
hold;
plotpro1(dh_ee2r,q)

disp('Pritisni razmaknicu da bi nastavio rad!');
pause;

% Matrica Dinamickih parametara aktuatora E2R manipulatora

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

% Inverzna dinamika
disp('Racunanje nominalnih napona motora - racunanje inverzne dinamike sa aktuatorima')

ur=idynact(dh_ee2r, act_ee2r, q, qd, qdd);

figure(5);
plot(t,ur);
xlabel('Vreme [s]')
ylabel('Nominalni naponi [V]')
legend('Ur1', 'Ur2')

% Direktna dinamika

%t111=t';
%taunom=[t111, ur];
%qt=[t111, q, qd, qdd];

%Pgain=[0 0];
%Dgain=[0 0];

%qd0=qd(1,:);

%ctlanme='nomctl'

%global taunom qt Pgain Dgain ctlname

%[tsim, q_os, qd_os]=fdynact(dh_ee2r, act_ee2r, 0, tf, 'nomctl', q0, qd0);

%q_os=interp1(tsim, q_os, t111);

%qd_os=interp1(tsim, qd_os, t111);

%plot(t111, q_os-q);

%plotbot(ee2rob, q_os);
