P3_1DOF

% P3
% Direct Dynamics
clear all; close all; clc;
L = 2; m = 1; g = 10;
syms t
omega1 = [0 0 diff('theta(t)',t)];
omega2 = [0 0 -diff('theta(t)',t)];
alpha1 = diff(omega1,t);
alpha2 = diff(omega2,t);
% diff(X,'t') or diff(X,sym('t'))
% differentiates a symbolic expression
% X with respect to t
% diff(X,'t',n) and diff(X,n,'t')
% differentiates X n times
% n is a positive integer

c = cos(sym('theta(t)'));
s = sin(sym('theta(t)'));

xC1 = (L/2)*c; yC1 = (L/2)*s;
rC1 = [xC1 yC1 0];

xB = L*c; yB = L*s;
rB = [xB yB 0];

xC2 = (3*L/2)*c; yC2 = (L/2)*s;
rC2 = [xC2 yC2 0];

rC=[2*L*c 0 0];

vC1 = diff(rC1,t);
aC1 = diff(vC1,t);

vC2 = diff(rC2,t);
aC2 = diff(vC2,t);

fprintf('aC1x =')
pretty(aC1(1))
fprintf('aC1y =')
pretty(aC1(2))
fprintf('\n')
fprintf('aC2x =')
pretty(aC2(1))
fprintf('aC2y =')
pretty(aC2(2))
fprintf('\n\n')

G1 = [0 -m*g 0];
G2 = [0 -m*g 0];

IO = m*L^2/3;
IC2 = m*L^2/12;

syms R21x R21y R02y
R21= [R21x R21y 0];
R02=[0 R02y 0];

% link 2
F2 = -R21+G2+R02;
eqII=m*aC2-F2;
eqIIx=simplify(eqII(1));
eqIIy=simplify(eqII(2));

MC2=cross(rB-rC2,-R21)+cross(rC-rC2,R02);
eqIIm=IC2*alpha2-MC2;
eqIIz=simplify(eqIIm(3));

soleq=solve(eqIIx, eqIIy, eqIIz,'R21x, R21y, R02y');
R21xs=soleq.R21x;
R21ys=soleq.R21y;
R02ys=soleq.R02y;


fprintf('R21x =')
pretty(simplify(R21xs))
fprintf('\n\n')

fprintf('R21y =')
pretty(simplify(R21ys))
fprintf('\n\n')

fprintf('R02y =')
pretty(simplify(R02ys))
fprintf('\n\n')


% link 1
R21s = [R21xs R21ys 0];
eq = -IO*alpha1+cross(rC1, G1)+cross(rB, R21s);
eqz = simplify(eq(3));
eqz1 = subs(eqz,diff('theta(t)',t,2),'ddtheta');
eqz2 = subs(eqz1, diff('theta(t)',t), sym('x(2)'));
eqz3 = subs(eqz2, 'theta(t)', sym('x(1)'));

% solve a second order ODE using MATLAB
% ODE45 function
% write the second order equation as a
% system of two first order equations,
% by introducing x(2) = x(1)'
%  x(1)' = x(2)  ==>   x' = g(t,x)
%  x(2)' = f
%
% define the vector x = [x(1); x(2)]
% (column-vector).

% first differential equation
dx1 = sym('x(2)');
% second differential equation
dx2 = solve(eqz3,'ddtheta');

% define right-hand side vector
dx1dt = char(dx1);
dx2dt = char(dx2);

t0 = 0;  % define initial time
tf = 1;  % define final time
time = [0 tf];
x0 = [pi/4 0]; % define initial conditions

fid = fopen('eP3.m','w+');
fprintf(fid,'function dx = eP3(t,x)\n');
fprintf(fid,'dx = zeros(2,1);\n');
fprintf(fid,'dx(1) = '); fprintf(fid,dx1dt); fprintf(fid,';\n');
fprintf(fid,'dx(2) = '); fprintf(fid,dx2dt); fprintf(fid,';');
fclose(fid);
cd(pwd);

optionP3 = odeset('RelTol',1e-3,'MaxStep',1e-3,'Events',@eventP3);

[t, xs, te, ye] = ode45(@eP3, time, x0, optionP3);

theta0 = ye(1);
omega0 = ye(2);

% fprintf('t0=  %g  [s] \n',t0);
fprintf('final values \n');
fprintf('tf =  %g [s] \n',te);
fprintf('thetaf = %g [rad]   \n',theta0);
fprintf('omegaf = %g [rad/s] \n',omega0);


% ode45  solves non-stiff differential equations,
% medium order method.
% [ts,ys] = ode45(f,tspan,y0) integrates
% the system of differential equations
% y' = f(t,y) with initial conditions y0.

x1 = xs(:,1); % extract x1 & x2 components from xs
x2 = xs(:,2);

subplot(2,1,1),plot(t,x1,'r'),...
xlabel('t'),ylabel('\theta [rad]'),grid,...
subplot(2,1,2),plot(t,x2),...
xlabel('t'),ylabel('d\theta/dt = \omega [rad/s]'),grid

aC1x =
                                 2
- cos(theta(t)) diff(theta(t), t)  -

   sin(theta(t)) diff(theta(t), t, t)
aC1y =
cos(theta(t)) diff(theta(t), t, t) -

                                  2
   sin(theta(t)) diff(theta(t), t)

aC2x =
                                   2
- 3 cos(theta(t)) diff(theta(t), t)  -

   3 sin(theta(t)) diff(theta(t), t, t)
aC2y =
cos(theta(t)) diff(theta(t), t, t) -

                                  2
   sin(theta(t)) diff(theta(t), t)


R21x =
                                 2
3 cos(theta(t)) diff(theta(t), t)  +

   3 sin(theta(t)) diff(theta(t), t, t)


R21y =
  /                                  2
  | sin(2 theta(t)) diff(theta(t), t)
- | ----------------------------------
  \                 2

    -

   cos(2 theta(t)) diff(theta(t), t, t)
   ------------------------------------
                    2

                             \
      7 diff(theta(t), t, t) |
    + ---------------------- | /
                6            /

   (cos(theta(t))) - 5


R02y =
    /                                  2
5 - | sin(2 theta(t)) diff(theta(t), t)
    \

    - cos(2 theta(t))

   diff(theta(t), t, t) +

   2 diff(theta(t), t, t) \
   ---------------------- | /
             3            /

   (cos(theta(t)))


final values 
tf =  0.699873 [s] 
thetaf = -5.81132e-17 [rad]   
omegaf = -3.25678 [rad/s]