% GRAVKOMP.M nelinearna kompenzacija gravitacionog momenta
             % materijal je namenjen vezbama iz Robotike iz
             % oblasti upravljanja
m6=0.09;

r=20e-2;
Xo=50e-2;
Yo=0e-2;
Zo=40e-2;

tf=3;
tc=tf/2;

V=2*r*pi/tf;
dt=1e-2;

TC=[];

t=[0:dt:tc]';
for ii=1:numrows(t),
  teta(ii)=-pi/2+V*t(ii)^2/2/r/tc;
  Y(ii)=Yo;
  X(ii)=Xo+r*cos(teta(ii));
  Z(ii)=Zo+r*sin(teta(ii));
  A=[1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, X(ii), Y(ii), Z(ii), 1]; % tekuca vrednost matrice homogene transformacije
  TC=[TC;A];
end

t=[tc+dt:dt:tf]';
for jj=1:numrows(t),
  ii=ii+1;
  teta(ii)=V/(r*(tf-tc))*(tf*(t(jj)-tc)-0.5*(t(jj)^2-tc*tc));
  Y(ii)=Yo;
  X(ii)=Xo+r*cos(teta(ii));
  Z(ii)=Zo+r*sin(teta(ii));
  A=[1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, X(ii), Y(ii), Z(ii), 1]; % tekuca vrednost matrice homogene transformacije
  TC=[TC;A];
end

t=[0:dt:tf]';

figure(1);
plot(X,Z);
xlabel('X [m]');
ylabel('Z [m]');

figure(2);
plot3(X,Y,Z);
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
   
%	alpha	A	theta	D	sigma	m	rx	ry	rz	Ixx	Iyy	Izz	Ixy	Iyz	Ixz	Jm	G		B		Tc+		Tc-
p560 = [pi/2	0	0	0	0	0	0	0	0	0	0.35	0	0	0	0	200e-6	-62.6111	1.48e-3		.395		-.435
         0	.4318	0	0	0	17.4	-.3638	.006	.0774	.13	.524	.539	0	0	0	200e-6	107.815		.817e-3		.126		-.071
         -pi/2	.0203	0	.1254	0	4.8	0	-.014	.070	.066	.086	.0125	0	0	0	200e-6	-53.7063	1.38e-3		.132		-.105
         pi/2	0	0	.4318	0	0.82	0	.019	0	1.8e-3	1.3e-3	1.8e-3	0	0	0	33e-6	76.0364		71.2e-6		11.2e-3		-16.9e-3
         -pi/2	0	0	0	0	0.34	0	0	0	.3e-3	.4e-3	.3e-3	0	0	0	33e-6	71.923		82.6e-6		9.26e-3		-14.5e-3
         0	0	0	0	0	m6	0	0	.032	.15e-3/.09*m6	.15e-3/.09*m6	.04e-3/.09*m6	0	0	0	33e-6	76.686		36.7e-6		3.96e-3		-10.5e-3];

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

%q0=[0 120 -140 0 20 0]*pi/180;
q0=[-18.5307  108.3507    2.6576    0.0000 -111.0082   18.5307];
M=[1 1 1 1 1 1]; % matrica vaznosti prostornih koordinata

q=ikine(p560,TC,q0,M); % racunanje vektora unutrasnjih koordinata
%q=ikine560(p560,TC,config(2));

qd=[0 0 0 0 0 0]; % pocetne vrednosti brzina u zglobovima
qdd=[0 0 0 0 0 0]; % pocetne vrednosti ubrzanja u zglobovima

for ii=2:numrows(q),
  qd(ii,:)=(q(ii,:)-q(ii-1,:))/dt; % generisanje brzina u zglobovima Ojlerovom metodom
  qdd(ii,:)=(qd(ii,:)-qd(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,qd);
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,qdd);
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]');

% animacija ostvarenog kretanja

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

save gravkomp.mat;

% Dinamicki parametri modela 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=[62.6111  107.815  53.7063  76.0364  71.9230  76.6860];
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];

% definisanje parametara aktuatora - DC motora upravljanih strujom u rotoru

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

%        R   Kem   Kme     J       N      B    U
%p560dyn=[1.1 0.034 0.034 0.00132 62.6111 0.0001 20
%         1.1 0.034 0.034 0.00132 107.815 0.0001 20
%         1.1 0.034 0.034 0.00132 53.7063 0.0001 20
%         1.1 0.034 0.034 0.00132 76.0364 0.0001 20
%         1.1 0.034 0.034 0.00132 71.9230 0.0001 20
%         1.1 0.034 0.034 0.00132 76.6860 0.0001 20];


tf=t(numrows(t));               % preuzimanje vremena trajanja kretanja iz programa P_KIN.M
q0=q(1,:);          % pocetni polozaj
q1=q(numrows(q),:); % krajnji polozaj
qd0=qd(1,:);        % pocetna brzina

t111=t;

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

%taunom=[t111, un]; 
qt=[t111, q, qd, qdd];

%****************************************
% odredjivanje parametara PD regulatora *
%****************************************

