Muïc luïc

I. GIÔÙI THIEÄU THIEÁT BÒ.......................................................................................................2

II. BOÁ TRÍ THÍ NGHIEÄM.............................................................................................................3
III. ÑAÁU DAÂY THÍ NGHIEÄM.....................................................................................................4
IV. THÖÏC HAØNH.............................................................................................................................5
Baøi 1: Vaän haønh vôùi 1 baäc töï do vôùi haøm ñieàu khieån PID ñôn giaûn.............................5
Baøi 2: Vaän haønh nhieàu baäc töï do............................................................................................9
Baøi 3: Thöïc haønh laäp trình hoaït ñoäng...................................................................................11
Baøi 4: Ñieàu khieån vaän haønh töø Controller – Vaän haønh vôùi thuaät toaùn ñieàu khieån
PID 14

1
 I. GIÔÙI THIEÄU THIEÁT BÒ
Toaøn boä heä thoáng ñieàu khieån tay maùy thoâng minh vôùi 5 baäc töï do SCS-376 ñöôïc giôùi
thieäu treân hình A.1, bao goàm:
 Heä xöû lyù vaø tay maùy
 Baêng taûi
 Phaàn meàm ñieàu khieån Scorbot.

Hình 1. 0 Heä thoáng ñieàu khieån tay maùy thoâng minh vôùi 5 baäc töï do SCS-376

2
II. BOÁ TRÍ THÍ NGHIEÄM
Caùc thieát bò thí nghieäm coù theå boá trí theo caùch: Baêng taûi chöùng minh ñöôïc ñaët ôû phía
tröôùc tay maùy Scorbot.

Hình 2. 0 Boá trí thí nghieäm vôùi 1 baêng taûi chöùng minh

3
III. ÑAÁU DAÂY THÍ NGHIEÄM
 Caùc daây ñieàu khieån töø CPU vaø töø tay maùy Scorbot ñaõ ñaáu noái ra caùc jaéc 50 chaân
töông öùng. Khi boá trí thi nghieäm caàn caém caùc jaéc naøy vôùi nhau.

 Noái caùp RS-232 töø oå 9 chaân ôû maët tröôùc CPU vôùi oå RS-232 ôû maët sau maùy tính.

 Noái caùc daây tín hieäu vaø caáp nguoàn cho baêng taûi:

Hình 3. 0 Sô ñoà ñaáu noái baêng taûi

 Caém caùc ñaàu caém caáp ñieän (220V/50Hz) cuûa CPU, baêng taûi, maùy tính voái oå caáp
ñieän löôùi.

4
IV. THÖÏC HAØNH

Baøi 1: Vaän haønh vôùi 1 baäc töï do vôùi haøm ñieàu khieån PID ñôn giaûn
Muïc ñích thí nghieäm: Thöïc haønh cô sôû chæ vôùi 1 khôùp.
1. Baät ñieän cho heä thieát bò (CPU, maùy tính).
2. Vaøo chöông trình Robot_Ctrl coù treân maùy tính, Maøn hình seõ hieån thò:

Hình 4. 0 Phaàn meàm vaän haønh robot

Caùc bieåu töôïng coù yù nghóa nhö sau:

 Options: Chaïy treân coång Com1 vaø choïn taét môû bieåu ñoà : on graf / off graft.
 Controller: Xaùc laäp thuaät toaùn ñieàu khieån.
 Control Mode: kieåu ñieàu khieån.

Ñeå vaän haønh vôùi 1 truïc, ta choïn : Move \ Single axis vaø nhaán chuoät, maøn hình hieån th ò:

5
 Hình 4. 1 Ñieàu chænh moät truïc baèng boä ñieàu khieån PID

Motor: Nhaán nuùt leân xuoáng ôû oâ naøy ñeå choïn motor 1,2,.. ñeå ñieàu khieån 1 khôùp töông
öùng.

