Nghiên cứu thiết kế chế tạo các robot thông minh phục vụ cho các ứng dụng quan trọng 5.

Nghiên cứu thiết kế chế tạo các robot thông minh phục vụ cho các ứng dụng quan trọng

BỘ KHOA HỌC VÀ CÔNG NGHỆ CHƯƠNG TRÌNH KC.03

YZ YZ YZ YZ YZ YZY YZ YZ YZY YZ YZ YZY

“NGHIÊN CỨU THIẾT KẾ, CHẾ TẠO CÁC ROBOT THÔNG MINH PHỤC VỤ CHO CÁC ỨNG DỤNG QUAN TRỌNG”

3.2 Thiết kế kết cấu và lập trình điều khiển chuyển động của robot 30

4.2 M« t¶ Robot “NhÖn n−íc” 58 4.3 TÝnh to¸n x©y dùng ch−¬ng tr×nh ®iÒu khiÓn Robot

Báo cáo các kết quả nghiên cứu theo nhiệm vụ 4 đề tài kc.03.08

Nhóm sản phẩm đồ gá cnc

I mở đầu

Đồ gá gia công phụ thuộc vào loại hình phôi gia công và quy trình công nghệ đối với loại sản phẩm đang được gia công Trên máy công cụ CNC mọi thao tác đều được điều khiển bằng số Các sản phẩm được gia công trên máy CNC rất đa dạng Ngoài những sản phẩm có thể gia công bằng những đồ gá có sẵn của máy, còn có nhiều trường hợp phải tự tạo ra đồ gá cho phù hợp Nếu sản phẩm không phải chỉ chế tạo đơn chiếc mà có số lượng lớn thì việc tạo ra các đồ gá nhánh, chính xác và cũng được điều khiển số là loại việc có nhu cầu bức xúc

Một trong những giải pháp kỹ thuật làm đồ gá CNC có thể dùng robot song song (RBSS) Vấn đề RBSS trở nên hấp dẫn nhiều nhà nghiên cứu từ giữa thập kỷ 90 khi nó được ứng dụng dưới dạng thiết bị có tên là Hexapod để tạo ra máy công cụ CNC 5 trục Thực chất, cũng có thể hiểu Hexapod là một loại đồ gá CNC

Hexapod là một môđun RBSS được kết cấu trên nguyên lý cơ cấu Stewart Cơ cấu này gồm có 6 chân, với độ dài thay đổi được, nối với giá và tấm động đều bằng các khớp cầu Bằng cách thay đổi độ dài của các chân có thể điều khiển được định vị và định hướng của tấm động theo ý muốn

Với ý thức tiếp cận một lĩnh vực mới mẻ của Robotics, vấn đề RBSS, Đề tài khi đăng ký muốn nghiên cứu tạo dựng các sản phẩm đồ gá CNC, trong đó nội dung chủ yếu là Hexapod Tuy nhiên nội dung Hexapod không được duyệt để cấp kinh phí vì nội dung này trùng lắp với một đề tài khác Vì thế nội dung về Hexapod không có trong Đề tài này

Cơ cấu RBSS còn có nhiều loại hình khác nhau Đề tài đã đi sâu nghiên cứu vào 3 loại sau và đạt được những kết quả bước đầu:

1 Cơ cấu RBSS phẳng, 3 chân là loại cơ cấu RBSS dạng đơn giản này rất thích hợp cho đồ gá CNC Phụ thuộc vào đối tượng được gia công, có thể dùng loại cơ cấu này với những biến thể rất khác nhau Các chân được thay đổi độ dài có thể bằng xilanh thủy lực, xilanh khí nén hoặc bằng vítme bi Liên quan đến vấn đề này Đề tài đã hoàn thành một công trình nghiên cứu tạo ra thiết bị gá lắp nhanh để gia công vỏ hộp động cơ xe máy trên Trung tâm gia công CNC nhập từ Anh Quốc của Công ty Đồng Tháp Phương án thiết kế đã chọn lựa là dùng hệ thống xilanh khí nén, bố trí tác động lần lượt vào 3 chân của cơ cấu RBSS Hợp đồng và bản thuyết minh công trình này giới thiệu trong phần phụ lục