% Odredivanje maximalne inercije na trajektoriji
disp('Odredivanje max. inercije');
Md=[];
for i=1:numrows(q),
    Mi=inertia(p560,q(i,:));
    mvec=diag(Mi);
    Md=[Md;mvec'];
end;

J=max(Md);                   % Maximalni moment inercije
w=[70 100 100 100 50 50]; % Strukturne rez. frekv.

% Formule podesavanja PD
Pgain=(J+Jm.*N.^2).*(w.^2).*(p560dyn(:,1)'./p560dyn(:,5)')./(p560dyn(:,2)')/4
Dgain=2*sqrt(Pgain.*(J+Jm.*N.^2).*(p560dyn(:,1)'./p560dyn(:,5)')./(p560dyn(:,2)'))-(p560dyn(:,5)'.*(p560dyn(:,2)'))

%Pgain=[1.1698e4	0.9562e4 0.3480e4 0.0505e4 0.0475e4 0.0505e4];
%Dgain=[216.3101	195.5726 117.9761 44.9463 43.5876 44.9279];

%ctlname='pdctl';

global qt Pgain Dgain p560dyn p560;

%********************
% direktna dinamika *
%********************
disp('DIREKTNA DINAMIKA BEZ KOMPENZACIJE');
[tsim1, q_os1, qd_os1]= fdynact(p560, p560dyn, 0, tf, 'pdctl', 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_osint1 i qd_osint1');
q_os1=interp1(tsim1, q_os1, t111); % interpolacija ostvarenih pozicija radi uskladjivanja sa t111
qd_os1=interp1(tsim1, qd_os1, t111); % interpolacija ostvarenih brzina radi uskladjivanja sa t111

e1=q-q_os1;                        % izracunavanje greske pozicije
ed1=qd-qd_os1;                     % izracunavanje greske brzine

upd=e1*diag(Pgain)+ed1*diag(Dgain);

disp('DIREKTNA KINEMATIKA');
%***********************
% DIREKTNA KINEMATIKA  *
%***********************
FK_OS1=fkine(p560,q_os1); % FKOS je (broj tacaka) x 16 matrica "razvucenih"
                          % homogenih transformacija tacaka ostvarene
 	                  % trajektorije vrha robota
figure(7);
plot3(FK_OS1(:,13),FK_OS1(:,14),FK_OS1(:,15),'y'); % crtanje zadate spojlasnje trajektorije
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plotpro2(p560,q_os1); % animacija ostvarenog kretanja

figure(8);
plot(t,(X-FK_OS1(:,13)')*1000);
xlabel('Vreme [s]');
ylabel('Greska po x [mm]');

figure(9);
plot(t,(Y-FK_OS1(:,14)')*1000);
xlabel('Vreme [s]');
ylabel('Greska po y [mm]');

figure(10);
plot(t,(Z-FK_OS1(:,15)')*1000);
xlabel('Vreme [s]');
ylabel('Greska po z [mm]');

figure(11);
plot(t,upd(:,1), t,upd(:,2), t,upd(:,3));
xlabel('Vreme [s]');
ylabel('Upravljacki naponi [V]');
legend('U1', 'U2', 'U3');


disp('DIREKTNA DINAMIKA SA KOMPENZACIJOM');
[tsim2, q_os2, qd_os2]= fdynact(p560, p560dyn, 0, tf, 'grctl', 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_osint2 i qd_osint2');
q_os2=interp1(tsim2, q_os2, t111); % interpolacija ostvarenih pozicija radi uskladjivanja sa t111
qd_os2=interp1(tsim2, qd_os2, t111); % interpolacija ostvarenih brzina radi uskladjivanja sa t111

e2=q-q_os2;                        % izracunavanje greske pozicije
ed2=qd-qd_os2;                     % izracunavanje greske brzine

ugr=gravload(p560,q);
ugrpom=[];
for ii=1:numrows(ugr),
  pom=ugr(ii,:)./[p560dyn(1,5)*p560dyn(1,2)/p560dyn(1,1) p560dyn(2,5)*p560dyn(2,2)/p560dyn(2,1) p560dyn(3,5)*p560dyn(3,2)/p560dyn(3,1) p560dyn(4,5)*p560dyn(4,2)/p560dyn(4,1) p560dyn(5,5)*p560dyn(5,2)/p560dyn(5,1) p560dyn(6,5)*p560dyn(6,2)/p560dyn(6,1)];
  ugrpom=[ugrpom; pom];
end
ugr=ugrpom;

updg=e2*diag(Pgain)+ed2*diag(Dgain);

disp('DIREKTNA KINEMATIKA');
%***********************
% DIREKTNA KINEMATIKA  *
%***********************
FK_OS2=fkine(p560,q_os2); % FKOS2 je (broj tacaka) x 16 matrica "razvucenih"
                          % homogenih transformacija tacaka ostvarene
 	                  % trajektorije vrha robota
figure(12);
plot3(FK_OS2(:,13),FK_OS2(:,14),FK_OS2(:,15),'y'); % crtanje zadate spojlasnje trajektorije
hold; % omogucava se dodavanje novog grafika u prostor prethodno nacrtanog
plotpro2(p560,q_os2); % animacija ostvarenog kretanja

figure(13);
plot(t,(X-FK_OS2(:,13)')*1000);
xlabel('Vreme [s]');
ylabel('Greska po x [mm]');

figure(14);
plot(t,(Y-FK_OS2(:,14)')*1000);
xlabel('Vreme [s]');
ylabel('Greska po y [mm]');

figure(15);
plot(t,(Z-FK_OS2(:,15)')*1000);
xlabel('Vreme [s]');
ylabel('Greska po z [mm]');

figure(16);
plot(t,updg(:,1), t,updg(:,2), t,updg(:,3));
xlabel('Vreme [s]');
ylabel('Upravljacki naponi [V]');
legend('U1', 'U2', 'U3');