3. Nhaäp caùc giaù trò choïn khôùp quay (choïn motor) thí nghieäm cho tay maùy.
Kyù hieäu motor töông öùng vôùi khôùp quay
Motor Khôùp tay maùy Khôùp tay maùy
STT
1 Motor 1 Truïc ñeá quay Base
2 Motor 2 Khôùp vai Shoulder
3 Motor 3 Khôùp khuyûu tay Elbow
4 Motor 4 Khôùp gaäp coå tay Wrist pitch
5 Motor 6 Khôùp laéc coå Wrist roll
6 Motor 6 Tay keïp Gripper

6
 Hình 4. 2 Scorbot ER V Plus vôùi caùc kích thöôùc hình hoïc vaø goùc quay toái ña

Choïn khôùp cho thí nghieäm öùng vôùi motor 1.

Caùc giaù trò ñieàu khieån K, Ki, Kd, Ts hieån thò trong baûng laø giaù trò maëc ñònh (cho saün).
Caùc bieåu ñoà treân maøn hình bieåu dieãn xung ñeám (töø Encoder cuûa motor vaän haønh
khôùp), vaän toác, gia toác.
Treân maøn hình hieån thò caùc giaù trò toaï ñoä OXYZ vaø xung ñeám cuûa motor.
Xung quay: nhaán nuùt leân xuoáng ôû oâ naøy ñeå xaùc laäp giaù trò ñaët, ví duï = 3000
Ñaët con troû vaøo nuùt Run vaø nhaán chuoät seõ ñieàu khieån tay maùy hoaït ñoäng, nhaán Exit
ñeå thoaùt.
4. Theo doõi hoaït ñoäng tay maùy vaø giaùm saùt baèng bieåu ñoà vaø caùc thoâng soá khaùc treân
maøn hình khi caùc giaù trò K, Ki, Kp, Kd, coù giaù trò maëc ñònh.

7
5. Thay ñoåi thoâng soá ñieàu khieån:
Thay ñoåi caùc giaù trò haøm PID: K, Ki, Kp, Kd, sau ñoù nhaán download ñeå taûi giaù trò xaùc
laäp vaøo Controller. Theo doõi hoaït ñoäng tay maùy vaø giaùm saùt baèng bieåu ñoà vaø caùc
thoâng soá khaùc treân maøn hình
Ví duï: K= 0.0300 -> quan saùt thaáy ñaàu Scorbot vaän haønh bò rung.

6. Laëp laïi thí nghieäm vôùi laàn löôït caùc khôùp khaùc (Motor -= 2,3,4,5,6). Theo doõi hoaït ñoäng
tay maùy vaø giaùm saùt baèng bieåu ñoà vaø caùc thoâng soá khaùc treân maøn hình.

7. Nhaän xeùt hoaït ñoäng cuûa tay maùy trong cheá ñoä 1 truïc.

8
Baøi 2: Vaän haønh nhieàu baäc töï do
Muïc ñích thí nghieäm: Thöïc haønh cô sôû vôùi vaän haønh ñoàng thôøi nhieàu khôùp.
1. Vaøo chöông trình Robot_Ctrl coù treân maùy tính, sau ñoù choïn Move\ Manual, maøn hình seõ
hieån thò:

Hình 4. 3 Vaän haønh nhieàu khôùp

2. Nhaäp soá lieäu ñaët cho töøng khôùp quay vaøo baûng
Baûng vò trí (Position):
Toaï ñoä X-Y-Z tính theo mm.
Pitch : goùc giöõa truïc tay gaép so vôùi truïc z .
Roll: goùc laät (quay) truïc tay gaép.
Gripper: open / close : ñoùng môû ñaàu gaép (tay keïp).
Caùc haøng troáng trong baûng ñeå ñieàn giaù trò ñaët.

Chuù yù:

