NGHIÊN cứu, THIẾT kế, CHẾ tạo CÁNH TAY ROBOT 4 bậc tự DO điều KHIỂN BẰNG màn HÌNH cảm ỨNG

Họ tên sinh viên: Tào Viết Bảo TÊN ĐỀ TÀI: NGHIÊN CỨU, THIẾT KẾ, CHẾ TẠO CÁNH TAY ROBOT BẬC Lê Trần Thảo My ĐIỀU KHIỂN BẰNG MÀN HÌNH CẢM ỨNG 2022 Phụ lục 01 ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT KHOA ĐIỆN – ĐIỆN TỬ ĐỒ ÁN TỐT NGHIỆP ĐẠI HỌC NGÀNH: CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ VIỄN THÔNG CHUYÊN NGÀNH: ĐIỆN TỬ MÁY TÍNH – MẠNG TRUYỀN THƠNG ĐỀ TÀI: NGHIÊN CỨU, THIẾT KẾ, CHẾ TẠO CÁNH TAY ROBOT BẬC TỰ DO ĐIỀU KHIỂN BẰNG MÀN HÌNH CẢM ỨNG Người hướng dẫn: ThS Phạm Văn Phát Sinh viên thực hiện: Tào Viết Bảo Mã sinh viên: 1811505410201 Lớp: 18DT2 Sinh viên thực hiện: Lê Trần Thảo My Mã sinh viên: 1811505410224 Lớp: 18DT2 Đà Nẵng, 06/2022 Phụ lục 01 ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT KHOA ĐIỆN – ĐIỆN TỬ ĐỒ ÁN TỐT NGHIỆP ĐẠI HỌC NGÀNH: CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ VIỄN THƠNG CHUN NGÀNH: ĐIỆN TỬ MÁY TÍNH – MẠNG TRUYỀN THÔNG ĐỀ TÀI: NGHIÊN CỨU, THIẾT KẾ, CHẾ TẠO CÁNH TAY ROBOT BẬC TỰ DO ĐIỀU KHIỂN BẰNG MÀN HÌNH CẢM ỨNG Người hướng dẫn: ThS Phạm Văn Phát Sinh viên thực hiện: Tào Viết Bảo Mã sinh viên: 1811505410201 Lớp: 18DT2 Sinh viên thực hiện: Lê Trần Thảo My Mã sinh viên: 1811505410224 Lớp: 18DT2 Đà Nẵng, 06/2022 TÓM TẮT Tên đề tài: Nghiên cứu, thiết kế, chế tạo cánh tay robot bậc tự điều khiển hình cảm ứng Sinh viên thực hiện: Tào Viết Bảo Mã SV: 1811505410201 Lớp: 18DT2 Sinh viên thực hiện: Lê Trần Thảo My Mã SV: 1811505410224 Lớp: 18DT2 Cánh tay robot loại robot phổ biến sản xuất công nghiệp Chúng có vai trị đặc biệt quan trọng ngày xuất nhiều sản xuất, lắp ráp, chế tạo Được sử dụng nhà máy, công xưởng đại giới Với đề tài “Nghiên cứu, thiết kế, chế tạo cánh tay robot bậc tự điều khiển hình cảm ứng” nhóm chúng em nghiên cứu để nắm rõ cấu tạo cách thức hoạt động cánh tay robot từ đưa giải pháp mơ hình cánh tay robot bậc tiến hành thiết kế, gia công, lắp ráp mơ hình, lựa chọn động phù hợp Đồng thời, áp dụng kiến thức chuyên ngành điện tử tìm kiếm giải pháp tối ưu cho việc điều khiển cánh tay robot trở nên đơn giản, dễ dàng Bên cạnh đó, nhóm em cịn đưa giải pháp điều khiển cánh tay robot hình cảm ứng với giao diện đơn giản, tất thao tác với cánh tay robot người dùng thao tác trực tiếp hình cảm ứng từ việc điều khiển cánh tay robot thiết lập vị trí cho chế độ tự động robot Kỹ thuật Robot sản xuất công nghiệp phát triển nên đòi hỏi khả xử lý liên quan đến trí tuệ nhân tạo hệ thống điều khiển thơng minh Học tập, thực hành với mơ hình cánh tay robot, sinh viên phát triển kiến thức kỹ xác định vấn đề, thiết kế, triển khai, vận hành, sử dụng khai thác hệ thống, sản phẩm ngành Kỹ thuật Robot Với mơ hình cánh tay robot nhóm chúng em cung cấp giải pháp tốt lĩnh vực giáo dục, phù hợp cho học sinh cấp trung học, phục vụ tốt cho đối tượng sinh viên đại học để nghiên cứu thực hành TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT BỘ MÔN: ĐIỆN TỬ - VIỄN THÔNG 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 Giảng viên hướng dẫn: ThS Phạm Văn Phát Sinh viên thực hiện: Tào Viết Bảo Mã SV: 1811505410201 Lê Trần Thảo My Mã SV: 1811505410224 Tên đề tài: Nghiên cứu, thiết kế, chế tạo cánh tay robot bậc tự điều khiển hình cảm ứng Các số liệu, tài liệu ban đầu: - Thiết kế cánh tay robot bậc tự do: + Bậc 1, bậc 2, bậc sử dụng động bước giảm tốc size 42 tỉ số truyền 1:14 + Bậc sử dụng động servo MG995 - Mạch Điều Khiển Động Cơ Bước TB6600 sử dụng IC TB6600HQ/HG, dùng cho loại động bước: 42 có dịng tải từ – 42VDC/4A - Phần tay gắp sử dụng động servo MG90S Giao tiếp hình TFT SPI 3.5’ với ATMEGA 2560 sử dụng hình cảm ứng Mạch Volume xoay Rotary Encoder 360 độ không giới hạn số vịng quay, encoder đưa xung vng 90 độ gọi phase A B, xung từ encoder đưa dùng để nhận biết chiều quay, tốc độ quay, vị trí, module cung cấp ngõ cho phase ngõ dạng nút nhấn Nội dung đồ án:  Chương 1: Tổng quan robot -  Chương 2: Hệ điều khiển cho robot -  Tổng quan robot Phân loại robot Ứng dụng robot đời sống cơng nghiệp Các thành phân robot Hệ truyền động cho robot Hệ điều khiển cho robot Hệ cảm biến- Giao tiếp- truyền thông Chương 3: Thiết kế mơ hình robot bậc tự - Sơ đồ tổng thể cánh tay robot - Thiết kế hệ cho robot - Thiết kế khối điều khiển cho robot Thiết kế module điều khiển giao tiếp Lập trình điều khiển  Chương 4: Lập trình – điều khiển/Thi cơng- đánh giá mơ hình - Lập trình điều khiển robot Thi cơng đánh giá mơ hình - Phân tích, đánh giá kết thực nghiệm Kết luận Các sản phẩm dự kiến - Báo cáo tổng hợp đồ án tốt nghiệp - Chương trình điều khiển - Mơ hình cánh tay Robot bậc điều khiển qua hình cảm ứng Ngày giao đồ án: 14/02/2022 Ngày nộp đồ án: 04/06/2022 Trưởng Bộ môn Đà Nẵng, ngày tháng năm 20… Người hướng dẫn ThS Phạm Văn Phát LỜI NÓI ĐẦU Theo xu hướng phát triển cơng nghiệp hóa – đại hóa ngày cao, nhu cầu tìm đến thiết bị đảm nhiệm việc cải thiện suất chất lượng sản phẩm ngày địi hỏi có xác tối ưu hóa Do đó, dây chuyền tự động có tính ứng dụng cao hình thành phát triển rộng rãi Robot dần thay người, tạo tự động hóa q trình sản xuất Giúp người giảm bớt sức lao động chân tay thủ cơng vốn có, số cơng việc nặng nhọc nguy hiểm Chính vậy, thiết bị robot đời ngày sử dụng nhiều vào hệ thống sản xuất công nghiệp tự động linh hoạt Qua lợi ích tiềm phát triển vượt bậc đó, doanh nghiệp nước ta ngày trọng vào việc nghiên cứu, đầu tư vào thi công nghệ Sáng tạo robot Việt nam-Robocon, robottics quốc tế, Ngoài ra, cánh tay robot đáp ứng nhu cầu mặt giải trí chăm sóc người Hơn nữa, góp phần khơng nhỏ vào việc giáo dục, làm mơ hình thực tế để học sinh, sinh viên thực hành dễ dàng, buổi học sinh động khơi nguồn sáng tạo họ vào việc tiếp cận sớm với công nghệ đại Đề tài: “Nghiên cứu, thiết kế, chế tạo cánh tay robot bậc tự điều khiển hình cảm ứng” khơng đảm bảo tính cấp thiết, quan trọng mà xã hội đặt mà cịn giúp em có hội vận dụng kiến thức chuyên ngành học trường học hỏi nhiều kiến thức để áp dụng giải vấn đề cụ thể Bên cạnh đó, việc sử dụng hình cảm ứng đề tài cho phép người sử dụng dễ dàng thao tác điều khiển trực tiếp cánh tay robot từ hình cảm ứng với giao diện thiết kế cách đơn giản, dễ hiểu, không cần nhiều thao tác để thiết lập vị trí, điều khiển cánh tay robot theo mong muốn Trong thời gian làm đồ án vừa qua, với cố gắng nhóm, với giúp đỡ tận tình thầy giáo hướng dẫn, nhóm em hồn thành xong Đồ án Tốt nghiệp khơng thể tránh sai sót Một lần nữa, nhóm em xin gửi lời cảm ơn chân thành đến thầy Phạm Văn Phát, quý thầy cô môn Điện – Điện Tử i LỜI CAM ĐOAN Nhóm em xin cam đoan kết đạt đồ án sản phẩm riêng cá nhân, không chép lại người khác Trong toàn nội dung đồ án, điều trình bày cá nhân tổng hợp từ nhiều nguồn tài liệu Tất tài liệu tham khảo có xuất xứ rõ ràng trích dẫn hợp pháp Nhóm em xin hồn tồn chịu trách nhiệm chịu hình thức kỷ luật theo quy định cho lời cam đoan Đã bổ sung, cập nhật theo yêu cầu Giảng viên phản biện Hội đồng chấm Đồ án tốt nghiệp họp ngày 17, 18/6/2022 Sinh viên thực Sinh viên thực Tào Viết Bảo Lê Trần Thảo My ii MỤC LỤC Nhận xét người hướng dẫn Nhận xét người phản biện Tóm tắt Nhiệm vụ đồ án Lời nói đầu i Lời cam đoan ii Mục lục iii Danh sách bảng, hìnhvẽ vi Danh sach chữ viết tắt x Trang MỞ ĐẦU CHƯƠNG 1: TỔNG QUAN VỀ ROBOT 1.1 Lịch sử đời Robot [3] 1.2 Phân loại 1.2.1 Phân loại theo không gian làm việc [2] 1.2.2 Phân loại theo điều khiển [4] 1.3 Các ứng dụng Robot CHƯƠNG 2: HỆ ĐIỀU KHIỂN CHO ROBOT .10 2.1 Cấu trúc robot 10 2.1.1 Các thành phần robot [1] .10 2.1.2 Kết cấu tay máy 10 2.1.3 Bậc tự tay máy [1] 11 2.2 Hệ truyền động cho robot 11 2.2.1 Hệ thống chấp hành 11 2.2.2 Hệ thống điều khiển (Controller) 12 2.2.3 Cảm biến 12 2.2.4 Phần công tác .13 iii 2.3 Hệ thống truyền dẫn động 13 2.3.1 Hệ truyền động điều khiển điện [2] 13 2.3.2 Hệ truyền động điều khiển thủy lực 15 2.3.3 Hệ truyền động khí nén .16 2.4 Hệ thống điều khiển 16 2.5 Giao tiếp – truyền thông 17 CHƯƠNG 3: THIẾT KẾ MƠ HÌNH ROBOT BẬC TỰ DO 20 3.1 Thiết kế hệ thống khí .20 3.1.1 Mơ hình cánh tay robot 20 3.1.2 Thiết kế khâu robot .22 3.2 Thiết kế hệ cho robot .33 3.2.1 Động sử dụng khớp .33 3.2.2 Động dùng cấu gắp đặt .35 3.3 Thiết kế hình điều khiển 36 3.4 Thiết kế hệ thống điều khiển .38 3.4.1 Sơ đồ khối hệ thống điều khiển 38 3.4.2 Các thành phần hệ thống điều khiển robot 39 3.4.3 Sơ đồ mô tổng quát hệ thống 43 CHƯƠNG 4: LẬP TRÌNH – ĐIỀU KHIỂN THI CƠNG – ĐÁNH GIÁ MƠ HÌNH .44 4.1 Lập trình điều khiển robot 44 4.1.1 Lưu đồ thuật tốn tồn hệ thống 44 4.1.2 Lưu đồ điều khiển đến vị trí ban đầu 46 4.1.3 Lưu đồ điều khiển tự động 47 4.1.4 Lưu đồ điều khiển khớp tay Encoder 48 4.2 Các phần mềm sử dụng 48 4.2.1 Phần mềm Arduino IDE 48 4.2.2 Phần mềm Solidworks 50 iv 4.3 Thi công đánh giá 51 4.3.1 Thi công phần cứng 51 4.3.2 Kết thi công 56 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 58 TÀI LIỆU THAM KHẢO 59 PHỤ LỤC v digitalWrite(dirPin1, HIGH); counter1 = counter1 + 1000 * (3969 / 289); giatri1 = giatri1 + 1000; } xung1 = giatri1; quay_step_1(); counter1 = 0; tft_bac_1(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(10, 40, 75, 70,5); show_string(bac1, 15, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(235, 155, 300, 220,15); show_string("1000", 232, 175, 3, RED, BLACK, 1); break; } } } } ////////////////////////////////////////bac_2//////////////////////////////////////////////////////// if (is_pressed(85, 80, 150, 145, px, py)) { if (status_2) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(85, 80, 150, 145,15); show_string("Y", 105, 95, 5, RED, BLACK, 1); show_heso_bac_2_3(); status_2 = false; } else { my_lcd.Set_Draw_color(YELLOW); my_lcd.Fill_Round_Rectangle(85, 80, 150, 145,15); show_string("Y", 105, 95, 5, RED, BLACK, 1); my_lcd.Set_Draw_color(WHITE); my_lcd.Fill_Rectangle(10, 155, 300, 220); status_2 = true; } } if (is_pressed(10, 155, 300, 220, px, py) && (!status_2)) { if (is_pressed(10, 155, 75, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(10, 155, 75, 220,15); show_string("1", 35, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { Phụ lục digitalWrite(dirPin2, LOW); counter2 = counter2 + (3969 / 289); giatri2 = giatri2 - 1; } else { digitalWrite(dirPin2, HIGH); counter2 = counter2 + (3969 / 289); giatri2 = giatri2 + 1; } xung2 = giatri2; quay_step_2(); counter2 = 0; tft_bac_2(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(85, 40, 150, 70,5); show_string(bac2, 90, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(10, 155, 75, 220,15); show_string("1", 35, 175, 3, RED, BLACK, 1); break; } } } if (is_pressed(85, 155, 150, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(85, 155, 150, 220,15); show_string("10", 100, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { digitalWrite(dirPin2, LOW); counter2 = counter2 + 10 * (3969 / 289); giatri2 = giatri2 - 10; } else { digitalWrite(dirPin2, HIGH); counter2 = counter2 + 10 * (3969 / 289); giatri2 = giatri2 + 10; } xung2 = giatri2; quay_step_2(); counter2 = 0; tft_bac_2(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(85, 40, 150, 70,5); show_string(bac2, 90, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); Phụ lục my_lcd.Fill_Round_Rectangle(85, 155, 150, 220,15); show_string("10", 100, 175, 3, RED, BLACK, 1); break; } } } if (is_pressed(160, 155, 225, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(160, 155, 225, 220,15); show_string("100", 165, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { digitalWrite(dirPin2, LOW); counter2 = counter2 + 100 * (3969 / 289); giatri2 = giatri2 - 100; } else { digitalWrite(dirPin2, HIGH); counter2 = counter2 + 100 * (3969 / 289); giatri2 = giatri2 + 100; } xung2 = giatri2; quay_step_2(); counter2 = 0; tft_bac_2(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(85, 40, 150, 70,5); show_string(bac2, 90, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(160, 155, 225, 220,15); show_string("100", 165, 175, 3, RED, BLACK, 1); break; } } } if (is_pressed(235, 155, 300, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(235, 155, 300, 220,15); show_string("200", 240 , 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { digitalWrite(dirPin2, LOW); counter2 = counter2 + 200 * (3969 / 289); giatri2 = giatri2 - 200; } else { digitalWrite(dirPin2, HIGH); Phụ lục counter2 = counter2 + 200 * (3969 / 289); giatri2 = giatri2 + 200; } xung2 = giatri2; quay_step_2(); counter2 = 0; tft_bac_2(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(85, 40, 150, 70,5); show_string(bac2, 90, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(235, 155, 300, 220,15); show_string("200", 240, 175, 3, RED, BLACK, 1); break; } } } } ////////////////////////////////////////////////bac_3////////////////////////////////////////////////////////////// if (is_pressed(160, 80, 225, 145, px, py)) { if (status_3) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(160, 80, 225, 145,15); show_string("Z", 180, 95, 5, RED, BLACK, 1); show_heso_bac_2_3(); status_3 = false; } else { my_lcd.Set_Draw_color(YELLOW); my_lcd.Fill_Round_Rectangle(160, 80, 225, 145,15); show_string("Z", 180, 95, 5, RED, BLACK, 1); my_lcd.Set_Draw_color(WHITE); my_lcd.Fill_Rectangle(10, 155, 300, 220); status_3 = true; } } if (is_pressed(10, 155, 300, 220, px, py) && (!status_3)) { if (is_pressed(10, 155, 75, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(10, 155, 75, 220,15); show_string("1", 35, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { digitalWrite(dirPin3, LOW); Phụ lục counter3 = counter3 + (3969 / 289); giatri3 = giatri3 - 1; } else { digitalWrite(dirPin3, HIGH); counter3 = counter3 + (3969 / 289); giatri3 = giatri3 + 1; } xung3 = giatri3; quay_step_3(); counter3 = 0; tft_bac_3(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(160, 40, 225, 70,5); show_string(bac3, 167, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(10, 155, 75, 220,15); show_string("1", 35, 175, 3, RED, BLACK, 1); break; } } } if (is_pressed(85, 155, 150, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(85, 155, 150, 220,15); show_string("10", 100, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { digitalWrite(dirPin3, LOW); counter3 = counter3 + 10 * (3969 / 289); giatri3 = giatri3 - 10; } else { digitalWrite(dirPin3, HIGH); counter3 = counter3 + 10 * (3969 / 289); giatri3 = giatri3 + 10; } xung3 = giatri3; quay_step_3(); counter3 = 0; tft_bac_3(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(160, 40, 225, 70,5); show_string(bac3, 167, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(85, 155, 150, 220,15); Phụ lục show_string("10", 100, 175, 3, RED, BLACK, 1); break; } } } if (is_pressed(160, 155, 225, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(160, 155, 225, 220,15); show_string("100", 165, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { digitalWrite(dirPin3, LOW); counter3 = counter3 + 100 * (3969 / 289); giatri3 = giatri3 - 100; } else { digitalWrite(dirPin3, HIGH); counter3 = counter3 + 100 * (3969 / 289); giatri3 = giatri3 + 100; } xung3 = giatri3; quay_step_3(); counter3 = 0; tft_bac_3(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(160, 40, 225, 70,5); show_string(bac3, 167, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(160, 155, 225, 220,15); show_string("100", 165, 175, 3, RED, BLACK, 1); break; } } } if (is_pressed(235, 155, 300, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(235, 155, 300, 220,15); show_string("200", 240, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { digitalWrite(dirPin3, LOW); counter3 = counter3 + 200 * (3969 / 289); giatri3 = giatri3 - 200; } else { digitalWrite(dirPin3, HIGH); counter3 = counter3 + 200 * (3969 / 289); Phụ lục giatri3 = giatri3 + 200; } xung3 = giatri3; quay_step_3(); counter3 = 0; tft_bac_3(); my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(160, 40, 225, 70,5); show_string(bac3, 167, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(235, 155, 300, 220,15); show_string("200", 240, 175, 3, RED, BLACK, 1); break; } } } } ////////////////////////////////////bac 4//////////////////////////////////// if (is_pressed(235, 80, 300, 145, px, py)) { if (status_4) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(235, 80, 300, 145,15); show_string("R", 255, 95, 5, RED, BLACK, 1); show_heso_bac_4(); status_4 = false; } else { my_lcd.Set_Draw_color(YELLOW); my_lcd.Fill_Round_Rectangle(235, 80, 300, 145,15); show_string("R", 255, 95, 5, RED, BLACK, 1); my_lcd.Set_Draw_color(WHITE); my_lcd.Fill_Rectangle(10, 155, 225, 220); status_4 = true; } } if (is_pressed(10, 155, 225, 220, px, py) && (!status_4)) { if (is_pressed(10, 155, 75, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(10, 155, 75, 220,15); show_string("1", 35, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { counter4 ; if (counter4179) counter4=179; } servo_to.write(counter4); //counter2 = 0; bac4[0] = (counter4 / 100) % 10 + 48; bac4[1] = (counter4 / 10) % 10 + 48; bac4[2] = (counter4 / 1) % 10 + 48; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(235, 40, 300, 70,5); show_string(bac4, 250, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(10, 155, 75, 220,15); show_string("1", 35, 175, 3, RED, BLACK, 1); break; } } } if (is_pressed(85, 155, 150, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(85, 155, 150, 220,15); show_string("10", 100, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { counter4 = counter4-10; if (counter4179) counter4=179; } servo_to.write(counter4); //counter3 = 0; bac4[0] = (counter4 / 100) % 10 + 48; bac4[1] = (counter4 / 10) % 10 + 48; bac4[2] = (counter4 / 1) % 10 + 48; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(235, 40, 300, 70,5); show_string(bac4, 250, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); Phụ lục my_lcd.Fill_Round_Rectangle(85, 155, 150, 220,15); show_string("10", 100, 175, 3, RED, BLACK, 1); break; } } } if (is_pressed(160, 155, 225, 220, px, py)) { my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(160, 155, 225, 220,15); show_string("100", 165, 175, 3, RED, BLACK, 1); for (int i = 0; ; i++) { currentStateCLK = digitalRead(CLK); if (currentStateCLK != lastStateCLK && currentStateCLK == 1) { if (digitalRead(DT) != currentStateCLK) { counter4 = counter4-100; if (counter4179) counter4=179; } servo_to.write(counter4); //counter3 = 0; bac4[0] = (counter4 / 100) % 10 + 48; bac4[1] = (counter4 / 10) % 10 + 48; bac4[2] = (counter4 / 1) % 10 + 48; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(235, 40, 300, 70,5); show_string(bac4, 250, 48, , RED, BLACK, 1); } lastStateCLK = currentStateCLK; int btnState = digitalRead(SW); if (btnState == LOW) { my_lcd.Set_Draw_color(GREEN); my_lcd.Fill_Round_Rectangle(160, 155, 225, 220,15); show_string("100", 165, 175, 3, RED, BLACK, 1); break; } } } } ///////////////////////////////////Gripper/////////////////////////////////////// if (is_pressed(310,80,369,139, px, py)) { if (status_5) { show_picture(gripperOn2,sizeof(gripperOn2)/2,310,80,369,139); for (angle_nho = 90; angle_nho > ; angle_nho ) { servo_nho.write(angle_nho); delay(15); } gripper = 1; status_5 = false; Phụ lục } else { show_picture(gripperOff2,sizeof(gripperOff2)/2,310,80,369,139); for (angle_nho = 0; angle_nho < 90; angle_nho++) { servo_nho.write(angle_nho); delay(15); } gripper = 2; status_5 = true; } } //////////////////////////////Save Pos 1///////////////////////////////////// if (is_pressed(385, 50, 470, 100, px, py)){ my_lcd.Set_Draw_color(RED); my_lcd.Fill_Round_Rectangle(385, 50, 470, 100,10); show_string("Save", 405, 60, 2, BLACK, BLACK, 1); show_string("Pos_1", 400, 80, 2, BLACK, BLACK, 1); AN1 = xung1; EEPROMWriteint(0,AN1); delay(5); AN2 = xung2; EEPROMWriteint(4,AN2); delay(5); AN3 = xung3; EEPROMWriteint(8,AN3); delay(5); AN4 = counter4; EEPROMWriteint(12,AN4); delay(5); AN5 = gripper; EEPROMWriteint(16,AN5); delay(5); giatri1=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(10, 40, 75, 70); show_string("00000", 15, 48, , RED, BLACK, 1); giatri2=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(85, 40, 150, 70); show_string("00000", 90, 48, , RED, BLACK, 1); giatri3=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(160, 40, 225, 70); show_string("00000", 167, 48, , RED, BLACK, 1); } //////////////////////////////Save Pos 2///////////////////////////////////// if (is_pressed(385, 110, 470, 160, px, py)){ my_lcd.Set_Draw_color(RED); my_lcd.Fill_Round_Rectangle(385, 110, 470, 160,10); show_string("Save", 405, 120, 2, BLACK, BLACK, 1); show_string("Pos_2", 400, 140, 2, BLACK, BLACK, 1); BN1 = xung1; EEPROMWriteint(20,BN1); delay(5); Phụ lục BN2 = xung2; EEPROMWriteint(24,BN2); delay(5); BN3 = xung3; EEPROMWriteint(28,BN3); delay(5); giatri1=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(10, 40, 75, 70); show_string("00000", 15, 48, , RED, BLACK, 1); giatri2=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(85, 40, 150, 70); show_string("00000", 90, 48, , RED, BLACK, 1); giatri3=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(160, 40, 225, 70); show_string("00000", 167, 48, , RED, BLACK, 1); } //////////////////////////////Save Pos 3///////////////////////////////////// if (is_pressed(385, 170, 470, 220, px, py)){ my_lcd.Set_Draw_color(RED); my_lcd.Fill_Round_Rectangle(385, 170, 470, 220,10); show_string("Save", 405, 180, 2, BLACK, BLACK, 1); show_string("Pos_3", 400, 200, 2, BLACK, BLACK, 1); CN1 = xung1; EEPROMWriteint(32,CN1); delay(5); CN2 = xung2; EEPROMWriteint(36,CN2); delay(5); CN3 = xung3; EEPROMWriteint(40,CN3); delay(5); CN4 = counter4; EEPROMWriteint(44,CN4); delay(5); CN5 = gripper; EEPROMWriteint(48,CN5); delay(5); giatri1=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(10, 40, 75, 70); show_string("00000", 15, 48, , RED, BLACK, 1); giatri2=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(85, 40, 150, 70); show_string("00000", 90, 48, , RED, BLACK, 1); giatri3=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(160, 40, 225, 70); show_string("00000", 167, 48, , RED, BLACK, 1); } ////////////////////////////////////Setting/////////////////////////////////// if (is_pressed(250, 270, 350, 315, px, py)) Phụ lục { if (status_6) { my_lcd.Set_Draw_color(BLUE); my_lcd.Fill_Round_Rectangle(70, 225, 170, 265,5); show_string("loop", 85, 235, 3, RED, BLACK, 1); my_lcd.Set_Draw_color(BLUE); my_lcd.Fill_Round_Rectangle(310, 225, 410, 265,5); show_string("Speed", 320, 235, 3, RED, BLACK, 1); status_6 = false; } else { my_lcd.Set_Draw_color(WHITE); my_lcd.Fill_Round_Rectangle(70, 225, 170, 265,5); my_lcd.Fill_Round_Rectangle(310, 225, 410, 265,5); my_lcd.Fill_Round_Rectangle(10, 225, 60, 265,5); my_lcd.Fill_Round_Rectangle(180, 225, 230, 265,5); my_lcd.Fill_Round_Rectangle(250, 225, 300, 265,5); my_lcd.Fill_Round_Rectangle(420, 225, 470, 265,5); status_6 = true; } } /////////////////////////////////Loop////////////////////////////////////// if (is_pressed(70, 225, 170, 265, px, py)) { if (status_7) { my_lcd.Set_Draw_color(CYAN ); my_lcd.Fill_Round_Rectangle(10, 225, 60, 265,5); show_string("+", 25, 230, , RED, BLACK, 1); my_lcd.Fill_Round_Rectangle(180, 225, 230, 265,5); show_string("-", 195, 230, , RED, BLACK, 1); status_7 = false; } else { my_lcd.Set_Draw_color(WHITE); my_lcd.Fill_Round_Rectangle(10, 225, 60, 265,5); my_lcd.Fill_Round_Rectangle(180, 225, 230, 265,5); status_7 = true; } } if (is_pressed(10, 225, 230, 265, px, py) && (!status_7)) { if (is_pressed(10, 225, 60, 265, px, py)) { my_lcd.Set_Draw_color(BLUE); my_lcd.Fill_Round_Rectangle(10, 225, 60, 265,5); show_string("+", 25, 230, , RED, BLACK, 1); my_lcd.Set_Draw_color(CYAN ); my_lcd.Fill_Round_Rectangle(10, 225, 60, 265,5); show_string("+", 25, 230, , RED, BLACK, 1); chuki = chuki + 1; } Phụ lục if (is_pressed(180, 225, 230, 265, px, py)) { my_lcd.Set_Draw_color(BLUE); my_lcd.Fill_Round_Rectangle(180, 225, 230, 265,5); show_string("-", 195, 230, , RED, BLACK, 1); my_lcd.Set_Draw_color(CYAN ); my_lcd.Fill_Round_Rectangle(180, 225, 230, 265,5); show_string("-", 195, 230, , RED, BLACK, 1); chuki = chuki - 1; if(chuki < 0){ chuki = 0; } } chukitft[0] = (chuki / 1000) % 10 + 48; chukitft[1] = (chuki / 100) % 10 + 48; chukitft[2] = (chuki / 10) % 10 + 48; chukitft[3] = (chuki / 1) % 10 + 48; my_lcd.Set_Draw_color(BLUE); my_lcd.Fill_Round_Rectangle(70, 225, 170, 265,5); show_string(chukitft, 85, 235, 3, RED, BLACK, 1); } //////////////////////////////Speed//////////////////////////////////////////// if (is_pressed(310, 225, 410, 265, px, py)) { if (status_8) { my_lcd.Set_Draw_color(CYAN ); my_lcd.Fill_Round_Rectangle(250, 225, 300, 265,5); show_string("+", 265, 230, , RED, BLACK, 1); my_lcd.Fill_Round_Rectangle(420, 225, 470, 265,5); show_string("-", 435, 230, , RED, BLACK, 1); status_8 = false; } else { my_lcd.Set_Draw_color(WHITE); my_lcd.Fill_Round_Rectangle(250, 225, 300, 265,5); my_lcd.Fill_Round_Rectangle(420, 225, 470, 265,5); status_8 = true; } } if (is_pressed(250, 225, 470, 265, px, py) && (!status_8)) { if (is_pressed(250, 225, 300, 265, px, py)) { my_lcd.Set_Draw_color(BLUE); my_lcd.Fill_Round_Rectangle(250, 225, 300, 265,5); show_string("+", 265, 230, , RED, BLACK, 1); my_lcd.Set_Draw_color(CYAN ); my_lcd.Fill_Round_Rectangle(250, 225, 300, 265,5); show_string("+", 265, 230, , RED, BLACK, 1); tocdo = tocdo-20; Speed = Speed + 1; if(Speed >10){ Speed = 10; Phụ lục } Serial.println(Speed); } if (is_pressed(420, 225, 470, 265, px, py)) { my_lcd.Set_Draw_color(BLUE); my_lcd.Fill_Round_Rectangle(420, 225, 470, 265,5); show_string("-", 435, 230, , RED, BLACK, 1); my_lcd.Set_Draw_color(CYAN ); my_lcd.Fill_Round_Rectangle(420, 225, 470, 265,5); show_string("-", 435, 230, , RED, BLACK, 1); tocdo = tocdo+20; Speed = Speed - 1; if(Speed < 0){ Speed = 0; } Serial.println(Speed); } speedtft[0] = (Speed / 1000) % 10 + 48; speedtft[1] = (Speed / 100) % 10 + 48; speedtft[2] = (Speed / 10) % 10 + 48; speedtft[3] = (Speed / 1) % 10 + 48; my_lcd.Set_Draw_color(BLUE); my_lcd.Fill_Round_Rectangle(310, 225, 410, 265,5); show_string(speedtft, 325, 235, 3, RED, BLACK, 1); my_lcd.Fill_Rectangle(397, 230, 410, 260); my_lcd.Set_Draw_color(WHITE); my_lcd.Fill_Rectangle(411, 230, 413, 260); Serial.println(speedtft); } //////////////////////////////Goto StartPos/////////////////////////////////// if (is_pressed(10, 270, 110, 315, px, py)){ my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(10, 270, 110, 315,10); show_string("Goto", 35, 275, , RED, BLACK, 1); show_string("StartPos", 12, 295, , RED, BLACK, 1); my_lcd.Set_Draw_color(YELLOW); my_lcd.Fill_Round_Rectangle(10, 270, 110, 315,10); show_string("Goto", 35, 275, , RED, BLACK, 1); show_string("StartPos", 12, 295, , RED, BLACK, 1); vi_tri_bat_dau(); giatri1=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(10, 40, 75, 70); show_string("00000", 15, 48, , RED, BLACK, 1); giatri2=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(85, 40, 150, 70); show_string("00000", 90, 48, , RED, BLACK, 1); giatri3=0; my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Rectangle(160, 40, 225, 70); show_string("00000", 167, 48, , RED, BLACK, 1); } /////////////////////////////////Start//////////////////////////////////// Phụ lục if (is_pressed(130, 270, 230, 315, px, py)){ my_lcd.Set_Draw_color(CYAN); my_lcd.Fill_Round_Rectangle(130, 270, 230, 315,10); show_string("Start", 150, 285, , RED, BLACK, 1); my_lcd.Set_Draw_color(YELLOW); my_lcd.Fill_Round_Rectangle(130, 270, 230, 315,10); show_string("Start", 150, 285, , RED, BLACK, 1); Serial.println(AN1); Serial.println(AN11); for(int j=0;j