ĐẠI HỌC QUỐC GIA HÀ NỘI

TRƯỜNG ĐẠI HỌC CÔNG NGHỆ

BÁO CÁO CUỐI KỲ II

Môn: Thiết bị điện
GV: Bùi Thanh Lâm

Đề tài: Cánh tay robot học lệnh gắp sản phẩm

Nhóm 6:
Họ và Tên Mã Sinh Viên
Nguyễn Văn Đồng 19021587
Nguyễn Xuân Chiến 19021580
Hoàng Ngọc Đức 19021588
Nguyễn Thành Công 19021581
Hồ Thức Nhân 19021613

Hà Nội 2022
 MỤC LỤC
LỜI MỞ ĐẦU................................................................................................................2
Chương I: Tổng quan về cánh tay robot học lệnh gắp sản phẩm....................................3
1. Đặt vấn đề..............................................................................................................3
2. Mục đích của đề tài.................................................................................................3
3. Đối tượng và phạm vi nghiên cứu...........................................................................3
4. Phương pháp nghiên cứu khoa học.........................................................................3
5. Ý nghĩa khoa học và thực tiễn đề tài.......................................................................4
Chương II: Thiết kế cánh tay robot gắp sản phẩm.........................................................4
2.1 Thiết kế mô hình cánh tay.....................................................................................4
2.2 Ứng dụng và lợi ích của cánh tay robot.................................................................6
Chương III: Phân tích và thiết kế mô hình.....................................................................7
3.1 Sơ đồ điều khiển vị trí cánh tay.............................................................................7
3.2 Các thiết bị sử dụng trong mô hình.......................................................................8
3.2.1 Arduino..........................................................................................................8
3.2.2 Động cơ servo.............................................................................................12
3. Phần mềm điều khiển...........................................................................................14
3.3.1: Phần mềm Arduino IDE..............................................................................14
3.3.2 Các thao tác hoạt động của cánh tay.............................................................15
3.3.3 Lưu đồ thuật toán.........................................................................................15
3.3.4 Mã nguồn chương trình điều khiển...............................................................16
KẾT LUẬN.................................................................................................................23
1. Kiểm tra tính ổn định...........................................................................................23
2. Hướng phát triển..................................................................................................23
TÀI LIỆU THAM KHẢO............................................................................................24
LỜI MỞ ĐẦ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óa ngày càng được chú trọng. Tự động hóa các quá trình sản
xuất cho phép cải thiện điều kiện sản xuất. Các quá trình sản xuất sử dụng quá nhiều
lao động sống rất dễ mất ổn định về giờ giấc, về chất lượng gia công và năng suất lao
động, gây khó khăn cho việc điều hành và quản lý sản xuất. Các quá trình sản xuất tự
động cho phép loại bỏ các nhược điểm trên . Đồng thời tự động hóa đã thay đổi tính
chất lao động, cải thiện điều kiện làm việc của công nhân, nhất là trong các khâu độc
hại, nặng nhọc, có tính lặp đi lặp lại nhàm chán, khắc phục dần sự khác nhau giữa lao
động trí óc và lao động chân tay.

Do vậy, ứng dụng robot vào sản xuất là tính tất yếu của quá trình tự động hóa. Việc áp
dụng các cánh tay robot vào dây chuyền sản xuất ngày càng phổ biến. Xuất phát từ
những vấn đề mà thực tiễn đề ra như trên, bản báo cáo này sẽ nghiên cứu cũng như
“Thiết kế và chế tạo cánh tay robot gắp sản phẩm”.

2
Chương I: Tổng quan về cánh tay robot học lệnh gắp sản phẩm