2 Robot song song RBSS - 322 có thể hoạt động như một thiết bị gia công CNC Các chân của RBSS được xếp thành 3 nhánh đôi, mỗi chân có 2 phần: phần chân trên và phần chân dưới Dáng dấp bên ngoài của RBSS - 322 giống như kiểu Robot Flex - Picker của Hãng ABB Khi thiết kế đã tham khảo các thông tin chào hàng của Hãng này, nhưng kết cấu các bộ truyền bên trong đã đơn giản hóa đi nhiều cho phù hợp khả năng tự tạo trong nước với khoản kinh phí dành cho công việc này rất hạn chế Định hướng ứng dụng RBSS trong công việc điêu khắc trên các vật liệu dễ gia công nhưng có kích thước lớn, cồng kềnh, không đưa lên các máy gia công thông thường được Phần đạt được kết quả hơn và thu hoạch được nhiều hơn là xây dựng chương trình phần mềm điều khiển RBSS - 322

3 Robot “Nhện nước” là một đề xuất mới, được phát triển trên nguyên lý RBSS và gồm 2 phần Phần thứ nhất là một RBSS hoạt động theo nguyên lý cơ cấu Stewart “Tấm động” được treo trên “tấm giá” cố định bằng 6 dây cáp có độ dài thay đổi được nhờ cơ cấu kiểu tời quay và đảm bảo độ linh hoạt nối ghép giữa dây cáp với các tấm nhờ có khớp cầu Phần thứ 2 gắn với tấm động là một hệ thống với 4 cơ cấu nhánh hình bình hành Hệ thống cơ cấu này có

thể “xòe rộng” ra hoặc “co cụm” lại Hệ thống này làm nhiệm vụ mang các ống nước mềm có vòi phun dùng để tẩy rửa các khoang hầm ngầm, mà ở đó con người rất khó khăn hoặc không thể thao tác được, ví dụ vì môi trường quá độc hại

Hệ thống này có thể “co cụm” lại và hạ thấp dần qua miệng hầm có kích thước hạn hẹp và khi đã lọt qua miệng hầm sẽ “xòe rộng” ra, đồng thời nhờ khả năng thay đổi “định vị và định hướng” của tấm động, gắn liền với hệ thống 4 cơ cấu này, mà quỹ đạo phun nước đang được điều khiển theo ý muốn

Mọi thao tác như làm thay đổi độ dài của 6 dây cáp treo tấm động và làm “co cụm” hoặc “xòe rộng” 4 cơ cấu bình hành mang các vòi phun nước, đều được điều khiển theo chương trình và các thiết bị điều khiển đều được lắp đặt phía trên miệng hầm, nên rất thuận tiện

Nội dung nghiên cứu của công trình này được xuất phát từ những lần trao đổi, bàn bạc với Vietxopetro về nhiệm vụ rất bức xúc thay thế các lao động dưới các khoang hầm tầu chở dầu thô, vừa rất độc hại, vừa rất khó rửa sạch nên rất chóng rỉ làm thủng vỏ tàu

Các kết quả nghiên cứu tính toán và mô phỏng hoạt hình các thao tác của “Robot - Nhện nước” phục vụ cho nhiệm vụ này đã được trình bày với Xí nghiệp Cơ điện dầu khi thuộc liên doanh Dầu khí Vietxopetro và đã được đưa vào kế hoạch triển khai Nhưng vì có sự thay đổi của cơ quan ứng dụng nên đến nay vẫn chưa thực hiện được

II C¬ cÊu RBSS 3 ch©n

2.1 Robot song song 3 chân RPS

