% KINDIN.M sracunavanje IK i inverzne dinamike za vezbu
% dinamickog skaliranja trajektorije

HTTRAJ=[];
for ii=1:numcols(t),
TCTEK=[cos(0), sin(0), 0, 0, -sin(0), cos(0), 0, 0, 0, 0, 1, 0, X(ii), Y(ii), 0, 1];
HTTRAJ=[HTTRAJ;TCTEK];
end

for ii=1:numcols(ac),
ac(ii)=ac(ii)*sign(Xdd(ii));
end

figure(1);

subplot(3,1,1);
plot(t,v,'y', t,ac,'r');
xlabel('t [s]');
legend('v [m/s]', 'a [m/s^2]');

subplot(3,1,2);
plot(t,Xd,'y', t, Xdd,'r');
xlabel('t [s]');
legend('Xd [m/s]', 'Xdd [m/s^2]');

subplot(3,1,3);
plot(t,Yd,'y', t, Ydd,'r');
xlabel('t [s]');
legend('Yd [m/s]', 'Ydd [m/s^2]');

disp('Pritisni razmaknicu radi nastavka !');
pause;

disp('Sracunavanje IK !');

paramrob;
%q0=[0 0];
q0=[2.1665 -2.7622];
q= ikine(ee2rob,HTTRAJ,q0,[1 1 0 0 0 0]);

qd=[0 0];
qdd=qd;
for ii=2:numrows(q),
qd=[qd; (q(ii,:)-q(ii-1,:))/dt];
qdd=[qdd; (qd(ii,:)-qd(ii-1,:))/dt];
end

save kin.mat;

figure(2);
subplot(3,1,1);
plot(t,q(:,1),'y');
xlabel('t [s]');
ylabel('Teta1 [rad]');

subplot(3,1,2);
plot(t,qd(:,1),'r');
xlabel('t [s]');
ylabel('Teta1d [rad]');

subplot(3,1,3);
plot(t,qdd(:,1),'b');
xlabel('t [s]');
ylabel('Teta1dd [rad]');

figure(3);
plot(t,q(:,1),'y', t,qd(:,1),'r', t,qdd(:,1),'b');
legend('Teta1 [rad]', 'Teta1d [rad/s]', 'Teta1dd [rad/s]');

figure(4);
subplot(3,1,1);
plot(t,q(:,2),'y');
xlabel('t [s]');
ylabel('Teta2 [rad]');

subplot(3,1,2);
plot(t,qd(:,2),'r');
xlabel('t [s]');
ylabel('Teta2d [rad]');

subplot(3,1,3);
plot(t,qdd(:,2),'b');
xlabel('t [s]');
ylabel('Teta2dd [rad]');

figure(5);
plot(t,q(:,2),'y', t,qd(:,2),'r', t,qdd(:,2),'b');
legend('Teta2 [rad]', 'Teta2d [rad/s]', 'Teta2dd [rad/s]');

FX=fkine(ee2rob,q);

disp('Pritisni razmaknicu radi animacije pokreta !');
pause;

figure(6);
plot3(FX(:,13),FX(:,14),FX(:,15),'r');
hold;
plotpro1(ee2rob,q);
xlabel('x [m]');
ylabel('y [m]');

disp('Pritisni razmaknicu radi nastavka !');
pause;

disp('Sracunavanje inverzne dinamike !');
for ii=1:numrows(q);
t1=q(ii,1);
t2=q(ii,2);
t1d=qd(ii,1);
t2d=qd(ii,2);
t1dd=qdd(ii,1);
t2dd=qdd(ii,2);

TauD(ii,1)=1/3*t1dd*m1*l^2+4/3*t1dd*m2*l^2+t1dd*m2*l^2*cos(t2)+1/3*t2dd*m2*l^2+1/2*t2dd*m2*l^2*cos(t2);
TauD(ii,2)=1/3*t1dd*m2*l^2+1/2*t1dd*m2*l^2*cos(t2)+1/3*t2dd*m2*l^2;

TauC(ii,1)=-t1d*t2d*m2*l^2*sin(t2)-1/2*t2d^2*m2*l^2*sin(t2);
TauC(ii,2)=1/2*m2*l^2*t1d^2*sin(t2);

TauG(ii,1)=1/2*g*l*(m1*cos(t1)+m2*cos(t1+t2)+2*m2*cos(t1));
TauG(ii,2)=1/2*m2*g*l*cos(t1+t2);