1. Đặt vấn đề
Thế giới hiện nay đang trong quá trình cách mạng công nghiệp 4.0, nó mang đến
cho nhận loại cơ hội để thay đổi bộ mặt các nền kinh tế. Cùng với đó là những hứa
hẹn về cuộc ‘đổi đời’ của các doanh nghiệp tại Việt Nam. Tự động hóa và robot là
một xu hướng quan trọng của tương lai nền công nghiệp. Tự động hóa đem lại
mức độ chính xác và năng xuất cao hơn. Công nghệ này thậm chí có thể hoạt động
tốt ở một số môi trường làm việc khắc nghiệt không an toàn với con người. Mục
đích để phục vụ sản xuất, thậm chí phục vụ nhu cầu giải trí cũng như chăm sóc con
người. Với sự phát triển mạnh mẽ về công nghiệp máy tính và cách mạng công
nghiệp 4.0 trên thế giới cũng như ở nước ta, công nghiệp tự động hóa và robot
cũng phát triển nở rộ đi kèm theo đó là nhu cầu về nguồn lao động có trình độ có
thể làm chủ được công nghệ.

2. Mục đích của đề tài

- Nghiên cứu về cấu tạo, nguyên lý hoạt động và các phương pháp điều khiển thích
hợp trên cơ sở ứng dụng Arduino để hiểu được bộ điều khiển cánh tay robot gắp
sản phẩm
- Vận dụng các kiến thức đã học và tìm hiểu xây dựng mô hình cánh tay robot có thể
hoạt động và lập trình được.

3. Đối tượng và phạm vi nghiên cứu

Đối tượng nghiên cứu của đề tài là mô hình cánh tay robot sử dụng mạch Arduino
để điều khiển. Nghiên cứu tổng quan về cấu tạo nguyên lý hoạt động của cánh tay
robot và mạch điều khiển.

4. Phương pháp nghiên cứu khoa học

- Nghiên cứu cơ sở khoa học và thực tiễn về chế tạo, điều khiển robot.
- Nghiên cứu về phương trình động học của robot
- Nghiên cứu và ứng dụng các phần gia công cơ khí để chế tạo cánh tay robot.
- Nghiên cứu và ứng dụng mạch Arduino để điều khiển cánh tay robot.

3
 5. Ý nghĩa khoa học và thực tiễn đề tài
- Kết quả nghiên cứu của đề tài có thể được ứng dụng xây dựng cánh tay robot tự
động gắp sản phẩm, dựa theo hình dáng, hay chia sản phẩm từ trên băng truyền
xếp vào thùng/khay. Ngoài ra cánh tay máy còn có thể ứng dụng vào hàn, cắt hay
chỉ đơn giản là nâng vật nặng.
- Mô hình kết quả của đề tài có thể làm thiết bị thí nghiệm thực hành tốt và trực quan
cho sinh viên .

Chương II: Thiết kế cánh tay robot gắp sản phẩm

Về mặt truyền động và điều khiển robot được cấu tạo từ những khối cấu trúc cơ
khí hoạt động nhờ các cơ cấu tác động. Về mặt kết cấu robot được chế tạo rất khác
biệt nhau nhưng chúng được xây dựng từ những thành phần cơ bản như tay máy,
nguồn cung cấp, bộ điều khiển. Trong đó tay máy là tập hợp các bộ phận và cơ cấu
cơ khí được thiết kế và hình thành các khối có chuyển động tương đối với nhau. Vì
vậy trong chương 2, chúng em sẽ đi xây dựng mô hình cánh tay của robot 3 bậc.
Chương này gồm những nội dung sau: Ứng dụng của robot trong đề tài, thiết kế mô
hình cánh tay robot, xây dựng phương trình động học cho robot 3 bậc tự do.

2.1 Thiết kế mô hình cánh tay

Hệ thống chuyển động robot công nghiệp đảm bảo cho robot có thể thực hiện các
nhiệm vụ trong không gian làm việc bao gồm các chuyển động của thân, cánh tay, cổ
tay giữa các vị trí hoặc chuyển động theo một quỹ đạo đặt trước. Bộ phận cơ bản của
robot là: Cánh tay (gồm một số thanh nối cứng liên kết với nhau bởi các khớp mềm);
thân (bệ); cổ tay; bàn tay.

● Khớp robot

4
 Khớp là khâu liên kết hai thanh nối có chức năng truyền chuyển động để thực
hiện di chuyển của robot.
Khớp robot gồm hai loại: khớp tịnh tiến và khớp quay.

Hình 2.2: Khớp robot