9
- Khi nhaäp giaù trò caùc haøng cuûa baûng phaûi lieân tuïc, tay maùy seõ chaïy töøng haøng tuaàn
töï ñeán cuoái roài quay trôû laïi haøng ñaàu.
- Trong cheá ñoä ñaët baèng tay, caàn ñaët caùc giaù trò ñeå caùnh tay ñoøn ôû xa truïc maùy, sao
cho truïc gaén tay gaép khi quay khoâng bò vöôùng.

Giaûi thích caùc nuùt ñieàu khieån treân baûng:

Control: Ñieàu khieån robot vaän haønh ñeán vò trí ñaët (baûng).
Set Px: xaùc laäp giaù trò thöïc tay maùy vaøo baûng (con troû ñaët ñuùng haøng).
Edit: söûa giaù trò töøng haøng ñeå ghi vaøo baûng.
Download: Taûi döõ lieäu thöïc hoaït ñoäng vaøo Controller, nhaèm xaùc ñònh caùc ñieåm
vaän haønh, hoaëc ñieåm goác.
New: laäp baûng môùi.
Load: taûi baûng cuõ ñaõ soaïn tröôùc.
Save: löu baûng vaøo boä nhôù.
Copy: choïn vuøng trong baûng (X:Roll) nhaán Copy, söû duïng ñeå gaùn (Paste) vaøo chöông
trình soaïn thaûo.
Exit: thoaùt.
Muoán chaïy chöông trình naøy phaûi nhaán Move \ Run from robot.

3. Theo doõi hoaït ñoäng tay maùy vaø giaùm saùt baèng bieåu ñoà vaø caùc thoâng soá khaùc treân
maøn hình.

10
Baøi 3: Thöïc haønh laäp trình hoaït ñoäng
Muïc ñích thí nghieäm: Laäp trình hoaït ñoäng vaø bieân dòch maõ trung gian laø cô sôû cho caùc
vaän haønh ñieàu khieån ôû möùc cao hôn. Trong phaàn naøy, hoïc vieân tìm hieåu quaù trình chuyeån
ñoåi döõ lieäu ñieàu khieån (ñöôïc nhaäp töø baûng) thaønh maõ trung gian söû duïng thoáng nhaát cho
heä ñieàu khieån tay maùy.

1. Höôùng daãn chuyeån Baûng khai baùo vò trí vaøo chöông trình soaïn thaûo:
- ÔÛ baøi treân, khi vaøo chöông trình Robot_Ctrl, choïn Move\ Manual, ta ñaõ nhaäp soá lieäu
ñaët cho töøng khôùp quay vaøo Baûng vò trí (Position) vôùi toaï ñoä X-Y-Z tính theo mm.
- Söû duïng leänh Copy: choïn vuøng trong baûng (X:Roll) nhaán Copy, söû duïng ñeå gaùn
(paste) vaøo chöông trình soaïn thaûo khai baùo vò trí.
- Söûû duïng CTRl-V ñeå gaùn caùc vò trí ñaõ choïn töø baûng treân (ñaõ nhaán Copy).
Ví duï ta coù:
Töø leänh Teân vò trí X Y Z Pitch Roll
Deft P1 186 0 300 0 0
Deft P2 350 0 300 0 0

2. Thöïc haønh:
- Nhaäp soá lieäu cho töøng khôùp quay theo caùc böôùc trong baøi treân.
- Söû duïng leänh Copy: choïn vuøng trong baûng (X:Roll) nhaán Copy, söû duïng ñeå gaùn
(paste) vaøo chöông trình soaïn thaûo khai baùo vò trí.
- Coù theå söûû duïng CTRl-V ñeå gaùn caùc vò trí ñaõ choïn töø baûng treân (ñaõ nhaán Copy)
vaøo chöông trình soaïn thaûo.
- Kieåm tra keát quaû nhaäp döõ lieäu.
3. Laäp trình hoaït ñoäng:
Heä phaàn meàm bieân dòch cô sôû SCOR-KC376 ñöôïc bieân soaïn treân neàn ngoân ngöõ C #.
Goàm caùc leänh toái thieåu:
“def” : Duøng ñeå khai baùo vò trí.
“label” : Taïo nhaõn trong chöông trình.
“goto” : Leänh nhaûy tröïc tieáp.
“move” : Leänh di chuyeån.
“wait” : Leänh chôø ñôïi.
“if…then goto” : Leänh ñieàu kieän reõ nhaùnh.
“open” : Môû ñaàu gaép.
“close” : Ñoùng ñaàu gaép.
Chöông trình ñieàu khieån hoaït ñoäng cuûa tay maùy ñöôïc vieát theo kieåu tuaàn töï caùc leänh.
Boä bieân dòch seõ taïo thaønh maõ leänh.
Chuù yù: Chöông trình bieân dòch coù phaân bieät chöõ hoa vaø chöõ thöôøng.