2.1.1 Kết cấu hình học

Rô-bốt song song 3 RPS (Tên gọi Rô-bốt song song 3 RPS là do loại rô-bốt này có cấu tạo gồm 3 chân, mỗi chân gồm có 1 khớp quay R, 1 khớp tịnh tiến P và 1 khớp cầu S), thường được thiết kế để mang phôi gia công hay mang công cụ để gia công Ba chân với chiều dài có thể thay đổi được điều khiển bởi các động cơ sẽ dẫn động cho bệ di động mang phôi hay công cụ chuyển động theo quĩ đạo xác định trước Hai đầu của các chân này được liên kết với đế cố định và bệ di động bằng các khớp cầu Ưu điểm của loại Rô-bốt này là khối lượng nhỏ, cấu trúc gọn nhẹ, độ cứng vững cao, có 3 bậc tự do và độ chính xác cao

Tất cả các thành phần cơ khí được lựa chọn và thiết kế càng nhỏ gọn càng tốt và không có khe hở theo chiều dọc trục của các chân, các chân được điều khiển của Rô-bốt được dẫn động bằng các cơ cấu chấp hành tuyến tính Hình 2.1 mô tả sơ đồ của rô-bốt này

- Chi tiết 1 : Bàn di động có 3 bậc tự do trong không gian, trong trường hợp cụ thể ở đây là phần bề mặt dùng để gá dụng cụ cắt kim loại (đầu dao phay, ) hoặc lắp đồ gá phôi (thước chia độ, kẹp phôi gia công ) có dạng tam giác (thường là tam giác đều) Trên bàn di động sẽ lắp đặt các loại đồ gá để kẹp chi tiết hoặc lắp đặt cụm động cơ - đài dao gia công Bàn được thiết kế có các lỗ, chốt định vị để lắp đồ gá Đồ gá được lắp chặt trên bàn di động bằng các bulông

Hình 4.2.1: Cơ cấu chấp hành song song 3 RPS

- Chi tiết 2: Là một phần của chi tiết thanh tr−ợt lồng, ống tr−ợt trong Tất cả các ống tr−ợt trong có dạng thanh trụ đặc Khớp tr−ợt đ−ợc truyền động bằng cơ cấu chấp hành sử dụng động cơ servo, bộ truyền động và cụm cơ cấu trục vít - đai ốc bi Các chân của Rô-bốt đ−ợc nối với bệ di động và đế cố định bằng các khớp cầu 6

- Chi tiết 3 : Là một phần của chi tiết thanh tr−ợt lồng, ống tr−ợt ngoài Tất cả các ống tr−ợt ngoài có dạng hình trụ rỗng

- Chi tiết 4 : Khớp quay, nối chân với đế cố định

- Chi tiết 5 : Mặt đế cố định, có dạng tấm phẳng tròn Bệ cố định đ−ợc lắp đặt trên bàn gá chi tiết của máy phay hoặc có thể đ−ợc lắp đặt cố định trên một vật khác Trên đế cố định có gia công các lỗ phục vụ, việc cố định đế trên bàn gá hoặc các vật khác bằng các bulông Trên đế cố định còn đ−ợc gia công các rãnh định vị phục vụ công tác căn chỉnh, lắp đặt rô-bốt

- Chi tiết 6 : Khớp cầu, nối chân với bàn máy di động

2.1.2 Bậc tự do

Sử dụng công thức:

Hình 4.2.2: Sơ đồ động học của robot song song 3 RPS

Do yêu cầu của kết cấu Robot nên AiBi ⊥ Zi (các trục quay) O và P là trọng tâm của hai tam giác A1A2A3 và B1B2B3

Ta đặt các hệ tọa độ: {Ox0y0z0} : Hệ cố định

Trong đó : Các ma trận A

e ee ee ee ee ee ee ee ee e

bi = +ai ARi 00

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥

Hình 4.2.3

