ỨNG DỤNG ROBOT KUKA KR6R700 ĐỂ PHÂN LOẠI VÀ LẮP RÁP SẢN PHẨM LUẬN VĂN THẠC SĨ KỸ THUẬT CƠ KHÍ

ĐẠI HỌC ĐÀ NẴNG TRƢỜNG ĐẠI HỌC BÁCH KHOA NGUYỄN NGỌC THIỆN ỨNG DỤNG ROBOT KUKA KR6R700 ĐỂ PHÂN LOẠI VÀ LẮP RÁP SẢN PHẨM LUẬN VĂN THẠC SĨ KỸ THUẬT CƠ KHÍ Đà Nẵng, Năm 2017 ĐẠI HỌC ĐÀ NẴNG TRƢỜNG ĐẠI HỌC BÁCH KHOA NGUYỄN NGỌC THIỆN ỨNG DỤNG ROBOT KUKA KR6R700 ĐỂ PHÂN LOẠI VÀ LẮP RÁP SẢN PHẨM Chuyên ngành Mã số : Kỹ thuật khí : 60.52.01.03 LUẬN VĂN THẠC SĨ Ngƣời hƣớng dẫn khoa học: PGS.TS PHẠM ĐĂNG PHƢỚC Đà Nẵng, Năm 2017 LỜI CAM ĐOAN Tơi cam đoan cơng trình nghiên cứu riêng Các số liệu, kết nêu luận văn trung thực chƣa đƣợc cơng bố cơng trình khác Tác giả luận văn Nguyễn Ngọc Thiện MỤC LỤC LỜI CAM ĐOAN MỤC LỤC TÓM TẮT LUẬN VĂN DANH MỤC CÁC HÌNH MỞ ĐẦU 1 LÝ DO CHỌN ĐỀ TÀI MỤC ĐÍCH NGHIÊN CỨU ĐỐI TƢỢNG VÀ PHẠM VI NGHIÊN CỨU PHƢƠNG PHÁP NGHIÊN CỨU Ý NGHĨA KHOA HỌC VÀ THỰC TIỄN CỦA ĐỀ TÀI CẤU TRÚC LUẬN VĂN CHƢƠNG TỔNG QUAN 1.1 TỔNG QUAN VỀ CÁC HỆ THỐNG SẢN XUẤT TỰ ĐỘNG .4 1.1.1 Lịch sử phát triển tự động hóa q trình sản xuất tự động 1.1.2 Các khái niệm tự động hóa 1.1.3 Các dạng tự động hóa (TĐH cứng, linh hoạt, lập trình, v v ) 1.1.4 Hiệu kinh tế tự động hóa 1.2 TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP 1.2.1 Các khái niệm định nghĩa robot công nghiệp .8 1.2.2 Kết cấu robot công nghiệp 1.2.3 Phân loại robot công nghiệp .10 1.3 TỔNG QUAN VỀ CÁC NGHIÊN CỨU LIÊN QUAN 11 1.4 TỔNG QUAN VỀ HOẠT ĐỘNG CỦA HỆ THỐNG SẢN XUẤT TẠI PHÒNG THỰC HÀNH CAD/CAM/CNC TRƢỜNG ĐH PHẠM VĂN ĐỒNG .12 1.4.1 Máy phay CNC hãng Tormach 12 1.4.2 Hệ thống cấp phôi tự động 13 1.4.3 Hệ thống máy khoan tự động 13 1.4.4 Mô tả hoạt động hệ thống sản xuất tự động 13 CHƢƠNG XÂY DỰNG CÁC PHƢƠNG TRÌNH ĐỘNG HỌCCHO ROBOT KUKA KR6R700 16 2.1 TỔNG QUAN VỀ ROBOT KUKA KR6R700 SIXX 16 2.1.1 Giới thiệu công nghệ robot hãng Kuka 16 2.1.2 Tổng quan hệ thống robot 17 2.1.3 Mô tả tay máy Kuka KR6 17 2.1.4 Bộ điều khiển KRC4 18 2.1.5 Bộ điều khiển lập trình dạy học KUKA smartPAD .19 2.1.6 Lập trình robot 21 2.2 THIẾT LẬP CÁC PHƢƠNG TRÌNH ĐỘNG HỌC CHO ROBOT KUKA KR6R700 Sixx 22 2.2.1 Gắn hệ toạ độ .23 2.2.2 Bảng thông số DH .23 2.2.3 Phƣơng trình động học 24 CHƢƠNG THIẾT KẾ HỆ THỐNG PHÂN LOẠI VÀ LẮP RÁP SẢN PHẨM .26 3.1 THIẾT KẾ VÀ CHẾ TẠO MODULE PHÂN LOẠI SẢN PHẨM .26 3.1.1 Tổng quan sản phẩm cần phân loại 26 3.1.2 Tính toán thiết kế hệ thống băng tải chiều .26 3.1.3 Tính tốn thiết kế hệ thống kho lƣu trữ tạm .29 3.1.4 Thiết kế hệ thống điều khiển có sử dụng cảm biến màu .32 3.2 THIẾT KẾ VÀ CHẾ TẠO MODULE LẮP RÁP SẢN PHẨM .36 3.2.1 Mơ hình sản phẩm cần lắp ráp 36 3.2.2 Thiết kế module lắp ráp sản phẩm 37 3.3 HỆ THỐNG PHÂN LOẠI VÀ LẮP RÁP SẢN PHẨM 39 CHƢƠNG LẬP TRÌNH MƠ PHỎNG HỆ THỐNG PHÂN LOẠIVÀ LẮP RÁP SẢN PHẨM BẰNG EASYROB 2.0 42 4.1 SƠ LƢỢC VỀ PHẦN MỀM EASYROB 42 4.1.1 Giới thiệu sơ lƣợc EasyRob 42 4.1.2 Tìm hiểu giao diện EasyRob 43 4.2 MÔ PHỎNG ROBOT KUKA KR6R700 sixx 51 4.3 MÔ PHỎNG HỆ THỐNG PHÂN LOẠI VÀ LẮP RÁP 53 4.4 LẬP TRÌNH MƠ PHỎNG 55 4.4.1 Lƣu đồ thuật toán hoạt động hệ thống 55 4.4.2 Mô hoạt động phân loại lắp ráp robot Kuka 56 CHƢƠNG LẬP TRÌNH ĐIỀU KHIỂN HỆ THỐNG PHÂN LOẠI VÀ LẮP RÁP SẢN PHẨM 60 5.1 MỘT SỐ VẤN ĐỀ CƠ BẢN VỀ NGƠN NGỮ LẬP TRÌNH CHO ROBOT KUKA 60 5.1.1 Các lệnh di chuyển 60 5.1.2 Các lệnh làm việc với vào/ra 60 5.1.3 Các lệnh điều khiển 61 5.1.4 Biến .65 5.2 LẬP TRÌNH SỬ DỤNG ROBOT KUKA PHÂN LOẠI SẢN PHẨM 66 5.2.1 Ghép nối hệ thống điều khiển KRC4 robot KuKa với hệ thống vi điều khiển nhận biết màu sắc sản phẩm 66 5.2.2 Thuật toán điều khiển robot phối hợp với hệ thống phân loại 68 5.3 LẬP TRÌNH ROBOT KUKA LẮP RÁP SẢN PHẨM 69 5.3.1 Ghép nối điều khiển thiết bị hệ thống lắp ráp với điều khiển KRC4 robot lắp ráp 69 5.3.2 Thuật toán điều khiển robot lắp ráp sản phẩm 70 KẾT LUẬN 71 TÀI LIỆU THAM KHẢO PHỤ LỤC QUYẾT ĐỊNH GIAO ĐỀ TÀI LUẬN VĂN (Bản sao) ỨNG DỤNG ROBOT KUKA KR6R700 ĐỂ PHÂN LOẠI VÀ LẮP RÁP SẢN PHẨM Học viên: Nguyễn Ngọc Thiện Chuyên ngành: Kỹ thuật khí Mã số: 60.52.01.03 Khóa: 31.Trƣờng Đại học Bách khoa- ĐHĐN Tóm tắt – Đề tài thực việc thiết kế chế tạo hệ thống phân loại lắp ráp sản phẩm tự động sử dụng robot Kuka Các chi tiết sau đƣợc robot đƣa vào gia công máy CNC đƣợc đƣa đến phận phân loại lắp ráp Sản phẩm cần lắp ráp sử dụng hai chi tiết có kích thƣớc hình học giống nhau, khác màu sắc Một cảm biến màu đƣợc tích hợp vào hệ thống để nhận biết đƣợc màu sắc báo cho robot biết để lựa chọn thao tác lắp ráp lƣu vào kho tạm Hệ thống lắp ráp sử dụng xylanh khí nén, cấu cấp bulông, đai ốc, cấu vặn bulông đƣợc thiết kế để thực việc lắp ráp Hệ thống đƣợc chế tạo, ứng dụng thực nghiệm phòng thực hành CAD/CAM/CNC trƣờng ĐH Phạm Văn Đồng Từ khóa – Robot Kuka; hệ thống phân loại; hệ thống lắp ráp; điều khiển robot, sản xuất tự động (5 từ khóa) CLASSIFYING AND ASEMBLING PRODUCT USING KUKA KR6R700 ROBOT Abstract - This thesis deals with the design and manufacture of classification and assembly products system using Kuka robot The parts been machined on CNC milling, after that will be taken into modules classification and module assembly The product is assembled using main parts that have the same geometry but diffrent color A color sensor is combined on system to recognize the color and call robot chose assembly or storing program The product assembly system includes cylinders, supply bolts and nuts modules, bolts tightening module The system is manufactured, applied at CAD/CAM/CNC laboratory in Pham Van Dong university Key words – Kuka robot; classification system; assembly system; control robot; automatic production DANH MỤC CÁC HÌNH Số hiệu Tên hình hình Trang Robot Kuka KR6R700 sixx 1.1 Một mơ hình hệ thống sản xuất linh hoạt Máy phay CNC trục Tormach 12 Hệ thống cấp phôi 13 Máy khoan tự động 13 Sơ đồ nguyên lý hệ thống 14 Robot công nghiệp FAMULUS năm 1973 16 2 Một phiên robot Kuka 17 Một mơ hình robot Kuka AGILUS sixx 17 Mơ hình robot Kuka KR6 18 Tủ điều khiển KR C4 18 Lựa chọn giao tiếp KR C4 19 KUKA smartPAD 19 Các chức KUKA smartPAD 20 Lập trình robot với KUKA smartPAD 22 10 Mơ làm việc trực quan với Robot KUKA 22 11 Gắn hệ tọa độ lên robot Kuka KR6R700 sixx 23 Sơ đồ tính lực cản băng tải 27 Cơ cấu Man hệ thống 32 3 Mạch nguồn 33 Khối vi điều khiển dao động 33 Khối I/O 34 Khối Led hiển thị 34 Khối hiển thị LCD 35 Khối công suất 36 Bản vẽ chi tiết chi tiết 36 Số hiệu Tên hình hình Trang 10 Mơ hình sản phẩm cần lắp ráp 37 11 Cụm định vị chi tiết 38 12 Định vị chi tiết 38 13 Bản vẽ chi tiết cụm chứa nạp đai ốc 39 14 Mô lắp ráp hệ thống 39 15 Các hình chiếu tồn hệ thống 40 16 Hình chiếu trục đo toàn hệ thống 41 Giao diện làm việc EasyRob 43 Menu chƣơng trình 43 Menu file 44 4 Menu Robotics 44 Menu mô 44 Menu thiết kế 3D 45 Menu view 45 Menu điều chỉnh lập trình 45 Thanh công cụ ngang 46 10 Thanh công cụ ngang dƣới 46 11 Thanh công cụ dọc 47 12 Giao diện lập trình robot 50 4.13 Mơ hình động học robot Kuka 52 14 Hệ tọa độ robot Kuka Kr6R700sixx EasyRob 53 15 Mơ hình robot Kuka EasyRob 53 16 Mơ hình hệ thống cấp phơi tự động 54 17 Mơ hình hệ thống băng tải chiều 54 18 Mơ hình hệ thống lắp ráp tự động 55 19 Mơ hình tồn hệ thống 55 20 Lƣu đồ thuật toán hoạt động hệ thống 56 21 Robot lấy sản phẩm từ hệ thống cấp phơi 57 Số hiệu Tên hình hình Trang 22 Robot đặt sản phẩm lên băng tải 57 23 Băng tải chuyển sản phẩm đến vị trí robot 58 24 Robot đƣa sản phẩm thứ vào vị trí lắp ráp 58 25 Robot đƣa sản phẩm vào lắp ráp 59 26 Kết lắp ráp 59 5.1 Các dạng di chuyển robot 60 5.2 Vào/ra Robot 61 5.3 Cấu trúc IF-ELSE 62 5.4 Cấu trúc SWICH-CASE 63 5.5 Cấu trúc vòng lặp FOR 63 5.6 Cấu trúc vòng WHILE 64 5.7 Cấu trúc REPEAT 65 5.8 Sơ đồ ghép nối hệ thống phân loại robot 67 5.9 Lƣu đồ thuật toán phối hợp robot hệ thống phân loại sản 68 phẩm 5.10 Sơ đồ ghép nối hệ thống lắp ráp KRC4 69 5.11 Thuật toán lập trình robot lắp ráp sản phẩm 70 DECL E6POS XP13={X -177.372589,Y 619.717896,Z 436.379852,A 177.403610,B 62.6224442,C -92.1281738,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP13={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT13={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP14={X -177.372589,Y 619.717896,Z 495.139404,A 177.403610,B 62.6224442,C -92.1281738,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP14={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT14={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP15={X -177.374268,Y 619.721375,Z 436.214203,A 177.405197,B 62.6228867,C -92.1299591,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP15={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT15={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP16={X -177.372269,Y 619.717224,Z 521.527466,A 177.402771,B 62.6222115,C -92.1272354,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP16={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT16={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP17={X -177.372269,Y 619.717224,Z 583.981384,A 177.402771,B 62.6222115,C -92.1272354,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP17={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT17={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP18={X -527.801697,Y 370.048279,Z 583.981384,A -143.603897,B 62.6222115,C -92.1272354,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP18={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT18={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP19={X -334.647156,Y 272.671722,Z 584.006104,A -143.599533,B 62.6237564,C -92.1321564,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP19={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT19={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP20={X -334.645294,Y 272.670868,Z 583.994446,A -122.706833,B 62.2857056,C -93.1041412,S 6,T 51,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP20={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT20={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP21={X -347.325226,Y 246.673538,Z 440.573914,A -117.383194,B 62.2313499,C -94.0131531,S 6,T 51,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP21={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT21={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP22={X -347.347870,Y 246.692520,Z 324.744232,A -117.374352,B 62.2303123,C -94.0231400,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP22={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT22={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP23={X -347.320831,Y 246.664566,Z 428.123718,A -117.440079,B 62.2436180,C -93.9476624,S 6,T 51,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP23={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT23={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP24={X -347.334290,Y 246.678,Z 575.359680,A -117.486092,B 62.2558784,C -93.8959885,S 6,T 51,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP24={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT24={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP25={X 2.01249266,Y 426.012,Z 575.356812,A -172.374725,B 62.2559700,C -93.8954544,S 6,T 51,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP25={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT25={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL PDAT PPDAT26={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP26={X 92.9433746,Y 259.535828,Z 496.158356,A 111.500175,B 46.5993042,C -31.5948982,S 2,T 11,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP26={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT27={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP27={X -354.090424,Y 250.346542,Z 307.809784,A -116.936066,B 62.3401718,C -94.0512924,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP27={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT28={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP28={X 92.9431152,Y 259.535522,Z 496.157776,A 111.500618,B 46.5990372,C -31.5951290,S 2,T 11,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP28={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT29={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} ENDDAT 1.2 Chƣơng trình thực thi &ACCESS RVP &REL 97 &PARAM EDITMASK = * &PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe DEF nnthien( ) ;FOLD INI;%{PE} ;FOLD BASISTECH INI GLOBAL INTERRUPT DECL WHEN $STOPMESS==TRUE DO IR_STOPM ( ) INTERRUPT ON BAS (#INITMOV,0 ) ;ENDFOLD (BASISTECH INI) ;FOLD USER INI ;Make your modifications here ;ENDFOLD (USER INI) ;ENDFOLD (INI) ;FOLD PTP P28 CONT Vel=100 % PDAT29 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P28, 3:C_DIS, 5:100, 7:PDAT29 $BWDSTART=FALSE PDAT_ACT=PPDAT29 FDAT_ACT=FP28 BAS(#PTP_PARAMS,100) PTP XP28 C_DIS ;ENDFOLD $out[12]=false $out[1]=true $out[4]=false ;FOLD PTP P1 CONT Vel=100 % PDAT1 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT1 $BWDSTART=FALSE PDAT_ACT=PPDAT1 FDAT_ACT=FP1 BAS(#PTP_PARAMS,100) PTP XP1 C_DIS ;ENDFOLD ;FOLD PTP P14 CONT Vel=100 % PDAT14 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P14, 3:C_DIS, 5:100, 7:PDAT14 $BWDSTART=FALSE PDAT_ACT=PPDAT14 FDAT_ACT=FP14 BAS(#PTP_PARAMS,100) PTP XP14 C_DIS ;ENDFOLD ;FOLD PTP P15 CONT Vel=100 % PDAT15 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P15, 3:C_DIS, 5:100, 7:PDAT15 $BWDSTART=FALSE PDAT_ACT=PPDAT15 FDAT_ACT=FP15 BAS(#PTP_PARAMS,100) PTP XP15 C_DIS ;ENDFOLD ;FOLD ; cloe gripper;%{PE}%R 8.3.34,%MKUKATPBASIS,%CCOMMENT,%VNORMAL,%P 2:cloe gripper ;ENDFOLD $out[12]=true $out[1]=false $out[4]=true $out[17]=false ;FOLD PTP P16 CONT Vel=100 % PDAT16 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P16, 3:C_DIS, 5:100, 7:PDAT16 $BWDSTART=FALSE PDAT_ACT=PPDAT16 FDAT_ACT=FP16 BAS(#PTP_PARAMS,100) PTP XP16 C_DIS ;ENDFOLD ;FOLD PTP P17 CONT Vel=100 % PDAT17 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P17, 3:C_DIS, 5:100, 7:PDAT17 $BWDSTART=FALSE PDAT_ACT=PPDAT17 FDAT_ACT=FP17 BAS(#PTP_PARAMS,100) PTP XP17 C_DIS ;ENDFOLD ;FOLD PTP P18 CONT Vel=100 % PDAT18 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P18, 3:C_DIS, 5:100, 7:PDAT18 $BWDSTART=FALSE PDAT_ACT=PPDAT18 FDAT_ACT=FP18 BAS(#PTP_PARAMS,100) PTP XP18 C_DIS ;ENDFOLD ;FOLD PTP P19 CONT Vel=100 % PDAT19 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P19, 3:C_DIS, 5:100, 7:PDAT19 $BWDSTART=FALSE PDAT_ACT=PPDAT19 FDAT_ACT=FP19 BAS(#PTP_PARAMS,100) PTP XP19 C_DIS ;ENDFOLD ;FOLD PTP P20 CONT Vel=100 % PDAT20 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P20, 3:C_DIS, 5:100, 7:PDAT20 $BWDSTART=FALSE PDAT_ACT=PPDAT20 FDAT_ACT=FP20 BAS(#PTP_PARAMS,100) PTP XP20 C_DIS ;ENDFOLD ;FOLD PTP P21 CONT Vel=100 % PDAT21 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P21, 3:C_DIS, 5:100, 7:PDAT21 $BWDSTART=FALSE PDAT_ACT=PPDAT21 FDAT_ACT=FP21 BAS(#PTP_PARAMS,100) PTP XP21 C_DIS ;ENDFOLD ;FOLD PTP P22 CONT Vel=100 % PDAT22 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P22, 3:C_DIS, 5:100, 7:PDAT22 $BWDSTART=FALSE PDAT_ACT=PPDAT22 FDAT_ACT=FP22 BAS(#PTP_PARAMS,100) PTP XP22 C_DIS ;ENDFOLD ;FOLD PTP P27 CONT Vel=100 % PDAT28 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P27, 3:C_DIS, 5:100, 7:PDAT28 $BWDSTART=FALSE PDAT_ACT=PPDAT28 FDAT_ACT=FP27 BAS(#PTP_PARAMS,100) PTP XP27 C_DIS ;ENDFOLD ;FOLD ; open gripper;%{PE}%R 8.3.34,%MKUKATPBASIS,%CCOMMENT,%VNORMAL,%P 2:open gripper ;ENDFOLD $out[12]=false $out[1]=true $out[4]=false ;FOLD PTP P23 CONT Vel=100 % PDAT23 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P23, 3:C_DIS, 5:100, 7:PDAT23 $BWDSTART=FALSE PDAT_ACT=PPDAT23 FDAT_ACT=FP23 BAS(#PTP_PARAMS,100) PTP XP23 C_DIS ;ENDFOLD $out[17]=true ;FOLD PTP P24 CONT Vel=100 % PDAT24 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P24, 3:C_DIS, 5:100, 7:PDAT24 $BWDSTART=FALSE PDAT_ACT=PPDAT24 FDAT_ACT=FP24 BAS(#PTP_PARAMS,100) PTP XP24 C_DIS ;ENDFOLD ;FOLD PTP P25 CONT Vel=100 % PDAT25 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P25, 3:C_DIS, 5:100, 7:PDAT25 $BWDSTART=FALSE PDAT_ACT=PPDAT25 FDAT_ACT=FP25 BAS(#PTP_PARAMS,100) PTP XP25 C_DIS ;ENDFOLD ;FOLD PTP P26 CONT Vel=100 % PDAT27 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P26, 3:C_DIS, 5:100, 7:PDAT27 $BWDSTART=FALSE PDAT_ACT=PPDAT27 FDAT_ACT=FP26 BAS(#PTP_PARAMS,100) PTP XP26 C_DIS ;ENDFOLD ;FOLD WAIT Time=5 sec;%{PE}%R 8.3.34,%MKUKATPBASIS,%CWAIT,%VWAIT,%P 3:5 WAIT SEC ;ENDFOLD $out[17]=false END Chƣơng trình thực thi Robot KuKa 2( robot có nhiệm vụ lắp ráp) 2.1 Chƣơng trình file DAT khai báo tọa độ điểm chuyển động robot &ACCESS RVP &REL 41 &PARAM EDITMASK = * &PARAM TEMPLATE = C:\KRC\Roboter\Template\ExpertVorgabe DEFDAT nnthien2 PUBLIC DECL BASIS_SUGG_T LAST_BASIS={POINT1[] "P10 ",POINT2[] "P10 ",CP_PARAMS[] "CPDAT0 ",PTP_PARAMS[] "PDAT10 ",CONT[] "C_DIS ",CP_VEL[] "2.0 ",PTP_VEL[] "100 ",SYNC_PARAMS[] "SYNCDAT ",SPL_NAME[] "S0 ",A_PARAMS[] "ADAT0 "} DECL E6POS XP1={X 92.9431229,Y 259.535522,Z 496.157745,A 111.500618,B 46.5990372,C -31.5951271,S 2,T 11,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP1={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT1={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP2={X -176.379059,Y 620.104736,Z 484.307861,A 177.226425,B 62.6645279,C -91.9602661,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP2={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT2={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP3={X -176.379059,Y 620.104736,Z 447.186462,A 177.226425,B 62.6645279,C -91.9602661,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP3={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT3={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP4={X -176.400314,Y 620.125488,Z 553.398804,A 177.227463,B 62.6670609,C -91.9583054,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP4={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT4={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP5={X -176.484329,Y 620.106445,Z 553.399,A -156.630768,B 62.7331429,C -89.5645218,S 6,T 51,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP5={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT5={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP6={X -359.190613,Y 348.390686,Z 553.513062,A -156.608490,B 62.7343254,C -89.5866470,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP6={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT6={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP7={X -346.384155,Y 251.975052,Z 487.267578,A -118.734550,B 62.7623253,C -89.5635529,S 6,T 51,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP7={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT7={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP8={X -346.496429,Y 252.087601,Z 306.768890,A -118.701958,B 62.7309761,C -89.6014252,S 6,T 19,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP8={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT8={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP9={X -346.514709,Y 252.104050,Z 473.465057,A -118.694046,B 62.7540207,C -89.6103668,S 6,T 51,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP9={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT9={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} DECL E6POS XP10={X 92.9431229,Y 259.535522,Z 496.157745,A 111.500618,B 46.5990372,C -31.5951271,S 2,T 11,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0} DECL FDAT FP10={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " ",TQ_STATE FALSE} DECL PDAT PPDAT10={VEL 100.000,ACC 100.000,APO_DIST 100.000,APO_MODE #CDIS,GEAR_JERK 50.0000,EXAX_IGN 0} ENDDAT 2.2 Chƣơng trình thực thi &ACCESS RVP &REL 41 &PARAM EDITMASK = * &PARAM TEMPLATE = C:\KRC\Roboter\Template\ExpertVorgabe DEF nnthien2( ) ;FOLD PTP P1 CONT Vel=100 % PDAT1 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT1 $BWDSTART=FALSE PDAT_ACT=PPDAT1 FDAT_ACT=FP1 BAS(#PTP_PARAMS,100) PTP XP1 C_DIS ;ENDFOLD ;FOLD OUT 12 'stop_banxoay' State=FALSE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:12, 3:stop_banxoay, 5:FALSE, 6:CONTINUE CONTINUE $OUT[12]=FALSE ;ENDFOLD ;FOLD WAIT Time=3 sec;%{PE}%R 8.3.34,%MKUKATPBASIS,%CWAIT,%VWAIT,%P 3:3 WAIT SEC ;ENDFOLD loop ;FOLD OUT 17 'dongco_bangtai_trong' State=FALSE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:17, 3:dongco_bangtai_trong, 5:FALSE, 6:CONTINUE CONTINUE $OUT[17]=FALSE ;ENDFOLD ;FOLD OUT 12 'stop_banxoay' State=TRUE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:12, 3:stop_banxoay, 5:TRUE, 6:CONTINUE CONTINUE $OUT[12]=TRUE ;ENDFOLD ;FOLD WAIT FOR ( IN 22 'robot_xanh' );%{PE}%R 8.3.34,%MKUKATPBASIS,%CEXT_WAIT_FOR,%VEXT_WAIT_FOR,%P 2:, 4:, 5:$IN, 6:22, 7:robot_xanh, 9: WAIT FOR ( $IN[22] ) ;ENDFOLD ;FOLD PTP P2 CONT Vel=100 % PDAT2 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P2, 3:C_DIS, 5:100, 7:PDAT2 $BWDSTART=FALSE PDAT_ACT=PPDAT2 FDAT_ACT=FP2 BAS(#PTP_PARAMS,100) PTP XP2 C_DIS ;ENDFOLD ;FOLD PTP P3 CONT Vel=100 % PDAT3 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P3, 3:C_DIS, 5:100, 7:PDAT3 $BWDSTART=FALSE PDAT_ACT=PPDAT3 FDAT_ACT=FP3 BAS(#PTP_PARAMS,100) PTP XP3 C_DIS ;ENDFOLD ;FOLD OUT 'Gripper_Open' State=FALSE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:1, 3:Gripper_Open, 5:FALSE, 6:CONTINUE CONTINUE $OUT[1]=FALSE ;ENDFOLD ;FOLD OUT 'Gripper_Close' State=TRUE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:4, 3:Gripper_Close, 5:TRUE, 6:CONTINUE CONTINUE $OUT[4]=TRUE ;ENDFOLD ;FOLD OUT 12 'stop_banxoay' State=TRUE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:12, 3:stop_banxoay, 5:TRUE, 6:CONTINUE CONTINUE $OUT[12]=TRUE ;ENDFOLD ;FOLD PTP P4 CONT Vel=100 % PDAT4 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P4, 3:C_DIS, 5:100, 7:PDAT4 $BWDSTART=FALSE PDAT_ACT=PPDAT4 FDAT_ACT=FP4 BAS(#PTP_PARAMS,100) PTP XP4 C_DIS ;ENDFOLD ;FOLD PTP P5 CONT Vel=100 % PDAT5 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P5, 3:C_DIS, 5:100, 7:PDAT5 $BWDSTART=FALSE PDAT_ACT=PPDAT5 FDAT_ACT=FP5 BAS(#PTP_PARAMS,100) PTP XP5 C_DIS ;ENDFOLD ;FOLD PTP P6 CONT Vel=100 % PDAT6 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P6, 3:C_DIS, 5:100, 7:PDAT6 $BWDSTART=FALSE PDAT_ACT=PPDAT6 FDAT_ACT=FP6 BAS(#PTP_PARAMS,100) PTP XP6 C_DIS ;ENDFOLD ;FOLD PTP P7 CONT Vel=100 % PDAT7 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P7, 3:C_DIS, 5:100, 7:PDAT7 $BWDSTART=FALSE PDAT_ACT=PPDAT7 FDAT_ACT=FP7 BAS(#PTP_PARAMS,100) PTP XP7 C_DIS ;ENDFOLD ;FOLD PTP P8 CONT Vel=100 % PDAT8 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P8, 3:C_DIS, 5:100, 7:PDAT8 $BWDSTART=FALSE PDAT_ACT=PPDAT8 FDAT_ACT=FP8 BAS(#PTP_PARAMS,100) PTP XP8 C_DIS ;ENDFOLD ;FOLD OUT 'Gripper_Open' State=TRUE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:1, 3:Gripper_Open, 5:TRUE, 6:CONTINUE CONTINUE $OUT[1]=TRUE ;ENDFOLD ;FOLD OUT 'Gripper_Close' State=FALSE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:4, 3:Gripper_Close, 5:FALSE, 6:CONTINUE CONTINUE $OUT[4]=FALSE ;ENDFOLD ;FOLD PTP P9 CONT Vel=100 % PDAT9 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P9, 3:C_DIS, 5:100, 7:PDAT9 $BWDSTART=FALSE PDAT_ACT=PPDAT9 FDAT_ACT=FP9 BAS(#PTP_PARAMS,100) PTP XP9 C_DIS ;ENDFOLD ;FOLD PTP P10 CONT Vel=100 % PDAT10 Tool[1]:PinGripper Base[0];%{PE}%R 8.3.34,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P10, 3:C_DIS, 5:100, 7:PDAT10 $BWDSTART=FALSE PDAT_ACT=PPDAT10 FDAT_ACT=FP10 BAS(#PTP_PARAMS,100) PTP XP10 C_DIS ;ENDFOLD ;FOLD OUT 17 'dongco_bangtai_trong' State=TRUE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:17, 3:dongco_bangtai_trong, 5:TRUE, 6:CONTINUE CONTINUE $OUT[17]=TRUE ;ENDFOLD ;FOLD OUT 12 'stop_banxoay' State=FALSE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:12, 3:stop_banxoay, 5:FALSE, 6:CONTINUE CONTINUE $OUT[12]=FALSE ;ENDFOLD ;FOLD WAIT Time=10 sec;%{PE}%R 8.3.34,%MKUKATPBASIS,%CWAIT,%VWAIT,%P 3:10 WAIT SEC 10 ;ENDFOLD ;FOLD OUT 17 'dongco_bangtai_trong' State=FALSE CONT;%{PE}%R 8.3.34,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:17, 3:dongco_bangtai_trong, 5:FALSE, 6:CONTINUE CONTINUE $OUT[17]=FALSE ;ENDFOLD endloop END Chƣơng trình sử dụng PIC18F4431 để phân loại màu sắc dựa vào cảm biến màu #include #include #include #fuses HS,NOWDT,NOPROTECT,NOLVP #use delay(clock=20000000) #use fast_io(b) #use fast_io(c) #use fast_io(a) #use fast_io(d) #bit TCS3200_S0=porta.5 #bit TCS3200_S1=porte.0 #bit TCS3200_S2=porte.1 #bit TCS3200_S3=porte.2 int16 F_Blue=0,F_Green=0,F_Red=0; int32 F_pulse; int8 Counter_Timer0=0; int16 Counter_Timer1=0; int32 dem,var,enable_display; #define FILTER_RED #define FILTER_BLUE #define NO_FILTER #define FILTER_GREEN //======================= #INT_TIMER1 void timer1() { Counter_Timer1++; } #INT_TIMER0 void count_pulse() { Counter_Timer0++; if (Counter_Timer0 == 500) { // sau 50ms tinh toan tan so F_pulse_get_timer1()+(Counter_Timer1*65536)))*20; Counter_Timer0=0; Counter_Timer1=0; enable_display = 1; } set_timer0(130); setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4); } #INT_TIMER2 void set_servo() { set_timer2(130); dem++; if (dem >= 200) { dem=0; RD6=1; } if (dem>= var) RD6=0; } unsigned int TCS3200_Read(unsigned int16 Temp) { if(Temp==FILTER_RED) { TCS3200_S2=0; TCS3200_S3=0; } if(Temp==FILTER_BLUE) { TCS3200_S2=0; TCS3200_S3=1; } if(Temp==NO_FILTER) { TCS3200_S2=1; TCS3200_S3=0; } if(Temp==FILTER_GREEN) { TCS3200_S2=1; TCS3200_S3=1; } delay_ms(300); return F_pulse; } void Check_Color() { F_Red=TCS3200_Read(FILTER_RED); F_Green=TCS3200_Read(FILTER_GREEN); F_Blue=TCS3200_Read(FILTER_BLUE); if((F_Red>F_Green)&&(F_Red>F_Blue)) { var=20; delay_ms(1000); var=9; } if((F_Green>F_Red)&&(F_Green>F_Blue)) { //var=20; ////delay_ms(1000); //var=9; } if((F_Blue>F_Red)&&(F_Blue>F_Green)) { //var=18; } } void main() { // i/o cac cong DDRC0 = 1; DDRA = DDROUT; DDRE = DDROUT; DDRD = DDROUT; enable_interrupts(int_timer0); //enable_interrupts(int_timer1); enable_interrupts(int_timer2); enable_interrupts(global); setup_timer_1(T1_EXTERNAL|T1_DIV_BY_1); set_timer1(0); setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4); set_timer0(130); setup_timer_2(T2_DIV_BY_4,255,1); SET_TIMER2(130); TCS3200_S0=1; TCS3200_S1=1; while(TRUE) { if(enable_display) { Check_Color(); } } }