● Cổ tay robot
Cổ tay robot có nhiệm vụ định hướng chính xác bàn tay robot (cơ cấu tác động
cuối) trong không gian làm việc. Thông thường cổ tay robot có 3 bậc tự do
tương ứng với 3 chuyển động có cấu tạo điển hình như hình 2.3: Cổ tay xoay
xung quanh trục thanh nối cuối cùng (Roll), cổ tay xoay xung quanh trục nằm
ngang tạo chuyển động lên xuống của bàn tay (Pitch) quay xung quanh trục
thẳng đứng tạo chuyển động lắc phải, trái của bàn tay (Yaw).

● Bàn tay robot ( cơ cấu tác động cuối)

Bàn tay được gắn lên cổ tay robot đảm bảo cho robot thực hiện các nhiệm vụ
khác nhau trong không gian làm việc. Cơ cấu bàn tay có hai dạng khác nhau
tùy theo chức năng của robot trong dây chuyền sản xuất: cơ cấu tay kẹp và cơ

5
 cấu dụng cụ (Các dụng cụ là: mũi hàn, dụng cụ cắt đá, mài đá, một bình sơn, cơ
cấu hàn điểm, hàn hồ quang...)
● Mô hình robot được thiết kế

2.2 Ứng dụng và lợi ích của cánh tay robot

 Ứng dụng
- Ứng dụng của cánh tay robot trong công nghiệp rất đa dạng, tùy vào những ngành
nghề, công việc khác nhau mà ta có thể áp dụng những robot công nghiệp riêng biệt.
Dưới đây là một số ngành trong hệ thống sản xuất mà áp dụng robot công nghiệp.
- Công nghiệp đúc: cánh tay robot làm nhiệm vụ rót kim loại nóng chảy vào khuôn,
cắt mép thừa, làm sạch vật đúc hoặc làm tăng bền vật đúc bằng cách phun cát.
- Ngành gia công áp lực: các quá trình hàn và nhiệt luyện thường bao gồm nhiều công
việc độc hại và ở nhiệt độ cao, điều kiện làm việc khá nặng nề, dễ gây mệt mỏi nhất là
ở trong các phân xưởng rèn dập.
- Ngành gia công và lắp ráp: cánh tay robot thường được sử dụng vào những việc như
tháo lắp phôi và sản phẩm cho các máy gia công bánh răng, máy khoan, máy tiện bán
tự động.
- Cánh tay robot được sử dụng để thực hiện các công đoạn lắp ráp linh kiện điện tử,
xử lý các công cụ máy và hàn hồ quang.
- cánh tay robot sử dụng trong ngành sản xuất ô tô

 Lợi ích
- Cài đặt nhanh: Ngay cả các nhân viên vận hành chưa được đào tạo cũng ngạc nhiên
khi lần đầu tiên cài đặt cánh tay robot. Tháo gỡ, lắp ráp robot và lập trình nhiệm vụ
đơn giản, đầu tiên thường chỉ mất ít hơn 1 giờ làm việc. Theo trải nghiệm của khách
hàng chúng tôi, thời gian trung bình để tiến hành cài đặt hoàn chỉnh chỉ mất nửa ngày.
6
- Lập trình đơn giản: Việc lập trình trên các cánh tay robot dễ dàng nhanh chóng cho
nên các nhân viên không có nhiều kinh nghiệm vẫn có thể nhanh chóng lập trình cánh
tay robot nhờ hiển thị 3D trực quan được cấp bằng sáng chế. Tất cả những gì nhân
viên vận hành cần làm là di chuyển cánh tay robot đến tọa độ mong muốn hoặc chạm
vào các phím mũi tên trên máy tính bảng có màn hình cảm ứng dễ sử dụng.
- Triển khai linh hoạt: Ngày nay các cài đặt sản phẩm cần linh hoạt và nhanh chóng để
đáp ứng các nhu cầu không ngừng thay đổi và mang tính cạnh tranh cao. cánh tay
robot không bao giờ hạn chế bạn; ngược lại, những cánh tay robot gọn nhẹ dễ dàng di
chuyển và tái triển khai sang các quy trình mới, cho phép bạn tự động hóa hầu như bất
kỳ công việc thủ công nào, kể cả công việc với phạm vi hoạt động rất nhỏ hoặc thay
đổi liên tục
- Nâng cao hiệu quả lao động: Nhờ được lập trình sẵn với các chức năng tương tự như
cánh tay người, các thiết bị này được ứng dụng vào rất nhiều lĩnh vực khác nhau với
mức năng suất cực kỳ hiệu quả, từ: hàn, sơn, lắp ráp các bảng mạch in, dán nhãn, hỗ
trợ xử lý vật liệu, kiểm tra sản phẩm và thử nghiệm…
- Tiết kiệm không gian và chi phí: Không cần phải sử dụng đến nhiều nguồn lực hay
các máy móc to lớn, 1 cánh tay robot 3 bậc có thể dư sức hoàn thành các công việc 1
cách nhanh chóng, giúp tiết kiệm chi phí thuê nhân công, chi phí bảo trì, bảo dưỡng
máy móc, chi phí thuê mặt bằng…
- Hạn chế sai sót: Với sự lập trình, tính toán cẩn thận trong từng chi tiết, những cánh
tay robot chắc chắn sẽ đảm bảo mọi thao tác được tiến hành một cách chuẩn xác, Điều
đó có nghĩa là chúng ta sẽ chẳng bao giờ để xảy ra các lỗi sai, các vấn đề trục trặc
trong quá trình sản xuất.

