Thiết kế và chế tạo robot scara phân loại pin

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP NGÀNH: CƠ ĐIỆN TỬ CHUYÊN NGÀNH: CƠ ĐIỆN TỬ C C R L T U D ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO ROBOT SCARA PHÂN LOẠI PIN Người hướng dẫn: ThS NGUYỄN ĐẮC LỰC Người duyệt: TS LÊ HOÀI NAM Sinh viên thực hiện: PHẠM VĂN THƯƠNG NGUYỄN BÁ HOÀNH Lớp: Đà Nẵng, 2019 14CDT2 TÓM TẮT ĐỒ ÁN TỐT NGHIỆP Tên đề tài: THIẾT KẾ VÀ CHẾ TẠO ROBOT SCARA PHÂN LOẠI PIN SV thực hiện: PHẠM VĂN THƯƠNG MSSV: 101140202 NGUYỄN BÁ HOÀNH MSSV: 101140183 Lớp: 14CDT2 Nhu cầu tính cấp thiết Đồ án tốt nghiệp học phần bắt buộc sinh viên ngành Cơ điện tử nói riêng sinh viên khối ngành kĩ thuật nói chung Để thiết kế thành cơng cấu, cụm chi tiết máy hay máy hồn chỉnh địi hỏi sinh viên phải có hiểu biết nắm kiến thức lĩnh vực khí điện tử Đây điều kiện thuận lợi cho chúng em để ứng dụng kiến thức học vào thực tế C C Robot cơng nghiệp có vị trí quan trọng sản xuất công nghiệp đại, R L T robot công nghiệp ứng dụng tiên tiến khoa học kỹ thuật lĩnh vực điều khiển tự động vào sản xuất Trong sản xuất tự động, robot công nghiệp đáp ứng công việc lặp lặp lại nhiều lần như: lắp ráp, đóng gói, vận chuyển U D sản phẩm…; cơng việc có độ xác cao như: lắp ráp chi tiết máy nhỏ, mạch điện tử; công việc nguy hiểm như: thao tác nhà máy điện nguyên tử, lò luyện kim loại, nơi có nhiệt độ áp suất cao, dễ xảy cháy nổ… Nó có vai trị quan trọng việc tạo sản phẩm cơng nghiệp có suất cao, chất lượng tốt đáp ứng nhu cầu xã hội Hiện nay, nhiều nước giới sử dụng Robot vào sản xuất Mỹ, Nhật, Pháp… Còn Việt Nam bắt tay vào nghiên cứu cịn mẻ Do mơ hình thích hợp, cấp thiết để sinh viên thực nghiệm nghiên cứu cho nhìn cụ thể robot Phạm vi nghiên cứu đề tài: Trong đề tài nhóm chúng em thiết kế hệ thống băng tải sản phẩm nghiên cứu ứng dụng xử lý ảnh để phân loại pin cách tối ưu - Tính tốn thiết kế băng tải - Tình tốn thiết kế cánh tay robot SCARA - Xử lý ảnh - Thiết kế, thi cơng mơ hình i Nội dung đề tài thực hiện: - Một báo cáo thuyết minh - Sáu vẽ A0 - Một slide thuyết trình - Một mơ hình Kết đạt được:  Phần lý thuết tìm hiểu: - Tổng quan đề tài - Thiết kế băng tải - Thiết kế cánh tay robot SCARA - Phần mềm xử lý ảnh ứng dụng xử lí ảnh  Đã lựa chọn thiết kế phần: C C - Lựa chọn đưa phương án tối ưu để thiết kế băng tải R L T - Lựa chọn đưa phương án tối ưu để thiết kế cánh tay robot  Thiết kế mơ hình tổng thể U D ii ĐẠ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 TT Họ tên sinh viên Nguyễn Bá Hoành Số thẻ SV 101140183 Lớp Ngành 14CDT2 Cơ Điện Tử Phạm Văn Thương 101140202 14CDT2 Cơ Điện Tử Tên đề tài đồ án: “Thiết kế chế tạo robot SCARA phân loại pin” Đề 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: …………………………………… …………………………………………… …… ………………………………………………………………………………………… … ………………………………….… ……………………… ……………………… Nội dung phần thuyết minh tính tốn: a Phần chung: TT Họ tên sinh viên Nguyễn Bá Hoành Phạm Văn thương b Phần riêng: TT Họ tên sinh viên Nguyễn Bá Hoành C C R L T Nội dung Chương 4: Thiết kế hệ thống điều khiển U D Chương 5: Xây dựng chương trình điều khiển Chương 6: Đánh giá kết Lắp ráp mơ hình thực tế Nội dung Chương 1: Tổng quan đề tài Chương 3: Thiết kế chế tạo robot Lập trình hệ thống Chương 2: Thiết kế băng tải Phạm Văn Thương Vẽ toàn hệ thống Các vẽ, đồ thị (ghi rõ loại kích thước vẽ): a Phần chung: TT Họ tên sinh viên Nội dung Nguyễn Bá Hoành Phạm Văn Thương Bản vẽ mơ hình thực tế - A0 iii b Phần riêng: TT Họ tên sinh viên Nội dung Nguyễn Bá Hoành Bản vẽ lưu đồ thuật toán - A0 Bản vẽ sơ đồ mạch điện - A0 Phạm Văn Thương Bản vẽ tổng thể hệ thống - A0 Bản vẽ chi tiết hệ thống - A0 Bản vẽ sơ đồ động -1 A0 Họ tên người hướng dẫn: Phần/ Nội dung: ThS Nguyễn Đắc Lực Ngày giao nhiệm vụ đồ án: 01/02/2019 Ngày hoàn thành đồ án: …/…/201… C C Đà Nẵng, ngày R L T Trưởng Bộ môn……………………… Người hướng dẫn U D iv tháng năm 201 LỜI CẢM ƠN Chúng em xin chân thành cảm ơn thầy Nguyễn Đắc Lực tận tình hướng dẫn, bảo chúng em suốt thời gian học tập thực đề tài Xin chân thành cảm ơn bạn tập thể lớp 14CDT2 tham gia đóng góp ý kiến suốt q trình thực để nhóm hồn thành tốt đề tài Mặc dù hướng dẫn nhiệt tình thầy Nguyễn Đắc Lực, cơng nghệ cịn hạn chế, kinh nghiệm thiết kế chưa trau dồi nhiều, tài liệu phục vụ cho cơng việc thiết kế cịn q nên khơng tránh khỏi sai sót Sau thời gian tháng làm đề tài nổ lực nhóm hướng dẫn thầy Nguyễn Đắc Lực, thầy cô giáo giúp đỡ bạn sinh viên khác khoa chúng em hoàn thành xong đồ án thời gian quy định Một lần cho phép C C chúng em gửi đến quý thầy cô bạn lòng biết ơn sâu sắc R L T Đà Nẵng, ngày… tháng… năm 2019 U D Sinh viên thực Phạm Văn Thương Nguyễn Bá Hoành v LỜI NÓI ĐẦU Ngành điều khiển tự động hóa thời đại cơng nghệ 4.0 ngày có nhiều đột phá phát triển nhanh chóng Q trình góp phần khơng nhỏ vào việc tăng xuất lao động, giảm giá thành, nâng cao chất lượng sản phẩm, cải thiện điều kiện làm việc người Ngày khái niệm “dây chuyền sản xuất tự động’’ hay “robot” trở nên quen thuộc với người, nhiều nước robot phát triển theo hướng phục vụ sản xuất công nghiệp mà theo hướng phục vụ sinh hoạt giải trí Ở nước có ngành cơng nghiệp phát triển đưa robot vào trình sản xuất nhà máy để dần thay người nơi có mơi trường độc hại, mơi trường phóng xạ, đồng thời khắc phục tình trạng khơng đồng chất lượng sản phẩm Đồng thời công tự động hóa dây chuyền sản xuất chiến C C lược phát triển tập đoàn sản xuất tương lai Vậy nên nói robot đem lại sống phần khơng thể thiếu đổi tương lai Vì R L T vậy, nhóm em định chọn đề tài “Thiết kế chế tạo robot Scara phân loại pin” để nghiên cứu thiết kế U D Sau tháng làm đồ án với giúp đỡ thầy Nguyễn Đắc Lực thầy môn Cơ điện tử, chúng em hoàn thành đồ án Nhưng với kiến thức có hạn chưa có nhiều kinh nghiệm tính tốn, thi cơng thực tế nên khó tránh sai sót Chúng em kính mong tiếp tục bảo thầy, để chúng em hồn thiện kiến thức Cuối cùng, em xin chân thành cảm ơn thầy, giáo khoa Cơ khí – Trường Đại học Bách khoa – Đại học Đà Nẵng, đặc biệt thầy Nguyễn Đắc Lực bạn gia đình động viên giúp đỡ chúng em suốt trình thực đồ án Đà Nẵng, ngày… tháng… năm 2019 Sinh viên thực Phạm Văn Thương Nguyễn Bá Hồnh vi CAM ĐOAN Kính gửi: Ban Giám Hiệu Nhà Trường Đại Học Bách Khoa - Đại học Đà Nẵng Khoa Cơ Khí - Trường Đại Học Bách Khoa - Đại học Đà Nẵng Em tên là: PHẠM VĂN THƯƠNG NGUYỄN BÁ HOÀNH Lớp: 14CDT2 Khoa: Cơ khí Đề tài: Thiết kế chế tạo robot Scara phân loại pin Em xin cam đoan:  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 C C 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 R L T học thuật kết từ 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, U D trình bày, chép, dịch đoạn, nêu ý tưởng người khác mà trích dẫn  Khơng tự đạo văn, sử dụng lại thơng tin nghiên cứu mà khơng có trích dẫn phân mảnh thơng tin kết nghiên cứu để cơng bố nhiều ấn phẩm vii MỤC LỤC TÓM TẮT ĐỒ ÁN TỐT NGHIỆP i NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP iii LỜI CẢM ƠN v LỜI NÓI ĐẦU vi CAM ĐOAN vii MỤC LỤC viii DANH SÁCH CÁC BẢNG, HÌNH VẼ x MỞ ĐẦU CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG 1.1 Giới thiệu hệ thống 1.1.1 Cấu trúc .3 1.1.2 Nguyên lý 1.1.3 Yêu cầu 1.2 C C R L T Đặt vấn đề hướng giải U D CHƯƠNG 2: THIẾT KẾ HỆ THỐNG BĂNG TẢI 2.1 Thiết kế băng tải .6 2.1.1 Yêu cầu kỹ thuật 2.1.2 Phương án thiết kế .6 2.1.3 Phương án truyền động .7 2.1.4 Tính tốn lựa chọn động 2.2 Hệ thống chiếu sáng CHƯƠNG 3: THIẾT KẾ CHẾ TẠO ROBOT 10 3.1 Định nghĩa phân loại Robot 10 3.2 Giới thiệu robot SCARA 14 3.3 Thiết kế robot SCARA 18 3.3.1 Phân tích lựa chọn cấu trúc 18 3.3.2 Bài toán động học 21 3.3.3 Thiết kế hệ thống dẫn động robot .36 CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 38 4.1 Cấu trúc hệ thống điều khiển 38 4.1.1 Sơ đồ kết nối mạch điện 39 viii 4.2 Các thành phần hệ thống 39 4.2.1 Bộ điều khiển 39 4.2.2 Ramps 1.4 41 4.2.3 Driver A4988 41 4.2.4 Step Motor 43 4.2.5 Servor Motor .44 CHƯƠNG 5: XÂY DỰNG CHƯƠNG TRÌNH ĐIỀU KHIỂN .46 5.1 Xây dựng chương trình xử lý ảnh 46 5.1.1 Giới thiệu xử lý ảnh 46 5.1.2 Các công đoạn xử lý ảnh 46 5.1.3 Một số khái niệm xử lý ảnh .48 5.1.4 Quy trình xử lý ảnh đồ án 50 5.2 C C Xây dựng chương trình điều khiển 51 5.2.1 Nguyên lý hoạt động 51 5.2.2 Lưu đồ thuật toán điều khiển 52 R L T CHƯƠNG 6: ĐÁNH GIÁ VÀ KẾT QUẢ 53 U D 6.1 Đánh giá đề tài 53 6.2 Kết hướng phát triển đề tài 54 TÀI LIỆU THAM KHẢO 55 PHỤ LỤC PHỤ LỤC ix PHỤ LỤC Code Windows Form UI using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Linq; using System.Text; using System.Threading; using System.Threading.Tasks; using System.Windows.Forms; C C using System.IO.Ports; using System.Timers; R L T //using System.IO; using Emgu.CV; using Emgu.CV.Structure; using Emgu.Util; using Emgu.CV.UI; using Emgu.CV.CvEnum; using Emgu.CV.Util; U D using Emgu.CV.Cvb; namespace DAHTCDT { public partial class Form1 : Form { System.Timers.Timer procTimer = new System.Timers.Timer(); // timer define private const int procTimerInterval = 1200; private Capture myCmr; private Mat frame = new Mat(); //khung hinh Mat Image_source; int Open = 1; int Detected = 1; int portcom = 1; //int Temp = 1; //int Tempp = 1; int Count2 = 0; int Count3 = 0; bool a = false; int u = 1; public Form1() { InitializeComponent(); btIdentifyObject.Enabled = false; InitSerialPort(); } // // KẾT NỐI CAMERA // private void btOpenCamera_Click(object sender, EventArgs e) { pictureBox1.Image = null; pictureBox2.Image = null; if (Open % != 0) C C { if (myCmr == null) R L T { myCmr = new Capture(1); myCmr.SetCaptureProperty(Emgu.CV.CvEnum.CapProp.FrameWidth, 800); // Kích thước khung hình myCmr.SetCaptureProperty(Emgu.CV.CvEnum.CapProp.FrameHeight, 600); // } U D myCmr.ImageGrabbed += capture_ImageGrabbled; myCmr.Start(); btOpenCamera.Text = "Đóng"; btIdentifyObject.Enabled = true; lbStatusCamera.Text = "Camera's Opening!"; } if (Open % == 0) { if (myCmr != null) { myCmr.Dispose(); myCmr = null; pictureBox1.Image = null; btOpenCamera.Text = "Khởi Động"; btIdentifyObject.Enabled = false; lbStatusCamera.Text = "Camera Close!"; } } Open++; } private void capture_ImageGrabbled(object sender, EventArgs e) { myCmr.Retrieve(frame); } // //KET NOI CONG COM // public void InitSerialPort() { var comList = SerialPort.GetPortNames(); for (int i = 0; i < comList.Length; i++) { cbSerialPorts.Items.Add(comList[i]); cbSerialPorts.SelectedIndex = 0; } } C C private void cbSerialPorts_SelectedIndexChanged(object sender, EventArgs e) { R L T if (portcom != && portcom % == 0) { serialPort.PortName = cbSerialPorts.Text; serialPort.Open(); U D lbCOM.Text = "ĐANG KẾT NỐI, " + cbSerialPorts.Text; MessageBox.Show("Bạn kết nối cổng " + cbSerialPorts.Text, "COM", MessageBoxButtons.OK); } if (portcom != && portcom % != 0) { serialPort.Close(); cbSerialPorts.SelectedText = ""; lbCOM.Text = "NOT COM"; MessageBox.Show("Bạn hủy kết nối cổng " + cbSerialPorts.Text, "COM", MessageBoxButtons.OK); } portcom++; } // // bat timer,chua ham tong // private void btIdentifyObject_Click(object sender, EventArgs e) { if (Detected % != 0) { timer1.Start(); btIdentifyObject.Text = "Tắt Nhận Dạng?"; } if (Detected % == 0) { timer1.Stop(); btIdentifyObject.Text = "Nhận Dạng DT"; pictureBox1.Image = null; } Detected++; } public void timer1_Tick(object sender, EventArgs e) { Image_source = myCmr.QueryFrame();//frame: khung //XuatDT(); if (u % == 0) { XuatDT(); button1.Text = "Stop"; } C C if (u % != 0) R L T { button1.Text = "Start"; } } // // // phương thức tiền xử lý U D public Image PreProcess(Image imgInput) { //int a = trackBar1.Value; //label4.Text = trackBar1.Value.ToString(); morphops(imgInput); Image grayImg = imgInput.Convert().SmoothGaussian(15); Image imgThresh = grayImg.ThresholdBinary(new Gray(120), new Gray(255)); return imgThresh; } // // // Tìm biên, trả vector private VectorOfVectorOfPoint contoursFinder(Image imgThresh) { VectorOfVectorOfPoint contours = new VectorOfVectorOfPoint(); Mat hierachy = new Mat(); CvInvoke.FindContours(imgThresh.Clone(), contours, Emgu.CV.CvEnum.ChainApproxMethod.ChainApproxSimple); return contours; } hierachy, Emgu.CV.CvEnum.RetrType.Tree, // // // Vẽ đường biên BR private Image drawExContour(Image imgInput) { VectorOfVectorOfPoint myContours = new VectorOfVectorOfPoint(); Mat noUse = new Mat(); Image imgExcontour = imgInput.Clone(); morphops(imgExcontour); CvInvoke.GaussianBlur(imgExcontour, noUse, new Size(13, 13), 1.5, 0, BorderType.Default); myContours = this.contoursFinder(PreProcess(imgExcontour)); List boxList = new List(); Dictionary dict = new Dictionary(); for (int i = 0; i < myContours.Size; i++) // lọc đường biên nhiễu { C C double area = CvInvoke.ContourArea(myContours[i]); R L T if (area > 50000) { dict.Add(i, area); } } U D var item = dict.OrderByDescending(v => v.Value); foreach (var it in item) { int key = int.Parse(it.Key.ToString()); RotatedRect rect = CvInvoke.MinAreaRect(myContours[key]); SizeF t = rect.Size; String vitri = (t.Width).ToString() + ";" + (t.Height).ToString()+ ";" + (rect).ToString(); CvInvoke.PutText(imgExcontour, vitri, new Point(50,100), FontFace.HersheyTriplex, 2, new MCvScalar(0, 0, 255), 4, LineType.EightConnected, false); boxList.Add(CvInvoke.MinAreaRect(myContours[key])); CvInvoke.DrawContours(imgExcontour, myContours, key, new MCvScalar(255, 0, 0), 3); foreach (RotatedRect box in boxList) { CvInvoke.Polylines(imgExcontour, Array.ConvertAll(box.GetVertices(), Bgr(Color.DarkOrange).MCvScalar, 3); } } return imgExcontour; } // //nhan dang pin lon bang dien tich, roi tra ve gia tri bool // private bool pinLon(Image imgInput) { bool cophaipinlon = false; Image imgExcontour = imgInput.Clone(); VectorOfVectorOfPoint myContours = new VectorOfVectorOfPoint(); double area = new double(); RotatedRect rect = new RotatedRect(); C C myContours = this.contoursFinder(PreProcess(imgExcontour)); R L T for (int i = 0; i < myContours.Size; i++) // lọc đường biên nhiễu { area = CvInvoke.ContourArea(myContours[i]); rect = CvInvoke.MinAreaRect(myContours[i]); } SizeF t = rect.Size; U D if(area > 56000 && area < 62000 && t.Width > 110) { cophaipinlon = true; } else { cophaipinlon = false; } return cophaipinlon; } // // nhan dang pin nho // private bool pinNho(Image imgInput) { bool cophaipinnho = false; Image imgExcontour = imgInput.Clone(); VectorOfVectorOfPoint myContours = new VectorOfVectorOfPoint(); Point.Round), true, new double area = new double(); RotatedRect rect = new RotatedRect(); myContours = this.contoursFinder(PreProcess(imgExcontour)); for (int i = 0; i < myContours.Size; i++) // lọc đường biên nhiễu { area = CvInvoke.ContourArea(myContours[i]); rect = CvInvoke.MinAreaRect(myContours[i]); } SizeF t = rect.Size; if (area > 50000 && area < 56000 && t.Width < 100) { cophaipinnho = true; } else C C { cophaipinnho = false; R L T } return cophaipinnho; } U D // // //nhận diện xong gửi tín hiệu kiểu char cho arduino private void recognitionArea(string sendSignal) { string str = "NOT COM"; if (a == true) { //Temp++; if (str == lbCOM.Text) { timer1.Stop(); myCmr.Dispose(); myCmr = null; pictureBox1.Image = null; pictureBox2.Image = null; MessageBox.Show("Bạn Chưa Kết MessageBoxButtons.OK); btOpenCamera.Text = "Khởi Động"; Open = 1; Nối Cổng COM, Làm truyền liệu!", "COM", btIdentifyObject.Enabled = false; btIdentifyObject.Text = "Nhận Dạng DT"; Detected = 1; } else serialPort.Write(sendSignal); } } // //hinh thai hoc // private void morphops(Image thresh) { Mat structuringElement1 = CvInvoke.GetStructuringElement(ElementShape.Rectangle, new Size(3, 3), new Point(-1, -1)); CvInvoke.MorphologyEx(thresh, thresh, MorphOp.Erode, structuringElement1, MorphOp.Erode, structuringElement1, C C BorderType.Default, new MCvScalar(0, 0, 0)); CvInvoke.MorphologyEx(thresh, thresh, R L T BorderType.Default, new MCvScalar(0, 0, 0)); new Point(-1, -1), 1, new Point(-1, -1), 1, Mat structuringElement2 = CvInvoke.GetStructuringElement(ElementShape.Rectangle, new Size(4, 4), new Point(-1, -1)); CvInvoke.MorphologyEx(thresh, U D thresh, MorphOp.Dilate, structuringElement2, new Point(-1, -1), 1, MorphOp.Dilate, structuringElement2, new Point(-1, -1), 1, BorderType.Default, new MCvScalar(0, 0, 0)); CvInvoke.MorphologyEx(thresh, thresh, BorderType.Default, new MCvScalar(0, 0, 0)); } private void XuatDT() { Image imgInput = Image_source.ToImage().Clone(); Rectangle rectroi = new Rectangle(99, 1, 598, 598); imgInput.ROI= rectroi; //int x = imgInput.Width; Image imgOut = drawExContour(imgInput); if(a = pinNho(imgInput)) { recognitionArea("3"); Thread.Sleep(200); Count3++; lbCount3.Text = Count2.ToString(); } if (a = pinLon(imgInput)) { recognitionArea("2"); Thread.Sleep(200); Count2++; lbCount2.Text = Count2.ToString(); } // Image picture = recognitionArea(imgInput, "2"); //if(Temp != Tempp) // { // Thread.Sleep(200); // Count3++; // lbCount3.Text = Count3.ToString(); // } C C //Temp = Tempp; //pictureBox1.Image = Image_source.Bitmap; R L T //pictureBox1.Image = imgOut.Bitmap; pictureBox1.Image = imgOut.Bitmap; pictureBox2.Image = PreProcess(imgInput).Bitmap; } // //nhan nut gui tin hieu // U D private void bt_Robhome_Click(object sender, EventArgs e) { serialPort.Write("1"); } private void bt_Vitri1_Click(object sender, EventArgs e) { serialPort.Write("2"); } private void bt_Vitri2_Click(object sender, EventArgs e) { serialPort.Write("3"); } private void button1_Click(object sender, EventArgs e) { u++; } } PHỤ LỤC Code Arduino #include #include "myServo.h" #include "BasicStepperDriver.h" #include "MultiDriver.h" #include "SyncDriver.h" //DC motor #define DC 10 //Servo // #define SERVO0 11 #define SERVO1 12 C C #define uStep // for convert function R L T #define ratioX 3.5 #define ratioY 3.5 #define ratioZ #define X_MIN_PIN #define Y_MAX_PIN 15 #define Z_MIN_PIN 18 // U D // Steppers // #define X_STEP_NUM #define X_uSTEP 200 // vi buoc #define X_STEP_PIN 54 #define X_DIR_PIN 55 #define X_ENABLE_PIN 38 BasicStepperDriver stepperX(X_STEP_NUM, X_DIR_PIN, X_STEP_PIN, X_ENABLE_PIN); #define Y_STEP_NUM #define Y_uSTEP 200 #define Y_STEP_PIN 60 #define Y_DIR_PIN 61 #define Y_ENABLE_PIN 56 BasicStepperDriver stepperY(Y_STEP_NUM, Y_DIR_PIN, Y_STEP_PIN, Y_ENABLE_PIN); #define Z_STEP_NUM #define Z_uSTEP #define Z_STEP_PIN 200 46 #define Z_DIR_PIN 48 #define Z_ENABLE_PIN 62 BasicStepperDriver stepperZ(Z_STEP_NUM, Z_DIR_PIN, Z_STEP_PIN, Z_ENABLE_PIN); #define X_MAX_DEG_PS 127 //max deg in postive #define X_MAX_DEG_NG -127 // negative #define Y_MAX_DEG_PS 133 #define Y_MAX_DEG_NG -133 #define Z_MAX_VAL_PS 75 SyncDriver controller(stepperX, stepperY); int pos = 0; int numOfStep; int n1, n2, n3, n4; String n5; C C float a, b, c, d; float prv_a, prv_b, prv_c; R L T int x_stepInc,y_stepInc; //bool stt = false; //bool ror = false; void setup() { // put your setup code here, to run once: pinMode(DC, OUTPUT); U D initPort(); // initializing PWM port to control Servo Motor' stepperY.setEnableActiveState(LOW); stepperX.setEnableActiveState(LOW); stepperZ.setEnableActiveState(LOW); stepperX.begin(30, X_uSTEP); stepperY.begin(30, Y_uSTEP); stepperZ.begin(30, Z_uSTEP); setDeg(SERVO1, 0); // init value for servo setDeg(SERVO0, 160); Serial.begin(9600); // to getting data //robHome(); } void loop() { // put your main code here, to run repeatedly: stepperX.setRPM(30); stepperY.setRPM(30); stepperZ.setRPM(120); digitalWrite(DC, LOW); if(Serial.available()){ String var = Serial.readStringUntil('\n'); if(var == "1") { robHome(); } else if(var == "2"){ delay(1200); gapPin(); //delay(100); thaPl(); } else if(var == "3") C C { gapPin(); R L T //delay(100); thaPn(); } } } // //gap pin void gapPin() { setHt(0,0,35,170,"on"); cal(a,b,c,d); // tinh toan: deg -> step show(); // hien thi cac gia tri nhan dc motRun(); //stepperZ.move(-mmToStep(15)); setHt(0,0,20,170,"on"); cal(a,b,c,d); // tinh toan: deg -> step show(); // hien thi cac gia tri nhan dc motRun(); } // //tha pinlon void thaPl() { setHt(45,40,50,160,"off"); cal(a,b,c,d); // tinh toan: deg -> step show(); // hien thi cac gia tri nhan dc motRun(); U D setHt(3,0,0,160,"off"); cal(a,b,c,d); // tinh toan: deg -> step show(); // hien thi cac gia tri nhan dc motRun(); } // //tha pinpho void thaPn() { setHt(45,60,50,160,"off"); cal(a,b,c,d); // tinh toan: deg -> step show(); // hien thi cac gia tri nhan dc motRun(); //stepperZ.move(-mmToStep(15)); setHt(3,0,0,160,"off"); cal(a,b,c,d); // tinh toan: deg -> step show(); // hien thi cac gia tri nhan dc motRun(); C C } // R L T // void robHome() { while (digitalRead(Z_MIN_PIN) == 0) { stepperZ.begin(80, Z_uSTEP); stepperZ.rotate(-360); } while (digitalRead(Z_MIN_PIN) != 0) { stepperZ.rotate(20); //stepperZ.setEnableActiveState(HIGH); } while (digitalRead(Y_MAX_PIN) == 0) { stepperY.begin(20, Y_uSTEP); stepperY.rotate(-360*ratioY); } while (digitalRead(Y_MAX_PIN) != 0) { stepperY.rotate(20*ratioY); //stepperX.setEnableActiveState(HIGH); } while (digitalRead(X_MIN_PIN) == 0) { //stepperX.setEnableActiveState(LOW); //stepperY.setEnableActiveState(LOW); stepperX.begin(20, X_uSTEP); stepperY.begin(20, Y_uSTEP); U D controller.rotate(360*ratioX, 360*ratioY); } while (digitalRead(X_MIN_PIN) != 0) { controller.rotate(-20*ratioX, -20*ratioY); //stepperX.setEnableActiveState(HIGH); //stepperY.setEnableActiveState(HIGH); } setHt(-110,150,0,0,"off"); cal(a,b,c,d); // tinh toan: deg -> step show(); // hien thi cac gia tri nhan dc motRun(); } void motRun() { Serial.println("Running "); Serial.read(); C C if (n3 >= 0) { controller.move(n1,n1); R L T stepperY.move(n2); setDeg(SERVO0, n4); stepperZ.move(n3); } else { stepperZ.move(n3); setDeg(SERVO0, n4); controller.move(n1,n1); stepperY.move(n2); } if (n5 == "on") { setDeg(SERVO1, 20); } else { setDeg(SERVO1, 0); } Serial.println("Done!"); } void cal(float ai, float bi, float ci, float di) { n1 = (int) (degToStep(ai) * ratioX); n2 = (int) (degToStep(bi) * ratioY); n3 = (int) mmToStep(ci); n4 = map(di, -90, 90, 180, 0); } U D void show() { Serial.print("teta1 ="); Serial.println(n1); Serial.print("teta2 ="); Serial.println(n2); Serial.print("d3 ="); Serial.println(c); Serial.print("teta4 ="); Serial.println(d); Serial.print("taykep ="); Serial.println(n5); } // //cai dat goc void setHt(float ai, float bi, float ci, float di, String fi) { for (int i = 0; i < 5; i++) { C C switch (i) { case 0: R L T a = - prv_a; prv_a = ai; break; case 1: b = bi - prv_b; prv_b = bi; break; case 2: c = ci - prv_c; prv_c = ci; break; case 3: d = di; break; case 4: n5 = fi; break; } } } //for X,Y float degToStep(float deg) { return (deg * 200 * uStep / 360); } //for only Z-Axis float mmToStep(float mm) { return (mm * 200 * uStep / ratioZ); } U D ... Nguyễn Đắc Lực Thiết kế chế tạo robot SCARA phân loại pin CHƯƠNG 3: THIẾT KẾ CHẾ TẠO ROBOT 3.1 Định nghĩa phân loại Robot 3.1.1 Phân loại robot Thế giới robot phong phú đa dạng, phân loại chúng khơng... CHƯƠNG 3: THIẾT KẾ CHẾ TẠO ROBOT 10 3.1 Định nghĩa phân loại Robot 10 3.2 Giới thiệu robot SCARA 14 3.3 Thiết kế robot SCARA 18 3.3.1 Phân tích lựa chọn... Đắc Lực Thiết kế chế tạo robot SCARA phân loại pin Hình 2: Robot bậc tự Phân loại theo hệ thống truyền động: Gồm có: C C R L T - Robot truyền động điện - Robot truyền động khí nén U D - Robot