Các ma trận cosin chỉ hướng ARi được xác định bởi các phép quay liên tiếp:

Ma trận AR1 được xác định bởi các phép quay sau :

• Quay một góc (π / 2ưβ1) quanh trục z

• Quay quanh trục x một góc (π/ 2)

• Quay quanh trục z một góc (π / 2+α1) Ma trận AR2 được xác định bởi các phép quay sau :

• Quay một góc η2 quanh trục z

• Quay một góc (π/ 2ưβ2) quanh trục z

• Quay quanh trục x một góc (π/ 2)

• Quay quanh trục z một góc (π/ 2+α2) Ma trận A

R được xác định bởi các phép quay sau :

• Quay một góc η3 quanh trục z

• Quay một góc (π/ 2ưβ3) quanh trục z

• Quay quanh trôc x mét gãc (π/ 2)

• Quay quanh trôc z mét gãc (π/ 2+α3)

NÕu ta coi Aαi lµ ma trËn cosin chØ h−íng cña 2 phÐp quay liªn tiÕp quanh trôc x mét gãc (π/ 2) vµ quanh trôc z mét gãc (π/ 2+αi) th× :

A

1 = z( ).γ1 α1

A1 =

R

Vµ Bi thuéc mÆt ph¼ng X0Y0 nªn biz = 0 (i=1,2,3)

+Víi i =1:

⎧ =⎪⎪

au dpu

bu dpu

bu dpu

(a) (b) (c)

pu bv bau dpu bv bau dpu bv bu d

(a) (b) (c)

pu bv bau dpu bv bau dpu bv bu d

−⎧

Víi 3

bu dp

B B (b1−b2) (Tb1−b2)=b32 2

1 3 =uuuur

B B (b1−b3) (Tb1−b3)=b22

22 3 =uuuur

B B (b2 −b3) (Tb2 −b3)=b12

víi : bi = +ai ARi 00

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥

(i=1,2,3)

⇒b =

au du du d

au dau d

au dau d

Hệ phương trình (2.14) chứa 9 ẩn số α α α1, , , , , , , ,2 3 d d d p p p1 2 3 1 2 3 Các thành phần u u uix, ,iyiz đã xác định được, các thành phần u u ux, ,yz xác định theo (2.9)

Khi giải quyết bài toán động học thuận hay ngược, ta biết trước được 3 ẩn Công việc còn lại chỉ phải giải hệ 6 phương trình 6 ẩn số

a Bài toán động học thuận

Bài toán động học thuận là bài toán biết độ dài các chân di (i=1,2,3), ta phải tìm vị trí của bàn máy động P và ma trận ARB

Theo phần trên ta thay các giá trị di (i=1,2,3) vào hệ (4.14), ta sẽ được hệ 6 phương trình với 6 ẩn là : α α α1, , , , ,2 3 p p p1 2 3

Chú ý là 3 phương trình sau của hệ (2.14) chỉ chứa di và αi nên việc giải 6 phương trình được đơn giản lại còn giải hệ 3 phương trình với 3 ẩn là

- PB1 = h; OA1 = g; η η2 = 3 =2 / 3π- Do kÕt cÊu cña c¬ cÊu ta cã zi ⊥ A Bii

; Bb2 =

; a2 =

2320

λ = = -1 ; λ1=b2xλ−b3x = h; Thay vµo hÖ (2.14) ta ®−îc :

3 cos cos cos (4.20)

a) Bài toán động học thuận

Bài toán động học thuận là bài toán biết độ dài các chân di (i=1,2,3), ta phải tìm vị trí của bàn máy động P và ma trận ARB

Theo phần trên ta thay các giá trị di (i=1,2,3) vào hệ (2.20), ta sẽ được hệ 6 phương trình với 6 ẩn là : α α α1, , , , ,2 3 p p p1 2 3

Chú ý là 3 phương trình sau của hệ (2.20) chỉ chứa di và αi nên việc giải 6 phương trình được đơn giản lại còn giải hệ 3 phương trình với 3 ẩn là

