212
M_q = [m1*l1^2+m2*l1^2, m2*l1*l2*cos(q1-q2);
m2*l1*l2*cos(q1-q2), m2*l2^2 ];
C_q = [ 0, m2*l1*l2*sin(q1-q2)*qd2;
-m2*l1*l2*sin(q1-q2)*qd1, 0];
g_q = [g*l1*sin(q1)*(m1+m2); g*m2*l2*sin(q2)];
ydot = zeros(4,1);
qdot = qd;
qd_dot = inv(M_q)*(-C_q*qd - g_q);
ydot = [qdot; qd_dot];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% main program
global m1 m2 l1 l2
m1 = 0.25; l1 = 0.5;
m2 = 0.25; l2 = 0.5;
t_start = 0; t_end = 6.0;
q10 = 10; q20 = 12;
y1 = q10*pi/180; y2 = q20*pi/180; y3 = 0; y4 = 0;
y0 = [y1 y2 y3 y4]';
[t,y] = ode45('conlactoan_kep',[0: 0.01: t_end],y0);
plot(t, y(:,1),'k-', t, y(:,2),'k--', 'linewidth',1), grid on
legend('q_1','q_2'), xlabel('t [s]')
axis([0 t_end -0.3 0.3]);
Chạy chương trình với các ñiều kiện ñầu khác nhau cho ta các kết quả như trên
hình 9-26 và 9-27.
0
2
4
6
-0.2
-0.1
0
0.1
0.2
0.3
t [s]
q
1
q
2
-0.4
-0.2
0
0.2
-1
-0.8
-0.6
-0.4
-0.2
0
x [m]
y
[
m
]
Hình 9-26. Kết quả mô phỏng với ñiều kiện ñầu q10 = 10; q20 = 12;
