Báo cáo môn học kỹ thuật robot (robot caro)
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (1.24 MB, 28 trang )
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
MỤC LỤC
Lời nói đầu......................................................................................................................... 2
I.
Tổng quan.................................................................................................................... 3
1.
Lịch sử phát triển robot.........................................................................................3
2.
Phân loại robot......................................................................................................4
3.
Ứng dụng robot công nghiệp................................................................................4
4.
Ưu điểm và nhược điểm........................................................................................6
5.
Cuộc thi “ Robot chơi CARO”..............................................................................7
II.
Cơ sở thiết kế hệ thống cơ khí..................................................................................8
1.
Số bậc tự do robot:................................................................................................8
2.
So sánh các cơ cấu cánh tay robot hiện tại:...........................................................9
III.
Bài tốn động học...................................................................................................11
1.
Mơ hình động học...............................................................................................11
2.
Bài tốn động học nghịch....................................................................................11
3.
Khơng gian làm việc của robot...........................................................................13
4.
Bài tốn động học nghịch...................................................................................14
5.
Xác định góc Ɵ cho các vị trí bàn cờ..................................................................15
IV.
Điều khiển quỹ đạo di chuyển................................................................................17
1.
Mạch điện...........................................................................................................17
2.
Mạch điều khiển.................................................................................................17
3.
Code.................................................................................................................... 19
V.
Thực nghiệm và kết quả.........................................................................................26
VI.
Kết luận.................................................................................................................. 26
1.
Về lý thuyết........................................................................................................26
2.
Về thực nghiệm...................................................................................................27
3.
Ưu điểm, nhược điểm.........................................................................................27
4.
Hướng phát triển.................................................................................................27
1
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Lời nói đầu
Trong sự nghiệp cơng nghiệp hóa, hiện đại hóa đất nước vấn đề tự động hóa
có vai trị đặc biệt quan trọng.
Nhằm nâng cao năng suất dây chuyền công nghệ, nâng cao chất lượng và khả
năng cạnh tranh của sản phẩm, cải thiện điều kiện lao động, nâng cao năng suất lao
động…vấn đề được đặt ra là hệ thống sản xuất phải có tính linh hoạt cao. Robot cơng
nghiệp, đặc biệt là những tay máy robot là bô phận quan trọng để tạo ra những hệ thống
đó.
Tay máy Robot đã có mặt trong sản xuất từ nhiều năm trước, ngày nay tay
máy Robot đã dùng ở nhiều lĩnh vực sản xuất, từ những ưu điểm mà tay máy Robot hoạt
động trong quá trình sản xuất, làm việc, chúng ta có thể đúc rút ra những tính năng mà
con người khơng thể có được : khả năng làm việc ổn định, có thể làm việc trong mơi
trường độc hại…..Do đó việc đầu tư nghiên cứu, chế tạo ra những tay máy Robot phục vụ
cho tự động hóa sản xuất là rất cần thiết cho hiện tại và tương lai.
Môn học giúp đỡ chúng em bước đầu làm quen, tìm hiểu, học cách giải quyết bài
toán động học thuận và động học nghịch hay những bài toán tĩnh học, động lực học… đối
với những mơ hình tay máy Robot cơng nghiệp cơ bản, giúp chung em mở rộng và củng
cố kiến thức phục vụ cho hoạt động học tập, nghiên cứu cũng như công việc sau này.
Trong q trình học cịn nhiều điều chưa hiểu cặn kẽ cũng như những thiếu
xót , mong thầy giúp đỡ chỉ bảo thêm.
Cuối cùng em xin chân thành cảm ơn và chúc thầy nhiều sức khỏe, gia đình hạnh
phúc!
2
Project Robot CN
I.
GVHD: PGS.TS Nguyễn Trường Thịnh
Tổng quan
1. Lịch sử phát triển robot.
-
Vào năm 1921 Thuật ngữ “Robot” xuất phát từ tiếng Séc (Czech)
“Robota” có nghĩa là cơng việc tạp dịch trong một vở kịch.
-
Năm 1950 ở Mỹ thành lập viện nghiên cứu đầu tiên.
-
Đầu năm 1960 công ty AMF cho ra đời sản phẩm đầu tiên có tên gọi
là Versatran.
-
Từ năm 1967, ở Anh, người ta đã bắt đầu nghiên cứu và chế tạo IR.
-
Từ năm 1968, ở Châu Á, Nhật bắt đầu nghiên cứu những ứng dụng của IR,
năm 1970, Robot đã được chú ý nhiều hơn và bắt đầu xuất hiện ở các nước
Đức, Ý, Pháp...
-
Nhất là vào những năm 1990 số lượng Robot công nghiệp đã gia tăng với
nhiều tính năng vượt bậc. Chính vì vậy mà robot cơng nghiệp có vị trí quan
trọng trong các dây chuyền sản xuất tự động hiện đại như ngày nay
-
Đến nay, trên thế giới có khoảng trên 200 cơng ty sản xuất IR. Trong đó Mỹ
và Nhật chiếm đa số.
3
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Hình 1.1:Sơ đồ phát triển robot công nghiệp theo thời gian
2. Phân loại robot
-
Phân loại theo kết cấu
Robot chuỗi: Là một chuỗi động học hở với một khâu cố định gọi là đế
và các khâu động, trong đó các khâu động được bố trí nối tiếp với nhau.
Mỗi khâu động được liên kết hay nối động với một khâu khác nhờ các
khớp liên kết.
Robot song song: Là một chuỗi động học kín, ở đó mỗi khâu ln ln
được liên kết với ít nhất hai khâu khác.
Robot có khớp nối: Là robot có những khớp quay. Robot có khớp có thể
có hai kết cấu nối với nhau rất đơn giản đến những hệ thống có tới hơn
10 kết cấu tương tác với nhau. Chúng có thể dùng để nhấc các chi tiết
nhỏ với độ chính xác cực cao.
-
Phân loại theo phương pháp điều khiển:
4
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Điều khiển hở: Dùng truyền động bước mà quãng đường hoặc góc dịch
chuyển tỷ lệ với xung điều khiển. Kiểu này đơn giản nhưng cho độ chính
xác thấp.
Điều khiển kín: Điều khiển kiểu servo, sử dụng tín hiệu phản hồi vị trí để
tăng độ chính xác điều khiển
-
Phân loại theo ứng dụng
Dựa vào những ứng dụng của robot trong sản xuất ta có những loại robot
sau: robot sơn, robot hàn, robot lắp ráp, robot chuyển phôi...
3. Ứng dụng robot công nghiệp
-
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
Hiện nay trên thế giới, do nhu cầu sử dụng robot ngày càng nhiều trong các
quá trình sản xuất phức tạp với mục đích 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à nâng cao khả
năng cạnh tranh của sản phẩm đồng thời cải thiện điều kiện lao động, nên
robot công nghiệp cần có những khả năng thích ứng tốt và thông minh hơn
-
với những cấu trúc đơn giản và linh hoạt.
Có thể kể đến một số ứng dụng điển hình của robot trên thế giới như:
Robot song song dùng trong phân loại và đóng gói sản phẩm: IRB 660
Flex Palletizer, IRB 340 FlexPicker, IRB 260 FlexPicker. Các robot này
có thể gắp lần lượt các hộp vắc xin bại liệt từ băng tải và đặt nó vào
thùng gồm 20 hộp một cách chính xác.
Robot sơn và phun sơn: Được ứng dụng khá nhiều trong sơn gỗ, sơn thép
5
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Robot dùng trong công nghệ ép phun nhựa: IRB 6650 của hãng ABB có
thể thao tác nhanh, dễ dàng lấy sản phẩm ra khỏi khn ở vị trí tách
khn, giám sát, làm sạch, điều khiển chất lượng dựa trên camera.
Robot gắp hàng, Robot bốc xếp hàng hóa hay Robot bốc dỡ hàng hóa:
Đây là thiết bị Robot đa dạng nhất với điều khiển tự động
Robot vận chuyển và đóng gói sản phẩm: Robot đóng gói và vận chuyển
trong phạm vi rộng các sản phẩm khác nhau: giường đóng gói phẳng và
ngăn kéo.
Robot hàn công nghiệp hay robot hàn tự động: phục vụ cho ngành ô tô
xe máy, với tỷ lệ hơn 70% số robot đang hoạt động, phải kể đến là robot
hàn tự động Panasonic.
Ngồi ra, robot cịn có nhiều lĩnh vực được nghiên cứu như robot dịch
vụ, robot dùng trong lĩnh vực quân sự, robot di động đồng thời kết hợp
với nhận dạng và điều khiển trên cơ sở xử lý những thơng tin hình ảnh,
đặc biệt là kết hợp với xử lý ngôn ngữ.
4. Ưu điểm và nhược điểm
-
Những ưu điểm chính của tự động hóa là:
Tăng năng suất: Hệ thống tự động hóa có thể cho phép một nhà máy
họat động 24/7 trong một tuần, một tháng và thậm chí là cả năm. Nhờ
vậy, năng suất sản xuất sẽ được cải thiện và nâng cao đáng kể.
Tăng mức độ an toàn: hệ thống tự động hóa có thể thay thế con người
trong những mơi trường nguy hiểm như ở dưới nước, khu vực có lửa, cơ
sở hạt nhân hay ngoài vũ trụ..
Cải thiện chất lượng sản phẩm: tự động hóa có khả năng giảm các sai
sót mà con người có thể gặp phải. Hơn nữa, các sản phẩm được tạo nên
bởi hệ thống tự động hóa sẽ có chất lượng đồng đều hơn.
Giảm chi phí vận hành: tự động hóa cơng nghiệp giúp giảm thiểu số
nhân công cần thiết để vận hành máy móc dẫn đến việc giảm được các
6
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
chi phí liên quan đến con người như tiền lương, tiền bảo hiểm y tế, tiền
lương nghỉ lễ, nghỉ ốm, tiền làm thêm giờ hay lương hưu.
Tăng tính linh hoạt: khi thêm một bước mới vào quy trình sản xuất, các
nhà quản lý sẽ thường phải hướng dẫn, đào tạo người lao động về bước
mới đó. Tuy nhiên, robot và các hệ thống máy tính hồn tồn có thể
được lập trình để thực hiện các thao tác mới mà không cần qua đào tạo
-
hay hướng dẫn, nhờ đó giúp quy trình sản xuất trở nên linh hoạt hơn.
Tuy nhiên, tự động hóa trong cơng nghiệp cũng có các hạn chế như:
Các mối đe dọa an ninh: hệ thống tự động điều khiển bằng máy tính hay
robot hồn tồn có nguy cơ bị hack. Khi hệ thống tự động hóa bị tấn
cơng hay xâm nhập, sản xuất của một nhà máy sẽ bị đình trệ và gây ra
những thiệt hại nghiêm trọng.
Chi phí đầu vào cao: việc chuyển từ sử dụng nhân cơng con người sang
dây truyền sản xuất tự động địi hỏi chi phí đầu tư ban đầu rất cao.
Ngồi ra, cũng cần có chi phí cho việc đào tạo nhân cơng vận hành các
loại thiết bị, máy móc hiện đại và phức tạp.
5. Cuộc thi “ Robot chơi CARO”
-
Giới thiệu cuộc thi:
Nhằm tạo sân chơi lành mạnh, phát triển khả năng tư duy sáng tạo của
sinh viên, đồng thời giúp các bạn ứng dụng được những kiến thức cơ
bản đã học vào thực tế. Với mong muốn sinh viên được tiếp cận gần
hơn với những công nghệ tiên tiến, phát triển ngành công nghiệp robot
của nước nhà trong tương lai. Đồn khoa Cơ Khí Chế Tạo Máy đã đứng
ra tổ chức cuộc thi “Thiết kế mơ hình Robot chơi cờ caro”.
Chơi cờ Caro một trò chơi khá quen thuộc với mỗi chúng ta. Với lối
chơi đơn giản nhưng yếu tố trí tuệ lại rất cao nên cờ Caro được rất nhiều
người yêu thích đặc biệt là các bạn học sinh, sinh viên. Cùng với thiết
7
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
kế, chế tạo và điều khiển robot
chơi cờ Caro khơng chỉ mang tính
chất nghệ thuật, kỹ thuật mà là
một cuộc đấu trí vơ cùng gay cấn
dựa trên nền tảng trí tuệ nhân tạo
nhằm thể hiện khả năng thiết kế
robot và đẳng cấp bộ điều khiển
được thiết kế của sinh viên Sư
phạm Kỹ thuật.
Qua cuộc thi này, sinh viên sẽ có
thêm những kiến thức và kỹ năng mềm như: tư duy thiết kế, kỹ năng
giải quyết vấn đề, kỹ năng quản lý thời gian, tư duy sáng tạo, làm việc
nhóm, thuyết trình. Việc này sẽ góp phần nâng cao khả năng nghiên
cứu, phác thảo ý tưởng và trình bày các dự án khoa học của các bạn trẻ.
-
Thể lệ cuộc thi
Kích thước robot và bàn cờ:
8
Project Robot CN
-
GVHD: PGS.TS Nguyễn Trường Thịnh
Hình 1.2: Sơ đồ bàn cờ và vị trí đặt robot
Cách thức điều khiển robot:
Điều khiển cho robot di chuyển đầu bút từ vị trí ban đầu đến ơ trên bàn
cờ. Khi đầu bút chạm vào màn hình ở ơ nào thì coi như là đánh vào ơ đó.
Ban tổ chức sẽ cho hiển thị ơ đã đánh (vì đầu bút khơng tác động vào
màn hình cảm ứng được). Với cờ 3x3, nếu robot bên nào đánh được 3 ô
liên tiếp theo đường thẳng hoặc chéo là thắng.
II.
Cơ sở thiết kế hệ thống cơ khí
1. Số bậc tự do robot:
-
Để đáp ứng yêu cầu của cuộc thi, robot phải di chuyển được đầu bút đến
các ô và chạm vào các ô trên bàn cờ.
-
Vậy số bậc tự do tối thiểu của robot là 3 bậc:
TH1: Tịnh tiến trục x, tịnh tiến trục y, tịnh tiến trục z (DECAC)
TH2: 3 trục quay (DENTA)
TH3: 2 quay, 1 tịnh tiến trục z (SCARA)
2. So sánh các cơ cấu cánh tay robot hiện tại:
9
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Cơ cấu
Ưu điểm
Di chuyển
Nhược điểm Hình minh họa
Phức
tạp,
DENTA
nhanh, độ
khó
chính xác
khiển
điều
cao
Cơ cấu
Đơn giản
Chiếm
DECAC
hóa việc
khơng
điều khiển
lớn, kém linh
gian
hoạt
Cơ cấu
Không gian
SCARA
nhỏ, dễ điều
khiển, sử
Kém vững
bền theo
phương
ngang
dụng phổ
biết, di
chuyển linh
hoạt
Dựa vào yêu cầu kích thước bàn cờ, vị trí đặt robot, mục tiêu nhỏ gọn dễ
thiết kế và điều khiển và chi phí thấp. Nên chọn cơ cấu cánh tay robot
-
SCARA. Sử dụng 3 động cơ step
Chọn kích thước cho cơ cấu
2
2
Tọa độ điểm cần chạm xa nhất là l 350 80 360
10
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
l2 l3 360
Chọn: l2 220mm , l3 165mm
Khâu tịnh tiến không giới hạn về không gian tịnh tiến nên chọn:
l1 210mm
Hình 2.1: Mơ hình thiết kế
III. Bài tốn động học
1. Mơ hình động học
11
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Hình 3.1: Sơ đồ động học
Bảng DH như sau
2. Bài toán động học nghịch
-
Tổng quát: ma trận chuyển vị thứ i
cos i
�
�
sin
Ai � i
�0
�
�0
sin i .cos i
cos i .cos i
sin i
0
sin i .sin i
cos i .sin i
cos i
0
li .cos i �
li .sin i �
�
di �
�
1 �
12
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Ta được:
�
cos(1 + ) sin(1 + )
�
2
2
�
sin(1 + )
cos(1 + )
A1 �
�
2
2
� 0
0
�
�
0
� 0
cos( 2 ) sin( 2 )
�
�
sin( 2 ) cos( 2 )
A2 �
� 0
0
�
0
� 0
1
�
�
0
A3 �
�
0
�
0
�
0
1
0
0
�
)
2 �
�
�
0 l2 .sin (1 + )
2 �
�
1
l1
�
�
0
1
�
0 l2 .cos(1 +
0 l3 .cos( 2 ) �
0 l3.sin ( 2 ) �
�
�
1
0
�
0
1
�
0 0 �
0 0 �
�
1 l4 �
�
0 1 �
Kết quả
sin(1 + 2 ) cos(1 + 2 )
�
�cos( + ) sin( + )
1
2
1
2
U
TT A1. A2 . A3 �
�
0
0
�
0
0
�
0 l3 .sin(1 + 2 ) l2 .sin(1 ) �
0 l3 .cos(1 + 2 )+l2 .cos(1 ) �
�
�
1
l1 l4
�
0
1
�
Vậy:
Px l3 .sin(1 + 2 ) l2 .sin(1 )
Py l3 .cos(1 + 2 )+l2 .cos(1 )
Pz l1 l4
13
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
3. Không gian làm việc của robot
Hình 3.2: Khơng gian làm việc 2D
14
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Hình 3.3: Khơng gian làm việc 3D
4. Bài toán động học nghịch
Px l2 .sin 1 l3 .sin(1 2 )
Py l2 .cos 1 l3 .cos(1 2 )
Pz l1 l4
Px2 Py2 l22 l32 2.l2 .l3 .cos 2
� cos 2
Px2 Py2 l22 l32
2.l2 .l3
a tan 2(sin 2 ,cos 2 )
sin 2 � 1 cos 2 2
Px l2 .s1 l3 .s1 .c 2 l3.c1.s2 s1.( l 2 l3 .c 2 ) c1 ( l3 .s 2 )
Py l2 .c1 l3 .c1.c2 l3 .s1.s2 s1 ( l3 .s 2 ) c1 (l 2 l3c 2 )
Px � �
�
l l .s
�2 3 2
�
�
Py � � l3 .s2
�
l3 .s2 ��
s1 �
�
l2 l3 .s2 �
c1 �
�
��
15
Project Robot CN
s1
GVHD: PGS.TS Nguyễn Trường Thịnh
Px
Py
l3 .s2
l2 l3 .s2
(l2 l3 .c 2 ) 2 (l3 .s 2 ) 2
c1
l2 l3 .s2
l3 .s2
Px
Py
Px .(l2 l3 .c 2 ) Py .l3 .s2
l2 (l 2 l3 .c 2 ) 2 (l3 .s2 ) 2
Px .l3 .s2 Py .(l2 l3 .s 2 )
(l3 .s2 ) (l 2 l3 .c 2 )
(l3 .s2 ) 2 (l 2 l3 .c 2 ) 2
5. Xác định góc Ɵ cho các vị trí bàn cờ
2
2
Thơng qua tính tốn ta được:
Vị trí
x
y
1-1
-80
350
2-1
0
350
Góc 1 (o)
83.38
32.37
67.87
Góc 2 (o)
-43.44
-46.56
-36.97
16
Project Robot CN
IV.
GVHD: PGS.TS Nguyễn Trường Thịnh
3-1
80
350
1-2
-80
270
2-2
0
270
3-2
80
270
3-1
-80
190
3-2
0
190
3-3
80
190
22.13
57.63
6.62
70.64
52.37
52.43
37.57
37.63
19.36
68.10
67.57
44.10
45.90
22.43
21.90
-53.03
-43.44
-46.56
-1.12
-88.88
3.82
-93.82
-1.12
-88.88
28.43
-118.43
34.21
-124.21
28.43
-118.43
Điều khiển quỹ đạo di chuyển
1. Mạch điện
Nguồn
Adapter 12V, 10A
Thông số kĩ thuật:
-
Điện áp đầu vào: AC 100V-240V 50-60Hz
Điện áp ra: DC 12V Cường độ dòng điện: 10A
Điều chế: chỉnh độ rộng xung
Điện áp đầu ra chính xác: 95%
17
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Động cơ
-
Loại: Step motor NEMA 17 size 42 x 48mm
Số lượng: 3 cái
Thông số kỹ thuật:
-
Chiều dài: 48 (mm)
Đường kính trục 5mm. Đầu trục vát phẳng,
giúp puli/khớp nối không đỡ bị lỏng khi hoạt
-
động
Cường độ định mức 1.5A, mô men giữ 0.55 Nm, góc quay mỗi bước 1.8
-
°
Dây nối dài 1m, đầu dây chuẩn XH2.54. Tương thích với đầu ra động cơ
-
bước trên mạch RAMPS 1.5 hoặc CNC shield V3
Ít tỏa nhiệt, chuyển động êm
Khối lượng: 400g
2. Mạch điều khiển
Arduino Uno
Arduino Uno R3 là loại phổ biến và dễ sử dụng nhất trong các dịng
-
Arduino hiện nay cũng như tương thích với nhiều loại Arduino Shield nhất.
Thông số kỹ thuật:
-
Chip điều khiển chính: ATmega328P
Chip nạp và giao tiếp UART: ATmega16U2
18
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
-
Nguồn nuôi mạch: 5VDC từ cổng USB hoặc nguồn ngồi cắm từ giắc
-
trịn DC.
Flash Memory: 32 KB (ATmega328P), 0.5 KB dùng cho bootloader.
SRAM: 2 KB (ATmega328P)
EEPROM: 1 KB (ATmega328P)
Clock Speed: 16 MHz
Driver A4988
-
A4988 là một trình điều khiển vi bước để điều khiển động cơ bước lưỡng
cực có bộ dịch tích hợp để vận hành dễ dàng. Điều này có nghĩa là chúng ta
có thể điều khiển động cơ bước chỉ với 2
chân từ bộ điều khiển của chúng ta hoặc một
chân để điều khiển hướng quay và chân kia
để điều khiển các bước.
-
Driver cung cấp năm độ phân giải bước
khác nhau: bước , ẵ bc, ẳ bc, 1/8
bc v 1/16 bc. Ngoi ra, nó có một
biến trở để điều chỉnh đầu ra hiện tại, tắt
khi nhiệt độ quá cao và bảo vệ dịng điện chéo.
-
Nguồn vào của nó là từ 3 đến 5,5 V và dòng điện tối đa trên mỗi pha là 2A
nếu được làm mát bổ sung tốt hoặc dòng điện liên tục 1A mỗi pha mà
không cần tản nhiệt hoặc làm mát.
CNC Shield V3
19
Project Robot CN
-
GVHD: PGS.TS Nguyễn Trường Thịnh
Mạch Arduino CNC Shield V3 GRBL được sử dụng kết hợp với mạch
Arduino Uno và Driver động cơ bước (A4988, DRV8825,...) giúp bạn có
thể dễ dàng điều khiển động cơ bước, công tắc hành trình để setup hệ thống
điều khiển trong các ứng dụng CNC cỡ nhỏ 1 cách dễ dàng, mạch tương
thích với phần mềm điều khiển GRBL và có cách sử dụng đơn giản, chất
lượng gia công tốt, độ bền cao.
3. Code
#Include <math.h>
// để xài các hàm tốn học thì thêm vào thư viện math.h
// quy ước dựa trên skematic của shield cnc v3
// step 1, dùng để xoay
Const int steppin1 = 2; // trục x quy ước là pin 1
Const int dirpin1 = 5;
// step 2, dùng cho khâu 2
Const int steppin2 = 3; // trục y quy ước là pin 2
Const int dirpin2 = 6;
// step 3, dùng để di chuyển trục z lên xuống
Const int steppin3 = 4; // truc z quy uoc là pin 3
Const int dirpin3 = 7;
// khai báo biến
Const float pi=3.14;
Double x,y,z,i=0,t1,t2,t11,t21;
Float c1,s1,c2,s2,s21,c11,s11,x,y;
Float pulse1,pulse2,pulse3=600;
Float l2=225,l3=165;
Int a[5] = {0,-80,0,80}; // a là x
Int b[5] = {0,350,270,190};// b là y
20
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
Int k,val;
Void setup()
{
//khoi tao serial
serial.begin(9600);
// sets the two pins as outputs
pinmode(steppin1,output);
pinmode(dirpin1,output);
pinmode(steppin2,output);
pinmode(dirpin2,output);
pinmode(steppin3,output);
pinmode(dirpin3,output);
pinmode(8, output); // chân enable, low mới chạy
digitalwrite(8, low);
}
Void loop() {
int state=0,state2=0;
// pulse1 = x[k];
// pulse2 = y[k];
// chờ nhận dữ liệu từ serial
while (serial.available()==0);
//đọc dữ liệu
int val=serial.parsefloat();
//in giá trị nhận được
serial.println("đánh ô ");
serial.println(val);
//giải mã ký tự
21
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
x=a[val/10];
y=b[val%10];
//tính tốn góc
c2=(pow(x,2)+ pow(y,2)-pow(l2,2)-pow(l3,2))/(2*l2*l3);
s2=sqrt(abs(1 - pow(c2,2)));
//
c1=(-y*(l2+l3*c2)+x*l3*s2)/(-pow(l2+l3*c2,2)-pow(l3*s2,2));
s1=(x*(l2+l3*c2)+y*l3*s2)/(-pow(l2+l3*c2,2)-pow(l3*s2,2));
//đổi rad sang độ
t2=(atan2(s2,c2)*180)/3.14;
t1=(atan2(s1,c1)*180)/3.14;
// xong phần tính góc
//đổi độ ra số xung, đang chạy full-step, tương đương 1 xung ~1,8 độ, step
1 và step 2
t2=(atan2(s2,c2)*180)/3.14;
t1=(atan2(s1,c1)*180)/3.14;
t2=t2-90;
t1=t1+90;
serial.println("goc 1 la ");
serial.println(t1);
serial.println(" goc 2 la");
serial.println(t2);
// đổi độ sang xung
pulse1=(t1)/0.1125;
pulse2=(t2)/0.1125;
pulse3=360/0.1125;
serial.println("pulse 1 la ");
22
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
serial.println(pulse1);
serial.println(" pulse 2 la");
serial.println(pulse2);
//quy ước chiều step quay, chiều dương là theo chiều kim đồng hồ, high là
chiều dương, low chiều âm
//tính góc quy ước dương ngược kim đồng hồ
//xuất xung để quay đến ô cần đánh
//cách 1
delay(500);
serial.println(" neu pulse >0 thi state =1");
if(pulse1>0)
{state=1;}
if(pulse1<0)
{state=0;}
if(pulse2>0)
{state2=1;}
if(pulse2<0)
{state2=0;}
pulse1 = abs(pulse1);
pulse2 = abs(pulse2);
// in ra để kiểm tra
serial.println(" state 1 ban dau");
serial.println(state);
serial.println(" state 2 ban dau");
serial.println(state2);
serial.println("------");
//xét điều kiện lần 1
23
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
if(state==1)
digitalwrite(dirpin1,high);
if(state==0)
digitalwrite(dirpin1,low);
if(state2==1)
digitalwrite(dirpin2,low);
if(state2==0)
{digitalwrite(dirpin2,high);}
digitalwrite(dirpin3,high);
//delay(500);
// xuất xung chạy
//step 2 -> xoay
for(int i = 0; i < pulse2; i++) {
digitalwrite(steppin2,high);
delaymicroseconds(1000);
digitalwrite(steppin2,low);
delaymicroseconds(1000);
}
delay(400);
//step 1 -> xoay
for(int i = 0; i < pulse1; i++) {
digitalwrite(steppin1,high); //6000 là ok nếu ko micro
delaymicroseconds(1800);
digitalwrite(steppin1,low);
delaymicroseconds(1800);
}
24
Project Robot CN
GVHD: PGS.TS Nguyễn Trường Thịnh
delay(400);
//step 3 tịnh tiến
for(int i = 0; i < pulse3; i++) {
digitalwrite(steppin3,high); //6000 là ok nếu ko micro
delaymicroseconds(120);
digitalwrite(steppin3,low);
delaymicroseconds(120);
}
//quay lại vị trí ban đầu
state=!state;
state2=!state2;
serial.println(" state 1 khi đảo");
serial.println(state);
serial.println(" state 2 khi đảo");
serial.println(state2);
delay(400);
//xét điều kiện lại
if(state==1)
digitalwrite(dirpin1,high);
if(state==0)
digitalwrite(dirpin1,low);
if(state2==1)
digitalwrite(dirpin2,low);
if(state2==0)
digitalwrite(dirpin2,high);
digitalwrite(dirpin3,low);
delay(100);
25
