209
2
2
(
)
2 (
)
(
) sin
0
(
)
cos
0
m l
s
m l
s s
mg l
s
ms
m l
s
mg
cs
ϕ
ϕ
ϕ
ϕ
ϕ
+
+
+
+
+
=
−
+
−
+
=
ɺɺ
ɺ
ɺ
ɺ
ɺɺ
Triển khai trong Matlab
function ydot = conlac_danhoi(t,y)
% ptvp cd cua con lac dan hoi (m, c, L)
% vector y = [q1, q2, v1, v2]'
global m c L
g = 9.81;
q1 = y(1); q2 = y(2);
qd1 = y(3); qd2 = y(4);
qd = [qd1; qd2];
M_q = [m, 0; 0, m*(L+q1)^2];
C_q = [ 0, -m*(L+q1)*qd2; m*(L+q1)*qd2, -m*(L+q1)*qd1];
g_q = [c*q1-m*g*cos(q2); m*g*(L+q1)*sin(q2)];
ydot = zeros(4,1);
qdot = qd;
qd_dot = inv(M_q)*(-C_q*qd - g_q);
ydot = [qdot; qd_dot];
% save conlac_danhoi.m
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
% main program
global m L c
m = 0.5; L = 0.5; c = 50; % gan cac thong so he
t_start = 0; t_end = 3.0;
q10 = 0.05; q20 = 30.0;
y1 = q10; y2 = q20*pi/180;
y3 = 0; y4 = 0;
y0 = [y1 y2 y3 y4]';
[t,y] = ode45('conlac_danhoi',[0: 0.01: t_end],y0);
figure(1)
subplot(2,1,1), plot(t, y(:,1),'k-','linewidth',1), grid on
ylabel('q_1 [m]')
subplot(2,1,2), plot(t, y(:,2),'k-','linewidth',1), grid on
ylabel('q_2 [rad]'), xlabel('t [s]'),
figure(2)
x0 = 0; y0 = 0;
hold on, plot(x0,y0,'o-','linewidth',1)
r = 0.02;
for i = 1:5:length(t)/2
x1(i) =(L+y(i,1))*sin(y(i,3)); y1(i)=-(L+y(i,1))*cos(y(i,3));
x_day = [x0, x1(i)]; y_day = [y0, y1(i)];
ϕ
m
l
s
+
c
Hình 9-23
