MỤC LỤC

NG I
GIỚI THIỆU CHUNG

1.1 Giới thiệu chung

Năm 1921 từ Robot xuất hiện lần đầu trong vở kịch “Rossum’s Universal
Robots” của nhà viết kịch viễn tưởng người Sec, Karel Capek. Trong vở kịch này ông
dùng từ “Robot” biến thể của từ gốc slavo “Rabota” để gọi một thiết bị lao công do
con người tạo ra (nhân vật Rossum).
Vào những năm 40 nhà văn viễn tưởng Nga, Issac Asimov, mô tả robot là một
chiếc máy tự động, mang diện mạo của con người được điều khiển bằng một hệ thần
kinh khả trình Positron, do chính con người lập trình. Asimov cũng đặt tên cho ngành
nghiên cứu về robot là robotics, trong đó có 3 nguyên tắc cơ bản:
- Robot không được xúc phạm con người và không gây tổn hại cho con người.
- Hoạt động của robot phải tuân theo các quy tắc do con người đặt ra. Các quy
tắc này không được vi phạm nguyên tắc thứ nhất.
- Một robot cần phải bảo vệ sự sống của mình, nhưng không được vi phạm hai
nguyên tắc trước.
Các nguyên tắc trên sau này trở thành nền tảng cho việc thiết kế robot, từ sự hư
cấu của khoa học viễn tưởng robot dần dần được giới kĩ thuật hình dung như những
chiếc máy đặc biệt được con người phỏng tác theo cấu tạo và hoạt động của chính
mình, dùng để thay thế con người trong một số công việc nhất định. Để hoàn thành
nhiệm vụ đó robot cần có khả năng cảm nhận các thông số trạng thái của môi trường
và tiến hành các hoạt động tương tự con người.
Khả năng hoạt động của robot được đảm bảo bởi hệ thống cơ khí gồm: cơ cấu
vận động để đi lại và cơ cấu hành động để có thể làm việc. Việc thiết kế và chế tạo hệ
thống này thuộc lĩnh vực khoa học về cơ cấu truyền động, chấp hành và vật liệu cơ
khí. Chức năng cảm nhận gồm thu nhận tín hiệu về trạng thái môi trường và trạng thái
của bản thân hệ thống do các cảm biến và các thiết bị liên quan thực hiện, được gọi là
hệ thống thu nhận và xử lý tín hiệu, hay đơn giản là hệ thống cảm biến.
Muốn phối hợp hoạt động của hai hệ thống trên, đảm bảo cho robot có thể tự
điều chỉnh hành vi của mình và hoạt động đúng theo chức năng quy định trong điều

1
kiện môi trường thay đổi, trong robot phải có hệ thống điều khiển. Xây dựng các hệ
thống điều khiển thuộc phạm vi điện tử, kỹ thuật điều khiển và công nghệ thông tin.

Hình 1.1 Một số hình ảnh về robot công nghiệp

Robot sử dụng trong công nghiệp đòi hỏi tính linh hoạt, hoạt động nhanh, tinh vi
và chuẩn xác, có khả năng thay thế con người hoạt động trong môi trường nguy hiểm,
độc hại. Robot 3 bậc tự do tọa độ cầu là một robot linh hoạt, chính xác, có tính ứng
dụng trong các dây chuyền công nghiệp, có thể được phát triển để ứng dụng vào việc
lắp ráp các sản phẩm trong các dây chuyền sản xuất hiện đại.
1.2 Các vấn đề đặt ra
Với đề tài “Nghiên cứu, thiết kế Robot 3 bậc tự do tọa độ cầu” yêu cầu:
- Thiết kế và chế tạo hệ thống cơ khí chính xác, đảm bảo hệ thống hoạt động
chắc chắn, chạy êm, cơ cấu di chuyển tới vị trí mong muốn.
- Giải quyết bài toán động học và động lực học làm cơ sở để điều khiển robot.
- Sử dụng phần mềm matlab để tính toán, mô phỏng robot
- Thiết kế hệ thống điều khiển cho robot: mạch điều khiển, mạch công suất.
1.3 Phương pháp nghiên cứu

2
 Trong một hệ thống cơ điện tử, phương pháp thiết kế có ý nghĩa rất quan trọng,
mang tính tổng quát, tiên phong. Để hoàn thành đề tài, nhiều kiến thức chuyên ngành
cơ điện tử và kiến thức liên ngành đã được áp dụng trong các hệ thống cơ khí, điện tử,
công nghệ thông tin, điều khiển hệ thống.
Đồ án sử dụng các phương pháp:
- Nghiên cứu cơ sở lý thuyết các mô hình, hệ thống thực tế, thiết bị có sẵn

trên thực tế để lựa chọn phương pháp phù hợp.

- Sử dụng các công cụ thiết kế, mô hình hóa và mô phỏng trên máy tính để
đánh giá mô hình thiết kế, và đồng thời loại trừ các sai sót trước khi chế
tạo.
1.4 Phạm vi giới hạn của đề tài
Đây là một đề tài rộng và có nhiều ứng dụng trong thực tế, trên thị trường có rất
nhiều sản phẩm hoàn thiện về cả chất lượng và thẩm mỹ. Tuy nhiên với những giới
hạn về kiến thức, thời gian và kinh phí đề tài giới hạn với những tính năng sau:
- Kết cấu cơ khí:
Robot có ba bậc tự do
Robot có không gian làm việc: (x,y,z) = (475 mm, 475 mm, 715 mm)
- Độ phân giải điều khiển tại các khớp:
Khớp 1: 1 phút
Khớp 2: 1 phút
Khớp 3: 1 phút
- Robot có thể điều khiển theo góc và theo vị trí.
- Xây dựng bộ phần mềm giải quyết bài toán động học thuận, động học ngược,

bài toán thiết kế quỹ đạo trong không gian khớp.

- Xây dựng phần mềm điều khiển robot trên máy tính bằng phương pháp dạy
học.

3
 CHƯƠNG II
TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP

2.1 Tổng quan về robot công nghiệp

Robot công nghiệp có thể được hiểu là máy tự động linh hoạt thay thế từng phần
hoặc toàn bộ các hoạt động cơ bắp và hoạt động trí tuệ của con người trong nhiều khả
năng thích nghi khác nhau. Là thiết bị có thể thao tác với nhiều bậc tự do, được điều
khiển nhờ các chương trình đã được lập trình sẵn và có khả năng lập trình lại được.
Robot có thể thực hiện nhiều công việc thay cho con người với độ chính xác và tin
cậy cao. Ngày nay với nhiều ưu điểm, robot công nghiệp đã trở thành thiết bị tự đông
hóa không thể thiếu cho các hệ thống sản xuất linh hoạt. Robot công nghiệp góp phần
nâng cao năng suất dây chuyền công nghệ, giảm giá thành, nâng cao chất lượng và
khả năng cạnh tranh của sản phẩm, đồng thời cải thiện lao động. Đạt được các mục
tiêu trên là nhờ vào những khả năng to lớn của robot như: làm việc không mệt mỏi,
linh hoạt trong việc chuyển đổi các chức năng giữa các dây chuyền khác nhau, chịu
được phóng xạ và các môi trường làm việc độc hại, nhiệt độ cao, “Cảm nhận” được từ
trường và “Nghe” được cả siêu âm… Robot được dùng thay thế con người trong các
trường hợp trên hoặc thực hiện các công việc tuy không nặng nhọc nhưng đơn điệu,
dễ gây mệt mỏi, nhầm lẫn. Trong ngành cơ khí, robot được sử dụng nhiều trong công
nghệ đúc, hàn, cắt, kim loại, sơn, phun phủ kim loại, tháo lắp vận chuyển phôi, lắp ráp
sản phẩm… Tuy nhiên, trong quá trình hoạt động nếu robot vì một lý do nào đó mà
ngừng hoạt động thì sẽ làm ảnh hưởng đến hoạt động của toàn bộ dây chuyền. Vì vậy,
trong quá trình hoạt động của robot vẫn cần có sự giám sát của con người.
2.2 Cấu trúc cơ bản và phân loại Robot công nghiệp
2.2.1 Cấu trúc cơ bản của Robot công nghiệp
Tùy thuộc vào số khâu và sự tổ hợp các chuyển động quay và tịnh tiến mà tay
máy có các kết cấu khác nhau với vùng làm việc khác nhau. Các kết cấu thường gặp
của robot kiểu tọa độ đề các, tọa độ trụ, tọa độ cầu, robot kiểu SCARA, hệ tọa độ góc
(phỏng sinh)…
- Tay máy Robot công nghiệp: được ví như phần khung xương và cơ bắp
của
robot.

4
 Robot có cứng vững hay không, có tải trọng lớn hay không, có làm việc chính
xác hay không phụ thuộc phần lớn kết cấu của tay máy tobot công nghiệp.

Môi trường:

Đối tượng Phần công tác

Lực, moment
Sensor giám
sát trạng thái
Truyền động cơ khí hệ thống

Sensor giám sát

thông số môi
trường Cơ cấu chấp hành

Hệ thống điều khiển

Giao diện người - Robot

Hình 2.1 Cấu trúc chung của Robot công nghiệp

- Hệ thống cảm biến: được ví như giác quan của Robot, giúp đo đạc các
thông số về trạng thái bản thân robot: vị trí, tốc độ, gia tốc… cũng như
các thông số của môi trường: nhiệt độ, lực, momen…
- Cơ cấu chấp hành: Tạo chuyển động cho các khâu của tay máy, nguồn
động lực của các cơ cấu chấp hành là các loại động cơ như: điện, thủy
lực, khí nén, hoặc sự kết hợp giữa chúng.
- Hệ thống điều khiển: Hệ thống điều khiển được ví như não bộ của robot.
Hệ thống điều khiển đưa ra các lệnh điều khiển cho tay máy robot công
nghiệp, đọc các dữ liệu từ hệ thống cảm biến để so sánh với dữ liệu sẵn
có, tính toán và đưa ra lệnh điều khiển tay máy.
- Giao diện người – Robot: Thường là những phần mềm chuyên dụng, điều
khiển trên máy tính hoặc các tay cầm điều khiển.
Tùy thuộc vào số khâu và sự tổ hợp các chuyển động quay và tịnh tiến mà tay
máy có các kết cấu khác nhau với vùng làm việc khác nhau. Các kết cấu thường gặp

5
của robot là robot kiểu toạn độ Đề các, tọa độ trụ, tọa độ cầu, robot kiểu SCARA, hệ
tọa độ góc (phỏng sinh)…
2.2.2 Phân loại robot công nghiệp
a) Phân loại hình dạng hình học và không gian hoạt động

Robot kiểu tọa độ đề các

Hình 2.2 Robot kiểu tọa độ Đề Các

Robot kiểu tọa độ trụ

Hình 2.3 Robot kiểu tọa độ trụ

Robot kiểu tọa độ cầu

Hình 2.4 Robot kiểu tọa độ cầu

Robot kiểu tọa độ Scara

6
 Hình 2.5 Robot kiểu SCARA
b) Phân loại theo phương pháp điều khiển

Theo phương pháp điều khiển robot công nghiệp được phân ra làm 2 loại: điều
khiển theo mạch hở và điều khiển theo vòng kín.
- Điều khiển theo mạch hở:

Hệ thống Tay máy robot công Môi trường làm