2.2 Robot song song 3 RSS

2.2.1 Kết cấu hình học

Hình 4.2.5: Kết cấu hình học của Robot song song 3 RSS

Deltarca cấu tạo gồm ba chân, bàn di động 12 và bàn cố định 1 Mỗi chân gồm có khâu trên, khâu dưới và năm khớp, trong đó có bốn khớp cầu 4, 6, 8, 10 (S) và một khớp quay 2 (R) Khâu AC được gọi là khâu cánh tay, khâu BC được gọi là khâu cẳng tay Khâu AC nối với bàn cố định bởi khớp quay bản lề 2, khâu BC nối với bàn di động bởi hai khớp cầu 8, 10 và nối với khâu AC bởi hai khớp cầu 4, 6 Khâu BC thực chất là cơ cấu hình bình hành gồm các khâu 5, 7, 9, 11 với các khớp là khớp cầu chứ không phải là khớp quay Như vậy Deltarca có tất cả 11 khâu và 15 khớp, được gọi là rô-bốt song song 3 RSS (Hình 4.2.5)

Như vậy bàn di động sẽ chuyển động với ba bậc tự do trong không gian

2.2.3 Hệ trục tọa độ và kí hiệu

Hệ trục Oxyz gắn với tâm bàn cố định, hệ trục Puvw gắn với tâm bàn di động Các trục x, y nằm trong bàn cố định, các trục u, v nằm trong bàn di động Tại mọi thời điểm x và y luôn song song cùng chiều với u và v

Dùng hệ trục A x y zi i i i gắn với tâm của khớp quay Ai, sao cho xi nằm theo hướng kéo dài của OAi và trục yi dọc theo trục các khớp quay, còn trục zi

song song với z Góc αi đo từ x đến xi là một hằng số của quá trình thiết kế rô-bốt (Hình 2.6) Góc ϕi là góc tạo bởi trục xi và ACii

Hình 4.2.6: Đặt hệ trục tọa độ lên rô-bốt

2.2.4 Phương trình liên kết

Để thuận tiện cho quá trình thành lập hệ phương trình liên kết, ta quy định một số điểm sau:

 Chọn hệ trục cố định là hệ trục Oxyz

 Ta dùng chữ in nghiêng để kí hiệu véc-tơ đại số, ví dụ ai là véc-tơ đại số biểu diễn tọa độ của điểm Ai so với hệ tọa độ cố định Oxyz Còn biP là toạ độ của điểm Bi trong hệ tọa độ động Puvw

 Khi nói tới tọa độ của một điểm nào đó mà ta không nói rõ là xét trong hệ toạ độ cụ thể nào thì hiểu rằng ta đang xet tọa độ của điểm đó trong hệ tọa độ cố định Oxyz

Sau đây ta xác định tọa độ của từng điểm

 Tọa độ điểm Ai (Hình 4.2.7): tuỳ thuộc vào kích thước bàn cố định

là véc-tơ tọa độ của điểm P trong hệ trục tọa độ Oxyz

Š Rp là ma trận cosin chỉ hướng của hệ Puvw so với Oxyz Vì bàn di động chuyển động song phẳng so với bàn cố định nên:

1 0 00 1 00 0 1

*cos( )*sin( )

px rbbipy rb

lb =bicibici= xci xbiư + yci ybiư + zci zbiư (2.21)

Với i=1,2,3 thì hệ (2.21) là hệ gồm ba phương trình và sáu ẩn:

2.2.5 Bài toán động học thuận

Khi cho biết quy luật chuyển động của ba góc: ϕ ϕ ϕ1, ,2 3 ta sẽ xác định được px, py và pz từ hệ (2.21) và do vậy xác định được quy luật chuyển động của bàn di động

2.2.6 Bài toán động học ngược

