Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử 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
NGÀNH: KỸ THUẬT CƠ ĐIỆN TỬ

ĐỀ TÀI:

THIẾT KẾ VÀ CHẾ TẠO CÁNH TAY ROBOT
5 BẬC TỰ DO PHÂN LOẠI SẢN PHẨM TRÊN
BĂNG TẢI SỬ DỤNG CÔNG NGHỆ XỬ LÝ
ẢNH

Người hướng dẫn: TS. VÕ NHƯ THÀNH
Sinh viên thực hiện: VI ĐÔNG VŨ
LÊ MAI XUÂN
Số thẻ sinh viên
: 101140171
101140172
Lớp: 14CDT1

Đà Nẵng, 2019


ĐẠ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ự do - Hạnh phúc

NHẬN XÉT ĐỒ ÁN TỐT NGHIỆP
1. Thông tin chung:
1. Họ và tên sinh viên: Vi Đông Vũ - Lê Mai Xuân
2. Lớp: 14CDT1 Số thẻ SV: 101140171- 101140172
3. Tên đề tài: Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên
băng tải sử dụng cơng nghệ xử lí ảnh
4. Người hướng dẫn: Ngô Thanh Nghị - Nguyễn Danh Ngọc
5 Học hàm/ học vị: Tiến sĩ
II. Nhận xét, đánh giá đồ án tốt nghiệp:
1. Về tính cấp thiết, tính mới, khả năng ứng dụng của đề tài: (điểm tối đa là 2đ)
………………………………………………………………………………………..
………………………………………………………………………………………..
2. Về kết quả giải quyết các nội dung nhiệm vụ yêu cầu của đồ án: (điểm tối đa là 4đ)
………………………………………………………………………………………..
………………………………………………………………………………………..
3. Về hình thức, cấu trúc, bố cục của đồ án tốt nghiệp: (điểm tối đa là 2đ)
………………………………………………………………………………………..
………………………………………………………………………………………..
4. Đề tài có giá trị khoa học/ có bài báo/ giải quyết vấn đề đặt ra của doanh nghiệp
hoặc nhà trường: (điểm tối đa là 1đ)
………………………………………………………………………………………..
………………………………………………………………………………………..
5. Các tồn tại, thiếu sót cần bổ sung, chỉnh sửa:
………………………………………………………………………………………..
………………………………………………………………………………………..
III. Tinh thần, thái độ làm việc của sinh viên: (điểm tối đa 1đ)
………………………………………………………………………………………..
IV. Đánh giá:

1. Điểm đánh giá:

……../10 (lấy đến 1 số lẻ thập phân)

2. Đề nghị: ☐ Được bảo vệ đồ án

☐ Bổ sung để bảo vệ

☐ Không được bảo vệ

Đà Nẵng, ngày
Người hướng dẫn

tháng

năm 201


ĐẠ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ự do - Hạnh phúc

NHẬN XÉT PHẢN BIỆN ĐỒ ÁN TỐT NGHIỆP
I.
1.
2.

Thông tin chung:
Họ và tên sinh viên: Vi Đông Vũ – Lê Mai Xuân
Lớp: 14 CDT1
Số thẻ SV: 101140171 - 101140172

3.

Tên đề tài: Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm

trên băng tải sử dụng cơng nghệ xử lí ảnh
Người phản biện: PGS- TS : Trần Xuân Tùy
TS: Đặng Phước Vinh

4.

ThS: Nguyễn Đắc Lực
Nhận xét, đánh giá đồ án tốt nghiệp:

II.

TT Các tiêu chí đánh giá

1

1a

1b

1c

Sinh viên có phương pháp nghiên cứu phù hợp, giải quyết
đủ nhiệm vụ đồ án được giao
Tính mới (nội dung chính của ĐATN có những phần
mới so với các ĐATN trước đây).
Đề tài có giá trị khoa học, cơng nghệ; có thể ứng dụng
thực tiễn.

80

15

Kỹ năng giải quyết vấn đề; hiểu, vận dụng được kiến
thức cơ bản, cơ sở, chuyên ngành trong vấn đề nghiên cứu.
50
Chất lượng nội dung ĐATN (thuyết minh, bản vẽ,
chương trình, mơ hình,…).
Có kỹ năng vận dụng thành thạo các phần mềm ứng
dụng trong vấn đề nghiên cứu;
Có kỹ năng đọc, hiểu tài liệu bằng tiếng nước ngoài 15
ứng dụng trong vấn đề nghiên cứu;
-

