(Đồ án tốt nghiệp) thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm ứng dụng công nghệ xử lý ảnh

ĐẠ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 PHÂN LOẠI SẢN PHẨM ỨNG DỤNG CÔNG NGHỆ XỬ LÝ ẢNH Người hướng dẫn: Sinh viên thực hiện: TS ĐẶNG PHƯỚC VINH NGUYỄN ĐẶNG MẪN LÊ ĐÌNH MINH NHẬT Đà Nẵng, 2019 Thiết kế chế tạo cánh tay robot bậc tự phân loại sản phẩm ứng dụng cơng nghệ xử lý ảnh TĨM TẮT Tên đề tài: Thiết kế chế tạo cánh tay phân loại theo màu sắc ứng dụng xử lý ảnh Sinh viên thực hiện: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn Số thẻ SV: 101140156 - 101140152 Lớp: 14CDT1 Nợi dung tóm tắt: cánh tay phân loại theo màu sắc ứng dụng xử lý ảnh khâu: tiền xử lý liệu thu thập từ camera, phân tích phân loại Quá trình phân loại sản phẩm thực dựa việc thu thập thơng tin màu sắc, hình dạng, góc lệch sản phẩm thông qua hệ thống camera Khi vật thể đạt màu sắc, hình dạng tương ứng cánh tay gắp vật đưa vào vị trí quy định C C R L T U D SVTH: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn GVHD: TS Đặng Phước Vinh i Thiết kế chế tạo cánh tay robot bậc tự phân loại sản phẩm ứng dụng công nghệ xử lý ảnh LỜI NĨI ĐẦU Trong năm gần đây, cơng nghệ xử lý ảnh (image processing) đạt nhiều thành tựu tiến bộ vượt bậc Trong đó, nhận dạng phân loại hình ảnh mợt lĩnh vực theo đuổi một cách tích cực Ý tưởng cốt lõi việc nhận dạng phân loại hình ảnh phân tích ảnh từ liệu thu cảm biến hình ảnh camera, Webcam … Nhờ hệ thống xử lý ảnh, người giảm khối lượng công việc tăng chính xác việc đưa định liên quan đến xử lý hình ảnh nhiều lĩnh vực: quân quốc phòng, hệ thống kỹ thuật hóa sinh, giải phẫu, hệ thống giao thông thông minh, công nghệ robot, … Những phần mềm xử lý ảnh sử dụng phổ biến Visual Studio, Visual Studio, … Trong đó phần mềm Visual studio thư viện xử lý ảnh C C trở thành công cụ thiếu cán bộ nghiên cứu giảng dạy, sinh viên đại học, cao học nghiên cứu sinh thuộc ngành khoa học kỹ thuật nước ta Điều có Visual Studio cung cấp một cơng cụ tính tốn lập trình bậc cao dễ sử R L T dụng, hiệu thân thiện với người dùng Visual Studio cung cấp cho người sử dụng phương thức để thực mô máy tính, ngồi Visual Studio cịn có thể giao tiếp với Vi điều khiển, PLC, … với thiết bị phần cứng để thực ứng dụng thực tế Vì phạm vi hiểu biết mình, chúng em tìm hiểu thực đề tài “Thiết kế chế tạo cánh tay phân loại theo màu sắc ứng dụng xử lý ảnh” Đây nghiên cứu kết hợp Visual Vi điều khiển để phân loại sản phẩm Trong trình thực đồ án với nỗ lực thân áp dụng kiến thức trang bị trường, chúng em có nhiều cố gắng để hoàn thành tốt đồ án Tuy nhiên kiến thức khả có hạn nên khó tránh khỏi thiếu sót Vì mong nhận ý kiến đóng góp giúp đỡ chân U D thành thầy cô khoa Cơ Khí bạn sinh viên để đồ án hoàn thiện Chúng em xin chân thành cảm ơn thầy TS Đặng Phước Vinh tận tình hướng dẫn giúp đỡ chúng em hoàn thành đồ án Đà Nẵng, Ngày 04 tháng 06 năm 2019 Nhóm sinh viên thực Lê Đình Minh Nhật – Nguyễn Đặng Mẫn SVTH: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn GVHD: TS Đặng Phước Vinh ii Thiết kế chế tạo cánh tay robot bậc tự phân loại sản phẩm ứng dụng công nghệ xử lý ảnh CAM ĐOAN Chúng xin cam đoan: Những nội dung đồ án thực hướng dẫn trực tiếp Thầy TS Đặng Phước Vinh Mọi tham khảo dùng đồ án trích dẫn rõ ràng tên tác giả, tên cơng trình, thời gian, địa điểm cơng bố Mọi chép không hợp lệ, vi phạm quy chế đào tạo, hay gian dối, chúng tơi xin chịu hồn tồn trách nhiệm Nhóm Sinh viên thực C C R L T Lê Đình Minh Nhật – Nguyễn Đặng Mẫn U D SVTH: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn GVHD: TS Đặng Phước Vinh iii Thiết kế chế tạo cánh tay robot bậc tự phân loại sản phẩm ứng dụng công nghệ xử lý ảnh MỤC LỤC Trang Nhiệm vụ đồ án Tóm tắt Lời nói đầu cảm ơn i Lời cam đoan liêm chính học thuật ii Mục lục iii Danh sách bảng biểu, hình vẽ sơ đồ vi MỞ ĐẦU Chương 1: TỔNG QUAN VỀ HỆ THỐNG 1.1 Giới thiệu chung hệ thống C C 1.1.1 Khái quát xử lý ảnh ngành gia công chi tiết máy, kiểm tra mực nước cha R L T 1.1.2 Giới thiệu Robot 1.5.1 Bài toán 15 U D 1.5.2 Hướng giải 16 1.5.3 Phân tích hệ thống: 16 Chương 2: THIẾT KẾ VÀ TÍNH TOÁN PHẦN CƠ KHÍ 17 2.1 Xây dựng chọn phương án thiết kế 17 2.1.1 Cánh tay robot 17 2.2 Tính tốn cánh tay Robot 18 2.2.1 Tính chọn đợng bợ phận robot 18 2.2.2 Tính tốn đợng học thuận, đợng học nghịch 26 Chương 3: TÍNH TOÁN THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 34 3.1 Sơ đồ nguyên lý 34 3.2 Các Module sử dụng hệ thống 34 3.2.1 Arduino NANO 34 3.2.2 Nguồn một chiều 12V-20A 37 3.2.3 Module giảm áp 5v 3A 37 3.4 Lưu đồ thuật toán cánh tay Robot 38 SVTH: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn GVHD: TS Đặng Phước Vinh iv Thiết kế chế tạo cánh tay robot bậc tự phân loại sản phẩm ứng dụng công nghệ xử lý ảnh 3.3.1 Lưu đồ thuật tốn chương trình 38 3.3.2 Lưu đồ thuật tốn chương trình 39 Chương 4: Xử lý ảnh 42 4.1 Sơ lược xử lý ảnh 42 4.1.1 Một số khái niệm bản: 42 4.1.2 Các loại ảnh thường gặp: 43 4.2 Các phép toán sử dụng: 46 4.2.1 Chuyển đổi RGB sang HSV ngược lại: 46 4.2.2 Chuyển đổi HSV sang nhị phân: 46 4.2.3 Các phép tốn hình thái học ảnh: 46 4.2.4 Bộ lọc gaussionBlur 47 4.3 OpenCV 48 C C 4.3.1 Giới thiệu thư viện OpenCV 48 4.3.2 Phiên OpenCV hay OpenCV 48 R L T 4.4 Thiết kế giao diện điều khiển Visual Studio: 51 U D KẾT LUẬN 53 TÀI LIỆU THAM KHẢO 54 PHỤ LỤC SVTH: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn GVHD: TS Đặng Phước Vinh v Thiết kế chế tạo cánh tay robot bậc tự phân loại sản phẩm ứng dụng công nghệ xử lý ảnh DANH SÁCH CÁC BẢNG, HÌNH VẼ Hình 1.1 Ứng dụng xử lý ảnh để đếm số lượng sản phẩm cơng nghiệp Hình 1.2 Ứng dụng xử lý ảnh đọc mã barcode Hình 1.3 Ứng dụng nhận dạng ký tự Hình 1.4 Ứng dụng định vị cho robot Hình 1.5 Ứng dụng đo kích thước Hình 1.6 Ứng dụng kiểm tra tổng quát Hình 1.7 Robot kiểu tọa đợ Descarte Hình 1.8 Robot kiểu hệ tọa đợ Hình 1.9 Robot kiểu tọa đợ cầu Hình 1.10 Robot kiểu Scar Hình 1.11 Robot kiểu tay người Hình 1.12 Robot sản xuất 11 Hình 1.13 Robot lắp ráp bộ phận ô tô 12 C C R L T Hình 1.14 Đèn chiếu sáng 12 U D Hình 1.15 Cơ cấu bợ đẩy bằng khí nén 13 Hình 1.16 Cơ cấu cánh tay robot DOF 13 Hình 1.17 Ảnh trước chỉnh sửa 14 Hình 1.18 Ảnh sau chỉnh sửa 14 Hình 1.19 Trước lập trình chưa đọc chữ 14 Hình 1.20 Sau lập trình xong, thử sai hồn thiện chương trình 15 Hình 1.21 Sơ đồ phân loại gắp hàng tự động ứng dụng công nghệ xử lý ảnh 16 Hình 2.1 Cánh tay bậc tự 18 Hình 2.2 Đợng RC Servo LD-20MG 19 Hình 2.3 Đợng Servo Tower Pro MG996R 20 Bảng 2.1 Thông số khâu cánh tay robot 20 Hình 2.4 Sơ đồ tải trọng robot chịu trọng lượng lớn 21 Bảng 2.2 Thông số động bước 86BYGH450A 22 Hình 2.5 Đợng bước 86BYGH450A 23 Hình 2.6 Đợng bước 23 Bảng 2.5 Bảng thông số động bước nema 17 tỉ số 3969/289 24 Hình 2.7 Đợng Nema 23 25 Bảng 2.4 Bảng thông số động Nema 23 25 SVTH: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn GVHD: TS Đặng Phước Vinh vi Thiết kế chế tạo cánh tay robot bậc tự phân loại sản phẩm ứng dụng công nghệ xử lý ảnh Hình 2.6 Khâu 25 Hình 2.7 Khâu 2,3,4,5 26 Hình 2.8 Tay gắp 26 Hình 2.9 Vị trí ban đầu robot hệ tọa độ 26 Bảng 2.3 Bảng D-H 27 Hình 2.10 Trường công tác cánh tay robot 32 Hình 2.11 Sơ đồ vị trị gốc tọa độ 32 Hình 3.1 Sơ đồ nguyên lý 34 Hình 3.2 Arduino NANO 34 Hình 3.3 Nguồn cung cấp 12VDC 37 Hình 3.4 Module giảm áp LM2596 37 Hình 3.5 Lưu đồ thuật tốn chương trình chính 38 Hình 3.6: Điều khiển cánh tay di chuyển 39 C C Hình 3.7: Về vị trí góc 40 Hình 3.8: Phân loại vật 41 Hình 4.1 Thành phần RGB 43 R L T Hình 4.2 Bảng màu RBG 44 Hình 4.3 Thành phần HSV 45 U D Hình 4.4 Tọa độ điểm ảnh xám 45 Hình 4.5 Ma trận ảnh nhị phân 46 Hình 4.6 Kết biến đổi a) Hình ban đầu b) Hình biến đổi 47 Hình 4.7 Tạo project 49 Hình 4.8 Cấu hình cho OpenCV 50 Hình 4.9 Cấu hình cho openCV (linker) 50 Hình 4.10 Giao diện điều khiển chương trình 51 SVTH: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn GVHD: TS Đặng Phước Vinh vii C C U D R L T Thiết kế chế tạo cánh tay robot bậc tự phân loại sản phẩm ứng dụng công nghệ xử lý ảnh MỞ ĐẦU Mục đích: Thiết kế chế tạo hệ thống phân loại gắp hàng tự động ứng dụng công nghệ xử lý ảnh nhằm ứng dụng hệ thống tự động vào công nghiệp cánh tay robot xử lý ảnh, nó làm tăng suất tránh sai sót có thể xảy trình sản xuất Mục tiêu đề tài: Thực hành lại kiến thức lập trình khí, … học thời gian qua vào thực tế, nhằm củng cố lại kiến thức hỗ trợ cho việc làm sau Phạm vi đối tượng nghiên cứu: a) Phạm vi: Nghiên cứu, tính toán, thiết kế, chế tạo hệ thống phân loại gắp hàng tự động ứng dụng công nghệ xử lý ảnh Hệ thống bao gồm phần chính như: cánh tay robot, hệ thống băng tải, camera xử lý ảnh, máy tính b) Đối tượng nghiên cứu: Cánh tay robot bậc tự do, xử lý ảnh, máy tính C C c) Nghiên cứu lý thuyết: Nghiên cứu đặc điểm động học động lực học robot, lập trình cho robot xử lý ảnh để nhận diện sản phẩm d) Nghiên cứu thiết kế: Tính toán vẽ kết cấu robot, băng tải R L T U D e) Chế tạo: Dựa vào kết tính tốn thiết kế sau đó tiến hành thi cơng chế tạo mơ hình Cấu trúc đồ án gồm chương: - Chương 1: Tổng quan hệ thống - Chương 2: Thiết kế hệ thống khí - Chương 3: Thiết kế hệ thống điều khiển - Chương 4: Xử lý ảnh - Chương 5: Kết luận SVTH: Lê Đình Minh Nhật – Nguyễn Đặng Mẫn GVHD: TS Đặng Phước Vinh double *val_R_Slow; private: System::Void tbar_R_Slow_Scroll(System::Object^ sender, System::EventArgs^ e) { val_R_Slow = new double(5); char a[5]; tbar_R_Slow->SetRange(0, 255); *val_R_Slow = tbar_R_Slow->Value; itoa(*val_R_Slow, a, 10); txb_val_R_Slow->Text = gcnew System::String(a); //*val_R_Slow = 150; } double *val_R_Shigh; private: System::Void tbar_R_Shigh_Scroll(System::Object^ sender, System::EventArgs^ e) { val_R_Shigh = new double(5); char a[5]; tbar_R_Shigh->SetRange(0, 255); *val_R_Shigh = tbar_R_Shigh->Value; itoa(*val_R_Shigh, a, 10); txb_val_R_Shigh->Text = gcnew System::String(a); //*val_R_Shigh = 255; } C C R L T double *val_R_Vlow; private: System::Void tbar_R_Vlow_Scroll(System::Object^ sender, System::EventArgs^ e) { val_R_Vlow = new double(5); char a[5]; tbar_R_Vlow->SetRange(0, 255); *val_R_Vlow = tbar_R_Vlow->Value; itoa(*val_R_Vlow, a, 10); txb_val_R_Vlow->Text = gcnew System::String(a); //*val_R_Vlow = 0; } double *val_R_Vhigh; private: System::Void tbar_R_Vhigh_Scroll(System::Object^ sender, System::EventArgs^ e) { val_R_Vhigh = new double(5); char a[5]; tbar_R_Vhigh->SetRange(0, 255); *val_R_Vhigh = tbar_R_Vhigh->Value; itoa(*val_R_Vhigh, a, 10); txb_val_R_Vhigh->Text = gcnew System::String(a); //*val_R_Vhigh = 255; } U D private: System::Void ResetRed_Click(System::Object^ sender, System::EventArgs^ e) { //value blue HSV low-high 100 , 150, , 140 , 255, 255 //Red H low *val_R_Hlow = 100; txb_val_R_Hlow->Text = ("100"); tbar_R_Hlow->Value = 100; Phụ lục 24 //Red S low *val_R_Slow = 150; txb_val_R_Slow->Text = ("150"); tbar_R_Slow->Value = 150; //Red V low *val_R_Vlow = 0; txb_val_R_Vlow->Text = ("0"); tbar_R_Vlow->Value = 0; //Red H high *val_R_Hhigh = 140; txb_val_R_Hhigh->Text = ("140"); tbar_R_Hhigh->Value = 140; //Red S high *val_R_Shigh = 255; txb_val_R_Shigh->Text = ("255"); tbar_R_Shigh->Value = 255; //Red V high *val_R_Vhigh = 255; txb_val_R_Vhigh->Text = ("255"); tbar_R_Vhigh->Value = 255; C C } private: System::Void ResetYellow_Click(System::Object^ sender, System::EventArgs^ e) { //value yellow HSV low - high 16, 42, 140, 56, 255, 255 //yellow H low *val_Y_Hlow = 16; txb_val_Y_Hlow->Text = ("16"); tbar_Y_Hlow->Value = 16; //yellow S low *val_Y_Slow = 42; txb_val_Y_Slow->Text = ("42"); tbar_Y_Slow->Value = 42; //yellow V low *val_Y_Vlow = 140; txb_val_Y_Vlow->Text = ("140"); tbar_Y_Vlow->Value = 140; //yellow H high *val_Y_Hhigh = 56; txb_val_Y_Hhigh->Text = ("56"); tbar_Y_Hhigh->Value = 56; //yellow S high *val_Y_Shigh = 255; txb_val_Y_Shigh->Text = ("255"); tbar_Y_Shigh->Value = 255; //yellow V high *val_Y_Vhigh = 255; txb_val_Y_Vhigh->Text = ("255"); tbar_Y_Vhigh->Value = 255; } int myVar = 0; private: System::Void btn_configure_Click(System::Object^ sender, System::EventArgs^ e) { if (myVar % == 0) { R L T U D Phụ lục 25 tbar_R_Hhigh->Enabled = true; tbar_R_Hlow->Enabled = true; tbar_R_Shigh->Enabled = true; tbar_R_Slow->Enabled = true; tbar_R_Vhigh->Enabled = true; tbar_R_Vlow->Enabled = true; tbar_Y_Hhigh->Enabled = true; tbar_Y_Hlow->Enabled = true; tbar_Y_Shigh->Enabled = true; tbar_Y_Slow->Enabled = true; tbar_Y_Vhigh->Enabled = true; tbar_Y_Vlow->Enabled = true; ResetRed->Enabled = true; ResetYellow->Enabled = true; } else { tbar_R_Hhigh->Enabled = false; tbar_R_Hlow->Enabled = false; tbar_R_Shigh->Enabled = false; tbar_R_Slow->Enabled = false; tbar_R_Vhigh->Enabled = false; tbar_R_Vlow->Enabled = false; tbar_Y_Hhigh->Enabled = false; tbar_Y_Hlow->Enabled = false; tbar_Y_Shigh->Enabled = false; tbar_Y_Slow->Enabled = false; tbar_Y_Vhigh->Enabled = false; tbar_Y_Vlow->Enabled = false; C C R L T U D ResetRed->Enabled = false; ResetYellow->Enabled = false; } myVar = myVar++; } public: System::Void timer1_Tick(System::Object^ sender, System::EventArgs^ e) { Mat img_show2; Mat img_show1; out = new Mat; *out = (*imgOriginal_show).clone(); img_show1 = (*imgOriginal_show).clone(); img_show2 = (*imgOriginal_show).clone(); ShowImg(img_show1, *out, *val_R_Hlow, *val_R_Slow, *val_R_Vlow, *val_R_Hhigh, *val_R_Shigh, *val_R_Vhigh, "b"); DrawCVImage(pictureBox1, *out); char a[10]; itoa(show, a, 10); NumRed->Text = gcnew System::String(a); Phụ lục 26 //Tran(); int tru; tru = show; ShowImg(img_show2, *out, *val_Y_Hlow, *val_Y_Slow, *val_Y_Vlow, *val_Y_Hhigh, *val_Y_Shigh, *val_Y_Vhigh, "y"); DrawCVImage(pictureBox1, *out); char b[10]; itoa(show-tru, b, 10); NumYellow->Text = gcnew System::String(b); //Tran(); show = 0; } double *val_Y_Hlow; private: System::Void tbar_Y_Hlow_Scroll(System::Object^ sender, System::EventArgs^ e) { val_Y_Hlow = new double(100); char a[5]; tbar_Y_Hlow->SetRange(0, 255); *val_Y_Hlow = tbar_Y_Hlow->Value; itoa(*val_Y_Hlow, a, 10); txb_val_Y_Hlow->Text = gcnew System::String(a); } double *val_Y_Slow; private: System::Void tbar_Y_Slow_Scroll(System::Object^ sender, System::EventArgs^ e) { val_Y_Slow = new double(5); char a[5]; tbar_Y_Slow->SetRange(0, 255); *val_Y_Slow = tbar_Y_Slow->Value; itoa(*val_Y_Slow, a, 10); txb_val_Y_Slow->Text = gcnew System::String(a); } double *val_Y_Vlow; private: System::Void tbar_Y_Vlow_Scroll(System::Object^ sender, System::EventArgs^ e) { val_Y_Vlow = new double(5); char a[5]; tbar_Y_Vlow->SetRange(0, 255); *val_Y_Vlow = tbar_Y_Vlow->Value; itoa(*val_Y_Vlow, a, 10); txb_val_Y_Vlow->Text = gcnew System::String(a); } double *val_Y_Hhigh; private: System::Void trackBar4_Scroll(System::Object^ sender, System::EventArgs^ e) { val_Y_Hhigh = new double(5); char a[5]; tbar_Y_Hhigh->SetRange(0, 255); *val_Y_Hhigh = tbar_Y_Hhigh->Value; itoa(*val_Y_Hhigh, a, 10); txb_val_Y_Hhigh->Text = gcnew System::String(a); } double *val_Y_Shigh; private: System::Void tbar_Y_Shigh_Scroll(System::Object^ sender, System::EventArgs^ e) C C R L T U D Phụ lục 27 { val_Y_Shigh = new double(5); char a[5]; tbar_Y_Shigh->SetRange(0, 255); *val_Y_Shigh = tbar_Y_Shigh->Value; itoa(*val_Y_Shigh, a, 10); txb_val_Y_Shigh->Text = gcnew System::String(a); } double *val_Y_Vhigh; private: System::Void tbar_Y_Vhigh_Scroll(System::Object^ sender, System::EventArgs^ e) { val_Y_Vhigh = new double(5); char a[5]; tbar_Y_Vhigh->SetRange(0, 255); *val_Y_Vhigh = tbar_Y_Vhigh->Value; itoa(*val_Y_Vhigh, a, 10); txb_val_Y_Vhigh->Text = gcnew System::String(a); } int *flag; C C Thread^ thread2; private: System::Void Btn_OpenCamera_Click(System::Object^ sender, System::EventArgs^ R L T e) { /*MyForm^ printer2 = gcnew MyForm(); thread2 = gcnew Thread(gcnew ThreadStart(printer2, &MyForm::Th_2)); thread2->Start();*/ timer1->Enabled = false; timer_tran->Enabled = true; } U D void Th_2() { timer_tran->Enabled = true; } private: System::Void NumYellow_TextChanged(System::Object^ sender, System::EventArgs^ e) { } private: System::Void Stop_Click(System::Object^ sender, System::EventArgs^ e) { /*MyForm^ printer1 = gcnew MyForm(); Thread^ thread1 = gcnew Thread(gcnew ThreadStart(printer1, &MyForm::Th_1)); thread1->Start();*/ timer_tran->Enabled = false; timer1->Enabled = true; } void Th_1() { thread2->Abort(); } private: System::Void MyForm_FormClosing(System::Object^ sender, System::Windows::Forms::FormClosingEventArgs^ e) { Phụ lục 28 cap->release(); } private: System::Void timer_tran_Tick(System::Object^ sender, System::EventArgs^ e) { out = new Mat; *out = *imgOriginal_tran; btn_Start->Enabled = false; btn_configure->Enabled = false; ResetRed->Enabled = false; ResetYellow->Enabled = false; ThresImg(*imgOriginal_tran, *val_R_Hlow, *val_R_Slow, *val_R_Vlow, *val_R_Hhigh, *val_R_Shigh, *val_R_Vhigh, "b"); Tran(); ThresImg(*imgOriginal_tran, *val_Y_Hlow, *val_Y_Slow, *val_Y_Vlow, *val_Y_Hhigh, *val_Y_Shigh, *val_Y_Vhigh, "y"); Tran(); btn_Start->Enabled = true; btn_configure->Enabled = true; ResetRed->Enabled = true; ResetYellow->Enabled = true; C C if ((*all_data).size() > 0) { delete all_data; } R L T } }; } U D ❖ Code điều khiển Arduino // tu nguon sang day dieu khien : de ( day bi that - day ko bi that) // khau1 (vang-lam-do-xanh la cay) // khau2 (đen-xanh-đỏ-lam) ///////////////tính góc //góc sang bước : khâu đế : bước - 0.15 độ // khâu : bước - 0.09 độ // khâu : bước - 0.0655329 độ sai so // gốc khâu đế : // gốc khâu : 1079 // gốc khâu : 1678 #include #include #include #define dir1 A3 #define step1 #define en1 A0 #define ends1 12 Phụ lục 29 #define dir2 #define step2 #define en2 A1 #define ends2 #define dir3 #define step3 11 #define en3 A2 #define ends3 #define q1 0.15 #define q2 0.09 #define q3 0.0655329 #define servoxoay 13 #define servogap #define PI 3.14159265 #define d1 276 //251+38-18+2 , 255 #define a2 222.5 #define a3 189.6 #define d5 203.0 C C //212 #define ser 6.3 R L T //#define kep 93 //#define tha 55 //////////////////////////////// AccelStepper stp1(1, step1, dir1); // pin step, pin dir U D AccelStepper stp2(1, step2, dir2); AccelStepper stp3(1, step3, dir3); ServoTimer2 myservo; ServoTimer2 myservo1; byte b2=0, b3=0,b=0; float sv [2]= {0,0}; float dc [8]= {}; int bienx, bieny ,bienz, goc,u, delayservo = 70; String mau,hinhdang,a; long check,doi; int kep =108, tha=70; String RecData="90x0y90abcp"; float t1, t2, t3, t4, t5; /////////////////////////////////// void ctht2() { if (digitalRead (2)==0){ if (millis() - check > 500) { check = millis(); b2 = 1; } } } Phụ lục 30 void ctht3() { if (digitalRead (3)==0){ if (millis() - check > 500) { check = millis(); b3 = 1; } } } void autohome(); void chay(); void tachso(String &giatri); void runsv (float &goc); void gap(int &xoay,int &gap); void chay(float &goc1,float goc2,float &goc3,float &goc4); void tinh(int x, int y, int z); void phanloai(String a,String b); void setup() { C C Serial.begin(9600); millis(); pinMode(en1, OUTPUT) R L T ; pinMode(en2, OUTPUT); Enable digitalWrite(en1, HIGH); digitalWrite(en2, pinMode(en3, OUTPUT); // HIGH); digitalWrite(en3, HIGH); // Dặt Enable len high để khởi động động U D stp1.setMaxSpeed(1000); stp1.setAcceleration(150); stp2.setMaxSpeed(1000); stp2.setAcceleration(250); stp3.setMaxSpeed(1000); stp3.setAcceleration(250); pinMode(2, INPUT_PULLUP); pinMode(3, INPUT_PULLUP); pinMode(ends1, INPUT_PULLUP); attachInterrupt(0, ctht2, FALLING); attachInterrupt(1, ctht3, FALLING); while(1){ a= Serial.readString(); if (a=="m") break; } port_attach(PB1); // chọn pin ~9 port_attach(PB2); // chọn pin ~10 write(PB2, 90); // - hạ góc tối đa 57 ,70 thẳng góc write(PB1, 90); myservo.attach(servoxoay); // attach (đính) servo chân digital myservo1.attach(servogap); myservo.write(map(0,0,180,650,2250)); myservo1.write(map(tha,0,180,650,2250)); autohome(); } Phụ lục 31 void loop() { if (Serial.available()>0){ RecData=Serial.readString(); tachso(RecData); if (bieny > 416) {Serial.print("L");doi=millis();} else { tinh(bienx,bieny,70); tinh(bienx,bieny,7); chay(dc[4],35,dc[6],dc[3]); if (u>3||hinhdang=="c") gap(0,tha); else gap(t5,tha); chay(dc[0],dc[1],dc[2],dc[3]); if (u>3||hinhdang=="c") gap(0,kep); else gap(t5,kep); chay(dc[4],dc[5],dc[6],dc[3]); gap(0,kep); phanloai(mau,hinhdang); C C Serial.print("L"); doi=millis(); R L T } } if (millis()-doi>10000){ stp3.moveTo(1185); stp2.moveTo(-1000); stp1.moveTo(0); U D while(stp1.distanceToGo()||stp2.distanceToGo()||stp3.distanceToGo()){ stp1.run(); stp3.run(); stp2.run(); } }} /////////////////////////////////////////////////////////////// void autohome() { /////////////////////////// khâu đế stp1.setSpeed(150); while (digitalRead(ends1)) { stp1.runSpeed(); } stp1.setSpeed(-200); for(int i=0;i