% DS9797_3.M seminarski rad iz predmeta Dinamika mehanizama i masina: 3. grupa 97/98

disp('SEMINARSKI RAD IZ DINAMIKE MEHANIZAMA I MASINA');
disp('III grupa 97/98');
disp('')
disp('DEFINISANJE GEOMETRIJE ROBOTA I GENERISANJE VEKTORA SPOLJNIH KOORDINATA');

% definisanje geometrije robota

m1=20;
m2=10;
m3=0;
m4=5;
m5=0;
m6=2;

l1=0.15;
l2=0.6;
l3=0;
l4=0.7;
l5=0;
l6=0.2;


r=0.02;

% parametri motora

R=[0.176 0.176 1.35 1.35 1.89 1.89];
Kem=[0.08 0.08 0.1 0.1 0.06 0.06];
Kme=Kem;
N=[92.7 92.7 100 100 100 100];
Jm=[1.46e-4 1.46e-4 1.22e-4 1.22e-4 6.62e-5 6.62e-5];
B=[0 0 0 0 0 0];
U=[24 24 24 24 24 24];
No_load_speed=[2770 2770 2230 2230 3660 3660]*2*pi/60;
Stall_torque=[10.9 10.9 1.78 1.78 0.765 0.765];

% 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
	    pi/2   0       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                  
           -pi/2   0       0      l4      0    m4    0       l4/2    0    m4*(3*r*r+l4*l4)/12    m4*r*r/2              m4*(3*r*r+l4*l4)/12     0    0    0    Jm(4)   N(4)  B(4)   0     0
 	   -pi/2   0       0      0       0    m5   -l5/2    0       0    m5*r*r/2               m5*(3*r*r+l5*l5)/12   m5*(3*r*r+l5*l5)/12     0    0    0    Jm(5)   N(5)  B(5)   0     0
	     0     l6      0      0       0    m6   -l6/2    0       0    m6*r*r/2               m6*(3*r*r+l6*l6)/12   m6*(3*r*r+l6*l6)/12     0    0    0    Jm(6)   N(6)  B(6)   0     0];
	     
    
% planiranje kretanja u spoljasnjim koordinatama

tf=2.5;
dt=1e-2;
tc=0.5;

XA=0.55;

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

jj=0;
t=[0:dt:tc]; %11111 definisanje vektora vremenskih trenutaka pri ubrzanju
for ii=1:numcols(t),
  jj=jj+1;
  fi=(pi/4)*t(ii)^2;
  fivek(jj)=fi;
  Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
  Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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

t=[tc+dt:dt:tf-tc-dt]; %11112 definisanje vektora vremenskih trenutaka pri const. brzini
for ii=1:numcols(t),
  jj=jj+1;
  fi=(pi/4)*t(ii)-pi/16;
    fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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
   t=[tf-tc:dt:tf];%1111113usporavanje
   for ii=1:numcols(t),
   jj=jj+1;
   fi=(5*pi/4)*t(ii)-(pi/4)*t(ii)^2-17*pi/16;
       fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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
f0=fi;

 t=[dt:dt:tc]; % 222224definisanje vektora vremenskih trenutaka pri ubrzanju
for ii=1:numcols(t),
  jj=jj+1;
  fi=(pi/4)*t(ii)^2+f0;
  fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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

t=[tc+dt:dt:tf-tc-dt]; %333335 definisanje vektora vremenskih trenutaka pri const. brzini
for ii=1:numcols(t),
  jj=jj+1;
  fi=(pi/4)*t(ii)+f0-pi/16;
    fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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
   t=[tf-tc:dt:tf];%usporavanje
   for ii=1:numcols(t),
   jj=jj+1;
   fi=(5*pi/4)*t(ii)-(pi/4)*t(ii)^2-17*pi/16+f0;
     fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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
f0=fi;

t=[dt:dt:tc]; % definisanje vektora vremenskih trenutaka pri ubrzanju
for ii=1:numcols(t),
  jj=jj+1;
  fi=(pi/4)*t(ii)^2+f0;
    fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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