điều khiển nghiệp việc
Hình 2.6 Sơ đồ điều khiển robot theo mạch hở
Đây là phương pháp điều khiển không có phản hồi về trạng thái cũng như môi
trường làm việc của tay máy robot công nghiệp. Do đó, độ chính xác không cao,
phương pháp điều khiển này tương đối đơn giản, thường được áp dụng trong những
trường hợp không đòi hỏi cao về độ chính xác, như vận chuyển phôi liệu hay hàng
hóa… Động cơ được sử dụng trên tay máy robot thường là động cơ bước, động cơ
điện thông thường không phản hồi.
- Điều khiển theo vòng kín:

Hệ thống Tay máy robot công Môi trường

điều khiển nghiệp làm việc

Hệ thống cảm biến

Hình 2.7 Sơ đồ điều khiển robot theo vòng kín

Khác với phương pháp điều khiển theo mạch hở, ở phương pháp điều khiển theo
vòng kín có phản hồi về trạng thái làm việc của tay máy cũng như môi trường mà tay
máy tương tác. Do đó, điều khiển tay máy robot công nghiệp sẽ chính xác hơn. So với
phương pháp điều khiển theo mạch hở, phương pháp này phức tạp hơn do phải trang
bị hệ thống cảm biến để đo các giá trị trạng thái của tay máy như: vị trí, vận tốc, gia
tốc, momen… và các thông số môi trường làm việc.
Với ưu điểm như vậy nên phương pháp điều khiển theo vòng kín ngày càng
được sử dụng nhiều trên các tay máy robot công nghiệp.
7
 c) Phân loại theo ứng dụng

Robot được phân loại tùy theo ứng dụng trong nhiều chuyên ngành công nghiệp
như: robot hàn, robot sơn, robot lắp ráp, robot trong ngành luyện kim…
2.3 Cơ sở lý thuyết
2.3.1 Động học tay máy
a) Động học thuận
Bài toán động học thuận sử dụng phương pháp do Denavit-Hartenberg đề xuất.
Phương pháp đó được sử dụng để biểu thị mối quan hệ giữa 2 khâu với nhau:

Khâ
u
1
2
…
N
Bảng 2.1 Bảng thông số Denavit – Hartenberg

Quy tắc đặt hệ trục tọa độ theo D – H :

Trục zi trùng hướng với hướng của trục zi+1
Trục xi cùng phương với phương pháp tuyến chung của trục zi-1 và trục zi
Gốc tọa độ trên khâu được xác định bởi giao điểm của trục z và trục x đã xác định
của khâu đó. Hướng của trục y được chọn theo hướng của trục z, x theo quy tắc bàn
tay phải.
Với hệ tọa độ đặt cho khâu công tác sẽ chọn hệ tọa độ sao cho gần nhất với hệ tọa độ
gần nó. Hệ tọa độ gốc chọn trục x bất kì.
Phép rời trục tọa độ:
Bản chất của quy tắc D – H là việc rời hệ tọa độ theo quy tắc nhất định để 2 hệ trục
tọa độ trùng khớp nhau. Thứ tự rời như sau:
- Tịnh tiến một đoạn di theo trục zi-1 để xi-1 nằm trên mặt phẳng pháp

tuyến của zi-1 chứa xi.

- Quay một góc θ1 quanh trục zi-1 để xi-1 cùng phương với xi.
- Tịnh tiến một đoạn ai theo trục xi-1 để xi-1 trùng với xi.
- Quay một góc αi quanh trục xi-1(≡xi) để zi-1≡zi.
(d)(θ)(a)(α)
Ma trận biến đổi thuần nhất theo quy tắc D – H:
= (d)(θ)(a)(α)

8
Các bước thiết lập phương trình động học Robot bằng phương pháp Denavit –
Hartenberg:
Bước 1: Đặt hệ trục tọa độ theo quy tắc D - H
Bước 2: Lập bảng thông số động học D – H.
Bước 3: Từ bảng thông số động học D – H thay vào công thức tính được các ma
trận biến đổi thuần nhất tương ứng.
Bước 4: Nhân các ma trận biến đổi thuần nhất này với nhau theo đúng thứ tự
được phương trình động học Robot công nghiệp. =....
Khi các trục khớp cắt nhau nên di chuyển các trục tọa độ có gốc trùng nhau.
b) Động học ngược
Khái niệm: Động học ngược tay máy là việc giải phương trình động học robot
công nghiệp để tìm ra các giá trị của biến khớp khi cần di chuyển khâu công tác tới vị
trí xác định và hướng xác định.
Mục đích của việc giải phương trình động học robot công nghiệp: để điều khiển
tay máy của robot công nghiệp tới vị trí làm việc và có một hướng xác định thì chúng
ta phải tính toán giá trị các biến khớp đã quay hoặc tịnh tiến một lượng là bao nhiêu.
Động học ngược tay máy là cơ sở cho việc điều khiển robot.
Các phương pháp giải bài toán động học ngược :
- Phương pháp số.
- Phương pháp giải tích.
2.3.2 Động lực học tay máy
a) Khái niệm
Bài toán động lực học tay máy là bài toán nghiên cứu mối quan hệ giữa chuyển
động của tay máy và nguyên nhân gây ra các chuyển động đó.
b) Mục đích nghiên cứu bài toán động học tay máy
- Thiết kế bộ điều khiển robot công nghiệp.
- Tính toán chọn động cơ.
c) Phân loại bài toán động lực học tay máy
- Bài toán động lực học thuận: Cho trước lực và momen tác dụng ở mỗi khớp

tay máy, tìm chuyển động của tay máy.

- Bài toán động lực học ngược: Cho trước chuyển động của tay máy, tính toán

lực và momen tác dụng vào các khớp của tay máy.
d) Các phương pháp giải quyết bài toán động lực học tay máy
- Phương pháp sử dụng định lý động năng.
9
 - Phương pháp sử dụng phương trình Lagarange II.
- Phương pháp Newton-Euler.

2.4 Các hệ thống điển hình của Robot công nghiệp

Robot công nghiệp được cấu thành bởi 3 hệ thống chính: hệ thống chấp hành, hệ
thống cảm biến, hệ thống điều khiển.

2.4.1 Hệ thống chấp hành

Hệ thống chấp hành là hệ thống tạo ra nguồn động lực cho mọi chuyển động của
tay máy robot công nghiệp.

Hình 2.8 Sơ đồ hệ thống chấp hành

Công suất nguồn Pp.
Pc là tín hiệu điều khiển.
Công suất điện cung cấp cho động cơ Pa.
Công suất cơ học do động cơ phát ra Pm.
Công suất cơ học cần thiết để làm chuyển động khớp Pu.
Công suất tổn hao: Pda; Pds; Pdt trên mạch khuếch đại công suất, động cơ và hệ thống
dẫn động cơ khí.
a) Truyền dẫn cơ khí

Hệ thống truyền dẫn cơ khí trên tay máy robot công nghiệp có tác dụng truyền
chuyển động và momen từ các động cơ tới các khớp và bộ phận công tác của robot.
Các loại truyền dẫn cơ khí:
- Cơ cấu trục vít bánh vít.
- Cơ cấu vít me đai ốc.
- Truyền động xích.
- Gắn trực tiếp rotor của động cơ tuyến tính lên trục của khớp.
- Truyền động đai răng.
10
Truyền động đai răng được sử dụng rộng rãi trên robot công nghiệp, chúng có ưu
điểm hạn chế trượt, khối lượng nhỏ, truyền được chuyển động, momen giữa các trục
tương đối xa.
b) Động cơ

Động cơ là nguồn tạo động lực chuyển động cho các khớp.
Yêu cầu động cơ trên robot công nghiệp:
Quán tính nhỏ, dải tốc độ làm việc rộng, độ chính xác định vị cao, có thể làm việc
trơn tru ở vận tốc thấp.
Các loại động cơ sử dụng trên robot công nghiệp:
-Động cơ khí nén.
-Động cơ điện một chiều, xoay chiều thông dụng.
-Động cơ Servo.
-Động cơ bước.
-Động cơ thủy lực.
c) Khuếch đại công suất

Biến đổi trạng thái năng lượng của nguồn cung cấp cho phù hợp với yêu cầu của
động cơ.
d) Nguồn cung cấp chính
Nhiệm vụ của nguồn cung cấp chính là cung cấp năng lượng tới đầu vào của bộ
biến đổi.
2.4.2 Hệ thống cảm biến
Cảm biến là thiết bị dùng để nhận giá trị của đại lượng vật lý cần đo và biến đổi
nó thành tín hiệu mà thiết bị đo hay điều khiển có thể xử lý được. Dạng và tín hiệu
xuất ra được chuẩn hóa để dễ ghép nối vào các mạch xử lý.
Cảm biến trong:
- Cảm biến vị trí.
- Cảm biến vận tốc.

Cảm biến ngoài:

- Cảm biến lực.
- Cảm biến kiểu điện trở.
- Cảm biến đo khoảng cách.
- Thiết bị quan sát.

2.4.3 Hệ thống điều khiển

Hệ thống điều khiển có chức năng:
- Chuyển động của các cơ cấu cơ khí.
11
- Thu nhận thông tin trạng thái của hệ thống và môi trường công tác.
- Phân tích thông tin và phản ứng trước điều kiện thực tế.
- Lưu trữ, xử lý và cung cấp thông tin về hệ thống.

12
 CHƯƠNG III
MÔ HÌNH HÓA VÀ MÔ PHỎNG HỆ THỐNG
3.1 Mô hình vật lý
a) Mô hình robot

Hình 3.1 Mô hình robot

Tính toán kích thước các khâu:
Robot có không gian làm việc: (x,y,z) = (475 mm, 475 mm, 715 mm)
 Ta có: l1 + l2 + l3=715(mm)
l2 + l3=475(mm)
Chọn độ dài khâu 1: l1 = 240 mm
Chọn độ dài khâu 2: l2 = 230 mm
Chọn độ dài khâu 3: l3= 245 mm
b) Mô hình được thiết kế trên solidworks

Hình 3.2 Cấu hình và các hệ trục tọa độ gắn trên robot cầu RRR
Khối lượng toàn bộ canh tay: 11,1 kg (tính theo solidworks)

13
 Hình 3.3 Thân dưới.
Khối lượng thân dưới (khâu 1) là: 9295,54 g (tính theo solidworks)

Hình 3.4 Khâu thứ 2

Khối lượng khâu thứ 2: 1594,9 g (tính theo solidworks)

14
 Hình 3.5 Khâu 3
Khối lượng khâu thứ 3: 216,05 g (tính toán theo solidworks)
3.2 Bài toán động học thuận
Quy tắc đặt hệ trục tọa độ theo D – H :
- Trục zi trùng hướng với hướng của trục zi+1.
- Trục xi cùng phương với phương pháp tuyến chung của trục z i-1 và trục zi.
- Gốc tọa độ trên khâu là được xác định bởi giao điểm của trục z và trục x

đã xác định của khâu đó. Hướng của trục y được chọn theo hướng của
trục z, x theo quy tắc bàn tay phải.
Với hệ tọa độ đặt cho khâu công tác sẽ chọn hệ tọa độ sao cho gần giống nhất với hệ
tọa độ gần nó. Hệ tọa độ gốc chọn trục x bất kì:
Khâu
1
2
…
n
Bảng 3.1 Bảng thông số Denavit – Hartenberg
Phép rời trục tọa độ:
Bản chất của quy tắc D – H là việc rời hệ tọa độ theo quy tắc nhất định để 2 hệ trục
tọa độ trùng khớp nhau. Thứ tự rời như sau:
- Tịnh tiến một đoạn di theo trục zi-1 để xi-1 nằm trên mặt phẳng pháp tuyến của z i-1

