% V1DIN.M vezba iz robotike u kokoj se razmatra dinamika manipulatora sa 2 SS. u vertikalnoj ravni

function v1din(Tseg);

q0=[30 -60]*pi/180;
q1=[80 -30]*pi/180;

t=[0:0.01:Tseg];

[q,qt,qtt]=jtraj(q0,q1,t);

figure(1);
plot(t,q(:,1),'y', t,qt(:,1),'r', t,qtt(:,1)/4,'c');
xlabel('t [s]');
legend('q1 [rad]','qt1 [rad/s]','qtt1/4 [rad/s^2]')
whitebg('k')

figure(2);
plot(t,q(:,2),'y', t,qt(:,2),'r', t,qtt(:,2)/4,'c');
xlabel('t [s]');
legend('q2 [rad]','qt2 [rad/s]','qtt2/4 [rad/s^2]');

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

g=9.81;
l=0.75;
m1=2*20;
m2=20;

for ii=1:numrows(q);
  t1=q(ii,1);
  t2=q(ii,2);
  t1d=qt(ii,1);
  t2d=qt(ii,2);
  t1dd=qtt(ii,1);
  t2dd=qtt(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);
end

figure(3);
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(4);
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]');