Khi cho biết quy luật chuyển động của bàn di động, tức là cho biết px, py, pz ta sẽ xác định được ϕ ϕ ϕ1, ,2 3 từ hệ (2.21)

2.3 Robot song song phẳng 3 chân

RBSS phẳng 3 chân có đặc điểm là mỗi chân gồm có 2 khâu và 3 khớp, mỗi khớp cần 1 bậc tự do, tức là khớp p5: khớp quay (R), khớp trượt (ký hiệu là T - Translatrion hoặc P – Prismatic) Vì vậy có 7 phương án:

RBSS phẳng 3 chân thường được dùng để tạo ra các đồ gá CNC để mang phôi gia công hay mang dụng cụ gia công

A1

Hình 4.2.10 RBSS phẳng 3 chân PRP

2.2 Đồ gá gia công vỏ hộp động cơ xe máy

Trong phần phụ lục trình bày bản thuyết minh “Công trình thiết kế chế tạo cơ cấu kẹp phôi nhanh cho máy gia công CNC” và giới thiệu một số bản vẽ thiết kế đồ gá về hệ thống các xi lanh khí nén đ−ợc dùng cho đồ gá này

P R P

Tấm động

Tấm cố định

III Robot song song RBSS – 322

3.1 Giíi thiÖu chung

Vấn đề Robot song song (RBSS) trở nên hấp dẫn nhiều nhà nghiên cứu từ giữa thập kỷ 90 khi nó được ứng dụng dưới dạng thiết bị có tên là Hexapod để tạo ra máy công cụ CNC 5 trục Hexapod là một môđun RBSS được kết cấu trên nguyên lý cơ cấu Stewart Cơ cấu này gồm có 6 chân, với độ dài thay đổi được, nối với giá và tấm động đều bằng các khớp cầu Bằng cách thay đổi độ dài của các chân có thể điều khiển sự định vị và định hướng của tấm động theo ý muốn

Ngoài các ứng dụng trong ngành chế tạo máy công cụ RBSS còn được áp dụng hiệu quả trong dụng cụ y học, trong hệ thống mô phỏng, trong thiết bị thiên văn, trắc địa và trong kỹ thuật phòng không v.v Cơ cấu RBSS cũng có nhiều biến thể khác nhau và xuất hiện nhiều tên gọi khác nhau, ví dụ như Hexapod, Hexaglide [4], Hexagons [5], Hexa [2], Delta [3] v.v

Dưới đây trình bày kết quả nghiên cứu, thiết kế, chế tạo và điều khiển một kiểu robot song song Trong thiết kế đã tham khảo phần tạo dáng từ mẫu chào hàng robot Flex Picker của hãng ABB

3.2 Thiết kế kết cấu và lập trình điều khiển chuyển động của Robot

Trên hình 3.1 là bản vẽ 3D của Robot đã thiết kế và trên hình 3.2 là ảnh chụp sau khi đã lắp đặt vận hành

Các chân của Robot được xếp thành 3 nhánh đôi và được truyền động từ 3 động cơ riêng biệt Mỗi chân gồm có 2 phần: phần trên (đùi) và phần dưới (ống chân) Chân nhánh nối với giá đỡ bằng khớp quay (khớp háng) và nối với tấm động bằng “khớp cổ chân” Chỗ “cổ chân” và đầu gối” đều bố trí một khớp quay với trục quay có 2 đầu khớp cầu

Hình 4.3.1 Bản vẽ 3D thiết kế RBSS-322

Hình 4.3.2 Ảnh chụp RBSS - 322

Gọi điểm đầu mút của dụng cụ gắn liền với tấm động là “điểm tác động cuối” (end - effector) của Robot Khi lập trình điều khiển chuyển động của Robot, ta quan tâm đến sự dịch chuyển của ‘điểm tác động cuối” này Ngôn ngữ lập trình là Visual C++ tạo điều kiện dễ dàng kết nối với các thư viện đồ hoạ mạnh và phổ biến như OpenGL