Chương III: Phân tích và thiết kế mô hình

3.1 Sơ đồ điều khiển vị trí cánh tay

7
Bài toán điều khiển cánh tay robot là đưa khâu chấp hành đến vị trí làm việc mong
muốn. Nguyên lý điều khiển trí cánh tay được trình bày như Hình. được so sánh với
các giá trị thông khớp hiện tại được mắt người phản hồi về, sai lệch sẽ qua bộ điều
khiển tác động đến các động cơ của cánh tay. Từ các thông số khớp hiện tại được lấy
liên tục từ mắt, vị trí hiện tại của khâu chấp hành sẽ tính được theo thời gian thực.

3.2 Các thiết bị sử dụng trong mô hình

Để có thể hoàn thành hệ thống thì phần cứng là một trong những yêu tố quan trọng
nhất. Chương này sẽ giới thiệu về các phần cứng sử dụng trong hệ thống và theo sau
đó là các thông số kỹ thuật cũng như cái nhìn chung về cách kết nối phần cứng

3.2.1 Arduino
● Giới thiệu về Arduino
Arduino là một bo mạch vi xử lý được dùng để lập trình tương tác với các thiết
bị phần cứng như cảm biến, động cơ, đèn hoặc một số thiết bị khác. Lý do
Arduino được biết đến rộng rãi là nhờ môi trường phát triển ứng dụng cực kỳ
dễ sử dụng, ngôn ngữ lập trình dễ tiếp cận. Hơn cả thế nữa điều đặc biệt nhất
tạo nên cộng đồng Arduino mạnh mẽ là vì Arduino có mức giá rất thấp, mã
nguồn từ phần cứng đến phần mềm đều mở.
● Giới thiệu Arduino Uno

8
  Vi điều khiển

Arduino UNO có thể sử dụng 3 vi điều khiển họ 8bit AVR là ATmega8,

ATmega168, ATmega328. Bộ não này có thể xử lý những tác vụ đơn giản như
điều khiển đèn LED nhấp nháy, xử lý tín hiệu cho xe điều khiển từ xa, làm một
trạm đo nhiệt độ - độ ẩm và hiển thị lên màn hình LCD

 Năng lượng

9
Arduino UNO có thể được cấp nguồn 5V thông qua cổng USB hoặc cấp nguồn
ngoài với điện áp khuyên dùng là 7-12V DC và giới hạn là 6-20V. Nếu cấp
nguồn vượt quá ngưỡng giới hạn trên sẽ làm hỏng Arduino UNO.

 Các chân năng lượng

GND (Ground): cực âm của nguồn điện cấp cho Arduino UNO. Khi dùng các
thiết bị sử dụng những nguồn điện riêng biệt thì những chân này phải được nối
với nhau.

5V: cấp điện áp 5V đầu ra. Dòng tối đa cho phép ở chân này là 500mA.