chứa xi.
- Quay một góc θ1 quanh trục zi-1 để xi-1 cùng phương với xi.
- Tịnh tiến một đoạn ai theo trục xi-1 để xi-1 trùng với xi.
- Quay một góc αi quanh trục xi-1(≡xi) để zi-1≡zi.
(d)(θ)(a)(α)
Ma trận biến đổi thuần nhất theo quy tắc D – H:
= (d)(θ)(a)(α)
15
Các bước thiết lập phương trình động học Robot bằng phương pháp Denavit –
Hartenberg:
- Bước 1: Đặt hệ trục tọa độ theo quy tắc D - H
- Bước 2: Lập bảng thông số động học D – H.
- Bước 3 : Từ bảng thông số dộng học D – H thay vào công thức chung ta
sẽ tính được các ma trận biến đổi thuần nhất tương ứng.
- Bước 4 : Nhân các ma trận biến đổi thuần nhất này với nhau theo đúng
thứ tự ta sẽ được phương trình động học Robot công nghiệp.
=....

Khi các trục khớp cắt nhau ta nên di chuyển các trục tọa độ có gốc trùng nhau.
Áp dụng đối với robot RRR ta có:
Bảng D-H:
Khâu Di θi ai
Bảng 3.2 Bảng 1 L1 0 90 thông số D-H

2 0 L2 0
Hình 3.6 Kết cấu tay máy robot
3 0 L3 0
tọa độ cầu RRR
Ma trận tổng quát:

Ta có:

Ta có:

16
=
=
=
3.3 Bài toán động học ngược
Khái niệm: động học ngược tay máy là việc giải phương trình động học robot
công nghiệp để tìm ra các giá trị của biến khớp khi cần di chuyển khâu công tác tới vị
trí xác định và hướng xác định.
Mục đích của việc giải phương trình động học robot công nghiệp: để điều khiển
tay máy của robot công nghiệp tới vị trí làm việc và có một hướng xác định thì chúng
ta phải tính toán giá trị các biến khớp đã quay hoặc tịnh tiến một lượng là bao nhiêu.
Động học ngược tay máy là cơ sở cho việc điều khiển robot
Các phương pháp giải bài toán động học ngược :
- Phương pháp số.
- Phương pháp giải tích.
Đối với robot cầu RRR:
=
=
=
Bình phương phương trình , sau đó cộng theo vế ta được:
2 2
= + 2 (1)

Ta có:

=
Bình phương phương trình sau đó cộng với phương trình (1) theo vế ta được:
l32+l22+2.l2.l3.cos=2+ 2+ (2
 =cos-1()
bằng cách vẽ hình và mô tả góc quay, ta xác định được các thông số yêu cầu của bài
toán.

17
 Hình 3.7 Phân tích cơ cấu các khâu

Ta tính được :

 = tan-1() – tan-1(l3cos+ l2)

3.4 Động lực học

a) Khái niệm:
Bài toán động lực học tay máy là bài toán nghiên cứu mối quan hệ giữa chuyển động
của tay máy và nguyên nhân gây ra các chuyển động đó.
b) Mục đích nghiên cứu bài toán động học tay máy:
- Tính toán thiết kế kết cấu tay máy.
- Mô phỏng chuyển động của tay máy.
- Thiết kế bộ điều khiển robot công nghiệp.
c) Phân loại bài toán động lực học tay máy:
- Bài toán động lực học thuận: Cho trước lực và momen tác dụng ở mỗi khớp

tay máy, tìm chuyển động của tay máy.

- Bài toán động lực học ngược: Cho trước chuyển động của tay máy, tính toán

lực và momen tác dụng vào các khớp của tay máy.
d) Các phương pháp giải quyết bài toán động lực học tay máy:
- Phương pháp sử dụng định lý động năng.
- Phương pháp sử dụng phương trình Lagrange II.

18
 - Phương pháp newton-euler.
Sử dụng phương pháp Lagrange II đối với robot tọa độ cầu RRR:
Phương trình Lagrange II có dạng như sau:
Mđc
Trong đó:
K là động năng của hệ, gồm:
K=K1+K2(chuyển động song phẳng)
K=K1 (chuyển động quay) K1=
K=K2 (chuyển động tịnh tiến) K2=

P là thế năng của hệ: P=m.g.h

Mđc là momen động cơ
Tính động năng (xét từng khâu):
• Khâu 1: Vì khâu 1 chuyển động quay quanh trục cố định nên ta có:
K1=
J1 là momen quán tính của khâu 1

Hình 3.8 Kết cấu tay máy robot tọa độ cầu RRR
Với khâu 1 được coi là đĩa đồng chất, bán kính r=40mm=0,04m, khối lượng :
kg

K1=0,00066
• Khâu 2: vì khâu chuyển động quay và tịnh tiến nên ta có
K2=K21+K22
Trong đó K21 là động năng tịnh tiến
K21=
19
Trong đó là tọa độ khối tâm của vật so với gốc .
Dựa vào công thức quan hệ giữa các hệ tọa độ trên tay máy công nghiệp

Áp dụng cho khâu 2 ta có:

Trong đó:
là tọa độ điểm xc2 so với gốc O0
là tọa độ điểm xc2 so với gốc O1

=>
=>
=>
=
K21= =
K22=

K22=
K2=+
• Khâu 3: vì khâu 3 chuyển động quay và tịnh tiến nên ta có:
K3=K31+K32
Trong đó:
K31 là động năng tịnh tiến
K32 là động năng quay
K31=
+

20







K31={
K32=
J3=
K32=
 K3={
Động năng của tay máy: K=K1+K2+K3
K=0,00066+++{
 =








Tính thế năng
P=mgh=P1+P2+P3
Trong đó:
P1=
P2= )
P3=
P= )+

Thay vào phương trình Lagarange II ta có:

21
 Mđci
Với động cơ 1 ta có:
Mđc1==
Với động cơ 2 ta có:
Mđc2=-{}+
Mđc3=-
3.5 Mô phỏng số
Để có thể mô phỏng cánh tay robot trên Winform ta cần áp dụng từ bài toán động
học thuận và ngược của cánh tay robot. Sử dụng bảng DH để thành lập các ma trận
truyền, ma trận mô tả vị trí và hướng của khâu cuối so với khâu cố định đầu tiên
Bảng D-H
Khâu Di θi ai αi

1 L1 0 90
2 0 L2 0

3 0 L3 0
Bảng 3.3 Thông số động học D-H
Thiết kế giao diện của bài toán động học thuận và ngược được viết trong chương 4,
dưới đây là giao diện đã thiết kế.
a) Bài toán động học thuận

Nhập các góc của các khâu robot trong bảng điều khiển, nhấn “Tăng hoặc Giảm” để
điều chỉnh sau đó nhấn “Dịch chuyển” ta được tọa độ vị trí công tác.

22
 Hình 3.9 Giao diện động học thuận
b) Bài toán động học ngược

Nhập tọa độ của vị công tác, nhấn “Tăng hoặc Giảm” để điều chỉnh sau đó nhấn “Dịch
chuyển” ta được các góc tại các khâu.

Hình 3.10 Giao diện động học ngược

23
3.6 Mô hình hóa hệ thống điều khiển
3.6.1 Hệ thống mạch điều khiển

Khối hiển thị

Khối bàn phím

ATMEGA 16 Cảm biến

Khối nguồn

Khối công suất

Động cơ

ROBOT

Hình 3.11 Hệ thống điều khiển

Khối điều khiển trung tâm được cấp nguồn 5V từ khối nguồn và kết nối với khối
điều khiển công suất để điều khiển tốc độ và đảo chiều động cơ.
Mạch công suất nhận tín hiệu điều khiển từ khối điều khiển trung tâm để điều
khiển công suất cung cấp cho động cơ, thay đổi chiều quay động cơ.
3.6.2 Khối điều khiển

Yêu cầu hệ thống điều khiển trung tâm là đáp ứng nhanh do đó đòi hỏi một loại
vi điều khiển mạnh, có tốc độ xử lý nhanh, có khả năng ghép nối với nhiều thiết bị như
RF, cảm biến góc và gia tốc, và có thể tích hợp thêm các modul khác nữa.
Các dòng chip trên thị trường rất đa dạng và nhiều chủng loại như chip PIC,
Atmel, Arm… Mỗi loại lại có một ưu điểm riêng song chip Atmel nổi tiếng là một
dòng chip mạnh với tốc độ xử lí khá cao, phù hợp với yêu cầu đề tài. Còn chip ARM
là loại rất mạnh nhưng giá thành cao nếu sử dụng loại này sẽ gây lãng phí.

24
 Vi điều khiển AVR do hãng Atmel sản xuất được giới thiệu lần đầu năm 1996,
AVR có rất nhiều dòng khác nhau bao gồm dòng Tiny AVR ( như AT tiny13, AT tiny
22…) có kích thước bộ nhớ nhỏ, ít bộ phận ngoaiij vi, rồi đến dòng AVR ( chẳng hạn
AT90S8535, AT90S8515,…) có kích thước bộ nhớ vào loại trung vình và mạnh hơn là
dòng Mega ( Atmega16, Atmega32, Atmega128,…) với bộ nhớ có kích thước vài
Kbyte đến vài trăm Kb cùng với các bộ ngoại vi đa dạng được tích hợp trên chip, cũng
có dòng tích hợp cả bộ LCD trên chip (dòng LCD AVR)). Tốc độ của dòng Mega
cũng cao hơn so với các dòng khác, sự khác nhau cơ bản giữa các dòng chính là cấu
trúc ngoại vi, còn nhân thì vẫn như nhau. Đặc biệt, năm 2008, Atmel lại tiếp tục cho ra
đời dòng AVR mới là XmegaAVR, với những tính năng mạnh mẽ chưa từng có ở các
dòng AVR trước đó. Có thể nói XmegaAVR là dòng MCU 8 bít mạnh mẽ nhất hiện
nay.

Hình 3.12 Các dòng chip atmega

Atmega16 là dòng vi điều khiển khá phù hợp, đủ để đáp ứng được các yêu cầu
bài toán đặt ra.

Hình 3.13 Atmega 16

Những đặc tính cơ bản:
25
 - Được chế tạo theo kiến trúc RISC hiệu suất cao mà điện năng tiêu thụ thấp.
- Tập lệnh gồm 131 lệnh, hầu hết đều chỉ thực thi trong 1 chu kì xung nhịp.

Bộ nhân hai chu kì.

- 32 x 8 thanh ghi làm việc đa dụng.
- Hoạt động tĩnh.
- 16 MIPS với thông lượng 16MHz.
- 8KB Flash ROM lập trình được ngay trên hệ thống.
- Giao diện nối tiếp SPI có thể lập trình ngay trên hệ thống.
- Cho phép 1000 lần ghi/xóa.
- Bộ EEPROM 512 byte, cho phép 100.000 lần ghi/xóa.
- Chu kì ghi/xóa: 10.000 Flash/ 100.000 EEPROM.
- Độ bền dữ liệu 20 năm ở 85°C và 100 năm ở 25°C.
- Bộ nhớ SRAM 512 byte.
- Bộ biến đổi ADC 8 kênh, 10 bit.
- 32 ngõ I/O lập trình được.
- Bộ truyền nối tiếp bất đồng bộ vạn năng UART.
- Vcc=2.7V đến 5.5V.
- Tốc độ làm việc: 8 MHz đối với Atmega16L, 16MHz đối với
Atmega16 tối đa .
- Tốc độ xử lí lệnh đến 8 MIPS ở 8 MHz nghĩa là 8 triệu lệnh trên