Có kỹ năng làm việc nhóm;

2

Kỹ năng viết:

2a

tích

Bố cục hợp lý, lập luận rõ ràng, chặt chẽ, lời văn súc

-

Thuyết minh đồ án khơng có lỗi chính tả, in ấn, định

2b

Điểm
Điểm
đánh
tối đa
giá

dạng

20
15
5


3

Tổng điểm đánh giá theo thang 100:
Quy về thang 10 (lấy đến 1 số lẻ)

1.

Các tồn tại, thiếu sót cần bổ sung, chỉnh sửa:

………………………………………..…………………………………………………
……………………………………...
2.
Câu hỏi đề nghị sinh viên trả lời trong buổi bảo vệ:
…………………………………………………………………………………………
………………………………...
3.

Đề nghị: ☐ Được bảo vệ đồ án

bảo vệ
Đà Nẵng, ngày

tháng

năm 201…

☐ Bổ sung để bảo vệ

☐ Không được


TÓM TẮT
Tên đề tài: Thiết kế và chế tạo cánh tay 5 bậc tự do phân loại sản phẩm trên
băng tải sử dụng cơng nghệ xử lí ảnh
Sinh viên thực hiện: Vi Đông Vũ – Lê Mai Xuân
Số thẻ SV: 101140136- 101140151
Lớp: 14CDT1

Nội dung tóm tắt: Hệ thống phân loại sản phẩm bằng cánh tay robot ứng dụng
công nghệ xử lí ảnh. Sản phẩm với các màu sắc khác nhau nằm trên băng tải động, khi
đến vùng xử lí.Q trình phân loại sản phẩm sẽ thực hiện dựa trên việc thu thập thơng
tin về, màu sắc, hình dạng và góc lệch của sản phẩm thông qua hệ thống camera.
Camera lấy dữ liệu, qua phân tích và tính tốn của máy tính. Xuất tín hiệu để điều
khiển cánh tay robot thực hiện công việc phân loại. Các sản phẩm được phân loại theo
mào sắc được phân chia vào các khay đúng theo yêu cầu.


ĐẠ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ự do - Hạnh phúc

NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP
Họ tên sinh viên: Vi Đông Vũ – Lê Mai Xuân
Số thẻ sinh viên: 10114071 - 101140172
Lớp:14CDT1
Khoa:Cơ khí

Ngành: Cơ điện tử

Tên đề tài đồ án: Thiết kế và chế tạo cánh tay 5 bậc tự do phân loại sản phẩm

1.

trên băng tải sử dụng cơng nghệ xử lí ảnh.

2.

Đề tài thuộc diện: ☐ Có ký kết thỏa thuận sở hữu trí tuệ đối với kết quả thực

hiện
3.

Các số liệu và dữ liệu ban đầu:

4. Nội dung các phần thuyết minh và tính tốn:
a.Phần chung.
STT

Họ và tên

1

Vi Đơng Vũ

2

Lê Mai Xn

Nội dung
Tìm hiểu một số một số cơ cấu ngoài thực tế để đưa ra
ý tưởng tốt nhất.
Tìm hiểu ngun lí hoạt động , lựa chọn các cơ cấu
phù hợp.
Tìm hiểu tài liệu và phần mềm liên quan.
Hoàn thiện thuyết minh.

b.Phần riêng.
STT

Họ và tên

Nội dung

1

Vi Đơng Đơng

Tìm hiểu các linh kiện điện tử, Lập trình hệ
thống.

2

Lê Mai Xuân

Thiết kế hệ thống bằng phần mềm solidworks,
tính tốn phần động học,

5. Các bản vẽ, đồ thị
a. Phần chung
Họ và tên

Nội dung
vẽ

STT

1

Bản

Vi Đông Vũ

Bản vẽ tổng thể

A0


Lê Mai Xuân

2

Bản vẽ sơ đồ động

b. Phần riêng
Họ và tên

Nội dung

Bản
vẽ

STT
1
2

Vi Đông Vũ

Lê Mai Xuân