3.3V: cấp điện áp 3.3V đầu ra. Dòng tối đa cho phép ở chân này là 50mA.

Vin (Voltage Input): để cấp nguồn ngoài cho Arduino UNO, nối cực dương của
nguồn với chân này và cực âm của nguồn với chân GND.

IOREF: Điện áp hoạt động của vi điều khiển trên Arduino UNO có thể được đo
ở chân này. Và dĩ nhiên nó luôn là 5V. Mặc dù vậy chúng ta không được lấy
nguồn 5V từ chân này để sử dụng bởi chức năng của nó không phải là cấp
nguồn.

RESET: việc nhấn nút Reset trên board để reset vi điều khiển tương đương với
việc chân RESET được nối với GND qua 1 điện trở 10KΩ.

 Bộ nhớ

Vi điều khiển Atmega328 tiêu chuẩn cung cấp cho người dùng:

● 32KB bộ nhớ Flash: những đoạn lệnh bạn lập trình sẽ được lưu trữ trong
bộ nhớ Flash của vi điều khiển. Thường thì sẽ có khoảng vài KB trong
số này sẽ được dùng cho bootloader nhưng đừng lo, bạn hiếm khi nào
cần quá 20KB bộ nhớ này đâu.
● 2KB cho SRAM (Static Random Access Memory): giá trị các biến bạn
khai báo khi lập trình sẽ lưu ở đây. Bạn khai báo càng nhiều biến thì
càng cần nhiều bộ nhớ RAM. Tuy vậy, thực sự thì cũng hiếm khi nào bộ
nhớ RAM lại trở thành thứ mà bạn phải bận tâm. Khi mất điện, dữ liệu
trên SRAM sẽ bị mất.
● 1KB cho EEPROM (Electrically Eraseble Programmable Read Only
Memory): đây giống như một chiếc ổ cứng mini – nơi bạn có thể đọc và
ghi dữ liệu của mình vào đây mà không phải lo bị mất khi cúp điện
giống như dữ liệu trên SRAM.

10
  Các cổng vào/ra

Arduino UNO có 14 chân digital dùng để đọc hoặc xuất tín hiệu. Chúng chỉ có
2 mức điện áp là 0V và 5V với dòng vào/ra tối đa trên mỗi chân là 40mA. Ở
mỗi chân đều có các điện trở pull-up từ được cài đặt ngay trong vi điều khiển
ATmega328 (mặc định thì các điện trở này không được kết nối).

Một số chân digital có các chức năng đặc biệt như sau:

● 2 chân Serial: 0 (RX) và 1 (TX): dùng để gửi (transmit – TX) và nhận

(receive – RX) dữ liệu TTL Serial. Arduino Uno có thể giao tiếp với
thiết bị khác thông qua 2 chân này. Kết nối bluetooth thường thấy nói
nôm na chính là kết nối Serial không dây. Nếu không cần giao tiếp
Serial, bạn không nên sử dụng 2 chân này nếu không cần thiết
● Chân PWM (~): 3, 5, 6, 9, 10, và 11: cho phép bạn xuất ra xung PWM
với độ phân giải 8bit (giá trị từ 0 → 28-1 tương ứng với 0V → 5V) bằng
hàm analogWrite(). Nói một cách đơn giản, bạn có thể điều chỉnh được
điện áp ra ở chân này từ mức 0V đến 5V thay vì chỉ cố định ở mức 0V
và 5V như những chân khác.
● Chân giao tiếp SPI: 10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK). Ngoài
các chức năng thông thường, 4 chân này còn dùng để truyền phát dữ liệu
bằng giao thức SPI với các thiết bị khác.
● LED 13: trên Arduino UNO có 1 đèn led màu cam (kí hiệu chữ L). Khi
bấm nút Reset, bạn sẽ thấy đèn này nhấp nháy để báo hiệu. Nó được nối
với chân số 13. Khi chân này được người dùng sử dụng, LED sẽ sáng.

Arduino UNO có 6 chân analog (A0 → A5) cung cấp độ phân giải tín hiệu
10bit (0 → 210-1) để đọc giá trị điện áp trong khoảng 0V → 5V. Với chân
AREF trên board, bạn có thể để đưa vào điện áp tham chiếu khi sử dụng các