giây.
- Bộ định thời gian thực (RTC) với bộ dao động và chế độ đếm

tách biệt.
- 2 bộ Timer 8 bit và 1 bộ Timer 16 bit với chế độ so sánh và chia

tần số tách biệt và chế độ bắt mẫu.

- 4 kênh điều chế độ rộng xung PWM.
- Có đến 13 ngắt ngoài và trong.
- Bộ so sánh Analog.
- 6 chế độ ngủ : Idle, ADC Noise Reduction, Power-save, Power-
down, Standby và Extended Standby.
- Giao tiếp nối tiếp Master/Slave SPI.

3.6.3 Khối công suất điều khiển động cơ

Động cơ bước là một thiết bị cơ điện chuyển đổi các xung điện thành những
chuyển động cơ học rời rạc. Trục của động cơ bước quay những bước tăng rời rạc
khi các xung điện điều khiển được áp đến nó theo một trình tự hợp lí. Sự quay
của các động cơ liên hệ trực tiếp với các xung được áp vào. Trình tự của các xung
áp vào quan hệ trực tiếp với hướng quay của trục động cơ. Tốc độ quay của trục
động cơ quan hệ trực tiếp với tần số các xung vào và chiều dài vòng quay thì liên
hệ trực tiếp với số lượng các xung được áp vào.

26
 Hình 3.14 Hoạt động của mạch Driver công suất

Hình 3.15 Cấu tạo nguyên lí động cơ bước

Nguyễn lí điều khiển: Ta cấp xung cho các cuộn dây 1a, 1b, 2a, 2b để động cơ có
thể hoạt động.
Mẫu 1a 1000100010001000100010001
Mẫu 1b 0010001000100010001000100
Mẫu 2a 0100010001000100010001000
Mẫu 2b 0001000100010001000100010

3.6.4 Khối giao tiếp máy tính

a) Khái niệm giao tiếp truyền thông nối tiếp đồng bộ và không đồng bộ
USART hay UART để chỉ thiết bị phần cứng không phải chuẩn giao tiếp. Nó cần
được kết hợp với một thiết bị chuyển đổi mức điện áp để tạo ra một chuẩn giao tiếp
nào đó.
VD: RS 232 (com) trên các máy tính cá nhân là sự kết hợp của chíp UART và
chíp chuyển đổi mức điện áp Max 232. Tín hiệu từ RS 232 là -12V cho mức cao và
+12V cho mức thấp, còn tín hiệu từ UART:TTL: Mức logic High (5V) và Low (0V)

27
 - Truyền thông nối tiếp đồng bộ : Để chỉ sự báo trước trong quá trình truyền,

cách truyền này đòi hỏi it nhất 2 đường truyền cho 1 quá trình: 1 đường dữ
liệu và 1 đường xung nhịp
- Truyền thông nối tiếp không đồng bộ: Chỉ cần 1 đường truyền cho quá trình

truyền hoặc nhận, “Khung dữ liệu” được chuẩn hóa bởi các thiết bị nên không
cần đường xung nhịp báo trước dữ liệu đến. Để quá trình truyền thành công thì
việc tuân thủ các tiêu chuẩn truyền là hết sức quan trọng.
VD: 2 thiết bị đang giao tiếp với nhau theo phương pháp này. Chúng thỏa thuận
với nhau cứ 1ms sẽ có 1 bit dữ liệu truyền đến, thiết bị nhận chỉ cần kiểm tra
hoặc đọc đường truyền theo chu kì đã đặt trước.

Hình 3.16 Tín hiệu tương đương của UART và RS 232

Tuy nhiên 2 thiết bị này cần có 1 tốc độ xung nhịp giống nhau, có một quy ước
khung truyền định sẵn để tránh thất lạc hoặc lỗi khi truyền nhận dữ liệu.

Hình 3.17 Truyền 8 bít theo phương pháp nối tiếp

b) Các khái niệm liên quan đến phương pháp truyền thông nối tiếp:
28
- Baud rate (tốc độ Baud): Như trong ví dụ trên về việc truyền 1 bít trong 1ms ta

thấy rằng, để việc truyền và nhận không đồng bộ diễn ra thành công thì các
thiết bị tham gia phải “Thống nhất” với nhau về 1 khoảng thời gian dành cho
1 bít truyền. Hay tốc độ truyền phải được cài đặt như nhau trước, tốc độ này
gọi là tốc độ Baud. Vậy tốc độ Baud là số bít truyền trong 1s. Ví dụ nếu tốc độ
baud được đặt 19200 thì thời gian dành cho 1 bít truyền là 1/19200~ 52,083us.
- Frame (khung truyền): Do truyền thông nối tiếp không đồng bộ nên rất dễ gặp

phải hiện tượng mất hoặc sai lệch dữ liệu. Do vậy trong quá trình truyền thông
phải tuân theo một số quy cách nhất định, bên cạnh tốc độ Baud thì khung
truyền đóng một vai trò quan trọng tạo nên sự thành công cho việc truyền
nhận. Đó là sự quy định về số bít trong mỗi lần truyền, các bít bắt đầu, kết
thúc, kiểm tra chẵn lẻ, dữ liệu…v..v… Đều được quy định bởi khung truyền.
- Start bit: Start là bit đầu tiên được truyền trong một frame truyền, bit này có

chức năng báo cho thiết bị nhận biết rằng có một gói dữ liệu sắp được truyền
tới. Ở module USART trong AVR, đường truyền luôn ở trạng thái cao khi
nghỉ (Idle), nếu một chip AVR muốn thực hiện việc truyền dữ liệu nó sẽ gởi
một bit start bằng cách “kéo” đường truyền xuống mức 0. Như vậy, với AVR
bit start là mang giá trị 0 và có giá trị điện áp 0V (với chuẩn RS232 giá trị điện
áp của bit start là ngược lại). Start là bit bắt buộc phải có trong khung truyền.
- Parity bit: Parity là bit dùng kiểm tra dữ liệu truyền đúng không (một cách

tương đối). Có 2 loại parity là parity chẵn (even parity) và parity lẻ (odd
parity). Parity chẵn nghĩa là số lượng số 1 trong dữ liệu bao gồm bit parity
luôn là số chẵn. Ngược lại tổng số lượng các số 1 trong parity lẻ luôn là số lẻ.
Ví dụ, nếu dữ liệu của bạn là 10111011 nhị phân, có tất cả 6 số 1 trong dữ liệu
này, nếu parity chẵn được dùng, bit parity sẽ mang giá trị 0 để đảm bảo tổng
các số 1 là số chẵn (6 số 1). Nếu parity lẻ được yêu cầu thì giá trị của parity bit
là 1. Hình 1 mô tả ví dụ này với parity chẵn được sử dụng. Parity bit không
phải là bit bắt buộc và vì thế chúng ta có thể loại bit này khỏi khung truyền
(các ví dụ trong bài này tôi không dùng bit parity).
- Stop bit: Stop bit là một hoặc các bit báo cho thiết bị nhận rằng một gói dữ

liệu đã được gởi xong. Sau khi nhận được stop bits, thiết bị nhận sẽ tiến hành
kiểm tra khung truyền để đảm bảo tính chính xác của dữ liệu. Stop bit là các
bit bắt buộc xuất hiện trong khung truyền, trong AVR USART có thể là 1

29
 hoặc 2 bit (Trong các thiết bị khác Stop bit có thể là 2.5 bit). Trong ví dụ ở
hình 1, có 2 stop bit được dùng cho khung truyền.Giá trị của stop bit luôn là
giá trị nghỉ (Idle) và là ngược với giá trị của start bit, giá trị stop bit trong
AVR luôn là mức cao (5V).

Hình 3.18 Sơ đồ khối giao tiếp máy tính

3.7 Mô phỏng hoạt động
a) Giới thiệu phần mềm mô phỏng
Easy-rob là một phần mềm phục vụ cho việc lập kế hoạch và mô phỏng sản xuất
khi sử dụng các robot trong dây chuyền. Tất cả các chuỗi xử lí khi sử dụng robot ví dụ
như: cầm nắm, lắp ráp, sơn phủ, hàn đều được lập chương trình cụ thể bằng phần mềm
này và các tính toán đó ngay lập tức được cụ thể hóa bằng mô hình 3D ngay trong
phần mềm. Các hoạt động của robot được mô phỏng có thể gồm chỉ 1 robot hoặc cùng
một lúc nhiều robot với các phiên bản cao cấp hơn của phần mềm.
Easy-rob là công cụ kiểm tra nhanh các ý tưởng đưa ra chuyển động của robot.
Easy –rob còn là một phần mềm để ứng dụng vào công việc học tập, nghiên cứu với
hiệu suất cao.
Nâng cao độ tin cậy khi lập trình cũng như thiết kế robot.
Giúp cho sự đầu tư vào robot an toàn.
Giảm thời gian lập trình, tính toán.
Ngoài ra, Easy-rob còn giúp một số khả năng khác như:
- Sắp đặt và lên kế hoạch cho một trạm robot hoạt động
- Kiểm tra khả năng vươn tới của cánh tay, sự xung đột va chạm và không gian

hoạt động của robot.

- Công cụ phục vụ cho việc đào tạo và học tập.
30
 - Khả năng kiểm tra đánh giá và chứng minh tính khả thi của một thiết kế robot

mới nào đó.

b) Thiết kế cánh tay robot
Robot tọa độ cầu RRR được thiết kế như sau:

Hình 3.17 Mô hình robot

c) Mô phỏng chuyển động của robot (file easy-rob đính kèm trong CD)

31
 CHƯƠNG IV
THIẾT KẾ VÀ THI CÔNG HỆ THỐNG
4.1 Thiết kế phần mềm
4.1.1 Trình tự thiết kế trong giao diện Guide
Giới thiệu lập trình Guide trong Winform của Visual Studio:
Giao diện trong Winform của Visual Studio tương đối giống với các chương trình lập
trình giao diện như Visual Basic, C++ hay Matlab,…
Visual studio là một môi trường phát triển tích hợp (IDE). Nó được sử dụng làm
nền tảng phát triển các phần mềm như : Windows API, Windows Form, …
Winform được sử dụng trên nền Visual Studio. Với sự tích hợp đa dạng nhiều ngôn
ngữ lập trình khác nhau: C C++, C# … Giúp người lập trình có thể tiếp cận lập trình
nhanh hơn.
Tổ chức bộ nhớ để lưu trữ các mảng rất tốt, các tính toán trên mảng được thực
hiện khá nhanh.

32
 Việc kiểm tra kết quả được nhanh chóng, dễ dàng kiểm tra chương trình. Đa số
các ngôn ngữ kahcs cần phải biên dịch cả một chương trình. Đa số các ngôn ngữ khác
cần phải biên dịch cả một chương trình không có lỗi rồi mới chạy,.
4.1.2 sử dụng lập trình Guide trong Winform để mô phỏng
Mở phần mềm Visual Studio>> Tạo 1 New Project mới :

Hình 4.1 Cửa sổ bắt đầu