Bản vẽ sơ đồ mạch điện
Bản vẽ chi tiết

A0

Bản vẽ lắp
Lưu đồ thuật toán

6. Họ tên người hướng dẫn: TS Ngô Thanh Nghị - TS Nguyễn Danh Ngọc.
7. Ngày giao nhiệm vụ đồ án: 01./02./2019
8. Ngày hồn thành đồ án:

01./06./2019
Đà Nẵng, ngày
tháng
Trưởng Bộ mơn ……………………..
Người hướng dẫn

năm 2019


LỜI NĨI ĐẦU

Trong sự nghiệp cơng nghiệp hóa và hiện đại hóa đất nước, tự động hóa ngày càng
đóng một vai trò quan trọng. Với tốc độ phát triển như hiện nay chúng ta không chỉ
cần một lượng lao động khổng lồ mà cịn địi hỏi có trình độ, chất lượng tay nghề, kỹ
thuật lao động và thiết bị sản xuất. Mức độ phát triển của khoa học kỹ thuật ngày càng
cao thì vấn đề tự động hố ngày càng được chú trọng.

Trong cách mạng công nghiệp 4.0 việc ứng dụng tự động hóa, trí tuệ nhân tạo vào sản
xuất, kinh doanh là một xu hướng tất yếu nhằm tạo ra năng suất lao động cao. Trên thế
giới đã có nhiều nhà máy sử dụng toàn bộ hệ thống dây chuyền sản xuất tự động hóa,
đưa rơ-bốt vào thay thế sức lao động của con người, tạo ra các sản phẩm tốt, độ chính
xác cao. Nhờ vào robot, cuộc sống của chúng ta ngày càng tiện nghi và an toàn hơn.
Thật vậy, robot đang dần thay thế con người trong những công việc nguy hiểm như
cứu nạn, cứu hộ, kiểm tra các môi trường độc hại, những nơi ảnh hưởng trực tiếp đến
sức khoẻ và tính mạng con người. Khơng chỉ vậy, trong các hộ gia đình, robot đã và
đang giúp chúng ta thực hiện những công việc nhàm chán và mất nhiều thời gian như
lau nhà, hút bụi… nhờ đó chất lượng cuộc sống ngày càng cao hơn.
Kết hợp xu thế phát triển của thời đại cũng những những kiến thức đã học, Nhóm
chúng em đã lựa chọn đề tài: “Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại
sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh”.
Trong thời gian làm đồ án, được sự chỉ bảo tận tình của thầy TS. Ngô Thanh Nghị và
TS. Nguyễn Danh Ngọc cùng với sự cố gắng của hai thành viên trong nhóm đến nay
đồ án đã hồn thành. Tuy nhiên với kiến thức cịn hạn chế, kinh nghiệm, kĩ năng cịn
thiếu, mặc dù có nhiều cố gắng song nhóm em vẫn cịn nhiều thiếu sót cần bổ sung,
hồn thiện. Kính mong các thầy cơ thơng cảm và góp ý để đề tài của nhóm em hồn
thiện hơn và có thể phát triển, ứng dụng trong thời gian sắp tới.
Đà Nẵng, ngày 03 tháng 6 năm 2019
Nhóm sinh viên thực hiện

Vi Đơng Vũ

i

Lê Mai Xn


CAM ĐOAN

Chúng tôi xin cam đoan:
Những nội dung trong đồ án này là do chúng tôi thực hiện dưới sự hướng dẫn trực
tiếp của Thầy TS. Ngô Thanh Nghị và TS. Nguyễn Danh Ngọc.
Mọi tham khảo dùng trong đồ án đều được 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 sao 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 hiện

Vi Đông Vũ

ii

Lê Mai Xuân


MỤC LỤC

TÓM TẮT ................................................................................................................... v
NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP ........................................................................... vi
LỜI NÓI ĐẦU.............................................................................................................. i
CAM ĐOAN ............................................................................................................... ii
MỤC LỤC .................................................................................................................. iii
CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI .................................................................... 2
1.1.Giới thiệu về đề tài................................................................................................. 2
1.2Phân tích đề tài ....................................................................................................... 4
1.3. Tính cấp thiết của đề tài. ....................................................................................... 4
1.4. Cấu trúc của đề tài................................................................................................. 4
CHƯƠNG 2. THIẾT KẾ HỆ THỐNG CƠ KHÍ........................................................... 6

