Thiết kế và chế tạo cánh tay robot 6 bậc tự do

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO CÁNH TAY ROBOT BẬC TỰ DO Người hướng dẫn: Sinh viên thực hiện: TS LÊ HOÀI NAM TRẦN VĂN ĐỔ NGUYỄN ĐÌNH DANH Đà Nẵng, 12/2019 TÓM TẮT ĐỒ ÁN TỐT NGHIỆP Tên đề tài: Thiết kế chế tạo robot bậc tự Sinh viên thực hiện: Nguyễn Đình Danh Lớp: 15CDT1 MSV: 101150159 Trần Văn Đổ Lớp: 15CDT1 MSV: 101150160 Giáo viên hướng dẫn: TS Lê Hoài Nam Nhu cầu thực tế đề tài Hiện nay, nhu cầu sử dụng robot để thay người làm công việc nặng nhọc, môi trường khắc nghiệt ngày cấp thiết Ngành công nghiệp robot ngày phát triển theo nhiều hình thức cách sử dụng khác Để xây dựng lên hệ thống, dây chuyền sản xuất tự động, thiếu robot tự động Chính thế, nhóm tác giả sâu nghiên cứu giải vấn đề Phạm vi nghiên cứu C C R L T - Nghiên cứu hệ thống, thiết kế robot bậc - Điều khiển, thiết kế phần mềm phần cứng cho robot bậc U D - Xây dựng mơ hình điều khiển robot Nội dung đề tài thực - Mơ hình: - Bản vẽ: (A0) - Thuyết minh gồm: 56 trang Kết đạt - Tìm hiểu chung loại robot - Giải động học thuận nghịch robot bậc - Nắm phương pháp, thuật toán điều khiển - Thiết lập điều khiển hoạt động robot Sinh viên thực Nguyễn Đình Danh Trần Văn Đổ ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ CỘNG HỊA XÃ HƠI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: NGUYỄN ĐÌNH DANH TRẦN VĂN ĐỔ Lớp: 15CDT1 MSSV: 101150159 MSSV: 101150160 Khoa: Cơ Khí Ngành: Kỹ Thuật Cơ Điện Tử Tên đề tài đồ án: Thiết kế chế tạo robot bậc tự Đề tài thuộc diện: ☐ Có ký kết thỏa thuận sở hữu trí tuệ kết thực Các số liệu liệu ban đầu: Vùng làm việc robot bậc Các kích thước thơng số robot Các vật liệu: nhơm ngun khối, nhựa, nhơm định hình C C R L T Nội dung phần thuyết minh tính tốn: Chương 1: Tổng quan đề tài U D Chương 2: Thiết kế khí Chương 3: Thiết kế hệ thống điều khiển robot Chương 4: Kết luận Các vẽ, đồ thị ( ghi rõ loại kích thước vẽ ): Bản vẽ tổng thể robot (1A0) Bản vẽ sơ đồ động (1A0) Bản vẽ sơ đồ mạch điện (1A0) Bản vẽ lưu đồ thuật toán (1A0) Bản vẽ cụm chi tiết (1A0) Họ tên người hướng dẫn: TS Lê Hoài Nam Ngày giao nhiệm vụ đồ án: …/…/2019 Ngày hoàn thành đồ án: …/…/2019 Đà nẵng, ngày 16 tháng 12 năm 2019 Trưởng Bộ môn: Người hưỡng dẫn TS Lê Hồi Nam LỜI NĨI ĐẦU Ngày nay, với ứng dụng khoa học kỹ thuật tiên tiến, giới ngày thay đổi, văn minh đại Sự phát triển kỹ thuật điện tử tạo hàng loạt thiết bị với đặc điểm bật xác cao, tốc độ nhanh, tính tự động hóa cao, yếu tố cần thiết góp phần cho hoạt động người đạt hiệu Có thể nói robot mang tới cho sống người sống mới, cách trải nghiệm sống đơi cịn người bạn Những hãng robot từ nước tiếng giới từ Đức, Nhật Bản, Nga, Hoa Kỳ ngày khẳng định diện robot phần không thiếu sống tương lai phía trước Nó xuất tất lĩnh vực từ khoa học vĩ mô vi mô ngày đa dạng C C Với xu phát triển thời đại kiến thức quý báu thầy cô truyền đạt qua năm học trường Nhóm tác giả định lựa chọn đề tài “Thiết kế chế tạo robot bậc tự do” R L T Sau thời gian học tập rèn luyện, với hướng dẫn bảo tận tình thầy U D giáo TS Lê Hoài Nam, trợ giúp quý thầy mơn tài liệu có liên quan, mà em hồn thành xong đề tài Đề tài hồn thành xong, khơng thể tránh nhiều thiếu sót mong q thầy giáo thơng cảm bảo thêm để đề tài phát triển ứng dụng rộng rãi thực tế Em xin chân thành gửi lời cảm ơn đến quý thầy cô! i CAM ĐOAN Chúng em xin cam đoan đồ án tuân thủ tốt quy định liêm học thuật: Không bịa đặt, đưa thông tin sai lệch so với nguồn trích dẫn Khơng ngụy tạo số liệu q trình khảo sát, thí nghiệm, thực hành, thực tập hoạt động học thuật khác Không sử dụng hình thức gian dối việc trình bày, thể hoạt động học thuật kết từ q trình học thuật Khơng đạo văn, sử dụng từ ngữ, cách diễn đạt người khác thể mình, trình bày, chép, dịch đoạn, nêu ý tưởng người khác mà không trích dẫn C C Khơng tự đạo văn, sử dụng lại thơng tin nghiên cứu mà khơng có trình dẫn phân mảnh thơng tin kết nghiên cứu để cơng bố nhiều ấn phẩm R L T U D Sinh viên thực Nguyễn Đình Danh ii Trần Văn Đổ MỤC LỤC TÓM TẮT ĐỒ ÁN TỐT NGHIỆP NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP LỜI NÓI ĐẦU i CAM ĐOAN ii MỤC LỤC iii DANH SÁCH CÁC BẢNG, HÌNH VẼ vi MỞ ĐẦU Chương 1: TỔNG QUAN VỀ ĐỀ TÀI 1.1 C C Giới thiệu cánh tay robot R L T 1.1.1 Lịch sử hình thành phát triển 1.1.2 Xu hướng phát triển 1.1.3 Cấu trúc robot công nghiệp 1.2 U D Ứng dụng cánh tay robot 1.2.1 Ứng dụng nông nghiệp 1.2.2 Ứng dụng robot y tế 1.2.3 Ứng dụng robot công nghiêp .4 1.3 Mục tiêu đề tài 1.3.1 Đặt vấn đề .5 1.3.2 Cấu trúc dự án: .5 Chương 2: THIẾT KÊ CƠ KHÍ .7 2.1 Lựa chọn phương án .7 2.1.1 Yêu cầu công nghệ 2.1.2 Sức nâng tay máy 2.1.3 Số bậc tự phần công tác ( DOF: Degrees Of Freedom) 2.1.4 Độ xác vị trí 2.1.5 Tốc độ dịch chuyển .8 iii 2.1.6 Xác định vùng làm việc robot phương án thiết kế mặt động học 2.2 Hệ thống dẫn động sức bền kết cấu 2.2.1 Mơ hình dẫn động tỷ số truyền 2.2.2 Nguyên tắc chung tính chọn hệ dẫn động 11 2.2.3 Tính tốn phương trình động lực học 11 2.2.4 Phân tích lực trọng trường tác dụng lên trục 16 2.2.5 Chọn động 18 2.3 Chế tạo mơ hình 20 2.3.1 Tấm đế 20 2.3.2 Khâu 21 2.3.3 Khâu 21 C C 2.3.4 Khâu khâu 22 R L T 2.3.5 Khâu khâu 23 2.3.6 Mơ hình thiết kế tổng thể 23 2.4 U D Chọn tay kẹp .24 2.4.1 Giới thiệu tay gắp mềm(soft grippers) 24 2.4.2 Thiết kế .25 2.4.3 Các bước chế tạo tay gắp mềm 26 Chương 3: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN ROBOT 28 3.1 Các thành phần hệ thống 28 3.1.1 Bộ điều khiển 28 3.1.2 Driver điều khiển động 30 3.1.3 Động bước .38 3.2 Tính tốn thuật tốn điều khiển 39 3.2.1 Động học thuận robot 39 3.2.2 Động học ngược robot 42 Chương 4: KẾT LUẬN 46 4.1 Kết 46 iv 4.2 Đánh giá 47 4.2.1 Phần khí 47 4.2.2 Phần điện tử 47 4.2.3 Phần điều khiển 47 4.3 Kết luận .47 TÀI LIỆU THAM KHẢO .48 PHỤ LỤC 1: CHƯƠNG TRÌNH ARDUINO .49 PHỤ LỤC 2: CHƯƠNG TRÌNH PYTHON 55 C C R L T U D v DANH SÁCH CÁC BẢNG, HÌNH VẼ Hình 1.1: Robot thu hoạch tự động Hình 1.2: Cánh tay robot hỗ trợ y tế Hình 1.3: Các cánh tay robot lắp ráp sản xuất ô tô Hình 1.4: Các thành phần hệ thống Hình 2.1: Vùng làm việc robot Hình 2.2: Bộ truyền đai khâu .10 Hình 2.3: Bộ truyện xích khâu 10 Hình 2.4: Bộ truyền đai khâu .11 Hình 2.5 : Hình biểu diễn tính động học .12 C C Hình 2.6: Sơ đồ lực đặt vào đầu trục khớp trạng thái giữ 17 R L T Hình 2.7: Động bước khâu 18 Hình 2.8: Động bước khâu 19 Hình 2.9: Động bước khâu 19 U D Hình 2.10: Động bước khâu 19 Hình 2.11: Động bước khâu 20 Hình 2.12: Động bước khâu 20 Hình 2.13: Tấm đế 21 Hình 2.14: Khâu 21 Hình 2.15: Khâu 22 Hình 2.16: Khâu & 22 Hình 2.17: Khâu & 23 Hình 2.18: Mơ hình hồn chỉnh .23 Hình 2.19: Tay gắp mềm gắp đồ ăn hãng Soft Robotics 24 Hình 2.20: Tay gắp mềm .25 Hình 2.21: Khn ngón tay mềm 26 Hình 2.22: Khn tay gắp 26 Hình 2.23: Tay gắp mềm sau khí chế tạo 27 Hình 3.1: Sơ đồ nguyên lý điều khiển 28 vi Hình 3.2: Sơ đồ chân Arduino Mega2560 .29 Hình 3.3: Driver điều khiển động .30 Hình 3.4: Các cổng kết nối driver 31 Hình 3.5: Các kiểu kết nối Driver 33 Hình 3.6: Các cơng tắc cài đặt thông số cho Driver 33 Hình 3.7: Biểu đồ trình tự xung điều khiển 35 Hình 3.8: Các cơng tắc cài đặt thơng số cho DM320T 36 Hình 3.9: Biểu đồ điều khiển Driver DM320T .38 Hình 3.10: Động bước 38 Hình 3.11: Hệ tọa độ mơ hình 39 Hình 3.12: Hình chiếu tính góc khớp 42 Hình 3.13: Hình chiếu tính góc thứ 43 C C Hình 3.14: Hình chiếu tính góc 44 R L T Hình 4.1: Robot bậc tự 46 U D Bảng 2.1: Bảng thông số khâu 1,2 12 Bảng 2.2: Khối lượng sơ khâu 16 Bảng 3.1: Thống số kĩ thuật board Arduino Mega2560 .29 Bảng 3.2: Bảng thông số kĩ thuật Driver DM542T 31 Bảng 3.3: Cổng kêt nối tín hiệu .32 Bảng 3.4: Cổng kết nối nguồn động 32 Bảng 3.5: Bảng thiết lập thông số công tắc 5,6,7,8 .34 Bảng 3.6: Bảng thiết lập thông số công tắc 1,2,3 35 Bảng 3.7: Bảng thông số kĩ thuật Driver DM320T 36 Bảng 3.8: Bảng thiết lập thông số công tắc 5,6,7 37 Bảng 3.9: Bảng thiết lập thông số công tắc 1,2,3 37 Bảng 3.10: Bảng thông số D-H .40 Bảng 4.1:Bảng thông số mô hình .46 vii Thiết kế chế tạo cánh tay robot bậc tự Động học ngược robot 3.2.2 Phương trình động học ngược tốn tìm quan hệ vị trí cuối gọc quay khớp robot Bài toán cho phép người điều khiển cần nhập hướng vị trí mong muốn, phương trình tính tốn để đưa góc điều khiển robot đến vị trí u cầu - Tính góc khớp C C R L T U D Hình 3.12: Hình chiếu tính góc khớp Ta có ma trận mơ ta hướng vị trí khâu chấp hành cuối có dạng là: 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥 𝑛 𝑜𝑦 𝑎𝑦 𝑝𝑦 T6 ] =[ 𝑦 𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧 0 ( 3.3 ) Với vector mô tả hướng khâu chấp hành cuối vector n, o, a Gọi vector 06 N k vector hướng khâu chấp hành cuối theo trục z  Nk 0 𝑎𝑥 = a = [ 𝑎𝑦 ] 𝑎𝑧 Gọi 64 Pk vector từ khâu đến khâu ta có: Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 42 Thiết kế chế tạo cánh tay robot bậc tự 6 Pk = N k d6 ∗ 𝑎𝑥 x d6 = [ d6 ∗ 𝑎𝑦 ] d6 ∗ 𝑎𝑧 Tương tự ta gọi 06 Pk 04 Pk vector từ khâu gốc đến khâu cuối từ khâu gốc đến khâu thứ P𝑥 Với Pk = [ 𝑃𝑦 ] 𝑃𝑧 Như hình vẽ ta thấy : P𝑥 − d6 ∗ 𝑎𝑥 Pk = Pk - Pk = [ 𝑃𝑦 − d6 ∗ 𝑎𝑦 ] 𝑃𝑧 − d6 ∗ 𝑎𝑧 6 Vậy từ hình ta dễ dàng tính được: C C  Góc θ1 = atan2(𝑃𝑦 − d6 ∗ 𝑎𝑦 , P𝑥 − d6 ∗ 𝑎𝑥 ) - R L T Tính góc θ3 a2 U D d4 Hình 3.13: Hình chiếu tính góc thứ Tương tự gọi vector 10 Pk 14 Pk vector từ gốc đến khâu từ khâu đến khâu Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 43 Thiết kế chế tạo cánh tay robot bậc tự Với Ta có Pk 0 P𝑥 − d6 ∗ 𝑎𝑥 = [ 𝑃𝑦 − d6 ∗ 𝑎𝑦 ] biết 𝑃𝑧 − d6 ∗ 𝑎𝑧 𝑎1 ∗ cos(𝜃1) Pk = [ 𝑎1 ∗ sin(𝜃1)] 𝑑1 P𝑥 − d6 ∗ 𝑎𝑥 − 𝑎1 ∗ cos(𝜃1) Từ ta tính vector Pk = Pk - Pk = [ 𝑃𝑦 − d6 ∗ 𝑎𝑦 − 𝑎1 ∗ sin(𝜃1)] 𝑃𝑧 − d6 ∗ 𝑎𝑧 − 𝑑1 4 Gọi độ dài vector 14 Pk P14k0 ta có: | 14 Pk | = P14k0 = √( P𝑥 − d6 ∗ 𝑎𝑥 − 𝑎1 ∗ cos(𝜃1))2 + (𝑃𝑦 − d6 ∗ 𝑎𝑦 − 𝑎1 ∗ sin(𝜃1)) + (𝑃 − d6 ∗ 𝑎𝑧 − 𝑑1 )2 C C Qua ta xác định góc Ø: a  d  P14 2ad Cos(Ø) = Vậy góc θ3 = - 𝜋 2 R L T 𝑧 a  d  P142  Ø = arccos( ) 2ad U D –Ø Tính góc θ2 Hình 3.14: Hình chiếu tính góc Ta có vector 14 Pk tính Vậy ta cần tính 14 Pk1 Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 44 Thiết kế chế tạo cánh tay robot bậc tự 𝑐𝑜𝑠(𝜃1) 𝑠𝑖𝑛(𝜃1) 𝑠𝑖𝑛(𝜃1) −𝑐𝑜𝑠(𝜃1) Tacó: 04 T = A1=[ 0 0 𝑐𝑜𝑠(𝜃1) 𝑠𝑖𝑛(𝜃1) Vậy 10 R = [ 𝑠𝑖𝑛(𝜃1) −𝑐𝑜𝑠(𝜃1)] 𝑎1 ∗ 𝑐𝑜𝑠(𝜃1) 𝑎1 ∗ 𝑠𝑖𝑛(𝜃1) ] 𝑑1 Suy ra: 14 Pk1 = 10 R  14 Pk Vậy ta tính góc: β = atan2( 14 Pk y , 14 Pk x ) β = arccos( a2  d  14 Pk1 2a2 d Vậy góc θ2 = β + β - ) 𝜋 C C Tính góc khớp thứ θ4,θ5 θ6 R L T Để tìm góc quay tiếp ta sử dụng ma trận quay 64 R Ta có: 64 R = 40 R 1 x 60 R Ngoài : 64 R = Rotx(θ4) x Roty(θ5) x Rotz(θ6) −𝐶4𝐶5𝐶6 − 𝑆4𝑆6 = [−𝑆4𝐶5𝐶6 + 𝐶4𝑆6 𝑆5𝐶6 U D 𝐶4𝐶5𝐶6 − 𝑆4𝐶6 𝑆4𝐶5𝑆6 + 𝐶4𝐶6 −𝑆5𝑆6 −𝐶4𝑆5 −𝑆4𝑆5 ] −𝐶5 Tử dễ dàng ta tính được: θ4 = atan2(-S4S5, -C4S5); θ5 = atan2(-C5, √1 − (−𝐶5)2 ); θ6 = Atan2(-(-S5S6), S5S6); Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 45 Thiết kế chế tạo cánh tay robot bậc tự Chương 4: KẾT LUẬN 4.1 Kết Hệ điều khiển đáp ứng yêu cầu toán điều khiển vị trí, chạy ổn định theo hành trình thiết lập, thực tất chức thiết lập Sau nhiều tháng làm việc thiết kế, cịn nhiều thiếu sót, bước đầu nhóm hồn thành mặt nội dung toàn đề tài theo yêu cầu ban đầu đặt C C R L T U D Hình 4.1: Robot bậc tự Các thơng số kĩ thuật chính: Bảng 4.1:Bảng thơng số mơ hình Số thứ tự Đặc tính Kí hiệu Thơng số Khối lượng robot mr 12 kg Tầm với tối đa H 62.9 cm Số trục quay Ax Khối lượng tủ điện mt 25 kg Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 46 Thiết kế chế tạo cánh tay robot bậc tự 4.2 Đánh giá 4.2.1 Phần khí Robot chuyển động tốt, điều khiển mượt mà, gặp cố Kiểu dáng chắn song bên cạnh cịn nhiểu điểm cần khắc phục liên kết khâu khớp, tính bền vững để hoạt động lâu dài, độ sai lệch hoạt động 4.2.2 Phần điện tử Các thiết bị mạch điện thiết kế chắn, có hộp chống va đập cách điện để tránh chập cháy tốt Các thành phần bảo vệ nguồn driver cho động lắp đặt 4.2.3 Phần điều khiển Đã điều khiển cánh tay robot hoạt động ổn định, xử lý tốt phần dạy học Bên cạnh cịn thiếu sót mặt xử lý ảnh, nhóm chưa xây dựng thể phần xử lý ảnh robot C C 4.3 Kết luận R L T Sau nhiều tháng liên tục tìm tịi phát triển nhóm tác giả hồn thành tiến độ đặt ra, đáp ứng yêu cầu nhiệm vụ thiết kế đồ án Cùng với việc áp dụng nghiên cứu mặt lí thuyết kết hợp với thiết kế thực tiễn nhóm có thêm nhiều kiến thức điều khiển tự động, robot, kết cấu khí Về mặt lí thuyết tìm hiểu quy trình thiết kế Robot việc U D Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hồi Nam 47 Thiết kế chế tạo cánh tay robot bậc tự TÀI LIỆU THAM KHẢO [1] Robot công nghiệp, Phạm Đăng Phước, Nhà xuất Xây Dựng, 2007 [2] Nhập Môn Robot Công Nghiệp Tập - Ts Lê Hoài Quốc [3] Cơ Sở Nghiên Cứu _ Sáng Tạo Robot - Trần Thế San [4] Modelling and Simulation of KUKA KR6 Robot Manipulator, NASR ABDO ALI MOHAMMED, 2017 [5] https://github.com/Chris-Annin/AR2 (truy cập lần cuối 28/11/2019) [6] https://www.anninrobotics.com/(truy cập lần cuối 28/11/2019) C C R L T U D Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 48 Thiết kế chế tạo cánh tay robot bậc tự PHỤ LỤC 1: CHƯƠNG TRÌNH ARDUINO #include Servo servo0; Servo servo1; Servo servo2; Servo servo3; Servo servo4; Servo servo5; Servo servo6; Servo servo7; C C String inData; String function; R L T char WayPt[101][50]; int WayPtDel; const int J1stepPin = 2; const int J1dirPin = 3; const int J2stepPin = 4; U D const int J2dirPin = 5; const int J3stepPin = 6; const int J3dirPin = 7; const int J4stepPin = 8; const int J4dirPin = 9; const int J5stepPin = 10; const int J5dirPin = 11; const int J6stepPin = 12; const int J6dirPin = 13; const int TRstepPin = 20; const int TRdirPin = 21; const int J1calPin = 14; const int J2calPin = 15; Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 49 Thiết kế chế tạo cánh tay robot bậc tự const int J3calPin = 16; const int J4calPin = 17; const int J5calPin = 18; const int J6calPin = 19; ///////khâu J1 ///////// if (J1dir == && J1rotdir == 1) { digitalWrite(J1dirPin, LOW); } else if (J1dir == && J1rotdir == 0) { digitalWrite(J1dirPin, HIGH); } else if (J1dir == && J1rotdir == 1) { digitalWrite(J1dirPin, HIGH); } else if (J1dir == && J1rotdir == 0) { digitalWrite(J1dirPin, LOW); } /////// khâu J2 ///////// if (J2dir == && J2rotdir == 1) { digitalWrite(J2dirPin, LOW); } else if (J2dir == && J2rotdir == 0) { digitalWrite(J2dirPin, HIGH); } else if (J2dir == && J2rotdir == 1) { digitalWrite(J2dirPin, HIGH); } else if (J2dir == && J2rotdir == 0) { digitalWrite(J2dirPin, LOW); } /////// khâu J3 ///////// if (J3dir == && J3rotdir == 1) C C R L T U D Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 50 Thiết kế chế tạo cánh tay robot bậc tự { digitalWrite(J3dirPin, LOW); } else if (J3dir == && J3rotdir == 0) { digitalWrite(J3dirPin, HIGH); } else if (J3dir == && J3rotdir == 1) { digitalWrite(J3dirPin, HIGH); } else if (J3dir == && J3rotdir == 0) { digitalWrite(J3dirPin, LOW); } ///////khâu J4 ///////// if (J4dir == && J4rotdir == 1) { digitalWrite(J4dirPin, LOW); } else if (J4dir == && J4rotdir == 0) { digitalWrite(J4dirPin, HIGH); } else if (J4dir == && J4rotdir == 1) { digitalWrite(J4dirPin, HIGH); } else if (J4dir == && J4rotdir == 0) { digitalWrite(J4dirPin, LOW); } ///////khâu J5 ///////// if (J5dir == && J5rotdir == 1) { digitalWrite(J5dirPin, LOW); } else if (J5dir == && J5rotdir == 0) { digitalWrite(J5dirPin, HIGH); } C C R L T U D Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 51 Thiết kế chế tạo cánh tay robot bậc tự else if (J5dir == && J5rotdir == 1) { digitalWrite(J5dirPin, HIGH); } else if (J5dir == && J5rotdir == 0) { digitalWrite(J5dirPin, LOW); } ///////khâu J6 ///////// if (J6dir == && J6rotdir == 1) { digitalWrite(J6dirPin, LOW); } else if (J6dir == && J6rotdir == 0) { digitalWrite(J6dirPin, HIGH); } else if (J6dir == && J6rotdir == 1) { digitalWrite(J6dirPin, HIGH); } else if (J6dir == && J6rotdir == 0) { digitalWrite(J6dirPin, LOW); } void loop() { //test led if (digitalRead(J1calPin) == HIGH || digitalRead(J2calPin) == HIGH || digitalRead(J3calPin) == HIGH || digitalRead(J4calPin) == HIGH || digitalRead(J5calPin) == HIGH || digitalRead(J6calPin) == HIGH) { digitalWrite(J6dirPin, HIGH); } else { digitalWrite(J6dirPin, LOW); } C C R L T U D //start loop WayPtDel = 0; while (Serial.available() > or WayPtDel == 1) { Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hồi Nam 52 Thiết kế chế tạo cánh tay robot bậc tự char recieved = Serial.read(); inData += recieved; // Process message when new line character is recieved if (recieved == '\n') { String function = inData.substring(0, 2); while (digitalRead(J1calPin) == LOW && J1done < J1step || digitalRead(J2calPin) == LOW && J2done < J2step || digitalRead(J3calPin) == LOW && J3done < J3step || digitalRead(J4calPin) == LOW && J4done < J4step || digitalRead(J5calPin) == LOW && J5done < J5step || digitalRead(J6calPin) == LOW && J6done < J6step) { if (J1done < J1step && (digitalRead(J1calPin) == LOW)) { digitalWrite(J1stepPin, LOW); } delayMicroseconds(5); if (J1done < J1step && (digitalRead(J1calPin) == LOW)) { digitalWrite(J1stepPin, HIGH); J1done = ++J1done; } delayMicroseconds(5); if (J2done < J2step && (digitalRead(J2calPin) == LOW)) { digitalWrite(J2stepPin, LOW); } delayMicroseconds(5); if (J2done < J2step && (digitalRead(J2calPin) == LOW)) { digitalWrite(J2stepPin, HIGH); J2done = ++J2done; } delayMicroseconds(5); if (J3done < J3step && (digitalRead(J3calPin) == LOW)) { digitalWrite(J3stepPin, LOW); } delayMicroseconds(5); if (J3done < J3step && (digitalRead(J3calPin) == LOW)) { digitalWrite(J3stepPin, HIGH); J3done = ++J3done; C C R L T U D Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hoài Nam 53 Thiết kế chế tạo cánh tay robot bậc tự } delayMicroseconds(5); if (J4done < J4step && (digitalRead(J4calPin) == LOW)) { digitalWrite(J4stepPin, LOW); } delayMicroseconds(5); if (J4done < J4step && (digitalRead(J4calPin) == LOW)) { digitalWrite(J4stepPin, HIGH); J4done = ++J4done; } delayMicroseconds(5); if (J5done < J5step && (digitalRead(J5calPin) == LOW)) { digitalWrite(J5stepPin, LOW); } delayMicroseconds(5); if (J5done < J5step && (digitalRead(J5calPin) == LOW)) { digitalWrite(J5stepPin, HIGH); J5done = ++J5done;; } delayMicroseconds(5); if (J6done < J6step && (digitalRead(J6calPin) == LOW)) { digitalWrite(J6stepPin, LOW); } delayMicroseconds(5); if (J6done < J6step && (digitalRead(J6calPin) == LOW)) { digitalWrite(J6stepPin, HIGH); J6done = ++J6done; } C C R L T U D Sinh viên thực hiện: Nguyễn Đình Danh Trần Văn Đổ Hướng dẫn: TS Lê Hồi Nam 54 Thiết kế chế tạo cánh tay robot bậc tự PHỤ LỤC 2: CHƯƠNG TRÌNH PYTHON savePosData() CalcFwdKin() else: almStatusLab.config(text="J1 AXIS LIMIT", bg = "red") almStatusLab2.config(text="J1 AXIS LIMIT", bg = "red") DisplaySteps() def J2jogNeg(): global JogStepsStat global J2StepCur C C global J2AngCur R L T global xboxUse if xboxUse != 1: U D almStatusLab.config(text="SYSTEM READY", bg = "cornflowerblue") almStatusLab2.config(text="SYSTEM READY", bg = "cornflowerblue") Speed = speedEntryField.get() ACCdur = ACCdurField.get() ACCspd = ACCspeedField.get() DECdur = DECdurField.get() DECspd = DECspeedField.get() J2Degs = float(J2jogDegsEntryField.get()) if JogStepsStat.get() == 0: J2jogSteps = int(J2Degs/J2DegPerStep) else: #switch from degs to steps J2jogSteps = J2Degs J2Degs = J2Degs*J2DegPerStep if (J2Degs