Khi bấm tạo 1 project sẽ có 1 cửa sổ mới hiện ra: Có rất nhiều lựa chọn, ở đây ta
xây dựng Guide trên nền Window form sử dụng ngôn ngữ C# nên ta sẽ chọn: Visual
C# >> Windows >> window Form Application.

Hình 4.2 Cửa sổ lựa chọn

Ta được cửa sổ Windows Form hiện ra.

33
 Hình 4.3 Giao diện thiết kế
Giao diện khá giống với các giao diện như : Matlab, Visual C++, … Di chuột qua
bên hộp Toolbox với các biểu tượng ở bên trái sẽ thấy tên của các điều khiển. Một số
nút điều khiển hay dùng.
Button: Là nút Command Button trong Win Form, có thể là các nút Ok , Yes hay
Cancel, … mà ta vẫn hay dung
Label : Đánh tên nhãn hay đối tượng
CheckBox
ListBox
ComboBox
Edit Text
Text box
Picture Box
Button Group
…
Ta sẽ kéo thử vào trong giao diện một vài đối tượng điều khiển: TextBox , Label và
Button.

34
 Hình 4.4 Lấy các đối tượng
Để thay đổi các thuộc tính của các điều khiển:
Ta quan sát bên phải có cửa sổ các thuộc tính của điều khiển. Có thể sắp xếp théo chức
năng hoặc theo thứ tự A-Z của tên thuộc tính bằng nút hiện ở gõ bên trái.

Hình 4.5 Thuộc tính của đối tượng

4.1.3 Phần mềm giải bài toán động học ngược
Giao diện được thiết kế như sau:

35
 Hình 4.1 Giao diện bài toán động học ngược
Thực hiện nhập độ dài các khâu, tọa độ vị trí công tác và nhấn nút “Dịch chuyển”. Kết
quả được thể hiện bằng “Góc”.
4.1.4 Thiết kế phần mềm điều khiển
Tạo giao diện điều khiển

Hình 4.7 Giao diện điều khiển chính

Trong giao diện điều khiển chính có:
- Bảng nhập dữ liệu:

36
 Hình 4.8 Bảng nhập dữ liệu
Bảng điều khiển thực hiện kết nối và ngắt kết nối với máy tính, cài đặt vị trí ban đầu
cho cánh tay, thực hiện di chuyển cánh tay từ vị trí hiện tại với vị trí được khai báo
trong bảng “Theo góc” hoặc “Tọa độ”.
- Bảng 4.11:

Hình 4.9 Bảng điều khiển

Thực hiện lưu lại vị trí trong ma trận, trạng thái cánh tay gắp nhả, tạo chương trình di
chuyển cho cánh tay. Có thể di chuyển với lượng tiến góc của cánh tay trong bảng này.
- Bảng thông báo việc lưu vị trí, tọa độ:

37
 Hình 4.10 Thông báo chương trình chạy của robot
4.2 Thiết kế hệ thống cơ khí
4.2.1 Tính chọn động cơ
Giả sử bán kính trục thiết kế có Rtr=5 mm
- Loại động cơ: Bước
- Fms=0.05*P*d/Rtr
- k=0.01÷0.05: hệ số ma sát của ổ lăn
- Rtr=0.005: bán kính trục (m)
- d=l2+l3 =0.23+0.245= 0.475 (m)
- P=(m2+m3)*g=(1.5949+0.21605)*9.8=17.74(N)
- Fms=0.05*P*d/Rtr = 0.05*17.74*0.475 /0.05= 8.4 (N)
- Ptr= (M x 2pi x n)/60 = (8.4*0.01*2*3.14*3000)/60= 26.5(W)
Chọn động cơ: PITTMAN DC GEAR MOTOR 24 VDC 65.5:1 RATIO MODEL
GM9434E470.
4.2.2 Thiết kế trục
Tính toán trục
Trục 1 (trục tại khớp 3)
Xác định momen lớn nhất tác dụng lên trục:
Giả sử cánh tay nâng vật nặng 100(g), khâu thứ 3 nặng 216,05(g) được thay thế bằng
lực tương đương tại trọng tâm.

38
 Hình 4.11 Khớp 3
Ta có tổng momen tác dụng lên trục 1:
M = 0.21605 75 + 0.1 150 = 305.8 (Nmm)
Gọi: +) M1 là momen canh tay tác dụng lên trục: M1 = M/2 = 152.9 (Nmm)
+) M2 là momen động cơ cần thiết tác dụng lên trục M2 = M1
Trục chịu lực cắt lớn nhất khi cánh tay ở vị trí cao nhất, khi đó lực cắt:
P= ( 0.21605 + 0.1 ) 9.8 = 3 (N)
Gọi: +) N3, N4 là hai lực cắt cánh tay tác dụng: N3 = N4 = P/2 =1,5 (N)
Khi đó
+) N1, N2 là hai phản lực: N1 = 1,8 (N), N2 =1,2 (N)
Biều đồ momen:

Hình 4.12 Biểu đồ momen trục 1

Xác định tiết diện tại điểm nguy hiểm

Dựa vào biểu đồ nội lực ta thấy điểm đặt lực N 4 là điểm nguy hiểm, ta có momen
tương đương tại điểm này:
Mtd = = 26.4 (Nmm)
Chọn vật liệu: thép C45 => [σ] = 80
Đường kính cần thiết:
d = = = 1.5 (mm)
39
Trục 2 (trục tai khớp 2)
Khối lượng khâu 2 và 3: m= 1594.9 + 216.05 = 1811 (g), cánh tay nâng vật nặng 100
(g). Ta có thể thay thế lực tác dụng như sau
Trục 2 chịu tác dụng của momen xoắn do khối lượng của khâu 2, khâu 3 và vật nâng
gây ra:

Hình 4.13 Khớp 2 và khớp 3

Từ hình trên ta có tổng các momen tác dụng:
M = M2126.5 + M3 328 + M0403 = 3066.44 (Nmm)
Gọi M1, M2 là momen xoắn do cánh tay và động cơ tác dụng lên truc 2
M1 = M2 = M/2 = 1533,22 (Nmm)
Trọng lượng của khâu 2,3:
P = M0 + M2 + M3 = 19 (N)
Gọi N3, N4 là lực cắt của cánh tay tác dụng lên trục 2: N3 = N4 =P/2 = 9,5 (N)
Khi đó hai phản lực ta có hai phản lực N1 = 11, N2 = 8 (N)
Biểu đồ nội lực:

40
 Hình 4.14 Biểu đồ nội lực
Dựa vào biểu đồ nội lực ta thấy điểm đặt lực N 4 là điểm nguy hiểm, ta có momen
tương đương tại điểm này:
Mtd = = 122.6 (Nmm)
Chọn vật liệu: thép C45 => [σ] = 80
Đường kính cần thiết:
d = = = 2.5 (mm)

Trục 3 (trục tại khớp 1)

Trục chịu tác dụng chủ yếu bởi momen uốn và momen xoắn

Hình 4.15 Khớp 1

Momen uốn lớn nhất được tạo ra bởi cánh tay tại vị trí xa nhất.
M = M2126.5 + M3 328 + M0403 = 3066.44 (Nmm)
Momen xoắn lớn nhất được tạo ra khi cho cánh tay quay với gia tốc lớn nhất, giả sử
gia tốc lớn nhất là 1 m/s2. Khi đó momen lớn nhất được tính như sau:
Mz = M2 a 126.5 + M3 a 328 + M1 a403 = 313 (Nmm)
Biểu đồ nội lực

41
 Hình 4.16 Biểu đồ nội lực
Dựa vào biểu đồ nội lực ta thấy điểm đặt momen M z2 là điểm nguy hiểm, ta có
momen tương đương tại điểm này:
Mtd = = 225.1 (Nmm)
Chọn vật liệu: thép C45 => [σ] = 80
Đường kính cần thiết:
d = = = 3.1 (mm)
4.2.3 Thiết kế khung cho robot cầu RRR
Khung đế phải được thiết kế gọn nhẹ,đủ độ cứng,đứng vững chịu được tải trọng của
toàn bộ robot. Phần đế được làm bằng thép d=3mm, đủ nặng để giữ robot không bị lật
về phía trước. Các phần khung robot được làm bằng nhôm tấm dày 3mm.

42
 Hình 4.17 Khung robot
4.2.4 Thiết kế cánh tay robot:
a) Cánh tay 1
Cánh tay gồm 2 tấm có trọng lượng nhẹ,thẩm mĩ, đơn giản dễ gia công và tháo lắp.
Vật liệu nhôm A1050. Bề dày d=2mm

Hình 4.18 Cánh tay 1

b) Cánh tay 2
Cũng giống như modul cánh tay 1, bao gồm 2 tấm nhôm nhẹ , dễ gia công và tháo lắp.
Vật liệu nhôm A1050.Bề dày d=2mm

43
 Hình 4.19 Cánh tay 2
c) Thiết kế động cơ khoan
Động cơ khoan có khả năng khoan được các vật liệu như gỗ, tường…Thiết kế điều
khiển bằng nguồn 5V: d=35mm, l=60mm

Hình 4.20 Động cơ khoan

4.2.5 Tính chọn ổ đỡ-ổ bi
Chọn ổ đỡ đã có sẵn ổ bi –tiết kiệm chi phí và gia công.
Ổ bi loại UCF 204
Thông số kĩ thuật:
Ổ bi cầu, chịu tải mức trung bình.
Đường kính trong: d=10mm, chiều cao: h=8mm, đường kính ngoài: D=28mm.
Tốc độ tải tối đa: Vmax= 1800RMP
44
 Số lượng bi: 10 viên
Vật liệu ổ đỡ: nhôm hợp kim
Ổ bi loại 62005RS
Thông số kĩ thuật:
Ổ bi cầu, chịu tải trong trung bình
Đường kính trong: d= 15mm, chiều cao 9mm, đường kính ngoài: D= 32mm
4.2.6 Tính chọn bộ truyền động đai răng robot RRR
Chọn đai răng theo tiêu chuẩn ISO

Bảng 4.1 Các ký hiệu L, H, XL và bước đai tương ứng

Ta có bảng lựa chọn bộ truyền đai với công suất và tốc độ tương ứng:

Bảng 4.2 Bảng lựa chọn bộ truyền đai

Động cơ bước điều khiển với tốc độ: n=5000 vòng/phút
Công suất động cơ: Pđc=35W
Dựa vào bảng ta chọn loại MXL

45
Thiết kế bánh đai theo tiêu chuẩn với phần mềm hỗ trợ GEARTRAX.
Tiến hành chọn:
Bước răng: Belt Pitch=3mm
Độ rộng đai: Belt Width=0.313in belt width (= 9mm)
Số lượng răng của bánh đai: Number of Teeth=16
Chọn tấm chắn cho bánh đai: Flange Creation
Chọn độ rộng của pulley: 12mm
Sau khi hoàn tất, ấn Create để xác nhận:

Hình 4.21 Thông số GearTrax pulley nhỏ

Phần mềm Solidwork 2010 tự động chạy quá trình tính toán và vẽ ra biên dạng
pulley, sau đó ta chỉnh sửa để được sản phẩm hoàn thiện

Hình 4.22 Mô hình pulley nhỏ trong SolidWork

46
 Ta làm tương tự với bánh đai lớn:

Hình 4.23 Thông số GearTrax pulley lớn

Bánh đai đã được thiết kế xong:

Hình 4.24 Mô hình pulley to trong SolidWork

Tiếp theo tiến hành chọn dây đai: Chọn dây đai theo tiêu chuẩn bằng phần mềm trực
tuyến từ trang web: http://www.hpcgears.com/calc.htm
Bộ truyền động thiết kế có thông số cơ bản là:
Số răng bánh đai nhỏ: No of teeth in samll pulley =25

47
Số răng bánh đai lớn: No of teeth in large pulley =50
Khoảng cách 2 trục: d=200mm
Bước răng: Pitch=3mm
Tiến hành chọn loại đai phù hợp theo thông số đã tính toán . Lựa chọn sao cho số răng
ăn khớp: Number of teeth in mesh>=6

Hình 4.25 Chọn loại đai

Theo bảng trên chọn được dây đai HTD531
4.2.7 Thi công hệ thống cơ khí:
Thống kê chi tiết cơ bản và phương án gia công

48
 Tên chi Vật liệu Phương án
STT Hình ảnh
tiết phôi gia công

1 Đế Thép Máy cắt tay

Máy cắt tay

Thân
2 Thép Hàn
dưới
Khoan lỗ

-Dập tạo
hình dạng
Gá động -cắt dây tạo
3 Thép
cơ số 1 rãnh
-Khoan lỗ
và taro

Thanh tay Nhôm tấm -Cắt dây

4
2 D = 3mm -Khoan lỗ

49
 Tiện
5 Trục số 1 Thép Ø35 Khoan lỗ và
taro

6 Trục số 2 Thép Ø20 Tiện

7 Trục số 3 Thép Ø 12 Tiện

Đệm thân Nhôm tấm Máy dập

8
số 1 D = 3mm Khoan

50
 Cắt dây
Nhôm tấm
9 Thân số 1 Máy dập
D = 3mm
Khoan lỗ

Tiện
10 Ca bi 1 Nhựa cứng
Khoan lỗ

Cắt dây
Nhôm tấm Máy cắt tay
11 Tay số 2
d = 2 mm Khoan

Nhôm tấm Máy cắt tay

12 Ca bi 3
d=2mm Khoan lỗ

Máy tiện
Pulley Cắt dây
13 Nhôm
nhỏ Khoan
và taro

51
 Máy tiện
Pulley số Cắt dây
14 Nhôm
2 Khoan
và taro

Máy tiện
Pulley
15 Nhôm Cắt dây
số 3
Khoan lỗ

Máy tiện
Pulley Cắt dây
16 Nhôm
số 4 Khoan lỗ và
taro

Tiện
Pulley Cắt dây
17 Nhôm
Số 5 Khoan lỗ và
taro

Tiện
Pulley
18 Nhôm Cắt dây
Số 6
Khoan lỗ

52
 Miếng ốp Nhôm tấm Dập và
19
thân 1 d = 1,5mm khoan

Dập và
Miếng ốp Nhôm tấm
20 khoan
thân 1.1 d = 1,5mm
lỗ

Dập và
Miếng ốp Nhôm tấm
21 khoan
thân 2.1 D = 1,5mm
lỗ

Dập và
Miếng ốp Nhôm tấm
22 khoan
thân 2.1 D = 1,5mm
lỗ

Dập và
Miếng ốp Nhôm tấm
23 khoan
thân 3.1 S = 1,5mm
lỗ

53
 Miếng ốp Nhôm tấm Dập và
24
thân 3.2 D = 1,5mm khoan tạo lỗ

4.3 Thiết kế, thi công mạch điện tử

a) Thiết kế mạch điều khiển
- Khối vi điều khiển Atmega16:

Hình 4.26 Khối vi điều khiển AVR

- Khối kết nối máy tính:

Hình 4.27 Khối giao tiếp máy tính

- Khối nguồn:
54
 Hình 4.28 Khối nguồn
- Khối cảm biến - hiển thị:

Hình 4.29 Khối cảm biến – hiển thị

- Khối driver điều khiển động cơ

55
 Hình 4.30 Khối driver điều khiển động cơ
- Mạch nguyên lí tổng :

Hình 4.31 Mạch nguyên lí

- Mạch in:

56
 Hình 4.32 Mạch in
CHƯƠNG V
KẾT QUẢ VÀ ĐÁNH GIÁ
5.1 Kết quả
Sau quá trình nghiên cứu, mô hình robot tọa độ cầu đã được mô phỏng thành công,
đáp ứng được yêu cầu của đề tài đặt ra. Ưu điểm, nhược điểm của mô hình:
Ưu điểm:
- Việc điều khiển dễ dàng theo góc, vị trí.
- Độ chính xác các khâu công tác (1mm).
- Tính thẩm mỹ cao, kích thước gọn.
- Dễ tháo lắp các khâu.
- Có thể điều khiển được từng khớp riêng biệt và 3 khớp kết hợp.
- Điều khiển được theo phương pháp dạy học.
Nhược điểm:
- Trục chính rung lắc nhẹ khi robot làm việc.
- Sử dụng công tắc hành trình để xác định vị trí ban đầu của robot nên có sự sai

lệch vị trí.
- Động cơ chưa có giảm tốc

Một số hình ảnh thật của robot:

57
 Hình 5.1 Sản Phẩm hoàn thiện
5.2 Nguyên nhân và biện pháp khắc phục
Trong quá trình mô phỏng, robot tọa độ cầu có xuất hiện một số lỗi khiến việc hoạt
động chưa đạt độ chính xác tuyệt đối.
Từ đó ta có biện pháp khắc phục như sau:
- Sử dụng ổ bi có đường kính lớn hơn để trục chính chống rung lắc, hoặc có thể

sử dụng ổ bi mềm chuyên dụng cho robot.

- Sử dụng động cơ bước có giảm tốc, độ phân giải cao.
- Sử dụng động cơ DC servo để điều khiển robot.
- Gia công các chi tiết bằng máy CNC để đạt độ chính xác cao.
- Phay các mặt ốp ca bi để đạt độ phẳng cao.
5.3 Hướng phát triển của đề tài
- Trong tương lai sẽ khắc phục các nhược điểm như: thay thế động cơ Bước
thường bằng động cơ bước có giảm tốc, để đề tài tối ưu hơn.
- Tăng số bậc tự do của robot để robot hoạt động linh hoạt hơn.
- Điều khiển robot bằng thuật toán điều khiển PID.
- Phát triển đề tài để ứng dụng trong các dây chuyền sản xuất tự động trong công
nghiệp.
- Tăng kích thước của robot để tăng khối lượng nâng của robot.

58
 TÀI LIỆU THAM KHẢO

[1]. Khoa Cơ Khí- Bộ môn Cơ Điện Tử, Kỹ Thuật Robot

[2]. PGS.TS.GVCC Trịnh Quang Vinh, PGS.TS Nguyễn Đăng Bình, ThS. Phạm
Thành Long, Robot công nghiệp.
[3]. Phạm Đăng Phước, Robot công nghiệp

59
 PHỤ LỤC I

I.1 Code lập trình vi điều khiển

#include <mega16.h>
#include <delay.h>
#include <stdio.h>
#include <alcd.h>
#include "dongcobuoc.h"
#include "hienthi.h"

/*************Transmite USART*************************/
#ifndef RXB8
#define RXB8 1
#endif

#ifndef TXB8
#define TXB8 0
#endif

#ifndef UPE
#define UPE 2
#endif

#ifndef DOR
#define DOR 3
#endif

#ifndef FE
#define FE 4
#endif
60
#ifndef UDRE
#define UDRE 5
#endif

#ifndef RXC
#define RXC 7
#endif

#define FRAMING_ERROR (1<<FE)

#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<DOR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)

// USART Receiver buffer

#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];

#if RX_BUFFER_SIZE <= 256

unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif

// This flag is set on USART Receiver buffer overflow

bit rx_buffer_overflow;

// USART Receiver interrupt service routine

interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;

61
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer[rx_wr_index++]=data;
#if RX_BUFFER_SIZE == 256
// special case for receiver buffer size=256
if (++rx_counter == 0)
{
#else
if (rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
if (++rx_counter == RX_BUFFER_SIZE)
{
rx_counter=0;
#endif
rx_buffer_overflow=1;
}
}
}

