220
b(2) = b(1)+deltax(2);
a(1) = a(2);
b(1) = b(2);
f = ptlk_bkbl(q1(i), a(1), b(1));
k = k+1;
end
q2(i)=a(1); q3(i)=b(1);
% van toc
[J1, J23] = Jacob_bkbl(q1(i), q2(i), q3(i));
dq23 = -inv(J23)*J1*dq1(i);
dq2(i) = dq23(1); dq3(i) = dq23(2);
% gia toc
[dJ1, dJ23] =
Jacdot_bkbl(q1(i),q2(i),q3(i),dq1(i),dq2(i),dq3(i));
ddq23 = -inv(J23)*(dJ23*dq23 + J1*ddq1(i) + dJ1*dq1(i));
ddq2(i) = ddq23(1); ddq3(i) = ddq23(2);
end
figure(1)
plot(t,q2,'k-', t, dq2,'k--', 'linewidth',1), grid on
xlabel('t [s]'), legend('q_2', 'dq_2')
axis([0 max(t) -0.5 1.1])
figure(2)
plot(t,q3,'k-', t, dq3,'k--', 'linewidth',1), grid on
xlabel('t [s]'), legend('q_3', 'dq_3')
axis([0 max(t) -0.8 2.2])
x0 = 0; y0 = 0; r = 0.005;
figure(3), hold on
plot(x0,y0,'o-','linewidth',1), plot(dx,dy,'o-','linewidth',1)
for i = 1:5:length(t)/2
x1(i) = l1*cos(q1(i)); x2(i) = x1(i) + l2*cos(q2(i));
y1(i) = l1*sin(q1(i)); y2(i) = y1(i) + l2*sin(q2(i));
x4(i) = dx; x3(i) = dx + l3*cos(q3(i));
y4(i) = dy; y3(i) = dy + l3*sin(q3(i));
x_day = [x0, x1(i), x2(i), x3(i), x4(i), x0];
y_day = [y0, y1(i), y2(i), y3(i), y4(i), y0];
plot(x_day, y_day, 'k-','linewidth',1), grid on, axis equal
tam1 = [x1(i), y1(i)]; vehinhtron(tam1,r);
tam2 = [x2(i), y2(i)]; vehinhtron(tam2,r);
end
axis([-0.2 0.8 -0.2 0.6])
ðồ thị của các góc
2
3
,
q q cùng các ñạo hàm
2
3
,
q q
ɺ ɺ ñược thể hiện như trên hình 9-
33. Cấu hình của cơ cấu tại một số vị trí ñược chỉ ra trên hình 9-34.