11
 chân analog. Tức là nếu bạn cấp điện áp 2.5V vào chân này thì bạn có thể dùng
các chân analog để đo điện áp trong khoảng từ 0V → 2.5V với độ phân giải
vẫn là 10bit.

Đặc biệt, Arduino UNO có 2 chân A4 (SDA) và A5 (SCL) hỗ trợ giao tiếp
I2C/TWI với các thiết bị khác.

3.2.2 Động cơ servo

- Động cơ servo có góc quay 0-180o

- Động cơ servo được thiết kế cho những hệ thống hồi tiếp vòng kín.

- Khi động cơ quay, vận tốc và vị trí sẽ được hồi tiếp về mạch điều khiển

- Chức năng chính của Servo đó là điều khiển vị trí, thay đổi tốc độ chính xác, điều
chỉnh momen phù hợp với những ứng dụng công việc.

 Động cơ RC servo 9G

Động cơ RC Servo 9G là động phổ biến dùng trong các mô hình điều khiển nhỏ và
đơn giản như cánh tay robot. Động cơ có tốc độ phản ứng nhanh, được tích hợp sẵn
Driver điều khiển động cơ, dễ dàng điều khiển góc quay bằng phương pháp điều độ
rộng xung PWM.

Đặc điểm:

- Kích thước: 23mmX12.2mmX2i9mm

- Trọng lượng: 9 gam

12
- Điện áp hoạt động: 4.2-6V
- Nhiệt độ: 0 ℃ – 55 ℃
- Tốc độ: 0.3 giây / 60 độ
 Khung cánh tay robot

13
 3. Phần mềm điều khiển

3.3.1: Phần mềm Arduino IDE

Hình 3.1: Giao diện phần mềm IDE

 Môi trường phát triển tích hợp (IDE) của Arduino là một ứng dụng
crossplatform (nền tảng) được viết bằng Java, và từ IDE này sẽ được sử
dụng cho ngôn ngữ lập trình xử lý (Processing programming language) và
project Wiring. Nó được thiết kế để dành cho những người mới tập làm
quen với lĩnh vực phát triển phần mềm. Nó bao gồm một chương trình code
editor với các chức năng như đánh đầu cú pháp, tự động brace matching, và
tự động canh lề, cũng như compile(biên dịch) và upload chương trình lên
board chỉ với 1 cú nhấp chuột. Một chương trình hoặc code viết cho
Arduino được gọi là một sketch.
 Các chương trình Arduino được viết bằng C hoặc C++. Arduino IDE đi kèm
với một thư viện phần mềm được gọi là "Wiring", từ project Wiring gốc, có thể
giúp các thao tác input/output được dễ dàng hơn. Người dùng chỉ cần định nghĩa 2
hàm để tạo ra một chương trình vòng thực thi (cyclic executive) có thể chạy được
- setup() : hàm này chạy mỗi khi khởi động một chương trình, dùng để thiết lập các cài
đặt
- loop() : hàm này được gọi lặp lại cho đến khi tắt nguồn board mạch
 Một chương trình điển hình cho một bộ vi điều khiển đơn giản chỉ là làm
cho một bóng đèn Led sáng/tắt. Trong môi trường Arduino, ta sẽ phải viết
một chương trình giống như sau:

14
3.3.2 Các thao tác hoạt động của cánh tay
● Thiết lập cánh tay robot về trạng thái ban đầu
● Quay cánh tay máy về hướng điểm gắp vật
● Hạ cánh tay máy
● Mở tay gắp và kẹp
● Hạ tay gắp, kẹp xuống
● Gắp, kẹp vật thể
● Di chuyển đến vị trí khác

3.3.3 Lưu đồ thuật toán

15
  Khi setup điều khiển cánh tay robot thông qua các chiết áp khi đạt được vị trí
mong muốn ấn record vị trí đó được lưu lại vào trong ram . Ấn nút play robot
tự động di chuyển theo những vị trí đã record theo tứ tự lần lượt . Khi tạm
dừng chương trình robot sẽ dừng lại khi nào ấn lại nút play robot sẽ hoạt động
trở lại .
 Trong trường hợp mất điện đột ngột khi khởi động lại chương trình ấn nút