t=[tc+dt:dt:tf-tc-dt]; % o v d e   definisanje vektora vremenskih trenutaka pri const. brzini
for ii=1:numcols(t),
  jj=jj+1;
  fi=(pi/4)*t(ii)+f0-pi/16;
  fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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
   t=[tf-tc:dt:tf];%usporavanje
   for ii=1:numcols(t),
   jj=jj+1;
   fi=(5*pi/4)*t(ii)-(pi/4)*t(ii)^2-17*pi/16+f0;
     fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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
f0=fi;

t=[dt:dt:tc]; % definisanje vektora vremenskih trenutaka pri ubrzanju
for ii=1:numcols(t),
  jj=jj+1;
  fi=(pi/4)*t(ii)^2+f0;
    fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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

t=[tc+dt:dt:tf-tc-dt]; % definisanje vektora vremenskih trenutaka pri const. brzini
for ii=1:numcols(t),
  jj=jj+1;
  fi=(pi/4)*t(ii)-pi/16+f0;
    fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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
   t=[tf-tc:dt:tf];%usporavanje
   for ii=1:numcols(t),
   jj=jj+1;
   fi=(5*pi/4)*t(ii)-(pi/4)*t(ii)^2-17*pi/16+f0;
     fivek(jj)=fi;
Y(jj)=0.1*(1+(cos(fi))^2)*sin(fi); 
 Z(jj)=0.05*((sin(fi))^2)*cos(fi);
  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]');

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

disp('Pritisnuti blanko za nastavak!');
pause;

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

q0=[0.278299659 2.024923079 -0.460079173 0 -2.030875498 -0.278299659];
M=[1 1 1 1 1 1]; % matrica vaznosti prostornih koordinata

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

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

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

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

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

figure(5);
plot(t,qtt);
xlabel('vreme[s]');

legend('qtt1 [rad/s^2]', 'qtt2 [rad/s^2]', 'qtt3 [rad/s^2]','qtt4 [rad/s^2]', 'qtt5 [rad/s^2]', 'qtt6 [rad/s^2]');

disp('Pritisnuti blanko 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]');

figure(9);
plot(t,tau(:,4),'y', t,tau_i(:,4),'r', t,tau_c(:,4),'b', t,tau_g(:,4),'c');
xlabel('Vreme [s]');
legend('Tau_4 [Nm]', 'Tau_i_4 [Nm]', 'Tau_c_4 [Nm]', 'Tau_g_4 [Nm]');

figure(10);
plot(t,tau(:,5),'y', t,tau_i(:,5),'r', t,tau_c(:,5),'b', t,tau_g(:,5),'c');
xlabel('Vreme [s]');
legend('Tau_5 [Nm]', 'Tau_i_5 [Nm]', 'Tau_c_5 [Nm]', 'Tau_g_5 [Nm]');

figure(11);
plot(t,tau(:,6),'y', t,tau_i(:,6),'r', t,tau_c(:,6),'b', t,tau_g(:,6),'c');
xlabel('Vreme [s]');
legend('Tau_6 [Nm]', 'Tau_i_6 [Nm]', 'Tau_c_6 [Nm]', 'Tau_g_6 [Nm]');

disp('Pritisnuti blanko za nastavak');
pause;

disp('DINAMICKE MEHANICKE KARAKTERISTIKE U ZGLOBOVIMA');

tau_pom=[0:Stall_torque(1)/100:Stall_torque(1)]; % generisanje pomocnog vektora momenata za crtanje karakteristike motora

teta1_pom=No_load_speed(1)*(ones(size(tau_pom))-tau_pom/Stall_torque(1));
figure(12);
plot(tau_pom,teta1_pom,'y'); % crtanje granice radne zone 1. motora (na osnovu kataloga)
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plot(tau(:,1)/N(1),N(1)*qt(:,1),'c'); % crtanje dinamicke mehanicke karakteristike 1. motora
xlabel('Tau_1/N1 [Nm]');
ylabel('N1*qt1 [rad/s]');