Xem ví duï chöông trình maãu demo1 söû duïng caùc leänh cuûa boä maõ bieân dòch SCOR-
KC376:
defp p1 186 0 306 0 0
defp p2 400 0 306 0 0
program

11
 label Lbl1
move p1
close
move p2
open
goto Lbl1
end

Chöông trình thöïc hieän di chuyeån tay maùy ñeán toaï ñoä p1, sau ñoù ñoùng tay gaép, tieáp
tuïc di chuyeån ñeán toaï ñoä p2, sau ñoù môû tay gaép vaø laëp laïi caùc ñoäng taùc naøy.

Sau khi soaïn thaûo xong, thöïc hieän nhôù (Save) chöông trình ñaõ soaïn thaûo.

Treân cô sôû boä leänh cuûa phaàn meàm bieân dòch cô sôû SCOR-KC376 tieán haønh laäp trình cho
caùc kieåu chuyeån ñoäng khaùc nhau:
- Di chuyeån giöõa hai ñieåm toaï ñoä baát kyø coù chôø ñôïi.
- Thay ñoåi ñoäng taùc tay gaép.

4. Bieân dòch chöông trình:

Bieân dòch chöông trình maãu demo1:
- Choïn File\ Compile -> nhaán OK
- Neáu chöông trình baùo khoâng coù loãi chöùng toû bieân dòch thaønh coâng. Xem hieån thò
treân maøn hình.
- Neáu coù baùo loãi ôû doøng naøo, caàn söûa cho ñeán khi heát loãi.
5. Taûi chöông trình xuoáng robot:
- Reset laïi robot, choïn Move\ Reset.
- Sau khi khôûi ñoäng xong roài taûi (Download) chöông trình ñaõ bieân dòch thaønh coâng
xuoáng Controller.

12
Hình 4. 4 Bieân dòch chöông trình

13
Baøi 4: Ñieàu khieån vaän haønh töø Controller – Vaän haønh vôùi thuaät toaùn ñieàu
khieån PID
Muïc ñích thí nghieäm: Cho pheùp thay ñoåi thuaät toaùn ñieàu khieån PID ñeå khaûo saùt hoaït ñoäng

Khaûo saùt hoaït ñoäng theo giaù trò PID

1. Vaøo chöông trình Robot_Ctrl neáu baét ñaàu töø ñaàu.
2. Vaøo File\ Open – goïi chöông trình ñaõ soaïn thaûo cho vaän haønh tay maùy, ví duï demo1 sau
ñoù thöïc hieän caùc böôùc bieân dòch – File \ Compile. Keát thuùc nhaán Close.
3. Vaøo Controller \ PID setting

Hình 4. 5 Baûng thoâng soá haøm hieäu chænh PID

4. Löïa choïn caùc thoâng soá PID.

5. Nhaán Download ñeå taûi xuoáng Controller .
6. Choïn Run from robot ñeå chaïy
7. Theo doõi hoaït ñoäng tay maùy vaø giaùm saùt baèng bieåu ñoà vaø caùc thoâng soá khaùc treân
maøn hình
Theo doõi thaáy tay maùy hoaït ñoäng toát, chaïy eâm.

14