Sơ đồ giải thuật chương trình điều khiển giới thiệu trên hình 4.3.3

Hình 4.3.3 Sơ đồ giải thuật chương trình điều khiển

Sau khi đã nhập dữ liệu, chúng kiểm tra có nằm trong vùng làm việc không, rồi được gửi tới các chương trình con để giải bài toán động học ngược Các kết quả đó được xử lý và truyền tới cổng nối tiếp R232C để điều khiển hoạt động của Robot qua bộ thiết bị điều khiển Sau khi di chuyển tới điểm đã định, tín hiệu phản hồi về sẽ là thông điệp gửi tiếp thông tin về điểm di chuyển tiếp theo Quá trình này được thực hiện lặp lại một cách nhịp nhàng, không phải chờ đợi, cho tới khi đã tới đích cuối cùng của hành trình

Trên hình 4.3.4 là minh hoạ kết quả tính toán về hành trình di chuyển của “điểm tác động cuối”

Hình 4.3 4 Trich kết quả tính toán về hành trình

Ngoài ra kết quả tính toán cho chương trình điều khiển cũng đồng thời được gửi tới chương trình mô phỏng hoạt hình của Robot

Hình 4.3.5: Sơ đồ chức năng bảng mạch chính

3.3 Sơ đồ hệ thống điều khiển

Trên hình 4.3.5 là sơ đồ chức năng bảng mạch chính (Main board) Chương trình điều khiển được nạp trong bộ nhớ trong bộ vi xử lý của 89C51

DB9 2 3 5

MAX232

TxD

PO RxD

89C51 P1 P2

Joy-AD 0809

ANALOG INPUT

CONTROL

BUS

thực hiện các chức năng kiểm soát cổng P1 để nhận lệnh điều khiển từ Joystick trong chế độ điều khiển bằng tay (manual), kiểm tra cổng nối tiếp để nhận lệnh điều khiển từ máy tính PC Chuỗi lệnh từ máy tính điều khiển truyền tới gồm 11byte Khi nhận xong một lệnh từ cổng nối tiếp thì Main board sẽ truyền trở về PC một byte trả lời Khi thực hiện xong lệnh đã nhận thì nó lại tiếp tục truyền trở về PC một byte báo cáo thứ 2

Trên hình 4.3.6 là sơ đồ chức năng bảng mạch bộ điều khiển động cơ (Driver board)

Hình 4.3.6 Sơ đồ chức năng bảng mạch bộ điều khiển động cơ

H×nh 4.3.7.Chu trình nhận và thực hiện lệnh điều khiển động cơ Driver board

89c2051CONTROL BUS

Mạch công suất

Kiểm soát dòng

Mạch phối

Mạch phối

BUS điều khiển

Nhận lệnh Thực hiện lệnh Kiểm soát dòng ĐC

Đọc EncoderSENSER

vị trí

ĐK Động cơ

Chương trình điều khiển được nạp vào bộ nhớ trong của bộ vi xử lý 89C 2051 thực hiện chức năng kiểm tra bus điều khiển (control bus) Giá trị xung đếm vòng quay từ encoder đưa về qua mạch phối hợp được so dánh với giá trị vòng quay từ lệnh điều khiển là tín hiệu để động cơ dừng hay hoạt động tiếp tục Dòng điện tiêu thụ của động cơ cũng được kiểm soát trong quá trình hoạt động Nếu giá trị này vượt quá ngưỡng đã đặt thì chương trình sẽ cho dừng động cơ và báo lỗi về bảng mạch chính Lỗi sẽ được hiển thị bằng đèn LED và có thể thông báo về máy tính điều khiển Các sensor chỉ báo vị trí ban đầu của Robot cũng được kiểm soát để khi có lệnh chúng sẽ trở về vị trí xuất phát Chu trình nhận và thực hiện lệnh của Driver board được mô tả trên hình 3.7

