II. MÔ TẢ CÁC VẬT THỂ VÀ NHIỆM VỤ.
4. Thể hiện chương trỡnh thao tỏc của robot.
Chương trỡnh thao tỏc của robot thực hiện cỏc nhiệm vụ được giao. Nhiệm vụ đú được mụ tả bằng một dóy liờn tiếp cỏc vị trớ thao tỏc. Trong vớ dụ là thao tỏc từ p1 đến p8 (hỡnh 5.8). Cỏc thao tỏc này được thể hiện bằn một chương trỡnh, trong đú từ cỏc phương trỡnh từ (5.5) đến (5.12) tương ứng với cỏc thao tỏc từ p1 đến p8, cần xỏc định ra T, ứng với mỗi vị trớ trạng thỏi của điểm cuối của tay robot trong hệ tọa độ cơ bản (hệ tọa độ cố định). Vớ dụ, ứng với thao tỏc p1, từ (5.5) ta viết:
ZT6E= PPA (5.60) Tacú: T6E = Z-1PPA (5.61) Và cứ thế tiếp đến p2 cho đến p8
Nhưng trước khi thể hiện chương trỡnh cỏc thao tỏc đú, cần sử dụng thờm 2 biến:
COORD – Mụ tả hệ tọa độ ứng với vị trớ đang xột TOOL – Mụ tả dụng cụ đang được sử dụng.
Như vậy tất cả cỏc phương trỡnh (tương tự 5.61) ứng với cỏc vị trớ thao tỏc đều cú thể viết dưới dạng sau:
T6TOOL = COORD POS
Vớ dụ, ứng với thao tỏc p1 ta cú : COORD : = - Z + p
TOOL : = E
Và thao tỏc là: MOVE PA:
Trong cỏch viết trờn, dựng dấu (+) để chỉ phộp nhõn ma trận và dấu (-) để chỉ phộp nhõn ma trận nghịch đảo.
Theo đú, chương trỡnh thể hiện cỏc thao tỏc của robot từ p1 đến p8 cú thể viết như sau:
TOOL : = E Gắn dụng cụ FOR I : = 1,2 DO
BEGIN
READ (CAMERA, PC): Thụng tin từ camera P : = CAM + PC Vị trớ của chốt COORD : = - Z + P Tọa độ của chốt MOVE PA: Hướng tới chốt MOVE PG : Sắp kẹp chốt GRASP : Kẹp
TOOL : = E – PG Dụng cụ bay giờ bao gồn bàn kẹp đó kộp chốt.
MOVE PD : Vị trớ xuất phỏt (bắt đầu mang chốt đi)
HT : = HR [I] Vị trớ lắp vao lỗ COORD : = - Z + H + HT Tọa độ cua lỗ
MOVE PHA : Hướng tới lỗ MOVE PCH : Chạm vào lỗ
MOVE PAL : Đặt đồng tõm với lỗ (lỳc chuẩn bị lắp )
MOVE PN : Lắp vào lỗ RELEASE : Dời đi
COORD : = - Z + H + HT + PN Tọa độ của lỗ đó lắp TOOL : + E
MOVE PA END