play robot sẽ tự động di chuyển theo chương trình đã setup ngay trước đó

3.3.4 Mã nguồn chương trình điều khiển

 Sơ đồ lắp mạch

 Lập trình
16
#define servo_max 5
#define step_max 200
#define start_pause_pin 12
#define record_pin 7
#define ENABLE_EEPROM_PIN 2
#define led_pin 13
#define delay_toc_do 20
byte pin_analog[servo_max] = { A0, A1};
byte pin_servo[servo_max] = {3, 5};
unsigned int A0_value, A1_value;
byte get_goc(byte servo_i)
{
switch (servo_i) {
case 0:
A0_value =analogRead(A0);
return map(A0_value, 0, 1023, 0, 179);
break;
case 1:
A1_value = analogRead(A1);
return map(A1_value, 0, 1023, 0, 179);
break;
default:
break;
}
}
const unsigned int SIZE_MEMORY_EEPROM = 1024;
#include <EEPROM.h>
#include <Servo.h>
Servo servo[servo_max];
int step_move = 0;
byte goc_servo[servo_max][step_max];
byte goc_tam_thoi[servo_max] = { 90 };
void setup()
{
pinMode(led_pin, OUTPUT);
Serial.begin(9600);
pinMode(start_pause_pin, INPUT_PULLUP);
pinMode(ENABLE_EEPROM_PIN, INPUT_PULLUP);
pinMode(record_pin, INPUT_PULLUP);

17
 for (byte i = 0; i < servo_max; i++) {
pinMode(pin_analog[i], INPUT);
servo[i].attach(pin_servo[i]);
}
ADCSRA = ((ADCSRA & (B11111000)) | B00000100);

learn_and_move();
}

void move_servo(byte i, byte goc_i)

{
servo[i].write(goc_i);
}
void nhap_nhay(unsigned int time_delay, byte count)
{
for (byte i = 0; i < count; i++) {

digitalWrite(led_pin, HIGH);
delay(time_delay);
digitalWrite(led_pin, LOW);
delay(time_delay * 2);
}
}
void record(unsigned int step_x)
{
for (byte i = 0; i < servo_max; i++) {
goc_servo[i][step_x] = get_goc(i);
}
nhap_nhay(100, 2);
Serial.println(goc_servo[0][step_x]);
}
void move_all()
{

for (byte i = 0; i < servo_max; i++) {

move_servo(i, goc_tam_thoi[i]);
} // di chuyển
}
void control_servo()
{
byte hieu;

18
 for (byte i = 0; i < servo_max; i++) {
hieu = abs(goc_tam_thoi[i] - get_goc(i));
if ((hieu >= 1) && (hieu < 170)) {
goc_tam_thoi[i] = get_goc(i);
}
} // lấy góc
move_all();
}

void nap_eeprom_sang_ram()
{

step_move = EEPROM.read(SIZE_MEMORY_EEPROM - 1);

for (byte i = 0; i < servo_max; i++) {

for (int step_j = 0; step_j < step_max; step_j++) {

goc_servo[i][step_j] = EEPROM.read(step_j + i * step_max);

}
}
}

void luu_vao_eeprom()
{

EEPROM.write(SIZE_MEMORY_EEPROM - 1, step_move);
delay(15);

nhap_nhay(500, 3);
digitalWrite(led_pin, HIGH);
for (byte i = 0; i < servo_max; i++) {
//Serial.print("luu");
//Serial.println(i);
for (int step_j = 0; step_j < step_move; step_j++) {
EEPROM.write(step_j + i * step_max, goc_servo[i][step_j]);
delay(15);
}
}

nhap_nhay(500, 3);

19
 digitalWrite(led_pin, LOW); // tắt đèn
}