2.1. Thiết kế cánh tay robot.......................................................................................... 6
2.1.1 Phân tích lựa chọn phương án . .......................................................................... 6
2.1.2. Tính chọn động cơ cho robot. ............................................................................. 7
2.1.3

Mô phỏng 3D cánh tay robot. ......................................................................... 9

2.1.4

Chọn vật liệu cho robot................................................................................. 10

2.1.5

Thiết kế bộ phận truyền chuyển động cho các khớp của robot .................... 11

Khớp 1 ( Khớp đế ) .................................................................................................... 11
2.2 Thiết kế hệ thống băng tải .................................................................................... 12
2.2.1 Giới thiệu .......................................................................................................... 12
2.2.2 Nguyên lí hoạt động: ........................................................................................ 13
2.2.3 Tính tốn và chọn thiết bị cho băng tải. ............................................................. 13
2.2.4. Lựa chọn và tính tốn băng tải ......................................................................... 15
2.3

Thiết kế khay chứa sản phẩm ........................................................................... 18
iii


2.4. Sơ đồ động hệ thống. ......................................................................................... 19
Chương 3: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN ................................................... 22
3.1Sơ đồ khối của hệ thống. ....................................................................................... 22

3.2. Vi điều khiển Arduno. ......................................................................................... 22
3.2.1.Tổng quan ......................................................................................................... 22
3.2.2. Thông số kĩ thuật –Uno R3 .............................................................................. 23
3.2.3. I/O Pins ............................................................................................................ 23
3.3 . Mạch điều khiển động cơ bước A4988. .............................................................. 24
3.3. Module LM2596 ................................................................................................. 25
3.4 . Khối nguồn 12V................................................................................................. 26
3.5. Sơ đồ mạch điện ................................................................................................. 27
CHƯƠNG 4: XÂY DỰNG CHƯƠNG TRÌNH ĐIỀU KHIỂN .................�� Lê Văn Uyển, Tính tốn thiết kế Hệ dẫn động cơ khí, Nhà xuất bản
Giáo Dục, 2009.
[3]. Trần Hữu Quế, Vẽ kĩ thuật cơ khí ( Tập 1 và 2 ), Nhà xuất bản Giáo Dục.
[4]. Lương Mạnh Bá, Nguyễn Thanh Thủy, Nhập mơn xử lí ảnh số, Nhà xuất bản
khoa học kỹ thuật, 2002
[5]. PGS. TS. Nguyễn Quang Hoan: Xử lý ảnh – Học viện cơng nghệ bưu chính viễn
thơng.
[6]. GS.TSKH Nguyễn Thiện Phúc : Robot công nghiệp- Nhà xuất bản khoa học và
kĩ thuật.
[7]. />[8]. Help trong Matlab
[9]. o/
[10]. />[11].
[12]. />[13]. />(Các tài liệu truy cập online cập nhật vào 05/06/2019)

46
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc

Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

PHỤ LỤC.
Code Arduino
#include <SerialCommand.h>
#include <AccelStepper.h>
#include <Servo.h>
#define stpbt 12 // output
#define dirbt 13
#define stp1 2
#define stp2 3
#define stp3
#define dir1
#define dir2
#define dir3

4
5
6
7

#define en 8
#define s0Pin 9
#define s1Pin 10
#define h1 0 // home
#define h2 90
#define h3
#define h5
#define e1
#define e2

-90
0
0 // endstop
97

#define e3 -150
#define gr0 126 // gripper
#define gr1 50
//200*ms*i/360, i=3969/289
#define q1 7.63 //ms4
#define q2 7.63 //ms4
#define q3 7.63 //ms4
//(200*ms*ib3)/(phi49*PI)
#define qb 15.59 //ms4
#define vb 80 //mm/s
#define td 1400 //ms
47
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

//parameter DH
#define a1 0
#define d1 139

