% Program 5.7
% RR robot arm
% Dynamics 
% Newton-Euler equations of motion

clear; clc; close;

L1 = 1; L2 = 1; m1 = 1; m2 = 1; g = 9.81;  
t = sym('t','real');
xB = L1*cos(sym('q1(t)')); yB = L1*sin(sym('q1(t)'));
rB = [xB yB 0];
rC1 = rB/2; vC1 = diff(rC1,t); aC1 = diff(vC1,t);
xD = xB + L2*cos(sym('q2(t)')); yD = yB + L2*sin(sym('q2(t)'));
rD = [xD yD 0];
rC2 = (rB + rD)/2; vC2 = diff(rC2,t); aC2 = diff(vC2,t);
omega1 = [0 0 diff('q1(t)',t)]; alpha1 = diff(omega1,t);
omega2 = [0 0 diff('q2(t)',t)]; alpha2 = diff(omega2,t);

G1 = [0 -m1*g 0]; G2 = [0 -m2*g 0];
IC1 = m1*L1^2/12; IA = IC1 + m1*(L1/2)^2; IC2 = m2*L2^2/12;

F21 = -m2*aC2 + G2;

b01 = 450; g01 = 300;
b12 = 200; g12 = 300;

q1f = pi/6; 
q2f = pi/3;

T01z = -b01*diff('q1(t)',t)-g01*(sym('q1(t)')-q1f)...
       +0.5*g*L1*m1*cos(sym('q1(t)'))+g*L1*m2*cos(sym('q1(t)'));
T01 = [0 0 T01z];

T12z = -b12*diff('q2(t)',t)-g12*(sym('q2(t)')-q2f)...
       +0.5*g*L2*m2*cos(sym('q2(t)'));
T12 = [0 0 T12z];

EqA = -IA*alpha1 + cross(rB, F21) + cross(rC1, G1) + T01 - T12;

Eq2 = -IC2*alpha2 + cross(rB - rC2, -F21) + T12;

slist={diff('q1(t)',t,2),diff('q2(t)',t,2),...
       diff('q1(t)',t),diff('q2(t)',t),'q1(t)','q2(t)'};
nlist = {'ddq1', 'ddq2', 'x(2)', 'x(4)', 'x(1)','x(3)'};

eq1 = subs(EqA(3),slist,nlist);
eq2 = subs(Eq2(3),slist,nlist);

sol = solve(eq1,eq2,'ddq1, ddq2');

dx2 = sol.ddq1;
dx4 = sol.ddq2;
 
dx2dt = char(dx2); 
dx4dt = char(dx4);

fid = fopen('RRrobot.m','w+'); 
fprintf(fid,'function dx = RRrobot(t,x)\n');
fprintf(fid,'dx = zeros(4,1);\n');
fprintf(fid,'dx(1) = x(2);\n');
fprintf(fid,'dx(2) = ');
fprintf(fid,dx2dt);
fprintf(fid,';\n');
fprintf(fid,'dx(3) = x(4);\n');
fprintf(fid,'dx(4) = ');
fprintf(fid,dx4dt);
fprintf(fid,';');
fclose(fid); 

%% insert cell divider

t0 = 0;  tf = 15; time = [0 tf];

x0 = [-pi/18 0 pi/6 0]; 

[t,xs] = ode45(@RRrobot, time, x0);

x1 = xs(:,1); 
x2 = xs(:,2);
x3 = xs(:,3); 
x4 = xs(:,4);  

subplot(2,1,1),plot(t,x1*180/pi,'r'),...
xlabel('t (s)'),ylabel('q1 (deg)'),grid,...
subplot(2,1,2),plot(t,x3*180/pi,'b'),...
xlabel('t (s)'),ylabel('q2 (deg)'),grid

[ts,xs] = ode45(@RRrobot,0:1:5,x0);

fprintf('Results \n'); fprintf('\n');
fprintf('    t(s)     q1(rad)  dq1(rad/s) q2(rad)  dq2(rad/s) \n'); 
[ts,xs]