% PROCMOM1.M procena potrebnih pogonskih momenata EE2R

function procmom1(proc,N1)

%N1=50;

%proc=20;

q0=-pi;
qf=pi;
tf=1;
tc=proc*tf/100;
dt=0.01;

qtc=(qf-q0)/(tf-tc);
qttc=qtc/tc;

jj=0;

for t=0:dt:tc,
  jj=jj+1;
  qtt(jj)=qttc;
  qt(jj)=qttc*t;
  q(jj)=q0+qttc*t*t/2;
end
q_tc=q(jj);

for t=tc+dt:dt:tf-tc,
  jj=jj+1;
  qtt(jj)=0;
  qt(jj)=qtc;
  q(jj)=q_tc+qtc*(t-tc);
end
q_tc=q(jj);

for t=tf-tc+dt:dt:tf,
  jj=jj+1;
  qtt(jj)=-qttc;
  qt(jj)=qtc-qttc*(t-tf+tc);
  q(jj)=q_tc+qtc*(t-tf+tc)-qttc*(t-tf+tc)^2/2;
end

t=[0:dt:tf];

q1=q;
q2=q;
q1t=qt;
q2t=qt;
q1tt=qtt;
q2tt=qtt;

%N1=30;
l1=0.42;
m1=3.312;
J1=122e-6;

N2=69;
l2=0.28;
m2=0.42;
%J2=2.0964e-6;
J2=1.1261e-6;

for ii=1:numcols(q1),

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

  h1=-q1t(ii)*q2t(ii)*m2*l2*l1*sin(q2(ii))-1/2*q2t(ii)^2*m2*l2*l1*sin(q2(ii));
  h2=1/2*m2*l2*l1*q1t(ii)^2*sin(q2(ii));

  M1nom(ii)=D11*q1tt(ii)+D12*q2tt(ii)+h1;
  M2nom(ii)=D21*q1tt(ii)+D22*q2tt(ii)+h2;

end

M1nom=M1nom/N1;
M2nom=M2nom/N2;

%           alfa   a     theta    d    sigma  m     rx      ry      rz       Ixx     Iyy            Izz           Ixy  Iyz  Ixz  Jm   G     B   Tc+   Tc-
ee2rob=[     0     l1      0      0      0    m1   -l1/2    0        0       0       m1*l1*l1/12    m1*l1*l1/12   0    0    0    J1   N1    0   0     0
 	     0     l2      0      0      0    m2   -l2/2    0        0       0       m2*l2*l2/12    m2*l2*l2/12   0    0    0    J2   N2    0   0     0];


figure(1);
plot(t,q1, t,q1t, t, q1tt);
legend('q1(t)', 'q1t(t)', 'q1tt(t)');

figure(2);
plot(t,q2, t,q2t, t, q2tt);
legend('q2(t)', 'q2t(t)', 'q2tt(t)');

figure(3);
plot(t,M1nom, t,M2nom,':');
legend('M1nom','M2nom');

[max(abs(M1nom)), max(abs(M2nom))]

figure(4);
plotpro1(ee2rob,[q1',q2'],'r');