#define a2 135
#define a3 160
#define a4 45.35
#define d5 92
#define ret 60 //
SerialCommand sc;
AccelStepper stepbt(1,stpbt,dirbt);
AccelStepper step1(1,stp1,dir1);
AccelStepper step2(1,stp2,dir2);
AccelStepper step3(1,stp3,dir3);
Servo sv[2];
bool m,n,t,r;
int w,b;
float A[5];
float th1,th2,th3,th5;
int st1,st2,st3;
int v,cv;
unsigned long tc,tb,tg;
const float des[3][4]={{65,220,50,90},{-30,220,50,90},{-125,220,50,90}};
//-----------------------------------void setup() {
Serial.begin(9600);
Serial.print("Arb\r");
pinMode(en,OUTPUT); digitalWrite(en,1);
step1.setMaxSpeed(1000);
step2.setMaxSpeed(1000);
step3.setMaxSpeed(1000);
step1.setAcceleration(3000);
step2.setAcceleration(4500);
step3.setAcceleration(4500);
stepbt.setMaxSpeed(1000);

stepbt.stop();
sc.addCommand("on", On);
48
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

sc.addCommand("off",Off);
sc.addCommand("h",Home);
sc.addCommand("obj",Obj);
sc.addCommand("bt0",bt0);
sc.addCommand("bt1",bt1);
sc.addCommand("start",Start);
sc.addCommand("stop",Stop);
sc.addCommand("g",G);
sc.addCommand("g0",G0);
sc.addCommand("g1",G1);
sc.addCommand("th",Th);
sc.addDefaultHandler(unrecognized);
}
//-----------------------------------void loop() {
sc.readSerial();
step1.run();
step2.run();
step3.run();

runSv();
stepbt.runSpeed();
wait();
grip();
}
//-----------------------------------void Obj(){
if(m&&b){
updateData();
tb=millis();
w=0;
MOVE();
}}
void updateData(){
char *arg;
for(int i=0;i<5;i++){
49
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

arg = sc.next();
if(arg == NULL) break;
A[i] = atof(arg);
}
}

void MOVE(){
w++;
int c=A[4];
c=constrain(c,0,2);
switch (w) {
case 1: Invert(A[0],A[1],A[2]+ret-15,A[3]);
M0(); t=1; break;
case 2: Invert(A[0],A[1],A[2],A[3]);
Mv(); break;
case 3: Invert(A[0],A[1],A[2]+ret,A[3]);
M1(); break;
case 4: Invert(des[c][0],des[c][1],des[c][2]+ret,des[c][3]);
Mv(); break;
case 5: Invert(des[c][0],des[c][1],des[c][2],des[c][3]);
Mv(); break;
case 6: Invert(des[c][0],des[c][1],des[c][2]+ret,des[c][3]);
M0(); break;
case 7: th1=h1; th2=h2; th3=h3; th5=h5;
Mv(); break;
case 8: w=0;
Serial.print("s1\r");
break;
default: break;
}
}
void wait(){
if(t&&!n&&(millis()-tb>=td)){
t=0;
MOVE();
}

}
50
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

//--------------------------------------void Invert(float Px, float Py, float Pz, float Or){
th1=atan2(Py,Px);
float n1=Px*cos(th1)+Py*sin(th1)-a1-a4;
float n2=Pz-d1+d5;
th3=-acos((sq(n1)+sq(n2)-sq(a2*1.0)-sq(a3*1.0))/(2.0*a2*a3));
th2=atan2((n2*(a2+a3*cos(th3))a3*sin(th3)*n1),(n1*(a2+a3*cos(th3))+a3*sin(th3)*n2));
//th4=-th2-th3;
th1*=180/PI; th2*=180/PI;
th3*=180/PI; //th4*=180/PI;
th5=th1-Or;
if (th5>180) th5-=360;
else if (th5<-180) th5+=360;
if (th5>90) th5-=180;
else if (th5<-90) th5+=180;
}
void convert(){
th3+=th2;
st1 = round(th1*q1);
st2 = round(th2*q2);

st3 = round(th3*q3);
v = map(th5,-90,90,2,168);
}
void Mv(){
convert();
step1.moveTo(st1);
step2.moveTo(st2);
step3.moveTo(st3);
cv=sv[0].read();
n=1;
}
void M0(){
sv[1].write(gr0);
r=1;
tg=millis();
51
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

}
void M1(){
sv[1].write(gr1);
r=1;
tg=millis();

}
void grip(){
if(r&&((millis()-tg)>=300)){
r=0;
Mv();
}
}
void runSv(){
if(n&&(millis()-tc>=15)){
tc=millis();
int ep;
ep=v-cv;
if(abs(ep)>1){
cv += (ep/abs(ep));
sv[0].write(cv);
}
else{
sv[0].write(v);
cv=v;
checkPos();
}}}
void checkPos(){
if((step1.distanceToGo()==0)&&(step2.distanceToGo()==0)&&(step3.distanceToGo()
==0)&&(cv==v)){
n=0;
if((w!=0)&&(w!=1))
MOVE();
}}
//---------------------------------------void On() {
52

SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

m=1;
bt0();
w=0; t=0;
th1=h1;
th2=h2;
th3=h3;
th5=h5;
convert();
sv[0].attach(s0Pin);
sv[1].attach(s1Pin);
sv[0].write(v);
sv[1].write(gr0);
digitalWrite(en,0);
step1.setCurrentPosition(round(e1*q1));
step2.setCurrentPosition(round(e2*q2));
step3.setCurrentPosition(round((e3+e2)*q3));
step1.moveTo(st1);
step2.moveTo(st2);
step3.moveTo(st3);
while(step1.distanceToGo()||step3.distanceToGo()){
step1.run();

step3.run();
}
while(step2.distanceToGo()){
step2.run();
}
}
void Off(){
m=0; n=0;
Stop();
digitalWrite(en,1);
}
void Home(){
if(m){
w=0; t=0;
th1=h1;
53
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

th2=h2;
th3=h3;
th5=h5;
M0();
}}

//---------------------------------------void Start(){
b=1;
bt1();
Home();
Serial.print("s1\r");
}
void Stop(){
b=0;
bt0();
Home();
//Serial.print("s0\r");
}
//---------------------------------------void bt0(){
stepbt.setSpeed(0);
stepbt.stop();
}
void bt1(){
stepbt.setSpeed(round(vb*qb));
}
void G(){
if(m){
updateData();
Invert(A[0],A[1],A[2],A[3]);
Mv();
}}
void G0(){
if(m){
updateData();
Invert(A[0],A[1],A[2],A[3]);
54

SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

M0();
}}
void G1(){
if(m){
updateData();
Invert(A[0],A[1],A[2],A[3]);
M1();
}}
void unrecognized(){
//Serial.print("?\r");
}
void Th(){
if(m){
updateData();
th1=A[0]; th2=A[1];
th3=A[2]; th5=A[3];
Mv();
}}

55
SVTH: Vi Đông Vũ

Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

Code Matlab.
function varargout = IRB0(varargin)
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',

mfilename, ...

'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @IRB0_OpeningFcn, ...
'gui_OutputFcn', @IRB0_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
function IRB0_OpeningFcn(hObject, eventdata, handles, varargin)

handles.output = hObject;
guidata(hObject, handles);
%---------------------bkdn=imread('bkdn.jpg');
ck =imread('ck.jpg');
axes(handles.axes1);
imshow(bkdn);
axes(handles.axes2);
imshow(ck);
%
global n c rx
n=0;
c=0;
rx='';
56
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

%---------------------function varargout = IRB0_OutputFcn(hObject, eventdata, handles)
varargout{1} = handles.output;
Connect_Callback(@Connect_Callback,eventdata,handles);
%-------------------------------------------------------------------------function figure1_DeleteFcn(hObject, eventdata, handles)
global s
if(strcmp(get(s,'Status'),'open'))
fclose(s);

delete(s)
clear s
end
function btnRetry_Callback(hObject, eventdata, handles)
set(handles.btnRetry,'visible','off');
Connect_Callback(@Connect_Callback,eventdata,handles);
function btnSwitch_Callback(hObject, eventdata, handles)
global s
sw=get(hObject,'value');
if (sw)
set(hObject,'string','Off');
fprintf(s,'on');
set(handles.btnSt,'enable','on');
set(handles.txtStatus,'string','ON');
else
set(hObject,'string','On');
fprintf(s,'off');
set(handles.btnSt,'string','Start');
set(handles.btnSt,'value',0);
set(handles.btnSt,'enable','off');
set(handles.txtStatus,'string','OFF');
end
function editTrans_Callback(hObject, eventdata, handles)
function editTrans_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
57
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