tau_pom=[0:Stall_torque(2)/100:Stall_torque(2)]; % generisanje pomocnog vektora momenata za crtanje karakteristike motora
figure(13);
teta2_pom=No_load_speed(2)*(ones(size(tau_pom))-tau_pom/Stall_torque(2));
plot(tau_pom,teta2_pom,'y'); % crtanje granice radne zone 2. motora (na osnovu kataloga)
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plot(tau(:,2)/N(2),N(2)*qt(:,2),'c'); % crtanje dinamicke mehanicke karakteristike 2. motora
xlabel('Tau_2/N2 [Nm]');
ylabel('N2*qt2 [rad/s]');

tau_pom=[0:Stall_torque(3)/100:Stall_torque(3)]; % generisanje pomocnog vektora momenata za crtanje karakteristike motora
figure(14);
teta3_pom=No_load_speed(3)*(ones(size(tau_pom))-tau_pom/Stall_torque(3));
plot(tau_pom,teta3_pom,'y'); % crtanje granice radne zone 3. motora (na osnovu kataloga)
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plot(tau(:,3)/N(3),N(3)*qt(:,3),'c'); % crtanje dinamicke mehanicke karakteristike 3. motora
xlabel('Tau_3/N3 [Nm]');
ylabel('N3*qt3 [rad/s]');

tau_pom=[0:Stall_torque(4)/100:Stall_torque(4)]; % generisanje pomocnog vektora momenata za crtanje karakteristike motora
figure(15);
teta4_pom=No_load_speed(4)*(ones(size(tau_pom))-tau_pom/Stall_torque(4));
plot(tau_pom,teta4_pom,'y'); % crtanje granice radne zone 4. motora (na osnovu kataloga)
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plot(tau(:,4)/N(4),N(4)*qt(:,4),'c'); % crtanje dinamicke mehanicke karakteristike 4. motora
xlabel('Tau_4/N4 [Nm]');
ylabel('N4*qt4 [rad/s]');

tau_pom=[0:Stall_torque(5)/100:Stall_torque(5)]; % generisanje pomocnog vektora momenata za crtanje karakteristike motora
figure(16);
teta5_pom=No_load_speed(5)*(ones(size(tau_pom))-tau_pom/Stall_torque(5));
plot(tau_pom,teta5_pom,'y'); % crtanje granice radne zone 5. motora (na osnovu kataloga)
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plot(tau(:,5)/N(5),N(5)*qt(:,5),'c'); % crtanje dinamicke mehanicke karakteristike 5. motora
xlabel('Tau_5/N5 [Nm]');
ylabel('N5*qt5 [rad/s]');

tau_pom=[0:Stall_torque(6)/100:Stall_torque(6)]; % generisanje pomocnog vektora momenata za crtanje karakteristike motora
figure(17);
teta6_pom=No_load_speed(6)*(ones(size(tau_pom))-tau_pom/Stall_torque(6));
plot(tau_pom,teta6_pom,'y'); % crtanje granice radne zone 6. motora (na osnovu kataloga)
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plot(tau(:,6)/N(6),N(6)*qt(:,6),'c'); % crtanje dinamicke mehanicke karakteristike 6. motora
xlabel('Tau_6/N6 [Nm]');
ylabel('N6*qt6 [rad/s]');

%****************************************
% 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)                     
             R(4)  Kem(4) Kme(4)  Jm(4)  N(4)  0   U(4)
             R(5)  Kem(5) Kme(5)  Jm(5)  N(5)  0   U(5)
             R(6)  Kem(6) Kme(6)  Jm(6)  N(6)  0   U(6)];
un=idynact(dmm_rob,dmm_rob_act,q,qt,qtt);

figure(18);
plot(t,un(:,1),'y', t,un(:,2),'r', t,un(:,3),'b',t,un(:,4),'g',t,un(:,5),'w',t,un(:,6),'c');
xlabel('t [s]');
legend('U1nom [V]','U2nom [V]', 'U3nom [V]','U4nom [V]','U5nom [V]', 'U6nom [V]');

% animacija ostvarenog kretanja

figure(19);
plot3(X,Y,Z); % crtanje zadate spojlasnje trajektorije
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plotpro2(dmm_rob,q,'r'); % animacija ostvarenog kretanja

