clear
close all
% M*xddot + C*xdot + K*x = F(t)
% System parameters
m1 = 2000; Icg = 2500; % kg
c1 = 3000; c2 = 3000; % kg/s
k1 = 30000; k2 = 30000; % N/m
l1 = 1; l2 = 1.5;
M = [m1 0;0 Icg];
C = [(c1+c2) (c1*l1-c2*l2);(c1*l1-c2*l2) ((c2*l2^2)+(c1*l1^2))];
K = [(k1+k2) (k1*l1-k2*l2);(k1*l1-k2*l2) ((k2*l2^2)+(k1*l1^2))];
Br = [k1 k2;k1*l1 -k2*l2];
Brdot = [c1 c2;c1*l1 -c2*l2];
r = @(t) [(0.01*sin(17.453*t)) (0.01*sin(17.453*t-pi))]';
r1 = @(t) [(0.17453*cos(17.453*t-pi)) (-0.17453*cos(17.453*t))]';
%%%%%%%%%%%
F = @(t) (Br*r(t)) + (Brdot*r1(t));
% Time grid
t0 = 0; tf = 10; dt = 0.01; t = t0:dt:tf;
% Set initial state and integrate equations of motion
s0 = [1 1 0 0]';
f = @(t,s) [s(3);s(4);M\F(t)-M\C*[s(3) s(4)]'-M\K*[s(1) s(2)]'];
[t,s] = ode45(f,t,s0);
% Plot system motion
figure
subplot(211),plot(t,s(:,1),t,s(:,2),'LineWidth',2)
grid minor
legend('y1','y2')
xlabel('Time (s)')
ylabel('Displacements (m)')
title('System Dynamic Response')
subplot(212),plot(t,s(:,3),t,s(:,4),'LineWidth',2)
grid minor
legend('theta1','theta2')
xlabel('Time (s)')
ylabel('Velocity (rad)')