#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index++];
#if RX_BUFFER_SIZE != 256
if (rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#endif

62
#asm("cli")
--rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif

/***************Define Step Motor**************/

// Step DC1
#define EN1 PORTA.0
#define DIR1 PORTA.1
#define CLK1 PORTA.2

// Step DC2
#define EN2 PORTA.3
#define DIR2 PORTA.4
#define CLK2 PORTA.5

// Step DC3
#define EN3 PORTB.0
#define DIR3 PORTB.1
#define CLK3 PORTB.2
/*************************************************************/
// Dinh nghia chan chieu cua step motor
#define nguoc1 0
#define thuan1 1
#define nguoc2 1
#define thuan2 0
#define nguoc3 0
#define thuan3 1

63
/************************************************************/
#define CB1 PIND.3
#define CB2 PIND.4
#define CB3 PIND.5
#define CB4 PIND.6

/***********************************************************/
// bien phan biet dau'
char k,j;
//bien cac phan tram, chuc, donvi
unsigned char hu1,do1,un1,hu2,do2,un2,hu3,do3,un3;
// bien khop
int rotate1=0,rotate2=0,rotate3=0;
//bien phan biet dau' tung goc
char a,b,c;
// luu so sanh gia tri goc duong
int r_r1=0,r_r2=0,r_r3=118;
// luu so sanh gia tri goc am
int rr_r1=0,rr_r2=0,rr_r3=0;

/********************************************************/
void display(unsigned int num,char x,char y)
// Reset robot before working
void reset_robot();
// Test Robot
void test_robot();
// Read rotate from Computer.
void read_rotate();
void control_rotate(int ro1,int ro2,int ro3);
/***************************************************/

void main(void)

64
{
// Declare your local variables here

// Input/Output Ports initialization

// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0xff;
DDRA=0xff;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x07;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0x0f;
DDRD=0x00;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF

65
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=0xFFFF
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF

66
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization

// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization

TIMSK=0x00;

// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud Rate: 9600
UCSRA=0x00;
UCSRB=0x98;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x33;

// Analog Comparator initialization

// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;

67
SFIOR=0x00;

// ADC initialization
// ADC disabled
ADCSRA=0x00;

// SPI initialization
// SPI disabled
SPCR=0x00;

// TWI initialization
// TWI disabled
TWCR=0x00;

// Alphanumeric LCD initialization

// Connections specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS - PORTC Bit 7
// RD - PORTC Bit 6
// EN - PORTC Bit 5
// D4 - PORTC Bit 4
// D5 - PORTC Bit 3
// D6 - PORTC Bit 2
// D7 - PORTC Bit 1
// Characters/line: 16
lcd_init(16);

// Global enable interrupts

#asm("sei")
//start_robot();
lcd_gotoxy(0,0);
lcd_puts("chao cau chu");

68
delay_ms(500);
lcd_clear();
while (1)
{

read_rotate();
control_rotate(rotate1,rotate2,rotate3);
// printf("chao cau chu\n");
// delay_ms(700);
// test_robot();
}
}

void read_rotate()
{
//Goc anfa
j=getchar();
while(RX_COMPLETE==1);
if(j==':')
{
k=getchar();
hu1=getchar()-48;
do1=getchar()-48;
un1=getchar()-48;
rotate1=hu1*100+do1*10+un1;
if(k=='-')
{
lcd_gotoxy(0,0);
lcd_puts("-");
display(rotate1,1,0);
a=0;
}

69
else
if(k=='+')
{
lcd_gotoxy(0,0);
lcd_puts("+");
display(rotate1,1,0);
a=1;
}
}
do
{
j=getchar();
while(RX_COMPLETE==1);
}
while(j!=',');
// Goc Beta
if(j==',')
{
k=getchar();
hu2=getchar()-48;
do2=getchar()-48;
un2=getchar()-48;
rotate2=hu2*100+do2*10+un2;
if(k=='-')
{
lcd_gotoxy(5,0);
lcd_puts("-");
display(rotate2,6,0);
b=0;
}
else
if(k=='+')

70
{
lcd_gotoxy(5,0);
lcd_puts("+");
display(rotate2,6,0);
b=1;
}
}
do
{
j=getchar();
while(RX_COMPLETE==1);
}
while(j!=',');
//Goc ga ma
if(j==',')
{
k=getchar();
hu3=getchar()-48;
do3=getchar()-48;
un3=getchar()-48;
rotate3=hu3*100+do3*10+un3;
if(k=='-')
{
lcd_gotoxy(0,1);
lcd_puts("-");
display(rotate3,1,1);
c=0;
}
else
if(k=='+')
{
lcd_gotoxy(0,1);

71
 lcd_puts("+");
display(rotate3,1,1);
c=1;
}
}
do
{
j=getchar();
while(RX_COMPLETE==1);
}
while(j!='.');
}
//read rotate to control
// xu li du~ lieu chuyen thanh goc quay cho motor
void control_rotate(int ro1,int ro2,int ro3)
{
int r1=0,r2=0,r3=0,i,j,k;
r1=ro1;
r2=ro2;
r3=ro3;

//Goc 3 gui? duong - quay nguoc

if(c==0)
{
if(rr_r3!=0) // Neu dang ben goc thuan, quay ve gôc
{
i=(rr_r3*200)/360;
run_step3(i+115,nguoc3);
rr_r3=0;
}
if((r3>=0)&&(r3<=118)&&(r3!=r_r3)) // goc r3 nho hon 40 do
{

72
 if(r3<r_r3) // goc nhap nho hon goc dang chay
{
r3=r_r3-r3;
i=(r3*200)/360;
run_step3(i+115,thuan3); // quay thuan3
r_r3=r_r3-r3;
}
else
if(r3>r_r3) // Goc nhap lon hon goc dang chay
{
r3=r3-r_r3;
i=(r3*200)/360;
run_step3(i+115,nguoc3); // Quay nguoc
r_r3=r_r3+r3;
}
}
else
if((r3>118)||(r3==r_r3)) // neu dat goc1 quay lon hon 118 hoac bang goc cu~
brake_motor3(); // Dung dong co1
}
else // Nguoc lai goc 3 quay thuan
if(c==1)
{
if(r_r3!=0)
{
i=(r_r3*200)/360;
run_step3(i+115,thuan3);
r_r3=0;
}
if((r3>=0)&&(r3<=90)&&(r3!=rr_r3))
{
if(r3<rr_r3)

73
 {
r3=rr_r3-r3;
i=(r3*200)/360;
run_step3(i+115,nguoc3);
rr_r3=rr_r3-r3;
}
else
if(r3>rr_r3)
{
r3=r3-rr_r3;
i=(r3*200)/360;
run_step3(i+115,thuan3);
rr_r3=rr_r3+r3;
}
}
}
else
if((r3>90)||(r3==rr_r3)) // neu dat goc quay lon hon 60
brake_motor3(); //dung dong co 1

//Goc 2 gui? duong - quay nguoc

if(b==1)
{
if(r2!=0)
brake_motor2(); // Dung dong co2
else
if(r2==0)
{
if(rr_r2!=0)
{
j=(rr_r2*200)/360;
run_step2(j+100,nguoc2); // khac phuc bang cam bien' CTHT

74
 rr_r2=0;
}
}
}
else // Nguoc lai goc 2 quay thuan
if(b==0)
{
if((r2>=0)&&(r2<=90)&&(r2!=rr_r2))
{
if(r2<rr_r2)
{
r2=rr_r2-r2;
j=(r2*200)/360;
run_step2(j+100,nguoc2);
rr_r2=rr_r2-r2;
}
else
if(r2>rr_r2)
{
r2=r2-rr_r2;
j=(r2*200)/360;
run_step2(j+100,thuan2);
rr_r2=rr_r2+r2;
}
}
else
if((r2>90)||(r2==rr_r2)) // neu dat goc quay lon hon 90
brake_motor2(); //dung dong co 1
}
//Goc 1 gui? duong - quay nguoc
if(a==1)
{

75
if(rr_r1!=0) //Neu dang ben goc thuan thi quay het goc thuan ve goc'
{
k=(rr_r1*200)/360; // chia % tinh ra buoc
run_step1(k+25,nguoc1);
rr_r1=0; //reset bien nho goc nguoc
}
if((r1>=0)&&(r1<=45)&&(r1!=r_r1)) // goc r1 nho hon 40 do
{
if(r1<r_r1) // goc nhap nho hon goc dang chay
{
r1=r_r1-r1;
k=(r1*200)/360;
run_step1(k+25,thuan1); // quay thuan1
r_r1=r_r1-r1; // luu goc' doc duoc tu may tinh
}
else
if(r1>r_r1) // Goc nhap lon hon goc dang chay
{
r1=r1-r_r1;
k=(r1*200)/360;
run_step1(k+25,nguoc1); // Quay nguoc
r_r1=r_r1+r1; // Luu goc doc duoc tu may tinh
}
}
else
if((r1>45)||(r1==r_r1)) // neu dat goc1 quay lon hon 40 hoac bang goc cu~
brake_motor1(); // Dung dong co1
}
else // Nguoc lai goc 1 quay thuan
if(a==0)
{
if(r_r1!=0)

76
 {
k=(r_r1*200)/360;
run_step1(k+25,thuan1); // Neu dang ben goc nguoc thi quay het goc' nguoc
ve gôc
r_r1=0; //reset goc nguoc ve 0
}
if((r1>=0)&&(r1<=45)&&(r1!=rr_r1)) // neu goc 1 < 40 do va khong bi trung goc'
truoc' do'
{
if(r1<rr_r1)
{
r1=rr_r1-r1;
k=(r1*200)/360;
run_step1(k+25,nguoc1);
rr_r1=rr_r1-r1;
}
else
if(r1>rr_r1)
{
r1=r1-rr_r1;
k=(r1*200)/360;
run_step1(k+25,thuan1);
rr_r1=rr_r1+r1;
}
}
}
else
if((r1>45)||(r1==rr_r1)) // neu dat goc quay lon hon 40
brake_motor1(); //dung dong co 1
}

void reset_robot()

77
{
// dua motor1 ve vi tri ban dau
do
{
run1(nguoc1);
}
while(CB1!=0);
brake_motor1();
// dua motor2 ve vi tri ban dau
do
{
run2(nguoc2);
}
while(CB2!=0);
brake_motor2();
// dua motor3 ve vi tri ban dau
do
{
run3(nguoc3);
}
while(CB3!=0);
brake_motor3();
}

void test_robot()
{
run_step2(60,0); // di len
// run_step2(60,0); // di xuong
// run_step1(60,0); // quay thuan
delay_ms(500);
run_step3(60,1); //
// run_step2(60,1);

78
// run_step1(60,1);
delay_ms(500);
}
/************Define some functions for step Motor*********/
// Brake Step Motor1
void brake_motor1()
{
EN1=0;
CLK1=0;
}

// Brake Step Motor2

void brake_motor2()
{
EN2=0;
CLK2=0;
}

// Brake Step Motor3

void brake_motor3()
{
EN3=0;
CLK3=0;
}

void run1(char direction)

{
DIR1=direction;
CLK1=0;
delay_ms(15);
CLK1=1;
delay_ms(15);

79
}

void run2(char direction)

{
DIR2=direction;
CLK2=0;
delay_ms(15);
CLK2=1;
delay_ms(15);
}

void run3(char direction)

{
DIR3=direction;
CLK3=0;
delay_ms(15);
CLK3=1;
delay_ms(15);
}

//Run step Motor 1

void run_step1(int step1, char direction)
{
int i;
EN1=0;
for(i=0;i<step1;i++)
{
DIR1=direction;
CLK1=0;
delay_ms(15);
CLK1=1;
delay_ms(15);

80
}
step1=0;
}

//Run step Motor 2

void run_step2(int step2,char direction)
{
int i;
EN2=0;
for(i=0;i<step2;i++)
{
DIR2=direction;
CLK2=0;
delay_ms(15);
CLK2=1;
delay_ms(15);
}
step2=0;
}

//Run step Motor 3

void run_step3(int step3,char direction)
{
int i;
EN3=0;
for(i=0;i<step3;i++)
{
DIR3=direction;
CLK3=0;
delay_ms(15);
CLK3=1;
delay_ms(15);

81
}
step3=0;
}
/*************calculator function**************/
void display(unsigned int num,char x,char y)
{
char num1,num2,num3;
num1=num/100;
num2=num%100/10;
num3=num%100%10;
lcd_gotoxy(x,y);
lcd_putchar(num1 + 48);
lcd_putchar(num2 + 48);
lcd_putchar(num3 + 48);
}
I.2 Code lập trình giao diện và giải toán
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO.Ports;

namespace VD1
{
public partial class Form1 : Form
{
string RxString, TxString;

82
 int number1, number2, number3;
double X = 0, Y = 0, Z = 0;
double goc1=0, goc2=0, goc3=-118;
double L1 = 240, L2 = 260, L3 = 170;
string selectedAngle;
public Form1()
{
InitializeComponent();
//int index=1;
//List<KeyValuePair<string, string>> data = new List<KeyValuePair<string,
string>>();

comboBox1.DataSource = null;
comboBox1.Items.Clear();
foreach (string s in SerialPort.GetPortNames())
{
Com.BaudRate = 9600;
Com.PortName = s;

// Add data to the List

//data.Add(new KeyValuePair<string, string>(index.ToString(), s));
comboBox1.Items.Add(s);

}
comboBox1.Sorted = true;
if (comboBox1.Items.Count > 0)
comboBox1.SelectedIndex = 0;
// Bind the combobox
//comboBox1.DataSource = new BindingSource(data, null);

//comboBox1.DisplayMember = "Value";
//comboBox1.ValueMember = "Key";

83
comboBox2.SelectedIndex = comboBox2.Items.Count - 1;

txt_Goc1.ReadOnly = true;
txt_Goc2.ReadOnly = true;
txt_Goc3.ReadOnly = true;

txt_L1.Text = L1.ToString();
txt_L2.Text = L2.ToString();
txt_L3.Text = L3.ToString();
//Nhap Cac khau

listView1.View = View.Details;
listView1.GridLines = true;
listView1.FullRowSelect = true;

//Add column header

listView1.Columns.Add("Góc 1", 70);

listView1.Columns.Add("Góc 2", 70);
listView1.Columns.Add("Góc 3", 70);
listView1.Columns.Add("X", 70);
listView1.Columns.Add("Y", 70);
listView1.Columns.Add("Z", 70);
//Add items in the listview

//Add second item

rd_Goc1.Select();
selectedAngle = "GOC1";
goc1 = goc2 = 0;

84
 goc3 = -118;
txt_Goc1.Text = goc1.ToString();
txt_Goc2.Text = goc2.ToString();
txt_Goc3.Text = goc3.ToString();

GocSangToado();
}

private void btn_Send_Click(object sender, EventArgs e)

{
if (comboBox2.SelectedIndex == comboBox2.Items.Count - 1)//Toa do sang
Goc
{
if (!ToadoSangGoc()) return;

}
else
{
if (!GocSangToado()) return;

}
if (Com.IsOpen == true)
{
truyenCom();
}
else
{
MessageBox.Show("Kết nối với cổng Com trước khi truyền lệnh!");
}
}

85
private void btn_Conect_Click(object sender, EventArgs e)
{
if (btn_Conect.Text == "Connect")
{

Com.BaudRate = 9600;
if (comboBox1.Text.ToString() == "")
return;
Com.PortName = comboBox1.Text.ToString();

try
{
Com.Open();
}
catch (Exception)
{
};
if (Com.IsOpen == true)
{
btn_Conect.Text = "Disconect";

if (Com.IsOpen == false)
{

MessageBox.Show("Thử kết nối lại. Không tìm thấy cổng Com");

}
}
else if (btn_Conect.Text == "Disconect")
{
if (Com.IsOpen == true)

86
 {
try
{
Com.Close();
}
catch (Exception)
{
MessageBox.Show("Error Close.");
};
}
if (Com.IsOpen == false)
{

btn_Conect.Text = "Connect";
}
}
}

private void Form1_FormClosing(object sender, FormClosingEventArgs e)

{
try
{
Com.Close();
}
catch (Exception)
{ };
}

private bool ToadoSangGoc()

{

87
try
{
X = Convert.ToInt32(txt_X.Text);
}
catch (FormatException)
{
MessageBox.Show("Hãy tọa độ X");
return false;
}

try
{
Y = Convert.ToInt32(txt_Y.Text);
}
catch (FormatException)
{
MessageBox.Show("Hãy tọa độ Y");
return false;
}

try
{
Z = Convert.ToInt32(txt_Z.Text);
}
catch (FormatException)
{
MessageBox.Show("Hãy tọa độ Z");
return false;
}

//Tinh goc phi 1

88
 if (X > 0 && Y >= 0)
{
goc1 = (int)(Math.Atan(Y / X) * 180 / Math.PI);
}
else if (X <= 0 && Y > 0)
{
goc1 = (int)(Math.Atan(Math.Abs(X) / Math.Abs(Y)) * 180 / Math.PI) + 90;
}
else if (X <= 0 && Y < 0)
{
goc1 = (int)(-Math.Atan(Math.Abs(X) / Math.Abs(Y)) * 180 / Math.PI) - 90;
}
else if (X > 0 && Y <= 0)
{
goc1 = (int)(-Math.Atan(Math.Abs(Y) / Math.Abs(X)) * 180 / Math.PI);
}

double tmpB, tmpC, x1;

x1 = Math.Sqrt(Math.Pow(X, 2) + Math.Pow(Y, 2));

//Tinh goc phi 2

tmpB = Math.Pow(L2, 2) + Math.Pow(x1, 2) + Math.Pow(L1 - Z, 2) -
Math.Pow(L3, 2);
tmpC = 2 * L2 * Math.Sqrt(Math.Pow(L1 - Z, 2) + Math.Pow(x1, 2));
double tmpTan, tmpCos;

if (tmpB > tmpC) goc2 = 0;

else
goc2 = -(int)((Math.Acos(tmpB / tmpC) - Math.Atan((Z - L1) / x1)) * 180 /
Math.PI);

89
 //Tinh goc phi 3
tmpB = Math.Pow(L2, 2) + Math.Pow(L3, 2) - Math.Pow(L1 - Z, 2) -
Math.Pow(x1, 2);

tmpC = 2 * L2 * L3;
goc3 = (int)((Math.Acos(tmpB / tmpC) * 180 / Math.PI)) - 180;
if (L1 < Z)
goc3 = -goc3;

if (Math.Abs(goc1) > 180 || Math.Abs(goc2) > 180 || Math.Abs(goc3) > 180)

{
MessageBox.Show("Ngoài phạm vi di chuyển của robot.");
return false;
}

txt_Goc2.Text = goc2.ToString();
txt_Goc1.Text = goc1.ToString();
txt_Goc3.Text = goc3.ToString();

return true;
}

private bool GocSangToado()

{

try
{
goc1 = Int32.Parse(txt_Goc1.Text) * Math.PI / 180;
}

90
catch (FormatException)
{
MessageBox.Show("Hãy nhập góc 1");
return false;
}

try
{
goc2 = Int32.Parse(txt_Goc2.Text) * Math.PI / 180;
}
catch (FormatException)
{
MessageBox.Show("Hãy nhập góc 2");
return false;
}

try
{
goc3 = (Int32.Parse(txt_Goc3.Text)) * Math.PI / 180;
}
catch (FormatException)
{
MessageBox.Show("Hãy nhập góc 3");
return false;
}

if (Math.Abs(goc1) > 180 || Math.Abs(goc2) > 180 || Math.Abs(goc3) > 180)

{
MessageBox.Show("Ngoài phạm vi di chuyển của robot.");
return false;
}

91
 X = (int)(Math.Cos(goc1) * (L3 * Math.Cos(goc2 + goc3) + L2 *
Math.Cos(goc2)));
Y = (int)(Math.Sin(goc1) * (L3 * Math.Cos(goc2 + goc3) + L2 *
Math.Cos(goc2)));
Z = (int)(L3 * Math.Sin(goc2 + goc3) + L2 * Math.Sin(goc2) + L1);
txt_X.Text = ((int)X).ToString();
txt_Y.Text = ((int)Y).ToString();
txt_Z.Text = ((int)Z).ToString();

goc1 = Int32.Parse(txt_Goc1.Text);
goc2 = Int32.Parse(txt_Goc2.Text);
goc3 = Int32.Parse(txt_Goc3.Text);
return true;
}

private void comboBox2_SelectedIndexChanged(object sender, EventArgs e)

{
if (comboBox2.SelectedIndex == comboBox2.Items.Count - 1)//Toa do sang
Goc
{
txt_X.ReadOnly = false;
txt_Y.ReadOnly = false;
txt_Z.ReadOnly = false;

txt_Goc1.ReadOnly = true;
txt_Goc2.ReadOnly = true;
txt_Goc3.ReadOnly = true;
}
else
{
txt_X.ReadOnly = true;

92
 txt_Y.ReadOnly = true;
txt_Z.ReadOnly = true;

txt_Goc1.ReadOnly = false;
txt_Goc2.ReadOnly = false;
txt_Goc3.ReadOnly = false;

}
}

private void rd_Goc1_Click(object sender, EventArgs e)

{
selectedAngle = "GOC1";
}

private void rd_Goc2_Click(object sender, EventArgs e)

{
selectedAngle = "GOC2";
}

private void rd_Goc3_Click(object sender, EventArgs e)

{
selectedAngle = "GOC3";
}

private void btn_Tang_Click(object sender, EventArgs e)

{
if (selectedAngle == "GOC1")
{
if (Math.Abs(goc1) < 180)
goc1++;
}

93
 else if (selectedAngle == "GOC2")
{
if (Math.Abs(goc2) < 180)
goc2++;
}
else if (selectedAngle == "GOC3")
{
if (Math.Abs(goc3) < 180)
goc3++;
}
txt_Goc1.Text = goc1.ToString();
txt_Goc2.Text = goc2.ToString();
txt_Goc3.Text = goc3.ToString();
}

private void button1_Click(object sender, EventArgs e)

{
if (selectedAngle == "GOC1")
{
if (Math.Abs(goc1) < 180)
goc1--;
}
else if (selectedAngle == "GOC2")
{
if (Math.Abs(goc2) < 180)
goc2--;
}
else if (selectedAngle == "GOC3")
{
if (Math.Abs(goc3) < 180)
goc3--;
}

94
 txt_Goc1.Text = goc1.ToString();
txt_Goc2.Text = goc2.ToString();
txt_Goc3.Text = goc3.ToString();
}

private void btn_Luu_Click(object sender, EventArgs e)

{
string[] arr = new string[6];
ListViewItem itm;

if (!GocSangToado()) return;

//Add first item

arr[0] = goc1.ToString();
arr[1] = goc2.ToString();
arr[2] = goc3.ToString();
arr[3] = ((int)X).ToString();
arr[4] = ((int)Y).ToString();
arr[5] = ((int)Z).ToString();

itm = new ListViewItem(arr);

listView1.Items.Add(itm);

}
private void btn_Reset_Click(object sender, EventArgs e)
{
goc1 = goc2 = 0;
goc3 = -118;
txt_Goc1.Text = goc1.ToString();
txt_Goc2.Text = goc2.ToString();

95
 txt_Goc3.Text = goc3.ToString();

if (comboBox2.SelectedIndex == comboBox2.Items.Count - 1)//Toa do sang

Goc
{
if (!ToadoSangGoc()) return;

}
else
{
if (!GocSangToado()) return;

}
if (Com.IsOpen == true)
{
truyenCom();
}
}

private void truyenCom()

{
Com.Write(":");
System.Threading.Thread.Sleep(100);
if (goc1 < 0)
{
Com.Write("-");

}
else
Com.Write("+");

Com.Write(Math.Abs(goc1).ToString().PadLeft(3, '0'));

96
 System.Threading.Thread.Sleep(100);
Com.Write(",");
System.Threading.Thread.Sleep(100);

if (goc2 < 0)
{
Com.Write("-");

}
else
Com.Write("+");
Com.Write(Math.Abs(goc2).ToString().PadLeft(3, '0'));
System.Threading.Thread.Sleep(100);
Com.Write(",");
System.Threading.Thread.Sleep(100);

if (goc3 < 0)
{
Com.Write("-");

}
else
Com.Write("+");
Com.Write(Math.Abs(goc3).ToString().PadLeft(3, '0'));
System.Threading.Thread.Sleep(100);
Com.Write(".");
}

private void btn_Tudong_Click(object sender, EventArgs e)

{
string tmp;
for(int i=0;i<listView1.Items.Count;i++)

97
 {
if(Com.IsOpen==true)
{

goc1 = (int)Int32.Parse(listView1.Items[i].SubItems[1].Text);
goc2 = (int)Int32.Parse(listView1.Items[i].SubItems[2].Text);
goc3 = (int)Int32.Parse(listView1.Items[i].SubItems[3].Text);
truyenCom();
System.Threading.Thread.Sleep(3000);//delay 3S
}
else
{
MessageBox.Show("Kết nối với cổng Com trước khi truyền lệnh!");
}

}
}

private void btn_Xoa_Click(object sender, EventArgs e)

{
if (listView1.Items.Count == 0)
return;
else
{
for (int i = listView1.SelectedItems.Count - 1; i >= 0; i--)
{
ListViewItem li = listView1.SelectedItems[i];
listView1.Items.Remove(li);

}
}

98
 }

private void btn_Nhap_Click(object sender, EventArgs e)

{
L1 = Int32.Parse(txt_L1.Text);
L2 = Int32.Parse(txt_L2.Text);
L3 = Int32.Parse(txt_L3.Text);
if (!GocSangToado()) return;
}

private void groupBox7_Enter(object sender, EventArgs e)

{

}
}

99