Tau(ii,1)=TauD(ii,1)+TauC(ii,1)+TauG(ii,1);
Tau(ii,2)=TauD(ii,2)+TauC(ii,2)+TauG(ii,2);

D11 =1/3*m1*l1^2+1/3*m2*l2^2+m2*l2*l1*cos(q(ii,2))+m2*l1^2;
D12=1/3*m2*l2^2+1/2*m2*l2*l1*cos(q(ii,2));
D21=1/3*m2*l2^2+1/2*m2*l2*l1*cos(q(ii,2));
D22=1/3*m2*l2^2;

h1=-qd(ii,1)*qd(ii,2)*m2*l2*l1*sin(q(ii,2))-1/2*qd(ii,2)^2*m2*l2*l1*sin(q(ii,2));
h2=1/2*m2*l2*l1*qd(ii,1)^2*sin(q(ii,2));

G1=1/2*g*(cos(q(ii,1))*m1*l1+cos(q(ii,1)+q(ii,2))*m2*l2+2*m2*l1*cos(q(ii,1)));
G2=1/2*m2*g*l2*cos(q(ii,1)+q(ii,2));

u1nom(ii)=(D11+Jm(1)*N(1)^2)/(N(1)*Kem(1)/Rr(1))*qdd(ii,1)+D12/(N(1)*Kem(1)/Rr(1))*qdd(ii,2)+(h1+G1+N(1)^2*Kem(1)*Kme(1)/Rr(1)*qd(ii,1))/(N(1)*Kem(1)/Rr(1));
u2nom(ii)=D21/(N(2)*Kem(2)/Rr(2))*qdd(ii,1)+(D22+Jm(2)*N(2)^2)/(N(2)*Kem(2)/Rr(2))*qdd(ii,2)+(h2+G2+N(2)^2*Kem(2)*Kme(2)/Rr(2)*qd(ii,2))/(N(2)*Kem(2)/Rr(2));

end

figure(7);

subplot(2,2,1);
plot(t,TauD(:,1),'y');
xlabel('t [s]');
ylabel('TauH1 [Nm]');

subplot(2,2,2);
plot(t,TauC(:,1),'r');
xlabel('t [s]');
ylabel('TauC1 [Nm]');

subplot(2,2,3);
plot(t,TauG(:,1),'b');
xlabel('t [s]');
ylabel('TauG1 [Nm]');

subplot(2,2,4);
plot(t,Tau(:,1),'c');
xlabel('t [s]');
ylabel('Tau1 [Nm]');

figure(8);
plot(t,TauD(:,1),'y', t,TauC(:,1),'r', t,TauG(:,1),'b', t,Tau(:,1),'c');
xlabel('t [s]');
legend('TauH1 [Nm]','TauC1 [Nm]','TauG1 [Nm]', 'Tau1 [Nm]');

figure(9);

subplot(2,2,1);
plot(t,TauD(:,2),'y');
xlabel('t [s]');
ylabel('TauH2 [Nm]');

subplot(2,2,2);
plot(t,TauC(:,2),'r');
xlabel('t [s]');
ylabel('TauC2 [Nm]');

subplot(2,2,3);
plot(t,TauG(:,2),'b');
xlabel('t [s]');
ylabel('TauG2 [Nm]');

subplot(2,2,4);
plot(t,Tau(:,2),'c');
xlabel('t [s]');
ylabel('Tau2 [Nm]');

figure(10);
plot(t,TauD(:,2),'y', t,TauC(:,2),'r', t,TauG(:,2),'b', t,Tau(:,2),'c');
xlabel('t [s]');
legend('TauH2 [Nm]','TauC2 [Nm]','TauG2 [Nm]', 'Tau2 [Nm]');

Mpom=[0:Mmax(1)/100:Mmax(1)];
qpom=polyval([-nmax(1)/Mmax(1),nmax(1)],Mpom);

figure(11);
plot(Tau(:,1),qd(:,1),'y');
hold;
plot(Mpom,qpom,'r');
xlabel('Tau1 [Nm]')
ylabel('Teta1d [rad/s]');

Mpom=[0:Mmax(2)/100:Mmax(2)];
qpom=polyval([-nmax(2)/Mmax(2),nmax(2)],Mpom);

figure(12);
plot(Tau(:,2),qd(:,2),'y');
hold;
plot(Mpom,qpom,'r');
xlabel('Tau2 [Nm]')
ylabel('Teta2d [rad/s]');

figure(13);
plot(t,u1nom,'y', t,u2nom,'r');
legend('U1nom [V]','U2nom [V]');