Hệ thống thiết bị đã được vận hành thử nghiệm tốt và đang chuẩn bị triển khai ứng dụng trong gia công điêu khắc

3.4 Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn RBSS - 322

Chương trình gồm các file vitridau.m, nhap.m, tinhdulieu.m, chayhopmay.m

%vitridau.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear;

s=serial('COM4'); set(s, 'Terminator', 'CR'); s.Timeout=7;

%s

so_xung1=2000; %21100 1 vong highbyte1=floor(so_xung1/256); lowbyte1=mod(so_xung1,256); so_xung2=1000; %21100 1 vong highbyte2=floor(so_xung2/256);

lowbyte2=mod(so_xung2,256); so_xung3=1000; %21100 1 vong highbyte3=floor(so_xung3/256); lowbyte3=mod(so_xung3,256); fopen(s)

fwrite(s,254) %ma Header1 fwrite(s,253) %ma Header2

fwrite(s,104) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115 dung) %(h or 104 vi tri dau)

fwrite(s,lowbyte1) %so xung lowbyte dc1 fwrite(s,highbyte1) %so xung highbyte dc1

fwrite(s,104) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau)

fwrite(s,lowbyte2) %so xung lowbyte dc2 fwrite(s,highbyte2) %so xung highbyte dc2

fwrite(s,104) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau)

fwrite(s,lowbyte3) %so xung lowbyte dc3 fwrite(s,highbyte3) %so xung highbyte dc3

fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung) fwrite(s,1)

fwrite(s,0) data=fscanf(s) if data=='b' fwrite(s,254) fwrite(s,253) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s')

fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0)

end fclose(s); %nhap.m

% eval(command);

% fprintf('\n Aa=%10.3f',Aa); % fprintf('\n Bb=%10.3f',Bb); % fprintf('\n Cc=%10.3f',Cc); % fprintf('\n OA=%10.3f',OA); % fprintf('\n OB=%10.3f',OB); % fprintf('\n OC=%10.3f',OC); % fprintf('\n TAa=%10.3f',TAa); % fprintf('\n TBb=%10.3f',TBb);

% fprintf('\n khoang cach TA den tam=%10.3f',OtTA); % fprintf('\n zTA=%10.3f',zTA);

% fprintf('\n khoang cach TB den tam=%10.3f',OtTB); % fprintf('\n zTB=%10.3f',zTB);

% fprintf('\n khoang cach TC den tam=%10.3f',OtTC); % fprintf('\n zTC=%10.3f',zTC);

% elseif (ch == 'n') % Aa=input('Aa='); % Bb=input('Bb='); % Cc=input('Cc='); % OA=input('OA='); % OB=input('OB='); % OC=input('OC='); % TAa=input('TAa='); % TBb=input('TBb='); % TCc=input('TCc=');

% OtTA=input('khoang cach TA den tam='); % zTA=input('zTA=');

% OtTB=input('khoang cach TB den tam='); % zTB=input('zTB=');

% OtTC=input('khoang cach TC den tam='); % zTC=input('zTC=');

% filesave=input('\n Nhap ten file save: ','s');

% [command,mss]=sprintf('save %s Aa Bb Cc OA OB OC TAa TBb % TCc OtTA zTA OtTB zTB OtTC zTC',filesave);

% eval(command);

% end

% -

Aa=800 Bb=800 Cc=800 OA=40 OB=40 OC=40 TAa=365 TBb=365 TCc=365 OtTA=200 zTA=615 OtTB=200 zTB=615 OtTC=200 zTC=615

xO=input('xO='); yO=input('yO='); zO=input('zO=');

xA=xO-OA yA=yO zA=zO

xB=xO+OB*sin(30*pi/180) yB=yO-OB*cos(30*pi/180) zB=zO

xC=xO+OC*sin(30*pi/180) yC=yO+OC*cos(30*pi/180)