% Program 5.5
% R robot arm
% Dynamics 
% Newton-Euler equation of motion

clear; clc; close;

syms  t 

L = 1; m = 1; g = 9.81;

c = cos(sym('theta(t)')); s = sin(sym('theta(t)'));
xC = (L/2)*c; yC = (L/2)*s;
rC = [xC yC 0];

omega = [0 0 diff('theta(t)',t)];
alpha = diff(omega,t);

G = [0 -m*g 0];

IO = m*L^2/3;

beta = 45; 
gamma = 30;
qf = pi/3;

T01z = -beta*diff('theta(t)',t)-gamma*(sym('theta(t)')-qf)+0.5*g*L*m*c;
T01 = [0 0 T01z];

eq = -IO*alpha + cross(rC,G) + T01;
eqz = eq(3);

slist = {diff('theta(t)',t,2),diff('theta(t)',t),'theta(t)'};
nlist = {'ddtheta', 'x(2)' , 'x(1)'};

eqI = subs(eqz,slist,nlist);

dx1 = sym('x(2)');
dx2 = solve(eqI,'ddtheta');

dx1dt = char(dx1);
dx2dt = char(dx2);

g = inline(sprintf('[%s; %s]', dx1dt, dx2dt), 't', 'x');
 
time = [0 10];
x0 = [pi/18; 0]; % define initial conditions

[ts,xs] = ode45(g, 0:1:10, x0);

plot(ts,xs(:,1)*180/pi,'LineWidth',1.5),...
xlabel('t (s)'), ylabel('\theta (deg)'), grid, axis([0, 10, 0, 70])

fprintf('Results \n'); fprintf('\n');
fprintf('    t(s)    theta(rad) omega(rad/s) \n'); 
[ts,xs]