void pause()
{
if (digitalRead(start_pause_pin) == 0) {

while (true) {
digitalWrite(led_pin, 1);
delay(300); // chống nhiễu
Serial.println("PAUSE");

if (digitalRead(ENABLE_EEPROM_PIN) == 0) {
// nhấn lưu eeprom
delay(300);
luu_vao_eeprom();
}

if (digitalRead(start_pause_pin) == 0) {
Serial.println("START");
digitalWrite(led_pin, 0);
delay(300);
goto out_pause;
}

}
}
out_pause:;

void auto_move()
{

Serial.println(step_move);
nhap_nhay(50, 3);
float hieu_f[servo_max];
unsigned int step = 0, step_next;

byte time;
byte thay_doi;
while (true) {

20
 //lấy hiệu góc hiện tại và góc sau

if (step < step_move) {

step_next = step + 1; // không viết : step++
}
else {

step_next = 0;
}

for (byte i = 0; i < servo_max; i++) {

hieu_f[i] = (float(goc_servo[i][step]) - float(goc_servo[i][step_next]));

}

Serial.println(step);
Serial.println(goc_servo[0][step]);
Serial.println(goc_servo[0][step_next]);
Serial.println(hieu_f[0]);
for (float loading = 1.0; loading <= 100.0; loading++) {

for (byte i = 0; i < servo_max; i++) {

goc_tam_thoi[i] = byte(float(goc_servo[i][step]) - (((hieu_f[i]) * loading) /

100.0));
}

Serial.print(loading);
Serial.print("‚");
Serial.print(hieu_f[0]);
Serial.print("‚");
Serial.println(goc_tam_thoi[0]);
if ((loading >= 0.0) && (loading < 20.0)) {
time = delay_toc_do * 2 + 5;
}
else if (loading < 30.0) {
time = delay_toc_do / 2 + 5;
}
else if (loading < 90.0) {
time = delay_toc_do + 5;
}

21
 else {
time = delay_toc_do * 2 + 5;
}

// delay(delay_toc_do);
while ((millis() % delay_toc_do) != 0) {

pause();
} //while

step = 0; // lại từ đầu

}
delay(100);
} //while
}

void learn_and_move()
{
step_move = 0;
while (digitalRead(start_pause_pin) != 0) {
//b1: điều khiển servo bằng biến trở
while (digitalRead(record_pin) != 0) {
control_servo();

if ((digitalRead(start_pause_pin) == 0) && (step_move == 0)) {

nap_eeprom_sang_ram();
}
if ((digitalRead(start_pause_pin) == 0) && (step_move != 0)) {
goto buoc_3;
}
}

record(step_move);
if (step_move < step_max) {
step_move++;
}
}
buoc_3:
step_move--;
auto_move();

22
}

void loop()
{
}

KẾT LUẬN

1. Kiểm tra tính ổn định

Hệ thống đã hoạt động ổn định, đạt được yêu cầu đề ra, tuy
nhiên hoạt động còn nhiều hạn chế chưa tích hợp được nhiều tính năng. Về
cơ bản hệ thống có thể sử dụng.
- Ưu điểm:
+ Mô hình nhỏ gọn
+ Đáp ứng được yêu cầu đề tài.
+ Kết nối ổn định
- Nhược điểm:
+Chưa khai thác hết các tính năng.
+ Vẫn còn độ trễ nhất định
+ Vẫn còn một số lỗi do nhiễu của cảm biến

2. Hướng phát triển

+ Hệ thống có thể được áp dụng trong thực tế, trong các nhà máy
+ Hệ thống có thể nâng cấp thành mô hình nhà máy thông minh với
các khả năng phân loại khác đặc thù hơn.
1.

23
TÀI LIỆU THAM KHẢO

https://uniduc.com/vi/blog/canh-tay-robot-la-gi
https://carnovn.com/tim-hieu-su-ra-doi-cua-canh-tay-robot-3-bac-va-loi-ich-cua-
chung/
https://123docz.net//document/1167908-thiet-ke-che-tao-va-dieu-khien-canh-tay-
robot-3-bac-tu-do.htm
https://baoanjsc.com.vn/tin-hang/arm-robot--nhung-ung-dung-cua-canh-tay-
robot_2_1_31596_vn.aspx
http://arduino.vn/bai-viet/42-arduino-uno-r3-la-gi
https://www.academia.edu/35289814/Canh_Tay_robot_arm_robot_

24