end
function btnClear_Callback(hObject, eventdata, handles)
set(handles.editTrans,'string','');
function btnSend_Callback(hObject, eventdata, handles)
global s
fprintf(s,get(handles.editTrans,'string'));
set(handles.editTrans,'string','');
function btnSt_Callback(hObject, eventdata, handles)
global s
st=get(hObject,'value');
if (st)
fprintf(s,'start');
set(hObject,'string','Stop');
set(handles.txtStatus,'string','START');
else
fprintf(s,'stop');
set(hObject,'string','Start');
set(handles.txtStatus,'string','STOP');
end
%-------------------------------------------------------------------------function Connect_Callback(obj,eventdata,handles)
global s Rx
c=0;
hwinfo = instrhwinfo('serial');

ac = hwinfo.AvailableSerialPorts;
if ~isempty(ac)
for i=1:length(ac)
s = serial(char(ac(i)));
s.Terminator = 'CR';
s.BytesAvailableFcnMode = 'terminator';
s.BytesAvailableFcn = {@Receiver_Callback,handles};
try fopen(s);
catch
end
set(handles.txtStatus,'foregroundColor','g')
set(handles.txtStatus,'string','Connecting...');
58
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

pause(2.5);
if strcmp(Rx,'Arb')
set(handles.btnRetry,'visible','off');
set(handles.txtStatus,'foregroundColor','b')
set(handles.txtStatus,'string',['Connect successful - ',char(ac(i))]);
Video_Callback(@btnVideo_Callback, eventdata, handles);
set(handles.btnSwitch,'enable','on');
set(handles.editTrans,'enable','on');

set(handles.txtRec,'enable','on');
set(handles.btnSend,'enable','on');
set(handles.btnClear,'enable','on');
c=1;
rx='';
break;
else
try fclose(s);
catch
end
end
end
if ~c
set(handles.txtStatus,'foregroundColor','r');
set(handles.txtStatus,'string','Connect failed');
set(handles.btnRetry,'visible','on');
end
else
set(handles.txtStatus,'foregroundColor','r');
set(handles.txtStatus,'string','No available serial port. Check connect');
set(handles.btnRetry,'visible','on');
end
%-----------------function Video_Callback(obj, eventdata, handles)
global video
video=videoinput('winvideo',1);
video.ReturnedColorspace = 'rgb';
vidRes = video.VideoResolution;
59
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc


Thiết kế và chế tạo cánh tay robot 5 bậc tự do phân loại sản phẩm trên băng tải sử dụng cơng nghệ xử lí ảnh

nBands = video.NumberOfBands;
hImage = image( zeros(vidRes(2), vidRes(1), nBands),'Parent', handles.axes3);
preview(video, hImage); % 160*120
%-------------------------------------------------------------------------function imageProccessing_Callback(obj,eventdata,handles)
global video nObj pObj
I = getsnapshot(video);
hsvI = rgb2hsv(I);
hueI = round(hsvI(:,:,1)*360);
satI = hsvI(:,:,2);
valI = hsvI(:,:,3);
threshI = (satI>=0.3)&(satI<=1)&(valI>=0.2)&(valI<=1);
red = ((hueI<15)|(hueI>=330))&threshI;
red = medfilt2(red, [3 3]);
red = bwareaopen(red,25000); %loai bo cac doi tuong nho hon 200px
red = imclearborder(red); %Xoa cac ket noi voi vien hinh anh - lket 8
bwRed = imfill(red, 'holes'); %lap day cac lo
yellow = ((hueI>30)&(hueI<=90))&threshI;
yellow = medfilt2(yellow, [3 3]);
yellow = bwareaopen(yellow,25000);
yellow = imclearborder(yellow);
bwYellow = imfill(yellow, 'holes');
blue = ((hueI>210)&(hueI<=270))&threshI;
blue = medfilt2(blue, [3 3]);

blue = bwareaopen(blue,25000);
blue = imclearborder(blue);
bwBlue = imfill(blue, 'holes');
axes(handles.axes4);
imshow(I);
hold on
% RED
60
SVTH: Vi Đông Vũ
Lê Mai Xuân

GVHD: TS. Ngô Thanh Nghị
TS. Nguyễn Danh Ngọc