% DMM_SEM.M primer seminarskog rada iz predmeta Dinamika mehanizama i masina

disp('DEFINISANJE GEOMETRIJE ROBOTA I GENERISANJE VEKTORA SPOLJNIH KOORDINATA');

% definisanje geometrije robota

m1=20;
m2=10;
m3=3;

l1=0.3;
l2=0.5;
l3=0.2;

r=0.01;

% parametri motora

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

% matricna reprezentacija dinamickih parametara manipulatora po DH konvenciji

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

% planiranje kretanja u spoljasnjim koordinatama

tf=3;
proc=20;
dt=1e-2;
tc=proc/100*tf;

XA=0.4;
YA=-0.1;
ZA=0.8;
XB=XA;
YB=0.2;
ZB=0.9;

a=(ZB-ZA)/(YB-YA);
b=ZA-a*YA;

V=(YB-YA)*sqrt(1+a*a)/(tf-tc);

TC=[]; % inicijalizacija matrice "ispeglanih" homogenih transformacija

jj=0;
t=[0:dt:tc]; % definisanje vektora vremenskih trenutaka pri ubrzanju
for ii=1:numcols(t),
  jj=jj+1;
  Y(jj)=YA+0.5*V*t(ii)^2/(tc*sqrt(1+a*a));
  Z(jj)=a*Y(jj)+b;
  X(jj)=XA;
  A=[1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, X(jj), Y(jj), Z(jj), 1]; % tekuca vrednost matrice homogene transformacije
  TC=[TC;A];
end
Y_tc=Y(jj);

t=[tc+dt:dt:tf-tc-dt]; % definisanje vektora vremenskih trenutaka pri const. brzini
for ii=1:numcols(t),
  jj=jj+1;
  Y(jj)=Y_tc+V*(t(ii)-tc)/sqrt(1+a*a);
  Z(jj)=a*Y(jj)+b;
  X(jj)=XA;
  A=[1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, X(jj), Y(jj), Z(jj), 1]; % tekuca vrednost matrice homogene transformacije
  TC=[TC;A];
end
Y_tf_tc=Y(jj);

t=[tf-tc:dt:tf]; % definisanje vektora vremenskih trenutaka pri usporenju
for ii=1:numcols(t),
  jj=jj+1;
  Y(jj)=Y_tf_tc+(V*(t(ii)-tf+tc)-V*(t(ii)-tf+tc)^2/(2*tc))/sqrt(1+a*a);
  Z(jj)=a*Y(jj)+b;
  X(jj)=XA;
  A=[1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, X(jj), Y(jj), Z(jj), 1]; % tekuca vrednost matrice homogene transformacije
  TC=[TC;A];
end

figure(1);
plot3(X,Y,Z);
title('3D oblik zadate trajektorije');
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');

figure(2);
plot(Y,Z);
title('Planarni oblik zadate trajektorije');
xlabel('Y [m]');
ylabel('Z [m]');

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

% *********************
% inverzna kinematika *
%**********************
disp('RACUNANJE INVERZNE KINEMATIKE');

q0=[-atan(0.1/0.5) pi/3 -pi/6]; % pocetni uslov pri resavanju IK
M=[1 1 1 0 0 0]; % matrica vaznosti prostornih koordinata

q=ikine(dmm_rob,TC,q0,M); % racunanje vektora unutrasnjih koordinata

t=[0:dt:tf]; % definisanje celokupnog vektora vremenskih trenutaka

qt=[0 0 0]; % pocetne vrednosti brzina u zglobovima
qtt=[0 0 0]; % pocetne vrednosti ubrzanja u zglobovima

for ii=2:numrows(ii),
  qt(ii,:)=(q(ii,:)-q(ii-1,:))/dt; % generisanje brzina u zglobovima
  qtt(ii,:)=(qt(ii,:)-qt(ii-1,:))/dt; % generisanje brzina u zglobovima
end

figure(3);
plot(t,q);
xlabel('Vreme [s]');
legend('q1 [rad]', 'q2 [rad]', 'q3 [rad]');

figure(4);
plot(t,qt);
xlabel('Vreme [s]');
legend('qt1 [rad/s]', 'qt2 [rad/s]', 'qt3 [rad/s]');

figure(5);
plot(t,qtt);
xlabel('Vreme [s]');
legend('qtt1 [rad/s^2]', 'qtt2 [rad/s^2]', 'qtt3 [rad/s^2]');

disp('Pritisni razmaknicu za nastavak');
pause;
 
%*************************
% drugi problem dinamike *
%*************************
disp('RACUNANJE NOMINALNIH MOMENATA');
tau=rne(dmm_rob,q,qt,qtt);

disp('RACUNANJE INERCIONIH MOMENATA');
tau_i=itorque(dmm_rob,q,qtt);

disp('RACUNANJE KORIOLISOVIH I CENTRIPETALNIH MOMENATA');
tau_c=coriolis(dmm_rob,q,qt);

disp('RACUNANJE GRAVITACIONIH MOMENATA');
tau_g=gravload(dmm_rob,q);

figure(6);
plot(t,tau(:,1),'y', t,tau_i(:,1),'r', t,tau_c(:,1),'b', t,tau_g(:,1),'c');
xlabel('Vreme [s]');
legend('Tau_1 [Nm]', Tau_i_1 [Nm]', Tau_c_1 [Nm]', Tau_g_1 [Nm]');

figure(7);
plot(t,tau(:,2),'y', t,tau_i(:,2),'r', t,tau_c(:,2),'b', t,tau_g(:,2),'c');
xlabel('Vreme [s]');
legend('Tau_2 [Nm]', Tau_i_2 [Nm]', Tau_c_2 [Nm]', Tau_g_2 [Nm]');

figure(8);
plot(t,tau(:,3),'y', t,tau_i(:,3),'r', t,tau_c(:,3),'b', t,tau_g(:,3),'c');
xlabel('Vreme [s]');
legend('Tau_3 [Nm]', Tau_i_3 [Nm]', Tau_c_3 [Nm]', Tau_g_3 [Nm]');

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

%****************************************
% drugi problem dinamike sa aktuatorima *
%****************************************
disp('RACUNANJE NOMINALNIH NAPONA');

% definisanje parametara aktuatora - DC motora upravljanih strujom u rotoru

%             Rr    Kem    Kme     Jm     N    B    U
dmm_rob_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)];

un=idynact(dmm_rob,dmm_rob_act,q,qt,qtt);

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

