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 - Nhóm sản phẩm ROBOT SCA

i¶ m tè c BR C L 79 H×nh 2.2.30. Hép gi¶m tèc BRCL cho m«®un quay robot T û lÖ : § ¹i h ä c B ¸c h K h o a h µ n é i D uy Öt K iÓ m tr a H ä vµ tª n T hi Õt k Õ T r. nh iÖ m K ý N gµ y b¶ n v Ï l¾ p h é p g i¶ m t è c i= 29 T hÐ p 45 02 æ bi 1 2x 24 x6 1718 V ßn g c¸ ch 01 P hÝ p 0201 T hÐ p 45 T hÐ p 45 B ¸n h r¨ ng 2 9 192 0 V µn h r¨ ng 06 C hè t ® Þn h vÞ 21 01 T hÐ p 45 N ¾p d uí i 22 X ev an it1 1 T hÐ p 45 T hÐ p 45 T hÐ p 45 T hÐ p 45 010101 0101 01 01 æ bi 1 7x 35 x1 0 V ßn g ch ¾n d Çu T he n 2 T he n 1 æ bi 2 0x 42 x1 2 P hÇ n l¾ p se n s¬ T rô c ®Ç u ra 91012 1115 14 13 T hÐ p 45 01 C hè t 16 5 C hè t ® Çu r a 06 T hÐ p 45 T hÐ p 45 01 T h© n ®Ç u ra 78 V ò m ì 01 3 C on l¨ n 30 T hÐ p 45 T hÐ p 45 06 C on l¨ n ch èt ® Çu r a 4 2 01 T hÐ p 45 M iÕ ng ® Öm T hÐ p 45 01 N ¾p tr ªn 1 S T T V Ët li Öu S è l\' ee ng T ªn c hi ti Õt 23 V ßn g lÖ ch t© m T hÐ p 45 02 80 3) Hép gi¶m tèc BRCL dïng cho c«ng t¸c häc tËp ý thøc r»ng lo¹i h×nh truyÒn ®éng BRCL lµ rÊt cã triÓn väng øng dông réng r·i trong nhiÒu lÜnh vùc c«ng nghiÖp vµ ®ang trong giai ®o¹n nghiªn cøu s¸ng t¹o phï hîp víi nh÷ng ph−¬ng ph¸p c«ng nghÖ míi, §Ò tai ®· lµm thªm mét viÖc lµ chÕ t¹o cho tr−êng häc nh÷ng hép gi¶m tèc BRCL cã thªn hép b»ng vËt liÖu trong suèt. Ng−êi häc cã thÓ hiÓu nã vËn hµnh ra sao nh×n thÊy râ trôc ®éng c¬ nèi liÒn víi trôc ®Çu vµo quay nhanh ®Õn thÕ mµ trôc ®Çu ra ®−îc quay rÊt chËm H×nh 2.2.31. Hép gi¶m tèc BRCL víi th©n hép chÕ t¹o b»ng vËt liÖu trong suèt ®Ó phôc vô ®µo t¹o 81 H×nh 2.2.32. S¶n xuÊt hép gi¶m tèc BRCL phôc vô ®µo t¹o 82 IV. KÕt luËn TriÓn khai thùc hiÖn nhiÖm vô 2 lµ nghiªn cøu c¶i tiÕn n©ng cÊp robot SCATM ®Ó ®¸p øng yªu cÇu th«ng minh, §Ò tµi ®· b¾t gÆp néi dung t¹o ra nh÷ng khíp quay robot, trong ®ã ph¶i cã truyÒn ®éng b¸nh r¨ng kh«ng cã khe hë c¹nh r¨ng ®Ó khi ®iÒu khiÓn kh«ng bÞ trÔ. Do vËy ®· xuÊt hiÖn nhu cÇu nghiªn cøu vÒ m«®un quay dïng BRCL. Víi néi dung nghiªn cøu nµy §Ò tµi ®· nhËn ®−îc c¸c kÕt qu¶ khoa häc chñ yÕu sau: 1) §Ò xuÊt ph−¬ng ph¸p x©y dùng d¹ng r¨ng b¸nh r¨ng l−în sãng theo yªu cÇu ®Æt tr−íc ®èi víi bé truyÒn BRCL (b¸nh r¨ng con l¨n) vµ thiÕt lËp ch−¬ng tr×nh m¸y tÝnh thiÕt kÕ hép gi¶m tèc BRCL. Ch−¬ng tr×nh nµy ®· ®−îc cÊp giÊy chøng nhËn b¶n quyÒn. 2) §Ò xuÊt ph−¬ng ph¸p chÕ t¹o b¸nh r¨ng con l¨n trªn m¸y gia c«ng CNC vµ kh«ng dïng dao (dao phay l¨n hoÆc dao xäc) ®Ó tr¸nh sai lÖch so víi d¹ng r¨ng lý thuyÕt. Trong tr−êng hîp s¶n xuÊt lo¹t lín ®Ò xuÊt dïng c¸c ph−¬ng ph¸p gia c«ng ¸p lùc tr−íc nguyªn c«ng mµi ®Ó chÕ t¹o BRCL, cã ®é chÝnh x¸c cao vÒ t¹o h×nh vµ gi¸ thµnh thÊp. 3) HiÖn thùc viÖc m«®un hãa thiÕt kÕ m«®un quay vµ kÕt cÊu robot ®Ó ®¸p øng nhu cÇu ®a d¹ng lo¹i h×nh robot trong lóc vÉn cã thÓ chuyªn m«n hãa s¶n xuÊt chÕ t¹o tõng lo¹i m«®un ®¶m b¶o ®−îc gi¸ thµnh gia c«ng vµ chÊt l−îng s¶n phÈm. 4) ChÕ t¹o thö nghiÖm thµnh c«ng nhiÒu s¶n phÈm BRCL. Trong ®ã cã m«®un quay robot dïng BRCL, c¸c hép gi¶m tèc BRCL dïng cho c«ng nghiÖp, hép gi¶m tèc BRCL phôc vô c«ng t¸c ®µo t¹o. 83 PHẦN III BÁO CÁO CÁC KẾT QUẢ NGHIÊN CỨU THEO NHIỆM VỤ 2 ĐỀ TÀI KC.03.08 MÔĐUN DÂY CHUYỀN TỰ ĐỘNG DÙNG ROBOT SCATM ĐÓ PHÂN LOẠI SẢN PHẨM THEO MÀU SẮC I. GIỚI THIỆU CHUNG Trong phần báo cáo này trình bày kết quả nghiên cứu xây dựng môđun dây chuyền tự động dùng robot SCATM để phân loại sản phẩm theo màu sắc. Đây là một sản phẩm minh họa về cách thức “thông minh hóa” hoạt động của robot. Tất nhiên khả năng “thông minh” của robot của robot phải thể hiện trên một công việc cụ thể. Trong môđun dây chuyền tự động này, máy tính sẽ trực tiếp điều khiển các chuyển dịch của robot và các sản phẩm trên dây chuyền theo các tín hiệu về màu sắc do các sensor nhận biết. Định hướng ứng dụng của nội dung nghiên cứu này là công nghiệp hàng tiêu dùng. Trong các dây chuyền sản xuất này bộ phận phân loại sản phẩm luôn luôn có mặt như một môđun của hệ thống thiết bị sản xuất. Ở đây dùng Robot SCATM thao tác phối hợp với các băng truyền điều chỉnh được tốc độ, có trang bị các sensor nhận biết được màu sắc và toàn bộ hệ thống được điều khiển bằng máy tính. Nội dung nghiên cứu này bao gồm một khối lượng lớn các công việc từ thiết kế, chế tạo, lắp ráp vận hành toàn bộ hệ thống chấp hành cơ, điện, 84 khí nén, các thiết bị phần cứng của hệ thống điều khiển và chương trình phần mềm điều khiển toàn bộ hệ thống nói trên. Các thiết bị chủ yếu của hệ thống gồm: Robot SCATM, bộ phận khí nén, 2 băng truyền, bộ phận truyền động với tốc độ thay đổi được, bộ phận thu hồi sản phẩm và bộ phận điều khiển. Ngoài Robot SCATM đã trình bày riêng ở phần trên, dưới đây giới thiệu tóm tắt các bộ phận còn lại trong 2 mục: Hệ thống băng truyền và hệ thống điều khiển. II. HỆ THỐNG BĂNG TRUYỀN Hệ thống băng truyền được thiết kế đảm bảo cho các sản phẩm đặt trên đó di chuyển đều đặn với tốc độ đã định. Đảm bảo mặt băng bằng phẳng và đủ rộng để phân nhiều luồng sản phẩm theo từng màu sắc. So với vị trí đặt Robot SCATM bố trí 2 băng truyền: Băng thứ nhất đem sản phẩm tới Robot và băng thứ 2 đưa sản phẩm đi. Trong điều khiển chật hẹp của phòng thí nghiệm các băng tải đều rút nhỏ kích thước và bổ xung thêm bộ phận thu hồi sản phẩm từ cuối băng chuyền thứ 2 về đầu băng truyền thứ 1. Dưới đây là bộ hồ sơ kỹ thuật của hệ thống băng truyền gồm: - H×nh 2.3.1. Các bản vẽ lắp Môđun dây chuyền dùng robot SCATM - H×nh 2.3.2. Các ảnh chụp Môđun dây chuyền dùng Robot SCATM (nhìn từ nhiều phía). - H×nh 2.3.3. Các bản vẽ và ảnh chụp về hệ thống khí nén. - H×nh 2.3.4. Các ảnh chụp hệ thống truyền dẫn động và biến tần - H×nh 2.3.5. Các ảnh chụp và bản vẽ lắp bộ phận thu hồi sản phẩm 85 H×nh 2.3.1. B¶n vÏ l¾p M«®un d©y chuyÒn s¶n xuÊt dïng Robot SCATM 1 2 3 A A A A 1 T æn g sè K hè i l −î ng V Ët li Öu S è l− în g § ¬n v Þ 1 1 G hi c hó R ob ot S C A 1 M « ®u n b¨ ng t¶ i V Þ t rÝ T ªn g äi T ñ ®i Ön 3 2 N gu y? n Th i? n Ph úc N gu y? n V an M in h D uy ?t Th i? t k ? Tr . n hi ?m K i? m tr a H ? và tê n K ý N gà y S? lu ?n g: Đ ? tà i K C - 0 3 - 0 8 Tr un g tâ m N CK T T? d ?n g hó a Tr u? ng Đ ?i h ?c B ác h K ho a H à N ?i B ?n v ? s? : T? l? : H oµ ng A nh § ¹i D ¢ Y T R U Y Òn p h © n l o ¹ i s¶ n p h È m t h eo m ¸ u s ¾ c B ¶ n v Ï l¾ p 86 H×nh 2.3.2. B¶n vÏ l¾p M«®un d©y chuyÒn s¶n xuÊt dïng Robot SCATM(nh×n tõ nhiÒu phÝa) 87 H×nh 2.3.3.S¬ ®å vµ ¶nh chôp vÒ hÖ thèng khÝ nÐn ®iÒu khiÓn b¨ng truyÒn dïng Robot SCATM 88 H×nh 2.3.4. Van khÝ nÐn ®iÖn tõ ®iÒu khiÓn Robot SCATM vµ b¨ng truyÒn H×nh 2.3.5. Bé phËn thu håi s¶n phÈm 89 M« t¶ th«ng sè kü thuËt cña hÖ thèng khÝ nÐn cho Robot SCATM vµ b¨ng truyÒn 1. Xy lanh bµn kÑp tay m¸y SCATM * Ký hiÖu: HGW-25-A * §Æc tÝnh kü thuËt: - DÉn ®éng b»ng xy lanh khÝ nÐn t¸c ®éng kÐp - §Þnh t©m tù ®éng - T¸c ®éng cña ngãn kÑp: KÑp bªn ngoµi hoÆc kÑp bªn trong. - Cã kh¶ n¨ng tæ hîp víi c¸c dÉn ®éng kh¸c - Cã thÓ g¾n c¶m biÕn vÞ trÝ tõ tÝnh hoÆc c¶m biÕn Hall. - Linh ho¹t víi kÑp tõ bªn ngoµi - Lùc kÑp lµ h»ng sè kh«ng phô thuéc vµo gãc kÑp. - Gãc më tèi ®a cña m¸ kÑp lµ 400 - §é chÝnh x¸c lËp l¹i: ± 0,02 mm - §é chÝnh x¸c kho¶ng c¸ch: < 0,2 mm - Sè ngãn kÑp: 2 - ¸p suÊt lµm viÖc: 2-8 bar - TÇn sè lµm viÖc lín nhÊt cña bµn kÑp: 4 Hz - Thêi gian më nhá nhÊt ë ¸p suÊt 6 bar: 10 ms - Thêi gian më ®ãng nhÊt ë ¸p suÊt 6 bar: 10 ms - NhiÖt ®é m«i tr−êng lµm viÖc: 5 - 60 0C - M« ment tay kÑp më ë ¸p suÊt 2 bar = 60 Ncm - M« ment tay kÑp më ë ¸p suÊt 4 bar = 120 Ncm - M« ment tay kÑp më ë ¸p suÊt 6 bar = 180 Ncm - M« ment tay kÑp ®ãng ë ¸p suÊt 2 bar = 53,5 Ncm - M« ment tay kÑp ®ãng ë ¸p suÊt 4 bar = 106,7 Ncm - M« ment tay kÑp ®ãng ë ¸p suÊt 2 bar = 160 Ncm - Lùc lín nhÊt ë m¸ kÑp Fz tÜnh = 54 N - M« ment lín nhÊt ë m¸ kÑp Mx tÜnh = 1,7 Nm - M« ment lín nhÊt ë m¸ kÑp My tÜnh = 0,6 Nm - M« ment lín nhÊt ë m¸ kÑp Mz tÜnh = 1,1 Nm - Träng l−îng: 250 g - §Çu nèi khÝ: M5 - VËt liÖu: Hîp kim nh«m 90 - M¹ anod cøng - M¸ kÑp ®−îc m¹ Niken KÝch th−íc: 2. Xy lanh quay: * Ký hiÖu: DSR-12-180 * Th«ng sè kü thuËt: - Nguyªn lý: xy lanh quay kiÓu c¸nh g¹t, gãc quay cã thÓ ®iÒu chØnh ®−îc. HÖ thèng dõng ®−îc t¸ch riªng khái c¸nh g¹t v× thÕ lùc qu¸n tÝnh ®−îc hÊp thô b»ng gi¶m chÊn vµ cam giíi h¹n hµnh tr×nh th«ng qua tÊm nhùa linh ho¹t. - Lùc ®Èy ®−îc truyÒn trùc tiÕp ®Õn trôc quay qua c¸c c¸nh g¹t b»ng chuyÓn ®éng l¾c qua l¹i. 91 - Lùc ®Èy max: 0,5 Nm - C¶m biÕn giíi h¹n hµnh tr×nh: c«ng t¾c c¬ ®iÖn mini hoÆc c¶m biÕn tõ tÝnh - Gãc l¾c cã thÓ hiÖu chØnh tõ 00 ®Õn 1800 . - Cã thÓ g¸ l¾p ë bÊt cø vÞ trÝ nµo. - ¸p suÊt lµm viÖc: 2 - 8 bar - TÇn sè l¾c lín nhÊt ë 6 bar : 3 Hz - NhiÖt ®é m«i tr−êng lµm viÖc: -100C ®Õn 600C - M« ment ë 6 bar: 1 Nm - §Çu nèi khÝ nÐn: M5 - VËt liÖu lµm trôc: ThÐp - M¹ Niken - Vßng lµm kÝn: NBR KÝch th−íc: 3. Xy lanh khÝ: * Ký hiÖu: DSN-25-100-PPV-A * Th«ng sè kü thuËt: - Hµnh tr×nh: 100 mm - §−êng kÝnh piston: 25mm - Ren cÇn piston: M10x1.25 - Gi¶m chÊn cuèi hµnh tr×nh: Vßng gi¶m chÊn ®µn håi ë c¶ hai phÝa 92 - ¸p suÊt lµm viÖc: 1 - 10 bar - ChÕ ®é lµm viÖc: t¸c dông kÐp - NhiÖt ®é m«i tr−êng lµm viÖc: -20 - 80 °C - Lùc ®Èy lý thuyÕt ë 6 bar, hµnh tr×nh trë vÒ: 247.4 N - Lùc ®Èy lý thuyÕt ë 6 bar, hµnh tr×nh tiÕn ra: 294.5 N - Khèi l−îng chuyÓn ®éng víi hµnh tr×nh 0 mm: 71 g - Khèi l−îng ccéng thªm cho mçi hµnh tr×nh 10 mm: 11g - Träng l−îng c¬ b¶n cho hµnh tr×nh 0 mm: 238 g - §Çu nèi ren khÝ nÐn: G 1/8" - VËt liÖu vá xy lanh: Hîp kim nh«m ®−îc an«d ho¸ - VËt liÖu vßng lµm kÝn: TPE-U(PU) NBR - VËt liÖu xy lanh: ThÐp hîp kim cao cÊp, kh«ng rØ - VËt liÖu piston: ThÐp hîp kim cao cÊp, kh«ng rØ Van ®iÖn tõ lo¹i nhá CPE: * Ký hiÖu: CPE14-M1BH-5L-1/8 * Th«ng sè kü thuËt: - Van ®iÖn tõ 5 cöa 2 vÞ trÝ t¸c ®éng ®¬n, håi b»ng lß xo - KÝch th−íc dÇy: 10mm - L−u l−îng khÝ ®i qua: 350l/phót - §iÖn ¸p ®iÒu khiÓn: 24 VDC - Cã chèt ®iÒu khiÓn van b»ng tay - §Çu nèi ren khÝ nÐn: G 1/8" 93 Van ®iÖn tõ lo¹i lín Tiger 2000 : * Ký hiÖu: JMFH-5-1/8-B * Th«ng sè kü thuËt: - Van kiÓu con tr−ît ®iÒu khiÓn ®iÖn tõ c¶ hai phÝa - Van ®iÖn tõ cã chèt ®iÒu khiÓn b»ng tay - L−u l−îng khÝ ®i qua: 1000l/phót - §iÖn ¸p lµm viÖc: 24VDC - Cã thÓ l¾p trùc tiÕp trªn ®−êng dÉn hoÆc trªn ®Õ thµnh tr¹m van tõ 2 ®Õn 10 van 94 Van Kho¸ HE : * Ký hiÖu: HE-2-1/8-QS-6 * Th«ng sè kü thuËt: - KiÓu van 2 cöa 2 vÞ trÝ - KÝch th−íc ®Þnh møc: 5 mm - ¸p suÊt lµm viÖc: 0,75 - 10 bar - L−u l−îng khÝ: 300 - 800 l/phót - §iÒu khiÓn b»ng tay - L¾p ®Æt ë vÞ trÝ bÊt kú - Dßng khÝ kh«ng ®¶o chiÒu ®−îc - Träng l−îng: 18 g - Ren nèi khÝ: G1/8" - VËt liÖu: Nhùa PBT Bé läc, ®iÒu ¸p khÝ nÐn LFR : * Ký hiÖu: LFR-1/4-D-MINI * Th«ng sè kü thuËt: - KÝch th−íc: lo¹i Mini - KiÓu: D - Cã kho¸ nót ®iÒu chØnh - L−u l−îng khÝ: 1400 l/phót - §é läc: 40 µm - L¾p ®Æt: Th¼ng ®øng +/- 5° - X¶ n−íc ng−ng tô: nót quay b»ng tay - ThÓ tÝch n−íc ng−ng tù lín nhÊt: 22 cm3 95 - Vá b¶o vÖ: b»ng kim lo¹i - Cã l¾p ®ång hå ¸p suÊt - D¶i ¸p suÊt ®iÒu chØnh: 0.5 - 12 bar - ¸p suÊt ®Çu vµo: 0 - 16 bar - NhiÖt ®é lµm viÖc: -10 - 60oC - NhiÖt ®é m«i tr−êng: -10 - 60oC - Träng l−îng: 460g - §−êng nèi khÝ: G1/4" - VËt liÖu van: KÏm ®óc - VËt liÖu bÇu chøa n−íc ng−ng tô: nhùa trong PC 96 H×nh 2.3.6. B¶n thiÕt kÕ 3D b¨ng t¶i 1 H×nh 2.3.7. B¶n thiÕt kÕ 3D b¨ng t¶i 2 (cã g¾n sensor nhËn mµu) 97 H×nh 2.3.8. HÖ thèng 2 b¨ng t¶i 98 III. hÖ thèng ®iÒu khiÓn dcsx dïng robot scatm ®Ó ph©n lo¹i s¶n phÈm theo mµu s¾c 3.1. Nguyên tắc hoạt động Hoạt động được điều khiển là môđun dây chuyền sản xuất (DCSX) gồm robot SCATM làm việc với hệ thống băng tải được điều khiển bằng biến tần và được trang bị các sensor nhận biết màu sắc. Nhờ chương trình phần mềm điều khiển, các thông số chuyển động của robot SCATM và của băng tải được tính toán hoặc nạp vào thông qua bàn phím và màn hình của máy PC. Khi khởi động môđun DCSX làm việc, chương trình điều khiển sẽ đưa các bộ phận chấp hành về vị trí ban đầu. Trong quá trình làm việc từ các khớp quay các giá trị xung tương ứng với độ chuyển dịch tức thời đều được ghi nhớ và so sánh với các giá trị đặt trước hoặc tính toán ra, chương trình điều khiển sẽ xuất ra các lệnh để làm chuyển dịch các khớp động đến vị trí cần thiết. Độ chuyển dịch của 3 động cơ bước sẽ đảm bảo tọa độ X,Y và hướng mở của bàn kẹp. Tọa độ Z của nó được thực hiện bằng độ chuyển dịch của xilanh khí nén. Hệ thống robot được dạy học nhận biết màu qua các sensor. Khi có tín hiệu xuất hiện vật thể có màu đã chọn, bằng cách phối hợp các chuyển động của băng tải và của bản thân mình, robot SCATM được ra lệnh nhặt lấy vật thể có màu đó. Phân loại sản phẩm bằng màu sắc có thể trở thành phương thức phân loại được dùng rộng rãi nhất nếu lưu ý rằng có thể dễ dàng tạo ra tín hiệu màu sắc để nhìn thấy ở vị trí thuận lợi trên bất kỳ loại bao bì hoặc trực tiếp trên sản phẩm. Trên hình 2.3.6 là ảnh chụp môđun DCSX với robot SCATM đang làm việc cùng băng tải phân loại sản phẩm theo màu sắc. 99 Hình 2.3.9. Tßan c¶nh m«®un DCSX dïng robot SCATM ®Ó ph©n lo¹i s¶n phÈm theo mµu s¾c 3.2. Các bộ phân chủ yếu Hệ thống điều khiển môđun DCSX dùng robot SCATM làm việc với băng tải phân loại sản phẩm, đã được thiết kế, chế tạo, lắp ráp và vận hành thử nghiệm đối với cả phần cứng và phần mềm. Phần cứng dùng bộ giao tiếp với máy tính PC qua cổng máy in, thực hiện các chuyển động của robot SCATM và của hệ thống băng tải theo các số liệu đã được lập trình. Các bộ phận phần cứng chủ yếu gồm có: Mạch giao tiếp với máy tính; Mạch logic và điều khiển động cơ bước; Mạch điều khiển khí nén v.v. Lấy ví dụ, trên hình 2.3.10. là sơ đồ điều khiển mạch động cơ bước. 100 Hình 3.2 H×nh 2.3.10. S¬ ®å c¸c m¹ch ®iÒu khiÓn ®éng c¬ b−íc Chương trình phần mềm được viết trên ngôn ngữ Visual C++. Việc thực hiện quỹ đạo chuyển động của bàn kẹp của robot SCATM được tự động tính toán theo chương trình con để giải các bài toán thuận và ngược động học, như đã trình bày ở phần trên. Trong quá trình điều khiển, chương trình bày tính toán giá trị của các góc khớp quay và so sánh với các giá trị nhận được từ các sensor gắn với các khớp quay để xuất ra các lệnh làm quay các động cơ tương ứng. Các số liệu được nhập vào từ bàn phím của máy PC. Khi khởi động, chương trình điều khiển sẽ đưa tay máy về vị trí ban đầu được xác định nhờ các bộ cảm biến vị trí gắn trên các hành trình của mỗi bậc tự do chuyển động. Hình 2.3.11 là trích đoạn chương trình phần mềm nói trên. Toàn bộ chương trình được trình bày ở phần IV của Báo cáo nhiệm vụ 2. Robot SCATM còn được điều khiển theo phương pháp “dạy học”, tức là dẫn dắt tay máy thực hiện các bước chuyển dịch cần thiết và các số liệu này được ghi vào bộ nhớ để sau đó lặp lại trong quá trình làm việc. Các “teaching box” này cũng được thiết lập trên màn hình (hình 2.3.12.) 1 2 3 4 A B C D 4321 D C B A Title Number RevisionSize A4 Date: 21-Sep-2001 Sheet of File: C:\NGHIA\DKM.S01 Drawn By: 11 12 13 IC14D 74LS32 8 9 10 IC14C 74LS32 4 5 6 IC14B 74LS32 1 2 3 IC14A 74LS32 11 12 13 IC13D 74LS02 89 10 IC13C 74LS02 4 56 IC13B 74LS02 1 23 IC13A 74LS02 +5 R2 1213 IC2F 74LS04 1011 IC2E 74LS04 8 9 IC2D 74LS04 5 6 IC2C 74LS04 1 2 IC2A 74LS04 +5 A1 B2 C3 E14 E25 E36 Y0 15 Y1 14 Y2 13 Y3 12 Y4 11 Y5 10 Y6 9 Y7 7 IC5 74LS138 8 9 10 IC3C 74LS32 1 14 2 15 3 16 4 17 5 18 6 19 7 20 8 21 9 22 10 23 11 24 12 25 13 D2 DB25 A02 A13 A24 A35 A46 A57 A68 A79 B0 18 B1 17 B2 16 B3 15 B4 14 B5 13 B6 12 B7 11 E19 DIR1 IC7 74LS245 D03 Q0 2 D14 Q1 5 D27 Q2 6 D38 Q3 9 D413 Q4 12 D514 Q5 15 D617 Q6 16 D718 Q7 19 OE1 LE11 IC8 74LS373 D 0 3 Q 0 2 D 1 4 Q 1 5 D 2 7 Q 2 6 D 3 8 Q 3 9 D 4 13 Q 4 12 D 5 14 Q 5 15 D 6 17 Q 6 16 D 7 18 Q 7 19 O E 1 LE 11 IC9 74LS373 D 0 3 Q 0 2 D 1 4 Q 1 5 D 2 7 Q 2 6 D 3 8 Q 3 9 D 4 13 Q 4 12 D 5 14 Q 5 15 D 6 17 Q 6 16 D 7 18 Q 7 19 O E 1 C LK 11 IC10 74LS374D 0 3 Q 0 2 D 1 4 Q 1 5 D 2 7 Q 2 6 D 3 8 Q 3 9 D 4 13 Q 4 12 D 5 14 Q 5 15 D 6 17 Q 6 16 D 7 18 Q 7 19 O E 1 C LK 11 IC11 74LS374 D 0 3 Q 0 2 D 1 4 Q 1 5 D 2 7 Q 2 6 D 3 8 Q 3 9 D 4 13 Q 4 12 D 5 14 Q 5 15 D 6 17 Q 6 16 D 7 18 Q 7 19 O E 1 L E 11 IC12 74LS373 1A 2 1Y 4 1B 3 2A 5 2Y 7 2B 6 3A 11 3Y 9 3B 10 4A 14 4Y 12 4B 13 G 15 A /B 1 IC15 74LS257 +5 +5 A1 B2 C3 E14 E25 E36 Y0 15 Y1 14 Y2 13 Y3 12 Y4 11 Y5 10 Y6 9 Y7 7 IC1 74LS138 1 2 3 IC3A 74LS32 4 5 6 IC3B 74LS32 11 1213 IC3D 74LS32 1 2 3 IC4A 74LS00 4 5 6 IC4B 74LS00 8 9 10 IC4C 74LS00 11 12 13 IC4D 74LS00 D0 3Q02 D1 4Q15 D2 7Q26 D3 8Q39 D4 13Q412 D5 14Q515 D6 17Q616 D7 18Q719 OE 1 LE 11 IC6 74LS373 VAO12 RA15RA14RA13 8X10K /RD /RD /WR /WR D0 D1 D2 D3 D4 D5 D6 D7 ACK SLCT PE BUSY STROBE D0 D1 D2 D3 D4 D5 D6 D7 D4 D5 D6 D7 =1 DU LIEU AF =1 WR =1 RDSLCTIN INIT SLCTIN INIT AF STROBE =0 DIA CHI =0 NUA BYTE THAP =1 NUA BYTE CAO J1 RD WR 101 Hình 2.3.11 Trích đoạn chương trình điều khiển robot SCA khí nén. Hình 2.3.12. Thiết lập “teaching box” trên màn hình 102 PHẦN IV. BÁO CÁO VỀ NHIỆM VỤ 2 CỦA ĐỀ TÀI KC.03.08 CƠ SỞ TÍNH TOÁN VÀ XÂY DỰNG CÁC CHƯƠNG TRÌNH MÁY TÍNH VỀ ĐIỀU KHIỂN VÀ ỨNG DỤNG ROBOT SCA I. GIỚI THIỆU CHUNG Trong phần IV của Báo cáo trình bày các cơ sở tính toán và xây dựng các chương trình máy tính. Trên cơ sở đó đã xây dựng các chương trình máy tính sau: - Ch−¬ng tr×nh tù ®éng thiÕt lËp m« h×nh ®éng häc robot (TDH) - Ch−¬ng tr×nh kiÓm nghiÖm lêi gi¶i bµi to¸n ®éng häc ng−îc (KN§H) - Ch−¬ng tr×nh tÝnh to¸n ®éng lùc häc (TDLH) - Chương trình điều khiển Robot SCA - Chương trình điều khiển môđun DCSX dùng robot SCA chọn lựa sản phẩm theo màu sắc. 103 II. Ch−¬ng tr×nh tù ®éng thiÕt lËp m« h×nh ®éng häc robot (tdh) Khi thiÕt lËp hÖ ph−¬ng tr×nh ®éng häc c¬ b¶n cña robot ph¶i lÆp l¹i nhiÒu phÐp tÝnh ®¹i sè ma trËn phøc t¹p. Ta cÇn cã hÖ ph−¬ng tr×nh (1.11) ë d¹ng gi¶i tÝch ®Ó cã thÓ trùc gi¸c ph©n tÝch vµ t×m lêi gi¶i cu¶ hÖ ph−¬ng tr×nh nµy nh»m x¸c ®Þnh c¸c th«ng sè ®iÒu khiÓn vÞ trÝ cña robot. MÆt kh¸c, nÕu thùc hiÖn c¸c phÐp tÝnh ma trËn b»ng tay th× rÊt mÊt thêi gian vµ rÊt dÔ nhÇm lÉn. V× vËy rÊt cÇn thiÕt tiÕn hµnh tÝnh to¸n, thiÕt lËp c¸c biÓu thøc gi¶i tÝch nãi trªn b»ng m¸y tÝnh. Lêi gi¶i ë ®©y ph¶i lµ c¸c biÓu thøc gi¶i tÝch (chø kh«ng ph¶i b»ng sè), biÓu diÔn mèi quan hÖ ®éng häc cña robot. Gi¶i quyÕt ®−îc c¸c vÊn ®Ò trªn, nh»m tù ®éng hãa mét sè c«ng viÖc tÝnh to¸n thiÕt kÕ ®éng häc robot, sÏ phôc vô cã c¸c b−íc tiÕp theo trong qóa tr×nh tù ®éng hãa s¶n xuÊt dïng robot. Ch−¬ng tr×nh tÝnh to¸n TDH ®−îc viÕt b»ng ng«n ng÷ Matlab. KÕt qu¶ tÝnh to¸n cã thÓ l−u vµo ®Üa vµ cã thÓ xem l¹i hoÆc in ra giÊy khi cÇn. D÷ liÖu nhËp trùc tiÕp trªn mµn h×nh mét c¸ch trùc quan, chñ yÕu lµ bé th«ng sè Denavit - Hartenberg (DH) cña c¬ cÊu robot , tªn robot vµ sè kh©u ®éng. Ch−¬ng tr×nh sÏ tù ®éng tÝnh to¸n vµ cho ra kÕt qu¶. L−u ®å thuËt to¸n cña ch−¬ng tr×nh chÝnh TDH nh− h×nh 2.4.1 104 Begin NhËp d÷ liÖu: Tªn robot, sè kh©u ®éng n, c¸c th«ng sè DH, kiÓu khíp X¸c ®Þnh c¸c ma trËn Ai TÝnh to¸n c¸c ma trËn Ti Xö lý kÕt qu¶ End Më File kÕt qu¶ cò Ghi kÕt qu¶ ra mµn h×nh In kÕt qu¶ ra m¸y in Ghi kÕt qu¶ vµo ®Üa H×nh 2.4.1. L−u ®å thËt to¸n c¸c ma trËn Ti nh− h×nh 2.4.2 H×nh 2.4.2 Begin Khai b¸o c¸c biÕn, c¸c ma trËn, ®äc c¸c th«ng tin tÖp d÷ liÖu, sè khíp §äc c¸c phÇn tö cña c¸c ma trËn Ai §Æt c¸c gi¸ trÞ ®Çu cho c¸c biÕn, c¸c ma trËn kh¸c nhau ... TÝnh tÝch c¸c ma trËn Ti = A1 *(A2... (An-1 * An)) TÝnh kÕt qu¶ ë d¹ng triÓn khai Rót gän c¸c biÓu thøc l−îng gi¸c, biÓu diÔn kÕt qu¶ ë d¹ng rót gän dïng c¸c ngoÆc ®¬n End 105 Mµn h×nh tÝnh to¸n ch−¬ng tr×nh TDH nh− h×nh 2.4.3 H×nh 2.4.3 PhÇn mÒm nµy còng cã t¸c dông tèt trong viÖc gi¶ng d¹y robot. Khi cho sinh viªn thiÕt lËp m« h×nh ®éng häc c¸c lo¹i robot, ta cã thÓ dÔ dµng kiÓm tra kÕt qu¶ sinh viªn thiÕt lËp m« h×nh ®éng häc mét c¸ch nhanh chãng. III. Ch−¬ng tr×nh kiÓm nghiÖm lêi gi¶i bµi to¸n ®éng häc ng−îc V× lêi gi¶i bµi to¸n ®éng häc ng−îc th−êng cã tÝnh ®a trÞ, kh«ng ph¶i lêi gi¶i nµo còng thÝch hîp nªn ë ®©y ta cÇn kiÓm nghiÖm c¸c lêi gi¶i. §Çu tiªn ta cho bé th«ng sè biÕn khíp (qi , i=1...4) øng víi n ®iÓm, sÏ x¸c ®Þnh ®−îc ®−êng ®i cña “ ®iÓm t¸c ®éng cuèi “. VËy ë ®©y ta cã n ®iÓm “®iÓm t¸c ®éng cuèi ”. Mçi mét “ ®iÓm t¸c ®éng cuèi “ sÏ cho ta mét bé nghiÖm cña bµi to¸n thuËn (nx, ny, nz, sx, sy, sz, ax, ay, az, px, py, pz ). Tõ bé nghiÖm cña bµi to¸n thuËn nµy ta thay vµo bµi to¸n ng−îc ®Ó t×m ra ®−îc 106 nghiÖm cña bµi to¸n ng−îc lµ c¸c biÕn khíp (qi_nguoc , i=1...4). Ta thay c¸c nghiÖm (qi_nguoc , i=1...4) vµo bµi to¸n thuËn vµ t×m ra ®−îc bé nghiÖm kh¸c cu¶ bµi to¸n thuËn (nx_sau, ny_sau, nz_sau, sx_sau, sy_sau, sz_sau, ax_sau, ay_sau, az_sau, px_sau, py_sau, pz_sau ). Cho (qi , i=1...4) → bµi to¸n thuËn →(nx, ny, nz, sx, sy, sz, ax, ay, az, px, py, pz ) → bµi to¸n ng−îc → (qi_nguoc , i=1...4) → bµi to¸n thuËn →(nx_sau, ny_sau, nz_sau, sx_sau, sy_sau, sz_sau, ax_sau, ay_sau, az_sau, px_sau, py_sau, pz_sau ). Tõ mét bé nghiÖm (nx, ny, nz, sx, sy, sz, ax, ay, az, px, py, pz ) ta x¸c ®Þnh ®−îc tr¹ng th¸i cña ®iÓm “ ®iÓm t¸c ®éng cuèi “. Ta sÏ so s¸nh (nx, ny, nz, sx, sy, sz, ax, ay, az, px, py, pz ) vµ (nx_sau, ny_sau, nz_sau, sx_sau, sy_sau, sz_sau, ax_sau, ay_sau, az_sau, px_sau, py_sau, pz_sau ) xem sù sai kh¸c cã nhiÒu hay kh«ng. KÕt qu¶ ch−¬ng tr×nh KNDH sau khi ch¹y nh− trªn h×nh 4.4. H×nh 2.4.4. 107 NhËn xÐt: Tõ nh÷ng h×nh ¶nh trùc quan trªn, ta thÊy râ sù thuËn lîi h¬n h¼n khi kiÓm nghiÖm bµi to¸n robot b»ng viÖc m¸y tÝnh ho¸ c¸c phÐp tÝnh to¸n vµ dïng c¸c ch−¬ng tr×nh øng dông ¸p dông vµo ®©y. KÕt qu¶ kiÓm nghiÖm cho ta thÊy sù tÝnh to¸n bµi to¸n ®éng häc thuËn vµ bµi to¸n ®éng häc ng−îc cña robot SCATM lµ hoµn toµn chÝnh x¸c. IV. Ch−¬ng tr×nh phÇn mÒm tÝnh to¸n ®éng lùc häc (tdlh). PhÇn mÒm nµy ®−îc viÕt trªn ch−¬ng tr×nh matlab ®Ó tÝnh to¸n ®éng lùc häc. KÕt qu¶ lµ c¸c biÓu thøc gi¶i tÝch (chø kh«ng ph¶i b»ng sè). KÕt qu¶ ch−¬ng tr×nh khi tÝnh ®éng lùc häc robot SCA nh− ë môc 2.3. PhÇn I Ch−¬ng tr×nh nguån cña phÇn mÒm xem ë phÇn phô lôc. V. Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn robot SCA (DKSCA) Ch−¬ng tr×nh phÇn mÒm ®−îc viÕt trªn ng«n ng÷ Visual C++. ViÖc thùc hiÖn quü ®¹o chuyÓn ®éng cña bµn kÑp cña robot SCA ®−îc tù ®éng tÝnh to¸n theo ch−¬ng tr×nh con ®Ó gi¶i c¸c bµi to¸n thuËn vµ ng−îc ®éng häc, nh− ®· tr×nh bµy ë phÇn trªn. Trong qu¸ tr×nh ®iÒu khiÓn, ch−¬ng tr×nh nµy sÏ tÝnh to¸n gi¸ trÞ cña c¸c gãc khíp quay vµ so s¸nh víi c¸c gi¸ trÞ nhËn ®−îc tõ c¸c sensor g¾n víi c¸c khíp quay ®Ó xuÊt ra c¸c lÖnh lµm quay c¸c ®éng c¬ t−¬ng øng. HÖ thèng ®iÒu khiÓn robot ®−îc kÕt nèi víi m¸y tÝnh, qua cæng m¸y in, cho phÐp xuÊt nhËp d÷ liÖu ®iÒu khiÓn ®éng c¬ b−íc, theo c¸c chiÒu kh¸c nhau, ®Ó thùc hiÖn ®−a c¸nh tay robot SCA ®i theo nh÷ng hµnh tr×nh ®−îc x¸c ®Þnh tr−íc nhê c¸c ch−¬ng tr×nh ®iÒu khiÓn kh¸c nhau nhËp vµo m¸y tÝnh. 108 M¹ch logic vµ ®iÒu khiÓn ®éng c¬ b−íc ®−îc thiÕt kÕ trªn c¸c IC: 4093, 7474, 74257, 7406, 4066 vµ c¸c transitor c«ng suÊt lín, cho phÐp t¹o ra c¸c xung ®iÒu khiÓn lµm quay ®éng c¬ theo c¸c ch−¬ng tr×nh ®iÒu khiÓn. Trªn h×nh 5.5 lµ mµn h×nh ®iÒu khiÓn ®éng häc thuËn robot SCA H×nh 2.4.5 Trªn h×nh 2.4.6 lµ mµn h×nh ®iÒu khiÓn ®éng häc ng−îc robot SCA H×nh 2.4.6 109 Khi lËp tr×nh trùc tuyÕn, ng−êi vËn hµnh robot trùc tiÕp tiÕn hµnh lËp tr×nh trªn b¶n th©n robot hoÆc thiÕt bÞ phô trî kÌm theo. ë ®©y theo ph−¬ng ph¸p lËp tr×nh dïng hép d¹y häc (teach box). "Hép d¹y häc" ®−îc thiÕt kÕt ngay trªn mµn h×nh m¸y tÝnh (h×nh 2.4.7). H×nh 2.4.7 C¸ch sö dông "hép d¹y häc" nh− sau: sau khi bËt m¸y, trªn mµn h×nh hiÖn sè liÖu tr¹ng th¸i hiÖn thêi cña c¸c khíp ®éng vµ th«ng tin vÒ kÕt qu¶ so s¸nh víi vÞ trÝ ban ®Çu cÇn cã cña tõng khíp ®éng. NÕu khíp ®éng nµo cßn ch−a ë vÞ trÝ ban ®Çu th× ta bÊm vµo nót lÖnh cho vÒ vÞ trÝ ®Çu. Sau ®ã dïng c¸c nót trªn "hép d¹y häc" ®Ó ®iÒu khiÓn c¸c khíp ®éng ®Õn vÞ trÝ mong muèn. Lóc ®ã sè liÖu sÏ ®−îc ghi l¹i vµ khi cã lÖnh chuyÓn ®éng sÏ ®−îc t¸i hiÖn vµ lËp l¹i nhiÒu lÇn theo ý muèn. Trong khi c¸c ®éng c¬ ®ang quay th× lu«n lu«n cã ch−¬ng tr×nh con kiÓm tra tr¹ng th¸i ho¹t ®éng cña tõng khíp ®éng. VI Ch−¬ng tr×nh ®iÒu khiÓn m«®un DCSX dïng Robot SCA chän lùa s¶n phÈm theo mµu s¾c. 110 111 // rbkhinenDlg.cpp : implementation file #include "stdafx.h" #include "rbkhinen.h" #include "rbkhinenDlg.h" #ifdef _DEBUG #define new DEBUG_NEW #undef THIS_FILE static char THIS_FILE[] = __FILE__; #endif ///////////////////////////////////////////////////////////////////////////// // CAboutDlg dialog used for App About class CAboutDlg : public CDialog { public: CAboutDlg(); // Dialog Data //{{AFX_DATA(CAboutDlg) enum { IDD = IDD_ABOUTBOX }; //}}AFX_DATA // ClassWizard generated virtual function overrides //{{AFX_VIRTUAL(CAboutDlg) protected: virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV support //}}AFX_VIRTUAL // Implementation protected: //{{AFX_MSG(CAboutDlg) //}}AFX_MSG DECLARE_MESSAGE_MAP() }; CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD) { //{{AFX_DATA_INIT(CAboutDlg) //}}AFX_DATA_INIT } void CAboutDlg::DoDataExchange(CDataExchange* pDX) { CDialog::DoDataExchange(pDX); //{{AFX_DATA_MAP(CAboutDlg) 112 //}}AFX_DATA_MAP } BEGIN_MESSAGE_MAP(CAboutDlg, CDialog) //{{AFX_MSG_MAP(CAboutDlg) // No message handlers //}}AFX_MSG_MAP END_MESSAGE_MAP() ///////////////////////////////////////////////////////////////////////////// // CRbkhinenDlg dialog CRbkhinenDlg::CRbkhinenDlg(CWnd* pParent /*=NULL*/) : CDialog(CRbkhinenDlg::IDD, pParent) { //{{AFX_DATA_INIT(CRbkhinenDlg) // NOTE: the ClassWizard will add member initialization here //}}AFX_DATA_INIT // Note that LoadIcon does not require a subsequent DestroyIcon in Win32 m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME); } void CRbkhinenDlg::DoDataExchange(CDataExchange* pDX) { CDialog::DoDataExchange(pDX); //{{AFX_DATA_MAP(CRbkhinenDlg) DDX_Control(pDX, IDC_DUNG, m_cDung); DDX_Control(pDX, IDC_CHAY, m_cChay); //}}AFX_DATA_MAP } BEGIN_MESSAGE_MAP(CRbkhinenDlg, CDialog) //{{AFX_MSG_MAP(CRbkhinenDlg) ON_WM_SYSCOMMAND() ON_WM_PAINT() ON_WM_QUERYDRAGICON() ON_BN_CLICKED(IDC_CHAY, OnChay) ON_BN_CLICKED(IDC_DUNG, OnDung) ON_BN_CLICKED(IDC_VITRIDAU, OnVitridau) ON_WM_TIMER() //}}AFX_MSG_MAP END_MESSAGE_MAP() ///////////////////////////////////////////////////////////////////////////// // CRbkhinenDlg message handlers BOOL CRbkhinenDlg::OnInitDialog() { 113 CDialog::OnInitDialog(); // Add "About..." menu item to system menu. // IDM_ABOUTBOX must be in the system command range. ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX); ASSERT(IDM_ABOUTBOX < 0xF000); CMenu* pSysMenu = GetSystemMenu(FALSE); if (pSysMenu != NULL) { CString strAboutMenu; strAboutMenu.LoadString(IDS_ABOUTBOX); if (!strAboutMenu.IsEmpty()) { pSysMenu->AppendMenu(MF_SEPARATOR); pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu); } } // Set the icon for this dialog. The framework does this automatically // when the application's main window is not a dialog SetIcon(m_hIcon, TRUE); // Set big icon SetIcon(m_hIcon, FALSE); // Set small icon // TODO: Add extra initialization here return TRUE; // return TRUE unless you set the focus to a control } void CRbkhinenDlg::OnSysCommand(UINT nID, LPARAM lParam) { if ((nID & 0xFFF0) == IDM_ABOUTBOX) { CAboutDlg dlgAbout; dlgAbout.DoModal(); } else { CDialog::OnSysCommand(nID, lParam); } } // If you add a minimize button to your dialog, you will need the code below // to draw the icon. For MFC applications using the document/view model, // this is automatically done for you by the framework. 114 void CRbkhinenDlg::OnPaint() { if (IsIconic()) { CPaintDC dc(this); // device context for painting SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0); // Center icon in client rectangle int cxIcon = GetSystemMetrics(SM_CXICON); int cyIcon = GetSystemMetrics(SM_CYICON); CRect rect; GetClientRect(&rect); int x = (rect.Width() - cxIcon + 1) / 2; int y = (rect.Height() - cyIcon + 1) / 2; // Draw the icon dc.DrawIcon(x, y, m_hIcon); } else { CDialog::OnPaint(); } } // The system calls this to obtain the cursor to display while the user drags // the minimized window. HCURSOR CRbkhinenDlg::OnQueryDragIcon() { return (HCURSOR) m_hIcon; } //*************************************** #include"stdio.h" #include"conio.h" #include"math.h" #include"stdlib.h" #include"iostream.h" #define basad 0x378 #define datreg basad #define statreg basad+1 #define contrreg basad+2 int xuat_cong_13(int); int xuat_cong_14(int); int xuat_cong_15(int); int dong_co_1(int); 115 int dong_co_2(int); int dong_co_3(int); int dong_co_1_ve(int); int dong_co_2_ve(int); int dong_co_3_ve(int); //------------------------------------ int vao_cong(int); int vi_tri_dau(int); int vao; int xuong4=255, dong5=255; //************************************************************** int vi_tri_dau(int cong) { int loc_1,loc_11,loc_sau_1; int loc_2,loc_21,loc_sau_2; int loc_3,loc_31,loc_sau_3; int loc_4,loc_41,loc_sau_4; int loc_5,loc_51,loc_sau_5; int luc_dau,tg,co_ban,chieu_quay; /* vao_cong(12); loc_2=vao&2; loc_sau_2=loc_2&0; Sleep(10); xuat_cong_14(34); Sleep(10); xuat_cong_15(253); while(loc_21!=loc_sau_2) { vao_cong(12); loc_21=vao&2; } xuat_cong_15(255); */ vao_cong(12); Sleep(10); loc_1=vao&1; loc_sau_1=loc_1&0; 116 loc_2=vao&2; loc_sau_2=loc_2&0; loc_3=vao&4; loc_sau_3=loc_3^4; xuat_cong_14(38); xuat_cong_15(248); xuat_cong_15(248); while(loc_11!=loc_sau_1) { vao_cong(12); loc_11=vao&1; xuat_cong_15(251); xuat_cong_15(251); } // xuat_cong_15(255); while(loc_21!=loc_sau_2) { vao_cong(12); loc_21=vao&2; xuat_cong_15(253); Sleep(0.1); xuat_cong_15(253); Sleep(0.1); xuat_cong_15(253); } while(loc_31!=loc_sau_3) { vao_cong(12); loc_31=vao&4; xuat_cong_15(254); } xuat_cong_15(255); //xuat_cong_15(255); 117 return(0); } //--------------------------------------- int vao_cong(int y8) { int tam1,tam2,thap,cao; _outp(contrreg,0); //mo dia chi _outp(datreg,y8); //d/c cong _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 Sleep(0.1); thap=_inp(statreg); //truyen _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 Sleep(0.1); cao=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam1=thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 tam2=cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 vao=tam1/16+tam2; return(0); } //------------------------------------------------ int xuat_cong_15(int x15) { _outp(contrreg,0); Sleep(0.1); _outp(datreg,15); _outp(contrreg,1); Sleep(0.1); _outp(datreg,x15); _outp(contrreg,9); //xung ghi Sleep(0.1); _outp(contrreg,1); //ket thuc ghi Sleep(0.1); return(0); } 118 int xuat_cong_14(int x14) { _outp(contrreg,0); Sleep(0.1); _outp(datreg,14); _outp(contrreg,1); Sleep(0.1); _outp(datreg,x14); _outp(contrreg,9); //xung ghi Sleep(0.1); _outp(contrreg,1); //ket thuc ghi Sleep(0.1); return(0); } int xuat_cong_13(int x13) { _outp(contrreg,0); _outp(datreg,13); _outp(contrreg,1); _outp(datreg,x13); _outp(contrreg,9); //xung ghi _outp(contrreg,1); //ket thuc ghi return(0); } //------------------------------------------ //************************************************ void CRbkhinenDlg::OnChay() { // TODO: Add your control notification handler code here int loc,loc_sensophathienvat1,loc_senso,loc1; dong_co_1(820);//780 Sleep(100); dong_co_2(500);//-510 Sleep(100); dong_co_3(150);//-510 Sleep(100); // xuat_cong_15(223); //ca len va mo // xuat_cong_15(215); //xuong // xuat_cong_15(199); //xuong va dong // xuat_cong_15(207); //len // xuat_cong_15(231); //xuong va dong//231 119 /* xuat_cong_14(40); // xi lanh ra delay(1000); xuat_cong_14(48); // xi lanh vao delay(1000); xuat_cong_14(34); // quay xl delay(1000); xuat_cong_14(32); // quay xl delay(1000); */ while(1) { vao_cong(12); Sleep(10); loc=vao&128; loc1=vao&64; Sleep(1); if(loc1==0) { xuat_cong_14(40); Sleep(300); Sleep(500); xuat_cong_15(215); Sleep(1500); xuat_cong_15(199); Sleep(1000); xuat_cong_15(207);//207 Sleep(1000); xuat_cong_14(48); Sleep(500); dong_co_1_ve(-550); Sleep(1000); xuat_cong_15(231); Sleep(800); xuat_cong_15(215);//255 Sleep(500); xuat_cong_15(255);//255 Sleep(1000); dong_co_1(550);//780 xuat_cong_14(2); Sleep(2000); 120 xuat_cong_14(32); } else if(loc==0) { Sleep(500); xuat_cong_14(40); Sleep(800); xuat_cong_15(215); Sleep(1500); xuat_cong_15(199); Sleep(1000); xuat_cong_15(207);//207 Sleep(1000); xuat_cong_14(48); Sleep(500); dong_co_1_ve(-650); Sleep(1000); xuat_cong_15(231); Sleep(800); xuat_cong_15(215);//255 Sleep(500); xuat_cong_15(255);//255 Sleep(1000); dong_co_1(650);//780 xuat_cong_14(2); Sleep(2000); xuat_cong_14(32); } else { xuat_cong_15(255); } } // m_cChay.EnableWindow(FALSE); // m_cDung.EnableWindow(TRUE); } 121 //************************************************ void CRbkhinenDlg::OnDung() { // TODO: Add your control notification handler code here m_cChay.EnableWindow(TRUE); m_cDung.EnableWindow(FALSE); } //***************************************** void CRbkhinenDlg::OnVitridau() { // TODO: Add your control notification handler code here xuat_cong_14(48); // xi lanh vao vi_tri_dau(1); Sleep(10); // vi_tri_dau(1); xuat_cong_13(255); Sleep(1); xuat_cong_13(0); /* xuat_cong_14(48); // xi lanh vao Sleep(1000); xuat_cong_14(32); // xi lanh quay ve dau //Sleep(1000); */ } //******************************************** //---------------------------------------------- int dong_co_1(int dc1) { int tam1,tam2,tam3,tam4,thap_thap,cao_thap, thap_cao,cao_cao,thap_that,cao_that; int gia_tri_tong_hop,gia_tri_tong_hop_that; xuat_cong_15(255-510+xuong4+dong5);//+(223-255)); Sleep(1); if (dc1<0) { xuat_cong_14(36); } else { xuat_cong_14(32); } xuat_cong_15(255+(-510+xuong4+dong5));//+(223-255)); 122 xuat_cong_13(255); xuat_cong_13(0); xuat_cong_15(251+(-510+xuong4+dong5));//+(223-255)); Sleep(1); do { _outp(contrreg,0); //mo dia chi _outp(datreg,4); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 // Sleep(100); thap_thap=_inp(statreg); //truyen tam1=thap_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 // Sleep(100); cao_thap=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam2=cao_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 thap_that=tam1/16+tam2; //--------------------------- _outp(contrreg,0); //mo dia chi _outp(datreg,5); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 //Sleep(100); thap_cao=_inp(statreg); //truyen tam3=thap_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 // Sleep(100); cao_cao=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam4=cao_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 cao_that=tam3/16+tam4; //--------------------------- // Sleep(1); gia_tri_tong_hop=(cao_that*256+thap_that); Sleep(1); }while (gia_tri_tong_hop<=abs(dc1)); 123 // xuat_cong_15(255); // Sleep(50); xuat_cong_15(255-510+xuong4+dong5);//+(223-255)); return(0); } //---------------------------------------- //---------------------------------------------- int dong_co_2(int dc2) { int tam1,tam2,tam3,tam4,thap_thap,cao_thap, thap_cao,cao_cao,thap_that,cao_that; int gia_tri_tong_hop,gia_tri_tong_hop_that; xuat_cong_15(255-510+xuong4+dong5);//+(223-255)); if (dc2<0) { xuat_cong_14(34); } else { xuat_cong_14(32); } xuat_cong_13(255); xuat_cong_13(0); xuat_cong_15(253+(-510+xuong4+dong5));//+(223-255)); do { _outp(contrreg,0); //mo dia chi _outp(datreg,2); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 // Sleep(0.1); thap_thap=_inp(statreg); //truyen tam1=thap_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 // Sleep(0.1); cao_thap=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 124 tam2=cao_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 thap_that=tam1/16+tam2; //--------------------------- _outp(contrreg,0); //mo dia chi _outp(datreg,3); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 //Sleep(0.1); thap_cao=_inp(statreg); //truyen tam3=thap_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 //Sleep(0.1); cao_cao=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam4=cao_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 cao_that=tam3/16+tam4; //--------------------------- gia_tri_tong_hop=(cao_that*256+thap_that); Sleep(1); }while (gia_tri_tong_hop<=abs(dc2)); // xuat_cong_15(255); xuat_cong_15(255-510+xuong4+dong5);//+(223-255)); return(0); } //---------------------------------------- //---------------------------------------------- int dong_co_3(int dc3) { int tam1,tam2,tam3,tam4,thap_thap,cao_thap, thap_cao,cao_cao,thap_that,cao_that; int gia_tri_tong_hop,gia_tri_tong_hop_that; xuat_cong_15(255-510+xuong4+dong5);//+(223-255)); if (dc3<0) { xuat_cong_14(32); 125 } else { xuat_cong_14(33); } xuat_cong_13(255); xuat_cong_13(0); xuat_cong_15(254+(-510+xuong4+dong5));//+(223-255)); do { _outp(contrreg,0); //mo dia chi _outp(datreg,0); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 //Sleep(0.1); thap_thap=_inp(statreg); //truyen tam1=thap_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 //Sleep(0.1); cao_thap=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam2=cao_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 thap_that=tam1/16+tam2; //--------------------------- _outp(contrreg,0); //mo dia chi _outp(datreg,1); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 //Sleep(0.1); thap_cao=_inp(statreg); //truyen tam3=thap_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 //Sleep(0.1); cao_cao=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam4=cao_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 cao_that=tam3/16+tam4; //--------------------------- gia_tri_tong_hop=(cao_that*256+thap_that); 126 Sleep(1); }while (gia_tri_tong_hop<=abs(dc3)); // xuat_cong_15(255); xuat_cong_15(255-510+xuong4+dong5);//+(223-255)); return(0); } //---------------------------------------- //***************************************** //---------------------------------------------- int dong_co_1_ve(int dc1) { int tam1,tam2,tam3,tam4,thap_thap,cao_thap, thap_cao,cao_cao,thap_that,cao_that; int gia_tri_tong_hop,gia_tri_tong_hop_that; xuat_cong_15(239); if (dc1<0) { xuat_cong_14(36); } else { xuat_cong_14(32); } xuat_cong_13(255); xuat_cong_13(0); xuat_cong_15(251+(-255+239)); do { _outp(contrreg,0); //mo dia chi _outp(datreg,4); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 // Sleep(0.1); thap_thap=_inp(statreg); //truyen tam1=thap_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 127 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 // Sleep(0.1); cao_thap=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam2=cao_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 thap_that=tam1/16+tam2; //--------------------------- _outp(contrreg,0); //mo dia chi _outp(datreg,5); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 // Sleep(0.1); thap_cao=_inp(statreg); //truyen tam3=thap_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 // Sleep(0.1); cao_cao=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam4=cao_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 cao_that=tam3/16+tam4; //--------------------------- gia_tri_tong_hop=(cao_that*256+thap_that); Sleep(1); }while (gia_tri_tong_hop<=abs(dc1)); // xuat_cong_15(255); xuat_cong_15(239); return(0); } //---------------------------------------- //---------------------------------------------- int dong_co_2_ve(int dc2) { int tam1,tam2,tam3,tam4,thap_thap,cao_thap, thap_cao,cao_cao,thap_that,cao_that; int gia_tri_tong_hop,gia_tri_tong_hop_that; xuat_cong_15(239); 128 if (dc2<0) { xuat_cong_14(34); } else { xuat_cong_14(32); } xuat_cong_13(255); xuat_cong_13(0); xuat_cong_15(253+(239-255)); do { _outp(contrreg,0); //mo dia chi _outp(datreg,2); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 // Sleep(0.1); thap_thap=_inp(statreg); //truyen tam1=thap_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 //Sleep(0.1); cao_thap=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam2=cao_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 thap_that=tam1/16+tam2; //--------------------------- _outp(contrreg,0); //mo dia chi _outp(datreg,3); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 //Sleep(0.1); thap_cao=_inp(statreg); //truyen tam3=thap_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 //Sleep(0.1); cao_cao=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam4=cao_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 cao_that=tam3/16+tam4; 129 //--------------------------- gia_tri_tong_hop=(cao_that*256+thap_that); Sleep(1); }while (gia_tri_tong_hop<=abs(dc2)); // xuat_cong_15(255); xuat_cong_15(239); return(0); } //---------------------------------------- //---------------------------------------------- int dong_co_3_ve(int dc3) { int tam1,tam2,tam3,tam4,thap_thap,cao_thap, thap_cao,cao_cao,thap_that,cao_that; int gia_tri_tong_hop,gia_tri_tong_hop_that; xuat_cong_15(239); if (dc3<0) { xuat_cong_14(32); } else { xuat_cong_14(33); } xuat_cong_13(255); xuat_cong_13(0); xuat_cong_15(254+(239-255)); do { _outp(contrreg,0); //mo dia chi _outp(datreg,0); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 // Sleep(0.1); thap_thap=_inp(statreg); //truyen tam1=thap_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 130 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 //Sleep(0.1); cao_thap=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam2=cao_thap&240; //giu nguyen 4 bit dau, 4 bit sau =0 thap_that=tam1/16+tam2; //--------------------------- _outp(contrreg,0); //mo dia chi _outp(datreg,1); //d/c cong 0 _outp(contrreg,1); //ghi ra _outp(contrreg,3); // doc vao nua byte thap,AF=1,Init=0 //Sleep(0.1); thap_cao=_inp(statreg); //truyen tam3=thap_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 _outp(contrreg,7); // doc nua cao cua byte thap,AF=1,Init=1 //Sleep(0.1); cao_cao=_inp(statreg); //truyen _outp(contrreg,1); // ket thuc xung doc,AF=0,Init=0 tam4=cao_cao&240; //giu nguyen 4 bit dau, 4 bit sau =0 cao_that=tam3/16+tam4; //--------------------------- gia_tri_tong_hop=(cao_that*256+thap_that); Sleep(1); }while (gia_tri_tong_hop<=abs(dc3)); xuat_cong_15(255); xuat_cong_15(239); return(0); } //---------------------------------------- void CRbkhinenDlg::OnTimer(UINT nIDEvent) { // TODO: Add your message handler code here and/or call default CDialog::OnTimer(nIDEvent); } 131 Tµi liÖu tham kh¶o 1. Morgan, C. Robots: planning and implementation, UK, IFS Publications, 1984. 2. Owen, T. Assembly with Robot. London: Kogan Page, 1985 3. Pham and Heginbotham – Robot Grippers. Bedford, UK, IFS Publications, 1986. 4. Pugh, A. (editor). Robot Sensors. Vol. 1 and Vol 2, Bedford UK, IFS Publications, 1986. 5. Vijay Kumar “Motion Planning and Control Robots” . Handbook of Industrial Robotics. Ed. S.Y.Nof. NewYork, John Wiley & Sons. Inc, 1999. 6. Refael C. Gonzaler & Richard E. Woods. Digital Image Processing. Addison – Wesley, 1992. 7. Mair G. Industrial Robotics. Prentice Hall, 1988. 8. Paul R.P., Modeling, Trajectory Caculation and Servoing of a Computer Controlled Arm. Palo Alto Calif, 1972. 9. Paul R.P., Robot Manipulators: Mathematics, Programming and Control. The MIT Press – Cambridge, Massachusetts and London, England, 1981. 10. Schilling R.J., Fundamentals of Robotics, Analysis & Control. Prentice Hall, 1990. 11. Snyder W.E., Industrial Robots: Computer Interfacing and Control. Prentice Hall Inc. New Jersey, 1985. 12. Shimon Y. Nof, Handbook of Industrial Robotics, 2nded. John Wiley & Sons. Inc. 1999. 132 13. Shimon Y. Nof, Handbook of Industrial Robotics, 2nded. John Wiley & Sons. Inc. 1999. 14. NguyÔn ThiÖn Phóc, TrÇn V¨n C−êng, VÒ mét ph−¬n−g ph¸p gi¶i bµi to¸n ng−îc ®éng häc khi tæng hîp quü ®¹o chuyÓn ®éng cña ng−êi m¸y. TuyÓn tËp c¸c b¸o c¸o khoa häc Héi nghÞ Tù ®éng hãa toµn quèc lÇn thø II, 1996. 15. NguyÔn ThiÖn Phóc, §iÒu khiÓn chuyÓn ®éng robot hµn theo quü ®¹o ®Þnh tr−íc. TuyÓn tËo c«ng tr×nh Héi nghÞ C¬ häc toµn quèc lÇn thø VI, Hµ Néi, 12/1997. 16. NguyÔn ThiÖn Phóc, Ph¸p triÓn ph−¬ng ph¸p “c¸c nhãm 3” ®Ó gi¶i bµi to¸n ng−îc ®éng häc khi tæng hîp quü ®¹o chuyÓn ®éng cña tay m¸y – ng−êi m¸y. TuyÓn tËp c¸c b¸o c¸o khoa häc Héi nghÞ C¬ häc m¸y ViÖt Nam, Hµ Néi, 1999. 17. NguyÔn ThiÖn Phóc, TrÇn Ngäc To¶n, TrÇn Minh NghÜa, NguyÔn TiÕn §øc, Nghiªn cøu thiÕt kÕ chÕ t¹o robot SCA mini. TuyÓn tËp c«ng tr×nh khoa häc Héi nghÞ khoa häc toµn quèc vÒ c¬ kü thuËt tËp 2, 10/2001. 18. Koltrin N.I. C¬ häc m¸y, M, 1972 (tiÕng Nga). 19. Litvin F. L. Lý thuyÕt ¨n khíp b¸nh r¨ng. Moscva, 1968 (tiÕng Nga) 20. Duxev I.I., NguyÔn ThiÖn Phóc – Ph©n tÝch ®Æc tÝnh tiÕp xóc côc bé trªn c¬ së lý thuyÕt b«i tr¬n thñy ®éng, Moscva, 1977 (tiÕng Nga). 21. NguyÔn ThiÖn Phóc. C¸c chØ tiªu ®¸nh gi¸ kh¶ n¨ng chÞu t¶i cña c¸c lo¹i truyÒn ®éng trôc vÝt kiÓu míi. Leningrat, 1971 (tiÕng Nga). 22. NguyÔn ThiÖn Phóc. C¬ së lý thuyÕt tiÕp xóc enlip trong ¨n khíp b¸nh r¨ng kh«ng gian, Leningrat, 1978 (tiÕng Nga). 23. NguyÔn ThiÖn Phóc. Planetary cycloid roller gear reducer. VN Journal of Mechanics, N2. 2002. 133 24. NguyÔn ThiÖn Phóc “Robot c«ng nghiÖp” NXB khoa häc vµ kü thuËt, Hµ Néi, 2004. 25. NguyÔn ThiÖn Phóc, (vµ c¸c céng sù). Hép gi¶m tèc b¸nh r¨ng con l¨n. TuyÓn tËp b¸o c¸o Héi nghÞ C¬ häc toµn quèc lÇn thø VII, 2002. 26. NguyÔn ThiÖn Phóc, NguyÔn TiÕn §øc, Lª Hoµng Giang. TÝnh to¸n thiÕt kÕ, chÕ t¹o c¸c m«®un quay trong robot. TuyÓn tËp b¸o c¸o khoa häc Héi nghÞ tù ®éng hãa toµn quèc, lÇn thø V.