ROBOT DI Ä Á NG THEO DAU TUONG

I HC QUC GIA TP.
H CH MINH
TRNG I HC BCH KHOA
KHOA C KH
BO MON C IEN T
&

BK

LUN VN TT NGHIP I HC

ROBOT DI NG THEO
DU TNG

Sinh vin thc hin : DON MINH NG
MSSV: P9900012
Cn b hng dn : TS. NGUYN TN TIN

CHNG TRNH O TO K S CHT LNG CAO
KHA 1: 1999 - 2004
TP.H Ch Minh, 07/2004

Li cam n
e thc hien e tai, tac gia a nhan c rat nhieu s ch dan, giup va
ong vien quy bau cua nhieu ngi, thieu mot trong cac s giup o cung co the
lam cho e tai khong at ket qua nh hien nay.
Trc het, em xin bay to long cam n sau sac oi vi thay TS. Nguyen Tan
Tien, ngi thay hng dan a tan tnh ch cho em phng phap nghien cu khoa
hoc, thay cung a cung cap cho em rat nhieu kien thc chuyen sau e thc hien e
tai.
Em cung vo cung cam n co Th.S Tran Th Ngoc Dung va cac thay co
chng trnh ao tao Ky s chat lng cao, thay TS. Nguyen Van Giap va cac thay
co bo mon C ien T, Khoa C kh, Trng ai Hoc Bach Khoa Tp.HCM a
tham gia qua trnh ao tao va hng dan em trong suot thi gian hoc ai hoc, nh
cac thay co ma em co u kien thc va long t tin e thc hien e tai nghien cu
nay cung nh cac e tai trong tng lai.
Ben canh o, s hp tac va giup cua ban be va cac the he an anh cung
giup toi rat nhieu trong viec thc hien e tai nay. Em xin c cam n KS. Lu
Tuan Anh, Khoa Cong nghe Vat Lieu, ngi a hng dan em i vao nghien cu
ve robot, Th.S Tran Van Tung va cac ban Phong th nghiem Thiet Ke May a
tch cc giup em trong thi gian thc hien e tai. Toi cung xin chan thanh cam
n cac ban cung hoc lp C ien T Viet Phap 99, ac biet la cac ban oan
Hiep, Nguyen Anh Kiet, Pham Huynh Phong, Nguyen Minh Trung, nhng ngi
cung nghien cu ve robot di ong a cho toi cac y kien ong gop quy gia!
Con cung xin cam n gia nh a luon cham soc va quan tam en viec hoc cua
con, con vo cung cam n va luon t hao v co Bo, Me, Ch luon ong vien con
trong qua trnh hoc tap.
Va cuoi cung, toi xin gi li cam n ti nhng ngi a tham gia giup toi
trong qua trnh thc hien luan van ma toi cha neu ten ay, s giup cua ho du
t hay nhieu cung ong gop mot phan vao ket qua thc hien e tai tot nghiep nay.
Tp. Ho Ch Minh, ngay 19 thang 7 nam 2004

Doan Minh ang

i
Muc luc
Li cam n ..........................................................................................................................................................
Muc luc...............................................................................................................................................................i
Danh muc cac hnh ve ...............................................................................................................................iii
Danh muc cac bang.....................................................................................................................................iii
Tom tat e tai................................................................................................................................................iv
Abstract.............................................................................................................................................................v
1 Tong quan va at van e .......................................................................................................................1
1.1 Gii thieu chung ve robot ........................................................................................ 1
1.2 Tong quan ve cac bai toan cua robot di ong [5] ...................................................... 4
1.3 Bai toan di chuyen theo tng va cac nghien cu lien quan ...................................... 5
1.3.1 Gii thieu bai toan ........................................................................................ 6
1.3.2 Mo hnh toan hoc.......................................................................................... 6
1.3.3 Muc tieu ieu khien...................................................................................... 8
1.4 Phng phap giai quyet van e ................................................................................ 8
2 Tom tat thuat toan ieu khien............................................................................................................9
2.1 Mo hnh bo ieu khien............................................................................................ 9
2.2 ac tnh bo ieu khien (theo ket qua chng minh va mo phong) ............................... 9
3 Thiet ke va thc hien phan cng.................................................................................................... 10
3.1 Kien truc robot ..................................................................................................... 10
3.2 Vi ieu khien PIC 16F877[13] ............................................................................... 11
3.3 Thiet ke khung giao tiep I2C................................................................................. 13
3.3.1 Ly do s dung giao tiep I2C......................................................................... 13
3.3.2 Khung giao tiep I2C trong robot................................................................... 13
3.4 Thiet ke e di chuyen va bo ieu khien ong c.................................................... 14
3.4.1 Thiet ke e di chuyen ................................................................................. 14
3.4.2 Bo ieu khien PID [15] ............................................................................... 14
3.5 Thiet ke cam bien................................................................................................. 16
3.5.1 Mo hnh toan hoc cua cam bien................................................................... 16
3.5.2 Thc hien cam bien .................................................................................... 17
3.6 Thiet ke cac mach ien t ..................................................................................... 19
3.6.1 Mach module master................................................................................... 19
3.6.2 Mach module slave..................................................................................... 20
4 Thc hien bo ieu khien va kiem chng giai thuat............................................................... 22
4.1 S o giai thuat chng trnh ................................................................................. 22
4.1.1 Giai thuat cho master module ...................................................................... 23
4.1.2 Giai thuat cho slave module ........................................................................ 24
4.2 Tien hanh th nghiem............................................................................................ 25
4.3 So sanh cac ket qua mo phong va th nghiem......................................................... 26
4.3.1 So sanh ket qua mo phong bang Matlab vi ket qua th nghiem.................... 26
4.3.2 Cac nhan xet bo sung.................................................................................. 28
5 Ket luan......................................................................................................................................................33

ii
5.1 o thch hp cua giai thuat.................................................................................... 33
5.2 Nhng han che cua e tai...................................................................................... 33
5.2.1 Ve viec che tao phan cng .......................................................................... 33
5.2.2 Nhng hien tng anh hng en ket qua va cach khac phuc........................ 33
5.3 Hng nghien cu tiep.......................................................................................... 34
TAI LIEU THAM KHAO...................................................................................................................... 35
PHU LUC A.................................................................................................................................................37
PHU LUC B..................................................................................................................................................39

iii
Danh muc cac hnh ve
Hnh 1.1 Mot so hnh anh ve robot va cac ng dung ................................................4
Hnh 1.2 Mo hnh bai toan robot di ong bam tng ................................................7
Hnh 3.1 S o khoi cua all-following mobile robot ...............................................11
Hnh 3.2 S o chan PIC 16F877 ............................................................................12
Hnh 3.3 Mo hnh e di chuyen lat ngc ...............................................................14
Hnh 3.4 Bo ieu khien PID van toc theo mo hnh song song ................................14
Hnh 3.5 ap ng cua bo ieu khien PID vi kp=8, ki=1, kd=1.............................15
Hnh 3.6 ap ng cua bo ieu khien PID vi kp=8.2, ki=1, kd=0.8.......................16
Hnh 3.7 Mo hnh toan hoc cua cam bien ...............................................................17
Hnh 3.8 Phan em tn hieu t encoder vao vi ieu khien module master..........18
Hnh 3.9 Hnh chup module cam bien.....................................................................18
Hnh 3.10 S o nguyen ly cua mach module master.............................................19
Hnh 3.11 Hnh chup module master .......................................................................20
Hnh 3.12 S o nguyen ly khoi x ly chnh cua module slave..............................20
Hnh 3.13 S o nguyen ly khoi khuech ai cong suat cua module slave..............21
Hnh 3.14 Hnh chup module slave .........................................................................21
Hnh 4.1 Lu o giai thuat cua master module .......................................................23
Hnh 4.2 Lu o giai thuat cua slave module..........................................................24
Hnh 4.3 Mo hnh th nghiem...................................................................................26
Hnh 4.4 So sanh o th cua van toc robot...............................................................27
Hnh 4.5 So sanh o th cua sai so khoang cach .....................................................27
Hnh 4.6 So sanh o th cua sai so goc....................................................................28
Hnh 4.7 Gia tr cua cam bien .................................................................................29
Hnh 4.8 Gia tr van toc goc cua robot va van toc goc (c lng) cua tng .......29
Hnh 4.9 Bien oi cua cac sai lech trong qua trnh hoat ong ................................30
Hnh 4.10 Gia tr van toc ra lenh cho 2 banh xe .....................................................30
Hnh 4.11 So sanh cac o th e
1
va e
2
cua hai th nghiem.......................................32

Danh muc cac bang
Bang 1.1 Tom tat lch s phat trien cua cong nghe robot .........................................2
Bang 4.1 Thong so th nghiem ................................................................................26
Bang 4.2 Thong so cua 2 th nghiem (TN) dung e so sanh...................................31
Bang 5.1 Cac hien tng anh hng en ket qua va cach khac phuc ....................33

iv
Tom tat e tai
Trong thi ai cong nghiep ngay nay, Robot ngay cang c s dung pho
bien trong san xuat cung nh trong cuoc song cua con ngi. Robot a co mot v tr
quan trong kho co the thay the c, no giup con ngi e lam viec trong cac ieu
kien nguy hiem, kho khan. Ngoai ra, Robot con c dung vao cac lnh vc tham
hiem khong gian, quan s, giai tr Lnh vc Robot di ong ang ngay cang chiem
c s quan tam cua cac nha nghien cu va xa hoi. T tnh hnh thc te o, viec
xay dng cac chng trnh hoat ong cho cac Robot la ieu thiet yeu ac biet oi
vi cac Robot di ong. Bai toan Robot di ong bam tng (wall-following problem)
la mot trong cac bai toan thng gap cua Robot kieu phan xa (reactive paradigm),
no a c giai bang nhieu cach khac nhau. Trong e tai "Robot di ong theo dau
tng", bai toan Robot di ong bam tng c giai quyet bang mot bo ieu khien
hoi tiep ay u trang thai ma ket qua a c chng minh bang mo phong. Mot
cam bien tiep xuc dung cac encoder c tao ra e s dung cho robot. Mo hnh
robot c che tao e tien hanh th nghiem nham kiem chng giai thuat cua bo
ieu khien. Ket qua th nghiem la can c e phat trien bo ieu khien danh cho bai
toan wall-following trong cac Robot sau nay.

v
Abstract
This project studies on control of a wall-following mobile robot. The wall is
assumed unknown. A tactile sensor is constructed to measure the angle and the
distance of mobile robot relatively to the wall that the mobile robot must follow. A
nonlinear controller is built based on Lyapunov stability. The experiment has been
carried out to verify the study. Based on this result, the proposed controller can be
used for control of a wall-following mobile robot problem.

Robot di ong theo dau tng 1. Tong quan va at van e
1
1
TONG QUAN VA AT VAN E

1.1 Gii thieu chung ve robot
Khai niem Robot theo ngha chung thng c hieu ong ngha vi khai
niem t ong hoa cong nghiep, ieu nay ch ung mot phan bi v: th nhat, Robot
ch la mot thanh phan trong he thong t ong hoa, th hai la t than viec trnh bay,
mieu ta Robot trong sinh hoat xa hoi t nhieu phong ai.
Nhng Robot xuat hien lan au tien NewYork vao ngay 9/10/1922 trong v
kchRossums Universal Robot cua nha soan kch ngi Tiep Khac la Karen
Chapek, con t Robot la mot cach goi khac cua t Robota-theo tieng Tiep co ngha
la cong viec lao dch. Khi o, Karen Chapek cho rang Robot la nhng ngi may
co kha nang lam viec nhng khong co kha nang suy ngh.
Gan mot the ky tiep theo, khai niem robot a lien tuc c phat trien, ong
gop them bi nhieu nha nghien cu, nhieu cong ty chuyen ve lnh vc robot. Di
ay la bang tom tat qua trnh lch s hnh thanh va phat trien cua cong nghe che
tao robot, va nhng tac ong cua khoa hoc cung nh xa hoi oi vi tng thi ky [4].

2
Bang 1.1 Tom tat lch s phat trien cua cong nghe robot
Moc
thi
gian
Nghien cu va phat
trien
ng dung trong
cong nghiep
Ky thuat ho
tr
Cac yeu to
anh hng
1920 Khai niem robot xuat
hien trong tieu thuyet

1940 Phat minh ra canh tay
may

1950 Phat sinh khai niem
robot thong minh

Gii thieu ve
bo nh vong

1960 Gii thieu ve robot
c ieu khien bang
may tnh

Tang cng nghien
cu

Phat trien robot
trong cong nghiep

ng dung
NASA va NAVY
May tnh
dung transitor

Gii thieu vi
x ly

1970 Robot co tr thong
minh nhan tao
S boc phat lan
au tien ve robot
Phat trien vi
x ly
S han che
cua nen
kinh te

1980 Robot dung trong
nhng cong viec nguy
hiem (1983)

Robot cong
nghiep thc te va
cac ng dung rong
rai khac
Ky thuat so

Ky thuat
quang

Nhu cau
tang cng
t ong
1990 Gii thieu ve
robot thong minh
trong san xuat
ieu khien
logic

Nghien cu
ve robot tr
thong minh
nhan to

Robot gay
nen tht
nghip
2000 Robot giong con
ngi
Cac tien bo
ve c kh

3
Trc nhng nam 1970, ngi ta ch tap trung vao viec phat trien nhng robot
tay may hoat ong trong cac nha may cong nghiep. Sau o mi xuat hien nhng
khai niem ve robot thong minh, va cac nghien cu bat au tap trung hn vao robot
di ong. Mot trong nhng chuyen gia au nganh ve robot di ong la Hans P.
Moravec (bat au nghien cu t nam 1964), va hien nay, chuyen nghien cu ve
robot di ong la Sebastien Thruns.
Cac robot di ong co ngi ieu khien a c dung cho cac muc ch quan
s, cac nhiem vu nguy hiem nh pha mn, tham do ay ai dng, ham mo, kiem
tra cac ng ong ngam, hay tham do sao Hoa
San pham robot di ong c san xuat ai tra va a vao th trng lan au
tien la robot hut bui Roomba va Trilobite cua hang Electrolux nam 2003.
hnh 1.1 la mot so hnh anh ve cac Robot va ng dung cua no:

a. Robot t hanh Sojourner tham hiem sao Hoa

b. Robot do mn
4

c. Tay may dung trong cong nghiep

d. Robot hut bui Trilobite
Hnh 1.1 Mot so hnh anh ve robot va cac ng dung
1.2 Tong quan ve cac bai toan cua robot di ong [5]
Van e "navigation" (tam dch la "di chuyen") la van e trong tam cua robot
di ong. e di chuyen c, robot phai thc hien mot loat cac tac vu, moi tac vu
gan vi mot bai toan nho trong bai toan "navigation". Cac bai toan o gom:
Mapping: la cong viec lap ban o moi trng hoat ong cua robot. Neu
khong c cung cap d lieu trc th robot phai co kha nang lap ban o.
Positioning: la viec nh v, robot phai co kha nang biet c mnh ang
au trong ban o toan cuc hoac a phng.
Path planning: la viec hoach nh ng i sap ti cua robot, sau khi no biet
c ban o va biet mnh ang v tr nao.
Motion control: la viec ieu khien cho robot di ong, tc la ieu khien cac c
cau e robot i theo con ng thu c t bai toan "path planning".
5
Obstacle avoidance: la nhiem vu tranh chng ngai vat khi robot ang di
chuyen.
1.3 Bai toan di chuyen theo tng va cac nghien cu
lien quan
Bai toan di chuyen theo tng:
Viec di chuyen theo tng (wall following) la mot tac vu thng thay robot
di ong, trong cac moi trng biet trc hoac khong biet trc. Tac vu nay c
dung vi cac nhiem vu: tranh chng ngai vat, i theo tng biet trc, i theo
tng khong biet trc.
Mot so nghien cu a thc hien:
Turennout et al., 1992 [6]: mot bo ieu khien hoi tiep dung bo quan sat e c
lng khoang cach va goc gia robot vi tng, dung cam bien sieu am.
Medromi et al., 1994 [7]: dung mot bo quan sat e c lng trang thai cua he
phi tuyen vi au vao khong biet trc.
Urzelai, J. et al., 1997 [8]: s dung bo ieu khien m e ieu khien robot
VEA-II dung cam bien sieu am.
Bemporad et al., 1997 [9]: a ra hng tiep can dung s chong chat cam bien
e c lng toa o cua robot, trong o dung mot bo loc Kalman e ket hp tn
hieu t cam bien sieu am va cam bien o dch chuyen.
Yata et al., 1998 [10]: a ra phng phap bam tng s dung viec o goc
nh cam bien sieu am.
Chung Tan Lam et al, 2004 [11]: mot bo ieu khien hoi tiep phi tuyen khi biet
khoang cach va goc gia robot vi tng, mot bo ieu khien da tren bo quan sat
khi ch biet khoang cach gia robot vi tng, dung cam bien c. Cac bo ieu
khien on nh theo tieu chuan Lyapunov, cac ket qua mo phong va thc nghiem a
chng to hieu qua cua cac bo ieu khien.
Nguyen Viet Hiep va Pham nh Anh Vu [3]: thiet ke bo ieu khien hoi tiep
phi tuyen va bo ieu khien da tren bo quan sat e ieu khien robot i theo tng,
mo phong cac bo ieu khien e kiem chng tnh hoi tu va on nh.
6
Cac ket qua nghien cu tren cho thay hng giai quyet bai toan robot di
chuyen theo tng bang cac bo ieu khien hoi tiep phi tuyen la mot hng i thch
hp. e tai cua Nguyen Viet Hiep va Pham nh Anh Vu, cac bo ieu khien a
c a ra va chng minh bang ly thuyet va mo phong, nhng cha c kiem
nghiem thc te. Da tren thanh qua o, luan van nay co nhiem vu la: Kiem chng
ly thuyet nghien cu ve bai toan robot di chuyen theo tng a c xay dng
trong luan van tot nghiep i trc.
e thc hien muc tieu tren, luan van nay tap trung vao cac van e sau:
- Nghien cu cac bo ieu khien cho robot bam tng.
- Thiet ke va che tao mot e di chuyen (mobile platform).
- Thiet ke va che tao mot cam bien tiep xuc gan len robot e do sai so gia
robot vi tng.
- Thiet ke va thc hien cac mach ieu khien cho robot.
- Lap trnh cho robot e hien thc cac bo ieu khien.
- Th nghiem va so sanh ket qua th nghiem vi ket qua mo phong.
- Nhan xet ket qua va ket luan.
1.3.1 Gii thieu bai toan
Thiet ke va thc hien bo ieu khien dung e ieu khien Mobile Robot di
chuyen doc theo tng vi van toc va khoang cach t Robot en tng cho trc.
Gia thiet cua bai toan:
* o c khoang cach d t tng en Robot.
* o c goc lech gia Robot va tng.
* Tng la ng cong trn bat ky vi ban knh cong cua tng khong nho
hn khoang cach d.
1.3.2 Mo hnh toan hoc
Mo hnh robot di ong bam tng c cho nh hnh 1.2
7

Hnh 1.2 Mo hnh bai toan robot di ong bam tng
Trong o:
* (x,y): he truc toa o tuyet oi.
* R: ban knh banh xe cua Mobile Robot.
* b: khoang cach t tam Mobile Robot en banh xe.
* W:giao iem cua truc Y cua Mobile Robot vi tng.
* t: tiep tuyen vi tng tai iem W.
*
: goc nh hng cua Mobile Robot.

*
w
: goc nghieng cua tng.

* d :khoang cach t Mobile Robot en tiep tuyen t.
* d0 :khoang cach yeu cau t Mobile Robot en tng.
*
2
e
: goc lech gia mobile robot vi tng.
Phng trnh ong hoc cua Mobile robot a c nghien cu bi cac cong
trnh trc ay cua nhieu tac gia [6]. Van toc c bieu dien nh sau:
8

1
]
1
1
1
1
]
1
1
1
1
]
1
v
y
x
1 0
0 sin
0 cos
&
&
&
[1-1]
Vi
v
: van toc dai cua Robot.
: van toc goc cua Robot.

Moi quan he gia
v
,
vi van toc goc cua hai banh xe nh sau:

1
]
1
1
]
1
1
]
1

v
R b R
R b R
lw
rw
/ / 1
/ / 1
[1-2]
vi
lw
,
rw
: van toc goc cua banh xe trai va banh xe phai cua Mobile Robot.
1.3.3 Muc tieu ieu khien
Yeu cau cua bai toan la phai ieu khien robot chay theo bien dang song song
vi tng, sao cho cac sai so khoang cach (e
1
) va (e
2
) hoi tu ve 0, robot hoat ong
on nh trong mot vung lan can iem hoi tu.
1.4 Phng phap giai quyet van e
e giai quyet bai toan robot di ong i theo tng, hai bo ieu khien a c
trnh bay trong e tai luan van tot nghiep nganh C ien T, trng HBK
Tp.HCM cua hai sinh vien Nguyen Viet Hiep va Pham nh Anh Vu. Mot bo ieu
khien la hoi tiep ay u trang thai (full-state feedback), mot bo ieu khien la da
tren bo quan sat (observer-based) [2]. Trong e tai nay, chung toi s dung cac ket
qua o e thc hien cac bo ieu khien, nham kiem tra tnh chnh xac cua ket qua
mo phong.
Theo gia thiet cua bai toan, ta biet c gia tr cua ca 2 bien trang thai la e
1

va e
2
, nh vay ta co the s dung mot bo hoi tiep tat ca bien trang thai (full-state
feedback controller). Bo ieu khien nay se c e cap en chng sau.
Robot di ong theo dau tng 2. Tom tat thuat toan ieu khien
9
2
TOM TAT THUAT TOAN IEU KHIEN

2.1 Mo hnh bo ieu khien
Cong thc cua bo ieu khien full-state feedback:

'
+
+ +
2 2 2 0 1 1
2 1 1 2 0 1 2
/ sin tan ) (
sin ] cos / ) ( [
k e e d e e
e k e e d e v k
v v
w
w w r
r

&
[2-1]
Trong o:
r
v
: van toc yeu cau
0
d
: khoang cach yeu cau
: c lng van toc goc cua tng

2 1
, k k
: cac tham so cua bo ieu khien e on nh Lyapunov
2 1
, e e
: sai so ve khoang cach va goc gia robot vi tng
v
: van toc dai cua robot
: van toc goc cua robot (van toc cua vect ch hng robot)
2.2 ac tnh bo ieu khien (theo ket qua chng minh
va mo phong)
Qua ket qua chng minh va mo phong, bo ieu khien nay co cac tnh chat
sau:
- On nh theo tieu chuan Lyapunov dang 2.
- Cac sai so e1 va e2 hoi tu ve zero sau thi gian 8-10s, vi cac tham so mo
phong lay theo mo hnh thc.
Robot di ong theo dau tng 3. Thiet ke va thc hien phan cng
10
3
THIET KE VA THC HIEN PHAN CNG

3.1 Kien truc robot
Robot c thiet ke theo kieu kien truc SA (subsumption architect) [12], phan
thanh 2 lp:
- Lp di la cac bo ieu khien ong c, bo thu tn hieu cam bien.
- Lp tren la bo ieu khien trung tam. Bo ieu khien nay khong can thiep vao
hoat ong cua cac bo phan lp di.
S o khoi ieu khien c cho hnh 3.1, gom 01 khoi x ly chnh (master
module) va 02 khoi x ly phu (slave module).
Master module co cac chc nang:
- oc tn hieu t cam bien, tnh cac sai so ve khoang cach va goc gia robot
vi tng.
- Dung giai thuat ieu khien full-state feedback e tm cac van toc dai va van
toc goc mi cho robot, nham giam cac sai so.
- Chuyen van toc cua robot thanh van toc cua 2 banh xe, gi cac gia tr o cho
cac slave module.
Slave module co cac chc nang:
- Nhan lenh t master module (van toc mong muon cua moi banh xe).
- Tnh van toc hien thi cua robot qua so xung hoi tiep trong 1 chu ky.
- Dung giai thuat ieu khien PID e tnh gia tr PWM, nham ieu khien banh
xe quay vi van toc c ra lenh.
Giao tiep gia master module va 2 slave module c thc hien da tren
chuan giao tiep I2C, chuan nay se c giai thch ro hn oan di.
11

Hnh 3.1 S o khoi cua Wall-following mobile robot
3.2 Vi ieu khien PIC 16F877[13]
Vi ieu khien c chon dung trong e tai la loai vi ieu khien PIC 16F877
cua cong ty Microchip. Sau ay la vai net chnh cua vi ieu khien nay:

12
Bo x ly chnh:
Loai bo x ly: RISC CPU
Ch co tat ca 35 lenh
Hau het cac lenh la 1 chu ky may,
ch cac lenh re nhanh la 2 chu ky
Tan so toi a: vi thach anh 20
MHz chu ky may la 200ns
Lu c 8K lenh trong bo nh
chng trnh, moi lenh 14 bits.
Co 368 x 8 bytes bo nh d lieu
(RAM)
Co 256 x 8 bytes bo nh
EEPROM
Nhieu loai ngat (14 loai)
Power-on Reset (POR)
Power-up Timer (PWRT) va
Oscillator Start-up Timer (OST)
Watchdog Timer (WDT) vi bo
dao ong tch hp ben trong
Bao mat chng trnh
Co che o SLEEP e tiet kiem
nang lng
In-Circuit Serial Programming
(ICSP - chuan ghi bo nh chng
trnh khi vi x ly van trong
mach)
Single 5V In-Circuit Serial
Programming
In-Circuit Debugging
Vi x ly truy cap c vao bo nh
chng trnh
ien ap hoat ong rong: 2.0V en
5.5V
Chu c nhiet o trong moi
trng cong nghiep
Nang lng tieu thu t:
< 0.6 mA 3V, 4 MHz
20 mA 3V, 32 kHz
< 1 mA che o standby

Hnh 3.2 S o chan PIC 16F877
Thiet b ngoai vi:
Timer0: 8-bit timer/counter vi bo chia
trc 8-bit
trc, co the hoat ong trong che o
SLEEP
trc va chia sau 8-bit
2 bo tch hp Capture, Compare, PWM
- Capture 16-bit, o phan giai toi a 12.5
ns
- Compare is 16-bit, o phan giai toi a
200 ns
- PWM co o phan giai toi a 10-bit
8 kenh bien oi Analog-to-Digital 10-bit.
Synchronous Serial Port (SSP) vi 2
chuan: SPI (Master mode) va I2C
(Master/Slave)
Universal Synchronous Asynchronous
Receiver Transmitter (USART/SCI)
Parallel Slave Port (PSP) 8-bits, co cac
chan ieu khien RD, WR va CS
Brown-out Reset (BOR)
13
3.3 Thiet ke khung giao tiep I2C
3.3.1 Ly do s dung giao tiep I2C
Vi ieu khien PIC 16F877 ho tr nhieu chuan giao tiep, trong o co chuan
giao tiep I2C c s dung lam cho giao tiep gia cac module trong robot nay.
Chung toi chon s dung chuan I2C v mot so ly do sau:
- I2C ho tr mot mang nhieu thiet b ket noi vi nhau. So thiet b toi a co the
co trong mang I2C gom 1 module master va 127 module slave (hien tai mi dung 2
module slave). Nh vay robot con nhieu kha nang m rong ve sau.
- Chuan I2C la chuan thong dung, c s dung nhieu trong cac linh kien
dung che tao robot di ong (nh cam bien sieu am SRF08, cam bien la ban
CMPS03). Neu sau nay che tao mot robot di ong da tren c s robot bam theo
tng, ta cung de dang tch hp cac linh kien khac vao robot.
- Moi trng lap trnh ang dung ho tr tot cho viec giao tiep bang I2C.
Chung toi a th nghiem va thay chng trnh giao tiep hoat ong on nh.
e hieu them ve chuan giao tiep I2C, xin xem phu luc A.
3.3.2 Khung giao tiep I2C trong robot
Cach thc truyen tn hieu qua I2C trong robot c quy nh nh sau:
- Module master ch ghi gia tr vao cac module slave, khong co che o oc.
- a ch cua module slave ieu khien di chuyen banh trai la 0xA0, cua
module ieu khien di chuyen banh phai la 0xC0. Moi khi muon gi d lieu cho
module slave nao, module master gi 1 byte a ch cua slave o, sau o gi tiep 2
byte d lieu.
- D lieu gi t module master en module slave la mot bien so nguyen 16
bit. Trc khi gi no phai c cat thanh 2 byte, sau khi module slave nhan th
ghep 2 byte o tr lai thanh so nguyen 16 bit ban au.
- moi chu ky hoat ong, module master gi cho moi module slave mot d
lieu, la van toc yeu cau module slave at c. 2 d lieu c gi lien tiep e
giam o tre ap ng gia 2 module slave.

14
3.4 Thiet ke e di chuyen va bo ieu khien ong c
3.4.1 Thiet ke e di chuyen
Cac thanh phan cua e di chuyen:
- 2 banh dan ong, gan vao 2 ong c co hop giam toc, co hoi tiep bang
encoder quang. Moi ong c c ieu khien bi 1 mach ien rieng. Mach ieu
khien, ong c va banh xe cau thanh mot module di chuyen, robot co 2 module di
chuyen trai va phai tach ri nhau.
- Banh tuy ong la mot banh cau, at sau xe.
Ban ve thiet ke: xem ban ve mo hnh robot (ban ve nh kem).

Hnh 3.3 Mo hnh e di chuyen lat ngc
3.4.2 Bo ieu khien PID [15]
Mo hnh ieu khien PID c s dung ay la:

Hnh 3.4 Bo ieu khien PID van toc theo mo hnh song song
15
Bo PID c s dung e am bao van toc quay ma module ieu khien trung
tam ra lenh cho module di chuyen thc hien.
- au vao cua bo PID: van toc yeu cau, n v la vong/phut.
- Tn hieu hoi tiep: van toc hien thi, t so xung encoder oc c trong 1 chu
ky, oi ra vong/phut.
- oi tng ieu khien: van toc ong c.
- au ra cua bo PID: gia tr chu ky o rong xung (PWM duty) cua ien ap hai
au ong c.
Phng phap thc hien bo ieu khien PID van toc: do khong co nhieu thi
gian e tm cac thong so cua ong c nham mo hnh hoa ong c, chung toi lap
trnh bo ieu khien ong c tren vi ieu khien PIC vi cac gia tr kp, ki va kd thay
oi c. Khi hieu chnh t t cac tham so kp, ki va kd va xem ap ng cua bo ieu
khien, chung toi la chon c bo tham so thch hp cho bo ieu khien.
Ta co the thay ap ng cua bo PID thay oi theo s thay oi nho cua cac he
so ieu khien thong qua cac o th hnh 3.5 va hnh 3.6. Bo PID c trnh bay
hnh 4.6 c xem la tot hn bo PID hnh 3.5 (ap ng at mc yeu cau, t dao
ong), o la bo PID tot nhat ma chung toi tm c cho cac module di chuyen cua
robot.
0 200 400 600 800 1000 1200 1400 1600 1800
0
5
10
15
20
25
30
PID,kp=8,ki=1,kd=1 - Dap ung cua bo dieu khien PID
t(ms)
v

(
r
p
m
)
command
response

Hnh 3.5 ap ng cua bo ieu khien PID vi kp=8, ki=1, kd=1
16
0 500 1000 1500 2000 2500 3000
0
5
10
15
20
25
30
PID,kp=8.2,ki=1,kd=0.8 - Dap ung cua bo dieu khien PID
t(ms)
v

(
r
p
m
)
command
response

Hnh 3.6 ap ng cua bo ieu khien PID vi kp=8.2, ki=1, kd=0.8
Cac ch tieu cua bo ieu khien PID:
- Thi gian at mc (rise time): 400ms
- o vot lo (overshoot): 40% (tng ng vi 2 xung encoder/10ms)
- Thi gian xac lap (settling time): 1000ms
Nhan xet: Do ong c c s dung la mot ong c khong tot (cong suat 4W,
toc o toi a khoang 1000 vong/phut, t so hop giam toc la 10), cong vi encoder co
o phan giai khong cao (gia tr vot lo 40% tng ng vi 2 xung encoder trong 1
chu ky lay mau la 10ms), hn na ta lai s dung ong c toc o quay thap, nen
ket qua cua bo ieu khien PID khong c tot. Tuy nhien, neu khong co cac thanh
phan I va D th bo ieu khien P thong thng se khong the ap ng c van toc
mong muon trong thi gian ngan, o la ly do phai s dung bo ieu khien PID.
3.5 Thiet ke cam bien
3.5.1 Mo hnh toan hoc cua cam bien
Viec thiet ke mot cam bien tot co y ngha rat quan trong trong robot di ong
nay, e lam c ieu o, ta can mot mo hnh toan hoc hp ly. Mot so mo hnh
toan hoc cua cam bien a c nghien cu, cuoi cung chung toi a ra mo hnh
toan hoc hnh 3.7 e thc hien cam bien cho robot.
17

Hnh 3.7 Mo hnh toan hoc cua cam bien
Cam bien c set v tr ban au ng vi e
2
=0 va d=d
0
. Khi robot chuyen
ong, 2 thanh trt tiep xuc vi tng qua 2 con lan tai cac tiep iem W va W'. Do
ban knh cong cua tng ln, ta xem tng trong oan WW' la thang. Hn na, nh
r nho nen e
3
khong ang ke, ta xem e
3
=0. Cong thc tnh e
1
va e
2
la:

) (
) cos( *
2 1
2
0 2 1 0 1
h
d d
arctg e
d e d d d e
w

[3-1]
vi d
1
, d
2
: o dch chuyen cua thanh trt 1 va 2
h: khoang cach gia 2 thanh trt.
3.5.2 Thc hien cam bien
Module cam bien bao gom 3 bo phan chnh: 2 thanh trt co con lan au e
tiep xuc vi tng, 2 encoder e do o dch chuyen cua cac thanh trt va 1 vi ieu
khien e oc o dch chuyen.
Thanh trt c gan lo xo ( ay dung day thun) ay ra e luon tiep xuc vi
tng. Tren moi thanh trt co gan day keo doc theo thanh, khi thanh trt chuyen
ong, day keo se keo con lan chuyen ong. Do con lan c noi chat vi truc
18
encoder nen chuyen ong cua con lan se lam quay a cua encoder, vi moi dch
chuyen nho cua a, encoder se gi xung ve cho vi ieu khien e x ly. Moi
encoder truyen tn hieu ve cho vi ieu khien qua 2 ng A va B, chung c cho
vao mot IC flip-flop e xac nh chieu quay cua encoder. Vi ieu khien nhan tn
hieu t cac encoder qua 2 cong CCP (capture) va dung 2 chan digital input (RE1 va
RE2) e nhan biet chieu quay gi t IC flip-flop, cac xung gi t encoder vao cong
CCP se tao ra ngat (interrupt) e tien viec tnh toan tren vi ieu khien, tn hieu
cac chan digital input se cho vi ieu khien biet c xung o ng vi chuyen ong
vao hay ra cua thanh trt.
S o mach thu tn hieu t encoder:
VCC
CCP2
VCC
R17
1k8
VCC
VCC
RE1
J3
Encoder 2
1
2
3
4
VCC
J2
Encoder 1
1
2
3
4
CCP1
U5A
7474
2
3
5
6
4
1
D
CLK
Q
Q
PRE
CLR
R16
1k8
RE2
R15
1k8
VCC
R18
1k8
U5B
7474
12
11
9
8
10
13
D
CLK
Q
Q
PRE
CLR

Hnh 3.8 Phan em tn hieu t encoder vao vi ieu khien module master
Ket qua thc hien:

Hnh 3.9 Hnh chup module cam bien
19
3.6 Thiet ke cac mach ien t
3.6.1 Mach module master
RX
SW2
SW PUSHBUTTON 1
J1
PORT D
1
2
3
4
5
6
7
8
RE2
VCC
LS2
SPEAKER
J5
Serial line
1
2
3
4
R11
10k
1
2
3
4
SW5
SW PUSHBUTTON 4
RE1
CCP1
C8
0.1uF
VCC
J10
TxCLK
1
2
Q2
C1815
R14
4k7
SDA
U1
PIC16F877
2
3
4
5
6
7
33
34
35
36
37
38
39
40
15
16
17
18
23
24
25
26
31
32
1
13
1
4
11
12
8
9
10
19
20
21
22
27
28
29
30
RA0
RA1
RA2
RA3
RA4
RA5
RB0
RB1
RB2
RB3
RB4
RB5
RB6
RB7
RC0
RC1
RC2
RC3
RC4
RC5
RC6
RC7
GND
VDD
MCLR
OSC1
O
S
C
2
VDD
GND
RE0
RE1
RE2
RD0
RD1
RD2
RD3
RD4
RD5
RD6
RD7
VCC
J7
PORTB_L
1
2
3
4
J6
I2C line sup
1
2
3
4
VCC
VCC
SCL
R10 10k
SW1
RESET button
CCP2
SW3 SW PUSHBUTTON 2
R13
4k7
R12 330
J8
IR2
1
2
3
J9
IR1
1
2
3
VCC
SW4 SW PUSHBUTTON 3
TX
+ C9
470uF
J4
I2C line
1
2
3
4

Hnh 3.10 S o nguyen ly cua mach module master
Cac thanh phan va chc nang cua chung trong mach master:
- Vi ieu khien PIC 16F877: bo x ly cua ca mach.
- Ngo giao tiep noi tiep: e giao tiep vi mach nap va chng trnh kiem tra
tren may tnh.
- Ngo giao tiep I2C: e giao tiep vi 2 vi ieu khien cua cac module slave.
- 2 ngo noi vi encoder e oc tn hieu t cam bien, co mot IC flip-flop e
em cho tn hieu t encoder.
- 4 nut bam phuc vu viec nhan lenh t ngi s dung.
- 1 loa danh e bao hieu cac giai oan trong chng trnh.
20

Hnh 3.11 Hnh chup module master
3.6.2 Mach module slave
Gom 2 khoi: khoi x ly chnh va khoi khuech ai cong suat.
C8
0.1uF
+ C9
470uF
BRAKE
SW1
RESET button
R14
4k7
U1
PIC16F877
2
3
4
5
6
7
33
34
35
36
37
38
39
40
15
16
17
18
23
24
25
26
31
32
1
13
1
4
11
12
8
9
10
19
20
21
22
27
28
29
30
RA0
RA1
RA2
RA3
RA4
RA5
RB0
RB1
RB2
RB3
RB4
RB5
RB6
RB7
RC0
RC1
RC2
RC3
RC4
RC5
RC6
RC7
GND
VDD
MCLR
OSC1
O
S
C
2
VDD
GND
RE0
RE1
RE2
RD0
RD1
RD2
RD3
RD4
RD5
RD6
RD7
VCC
VCC
VCC
VCC
SDA
PWM
R11
10k
LS2
SPEAKER
J2
To ENCODER
1
2
3
4
TX
1
2
3
4
VCC
R13
4k7
Q2
C1815
J1
4BIT7SEG_DISPLAY
1
2
3
4
5
6
7
8
9
J4
I2C line
1
2
3
4
R12 330
J5
Serial line
1
2
3
4
VCC
RX
DIR
R10 10k
U5A
7474
2
3
5
6
4
1
D
CLK
Q
Q
PRE
CLR
VCC
SCL
VCC
J3
PWM select
1 2
3 4
VCC
R16
1k8
R15
1k8

Hnh 3.12 S o nguyen ly khoi x ly chnh cua module slave
Cac thanh phan trong khoi x ly chnh:
- Vi ieu khien PIC 16F877: bo x ly cua module.
- Ngo giao tiep noi tiep: e giao tiep vi mach nap va chng trnh kiem tra
tren may tnh.
- Ngo giao tiep I2C: e giao tiep vi vi ieu khien cua module master.
21
- Ngo noi vi encoder e oc tn hieu hoi tiep, co mot IC flip-flop e em cho
tn hieu t encoder.
- 1 loa danh e bao hieu.
Khoi khuech ai cong suat:
R6 5k
C5
PWM
BRAKE
C1 0.1uF
MOTOR OUT 2
D2 LED
ISO1 PC817
1
2
3
4
R1 330
MOTOR OUT 1
VCC
U4A
74LS04
1
2
C4
C3 0.1uF
DIR
C6
24VDC
TLP250
U3
1
2
3
4
6
7
8
5
N.C.
Anode
Cathode
N.C.
VOUT
VOUT
Vs
GND
ISO2 PC817
1
2
3
4
C2 0.1uF
D1 LED
D3 LED
R9
R4 100
24VDC
R8 100
R2 680
R3 1k8
JP1
POWERS
1
2
3
4
R5 100
U2
LMD18200/TO
1 2
6
7
8
9
10 11
3
4
5
BTP1 OUT1
VS
GND
CSOUT
TFOUT
OUT2 BTP2
DIN
BIN
PIN
GND
JP2
To MOTOR
1
2
U4B
74LS04
3 4
MOTOR DRIVER BLOCK
R7 5k
C7
12VDC

Hnh 3.13 S o nguyen ly khoi khuech ai cong suat cua module slave
Cac thanh phan trong khoi khuech ai cong suat:
- Chip ieu khien ong c LMD18200.
- 3 opto e cach ly cac ngo vao cua chip LMD18200, bao ve phan mach pha
trc.
- Cac cong logic e nan tn hieu cho ngo vao PWM.

Hnh 3.14 Hnh chup module slave

Robot di ong theo dau tng 4. Thc hien bo ieu khien
va kiem chng giai thuat
22
4
THC HIEN BO IEU KHIEN
VA KIEM CHNG GIAI THUAT

4.1 S o giai thuat chng trnh
Giai thuat cho robot di ong theo tng c thc hien nh 2 chng trnh,
mot chng trnh danh cho master module (chng trnh chnh) va chng trnh kia
danh cho slave module (chng trnh phu).
Chng trnh chnh co nhiem vu la bo ieu khien full-state feedback cua toan
he thong, chng trnh phu co nhiem vu la bo ieu khien PID van toc cho moi banh
xe. Chc nang cu the cua tng chng trnh c e cap phan 3.1.
Cac chng trnh c lap trnh bang ngon ng C, bien dch cho cac vi ieu
khien PIC bang trnh bien dch PIC-C, sau o nap vao cac vi ieu khien (nap
chng trnh chnh vao vi ieu khien master module, nap chng trnh phu vao 2
vi ieu khien 2 slave module).
Ma nguon cua cac chng trnh: xem phu luc B.
23
4.1.1 Giai thuat cho master module

Hnh 4.1 Lu o giai thuat cua master module
24
4.1.2 Giai thuat cho slave module

Hnh 4.2 Lu o giai thuat cua slave module
25
4.2 Tien hanh th nghiem
Cac th nghiem c thc hien nh sau:
* Moi trng hoat ong: gom tng va san nha. Tng can c lot mot lp
em e co be mat em va trn. Be mat trn e giam lc ma sat tac ong len robot
thong qua cam bien tiep xuc, be mat cung can em e giam rung ong cho cam
bien, v theo quan sat, s rung ong cam bien se gay ra sai so oc encoder. San
nha cung c lot em e tang he so ma sat gia banh xe vi san, va cung nham
giam rung ong cho robot.
* Robot di ong: robot c nuoi bang nguon ien ngoai. Thong qua cac
thiet b bien the, on ap, t nguon ien xoay chieu 220V/50Hz ta co c cac nguon
mot chieu (5V, 12V va 24V) e nuoi robot. Tren module master cua robot co cam
day noi vi may tnh qua cong giao tiep noi tiep, e robot truyen cac ket qua o
c ve may tnh.
* May tnh: chung toi dung mot may tnh laptop co cong giao tiep noi tiep
(chuan RS-232) e va ra lenh cho robot, va quan sat cac trang thai cua robot.
May tnh co mot chng trnh dang "terminal" e gi lenh va nhan d lieu qua
cong noi tiep, d lieu nhan c se c lu thanh file ket qua, file nay se c x
ly bang phan mem Matlab e the hien ket qua th nghiem qua cac o th.
Tien trnh thc hien mot th nghiem gom cac bc:
- Bat nguon ien, reset cac vi ieu khien tren robot, cai at thong so cho
chng trnh terminal tren may tnh. Cai at v tr chuan cua cam bien, sau o dch
chuyen robot e tao o lech ban au cho cam bien.
- Truyen cac tham so cho module master cua robot, cac thong so c truyen
theo th t gom: khoang cach mong muon d0, van toc mong muon vr, thi gian lay
mau st, cac tham so cua bo ieu khien k1 va k2.
- i robot di chuyen en het oan ng can th nghiem, ngng robot va lu
ket qua th nghiem.
- Chay phan mem Matlab e x ly ket qua th nghiem va nhan xet.
Do b gii han ve thi gian nghien cu, cac th nghiem tren tng cong cha
c thc hien. Chung toi ch xin trnh bay ay cac th nghiem robot di chuyen
theo tng thang.
26

Hnh 4.3 Mo hnh th nghiem
4.3 So sanh cac ket qua mo phong va th nghiem
4.3.1 So sanh ket qua mo phong bang Matlab vi ket qua th
nghiem
Chung toi tien hanh rat nhieu th nghiem vi tng thang, sau ay la mot so
th nghiem cho ket qua ien hnh va cac nhan xet.
Bang 4.1 Thong so th nghiem
Tham so Gia tr n v Ghi chu
d
0
0.25 m d
0
la giong nhau trong tat ca cac th
nghiem
v
r
0.05 m/s
st 0.4 s st kha ln, do bo ieu khien ong c ap
ng cham
k
1
3.5
k
2
615
e
1
ban au 0.0178 m Tnh ngc t gia tr hai encoder cam
bien
e
2
ban au -0.1955 rad Tnh ngc t gia tr hai encoder cam
bien
o th so sanh gia ket qua mo phong va ket qua th nghiem:
27
0.4 5 10 15 20 25 30
-1
-0.5
0
0.5
1
1.5
2
Experiment 14 - GIA TRI CUA BIEN
t(s)
(
r
a
d
/
s
)
thuc
mo phong

Hnh 4.4 So sanh o th cua van toc robot
0.4 5 10 15 20 25 30
-10
-5
0
5
10
15
20
25
Experiment 14 - DO LECH KHOANG CACH e
1
t(s)
e
1

(
m
m
) mo phong
thuc

Hnh 4.5 So sanh o th cua sai so khoang cach
28
0.4 5 10 15 20 25 30
-15
-10
-5
0
5
10
15
20
Experiment 14 - DO LECH GOC e
2
t(s)
e
2

(
d
e
g
r
e
e
)
mo phong
thuc

Hnh 4.6 So sanh o th cua sai so goc
Nhan xet 1: Ket qua thc co o th kha giong dang o th cua ket qua mo
phong, ieu nay cho thay viec mo phong a ch ng ung an cho thc nghiem.
Nhan xet 2: Ket qua thc kha xau so vi ket qua mo phong. ieu nay cung
de hieu v robot thc cua chung ta chu nhieu tac ong cua cac sai so khi che tao,
cac sai so do linh kien phan cng va cac sai so nhieu, trong khi viec mo phong
c thc hien trong moi trng ly tng.
Nhan xet 3: Bo ieu khien la hoi tu. Thi gian hoi tu la khoang 10s theo mo
phong va khoang 15s theo ket qua th nghiem (ta anh gia gia tr thi gian nay la
da vao nhan xet rang t thi iem 15s, cac gia tr e1 va e2 khong thay oi nhieu,
chung dao ong gan iem can bang).
4.3.2 Cac nhan xet bo sung
Ta xet them cac o th ket qua cua th nghiem a neu tren:
29
0 5 10 15 20 25 30
-1000
-500
0
500
1000
Experiment 14 - GIA TRI CUA BIEN encoder1 VA encoder2
t(s)
e
1
,

e
2

(
p
u
l
s
e
)
encoder2
encoder1

Hnh 4.7 Gia tr cua cam bien
0.4 5 10 15 20 25 30
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
Experiment 14 - GIA TRI CUA BIEN VA
wall
t(s)
(
r
a
d
/
s
)
wall

Hnh 4.8 Gia tr van toc goc cua robot va van toc goc (c lng) cua tng
30
0.4 5 10 15 20 25 30
-0.25
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0.25
Experiment 14 - e
1
VA e
2
t(s)
e
1

(
m
*
1
0
-
1
)
/
e
2

(
r
a
d
)
e
2

e
1

Hnh 4.9 Bien oi cua cac sai lech trong qua trnh hoat ong
0 5 10 15 20 25 30
0
5
10
15
20
25
30
Experiment 14 - SO SANH
lef t
VA
right
t(s)
l
e
f
t
/
r
i
g
h
t

(
r
p
m
)
right

lef t

Hnh 4.10 Gia tr van toc ra lenh cho 2 banh xe
31
Nhan xet 4: Bo ieu khien la on nh vi o lech ban au nam trong vung lan
can iem can bang. (that ra th nghiem c neu tren ay co o lech ban au nam
gan bien cua vung lan can o).
Hai th nghiem sau ay se cho thay toc o hoi tu cua 2 sai so khoang cach va
goc:
Bang 4.2 Thong so cua 2 th nghiem (TN) dung e so sanh
Tham so Gia tr n v Ghi chu
TN1 TN2
d
0
0.25 0.25 m
v
r
0.05 0.05 m/s
st 0.4 0.4 s
k
1
3.5 3.5
k
2
615 615
e
1
ban au 0.0017 0.0160 m TH1: e
1
ban au rat nho; TH2: e
1
ban
au ln
e
2
ban au -0.1203 0.0516 m TH1: e
2
ban au ln; TH2: e
2
ban au
rat nho
Cac o th ket qua c cho hnh 4.11.
Nhan xet 5: Sai so ve goc (e
2
) hoi tu nhanh hn sai so ve khoang cach (e
1
).
Khi robot nhan e
1
ban au ln, thi gian hoi tu chung cua ca 2 sai so se b keo dai
hn so vi trng hp e
2
ban au ln.

32
0.4 5 10 15 20 25 30 35
-0.25
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0.25
Experiment 10 - e
1
VA e
2
t(s)
e
1

(
m
*
1
0
-
1
)
/
e
2

(
r
a
d
)
e
2
e
1

a. o th e
1
va e
2
cua TN1
0.4 5 10 15 20 25 30 35
-0.25
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0.25
Experiment 15 - e
1
VA e
2
t(s)
e
1

(
m
*
1
0
-
1
)
/
e
2

(
r
a
d
)
e
1

e
2

b. o th e
1
va e
2
cua TN2
Hnh 4.11 So sanh cac o th e
1
va e
2
cua hai th nghiem
Robot di ong theo dau tng 5. Ket luan
33
5
KET LUAN

5.1 o thch hp cua giai thuat
Luan van nay a kiem nghiem c bo ieu khien cho robot di ong bam
tng. Bo ieu khien c dung la bo ieu khien hoi tiep tat ca trang thai (full-
state feedback controller). Ket qua th nghiem a kiem chng tnh hoi tu va on nh
Lyapunov cua bo ieu khien.
5.2 Nhng han che cua e tai
5.2.1 Ve viec che tao phan cng
Do cha co nhieu kinh nghiem trong viec gia cong c kh, chung toi a phai
mat rat nhieu thi gian cho viec che tao phan c cua robot. Tuy viec che tao con
nhieu sai so, robot cung at ieu kien va u e phuc vu cho viec th nghiem.
Khuyet iem ln nhat robot nay la s dung cac ong c cu (2 ong c mua ch
ong c cu tren ng Vnh Vien, Q.10, Tp.HCM), vi cac thong so khong am
bao va momen tai nho, ong c co ap ng cham keo theo thi gian lay mau cua
he thong phai ln, lam cho ket qua th nghiem khong tot nh mong muon.
5.2.2 Nhng hien tng anh hng en ket qua va cach khac
phuc
Trong qua trnh thc hien e tai, chung toi quan sat thay mot so hien tng co
the gay sai lech ket qua. Nham giup cac ban sinh vien i sau tiet kiem thi gian,
chung toi xin trnh bay cac hien tng o va e xuat cach khac phuc.
Bang 5.1 Cac hien tng anh hng en ket qua va cach khac phuc
Hien tng Cach khac phuc
o r cua cac khp noi gia truc
ong c (qua hop giam toc) va banh xe,
ban knh 2 banh dan ong khong bang
nhau. Hau qua: bo ieu khien PID hoat
ong khong tot.
Gia cong banh xe bang kim loai
(nen dung nhom), co gang giam sai so
khi khoan lo truc banh xe.
Robot di ong theo dau tng 5. Ket luan
34
Rung ong cac thanh trt va
encoder tren cam bien se gay ra sai so
cua cam bien, sai so tch luy trong thi
gian hoat ong cua robot se u ln e
dan en ket qua xau bo ieu khien.
Ga chat encoder vao cam bien;
s dung cac thanh trt chnh xac;
giam rung ong cho robot bang viec
tao ra tng va san em.
Chng trnh ieu khien ln, khi
bien dch bang PICC co the khong hoat
ong ung (do loi cua trnh bien dch)
Tiet kiem bo nh chng trnh
bang cach han che dung bien so thc
va cac lenh xuat/nhap qua cong noi
tiep, nen gii han chng trnh trong
khoang <80% ROM.
Ket qua mo phong co the tot vi
nhieu bo (k1,k2), nhng khi a vao
robot thc th ch co mot so bo (k1,k2)
thch hp.
Cac tham so k1 va k2 cua bo ieu
khien can phai do lai trong khi th
nghiem bang cach ieu chnh sau moi
th nghiem.
5.3 Hng nghien cu tiep
e tiep tuc e tai nay cho en het nhiem vu kiem nghiem giai thuat, chung
toi xin e ngh hng nghien cu tiep nh sau: cai tien phan c cua robot e giam
cac sai so che tao, thay cac ong c hien co bang 2 ong c mi vi bo truyen ong
co t so truyen ln, ong thi gan encoder co o phan giai ln hn vao ong c e
tang kha nang ieu khien van toc; co gang giam thi gian lay mau cua chng trnh
chnh; lap trnh bo ieu khien dung bo quan sat e so sanh toc o hoi tu gia viec
dung bo ieu khien nay vi bo ieu khien hoi tiep tat ca trang thai a thc hien
trong e tai.

Robot di ong theo dau tng TAI LIEU THAM KHAO
35
TAI LIEU THAM KHAO

Tieng Viet:

[1] Tran Hu Que, Ve K Thuat C Kh, Tap 1&2, Nha xuat ban Giao Duc, 2000
[2] Nguyen Doan Phc, Phan Xuan Minh & Han ThanhTrung, Ly Thuyet ieu
Khien Phi Tuyen, Nha xuat ban Khoa Hoc va Ky Thuat, 2003, trang 53-75.
[3] Nguyen Viet Hiep & Pham nh Anh Vu, Mo hnh hoa mo phong va ieu
khien Robot theo dau tng, Luan van tot nghiep ai hoc, ai hoc Bach Khoa
Tp.HCM, 2004.

Tieng nc ngoai:

[4] Detriche, Jean-Marie, Systemes robotiques et Mecatroniques, Cours d'Ecole
Centrale Paris, 1999-2000.
[5] Lagoudakis, Michail G., Mobile Robot Local Navigation with a Polar Neural
Map, MSc Thesis, University of Southwestern Louisiana, 1998.
[6] P. van Turennout, G. Honderd, L.J. van Schelven,Wall following control of a
Mobile Robot, International Conference on Robotics and Automation, Nice, France,
1992, p. 280-285.
[7] Medromi, J.Y. Tigli, and M.C. Thomas, Posture Estimation of a Mobile Robot:
Observers-Sensors, Proceedings of the 1994 IEEE International Conference on
Multisensor Fusion and Integration for Intelligent Systems, p. 661-666, 1994.
[8] Urzelai J., Uribe J.P., and Ezkerra M., Fuzzy Controller for Wall-Following With
a non-holonomous Mobile Robot, Proceedings of the 6
th
IEEE International
Conference on Fuzzy System, Vol. 3, p.1361-1368, 1997.

[9] A. Bemporad, M.D. Marco, and A. Tesi, Wall-Following Controllers for a Sonar
Mobile Robot, Proceedings of the 36
th
Conference on Decision & Control,
California USA, p. 3063-3068, 1997.
[10] T. Yata, L. Kleeman, and S. Yuta, Wall Following Using Angle Information
Measured by a Single Ultrasonic Transducer, Proceedings of the 1998 IEEE
International Conference on Robotics and Automation, p1590-1596, 1998.
Robot di ong theo dau tng TAI LIEU THAM KHAO
36
[11] Chung Tan Lam, A nonlinear Feedback Control of Wall-Following Mobile
Robot, Ms.C Thesis, Pukyong National University, Korea, 2004.
[12] Rodney A. Brooks, A robust layered control system for a mobile robot,
Research report of Massachusette Institute of Technology, 1985.
[13] http://www.microchip.com
[14] http://www.semiconductors.philips.com/buses/i2c/
[15] John A. Shaw, The PID Control Algorithm - How it works, how to tune it, and
how to use it, 2
nd
edition, (ebook), 2003.

Robot di ong theo dau tng PHU LUC A
37
PHU LUC A
S LC VE CHUAN GIAO TIEP I2C [14]

I2C la chuan giao tiep ban au c hang Philips phat trien e phuc vu
cho cac giao tiep vi Tivi, sau o, do nhng li the cua chuan giao tiep nay
nen no a tr nen rat pho bien trong cong nghiep. Cac tai lieu ve chuan I2C
c cung cap rat ay u va chi tiet tai trang web cua hang Philips.
Microchip a tch hp chuan giao tiep nay vao phan cng cho mot so vi
ieu khien, trong o co PIC16F877. Chuan nay c biet di dang chuan
giao tiep ong bo SSP. Trong chuan giao tiep SSP co hai chuan con la chuan
SPI va I2C. SPI la chuan giao tiep noi tiep dung 1 day noi, va I2C la chuan
giao tiep noi tiep 2 day. Day SDA la day e truyen d lieu va day SCL la
day e gi nhp cho d lieu truyen.
Chuan I2C la chuan giao tiep trong o dung mot thiet b lam Master,
va cac thiet b khac trong mang la Slave trong mot thi iem nhat nh. Tai
mot thi iem, Master co quyen oc va xuat d lieu qua tat ca cac Slave
thong qua a ch cua Slave o.
Master bat au qua trnh oc hoac ghi d lieu vao mot Slave bang cach
at tn hieu Start (S) vao ng truyen (hnh A.1 va A.2). Byte tiep theo
c gi i la byte a ch cua Slave can giao tiep. Qua trnh truyen nhan
c ket thuc bang bit Stop (P).
Sau ay la bieu o thi gian truyen va nhan trong chuan giao tiep I2C

Hnh A.1: Bieu o nhan d lieu, a ch 7 bit

Robot di ong theo dau tng PHU LUC A
38

Hnh A.2: Bieu o truyen d lieu, a ch 7 bit
I2C phan cng cua PIC 16F877 ho tr hai che o nh a ch 10 bit va
7 bit. Tuy nhien, chung ta van ch thng s dung a ch 7 bit, ho tr giao
tiep 128 thiet b. Tuy vay, chuan I2C cung giong nh chuan RS232, khi thao
tac vi cac vi ieu khien, co the c thiet lap bang phan mem.
Mot lan truyen a ch, se co 8 bit, bit thap nhat la bit xac nh che o
oc hoac ghi (R/W) (hnh A.2).
Chuan I2C rat de s dung, co cac toc o truyen nhan la 100Kbps,
400Kbps va 1Mbps, vi ieu khien PIC 16F877 ho tr toc o 100Kbps va
400Kbps, nhanh hn nhieu lan so vi chuan RS232. Ngoai ra, khong can
dung bat ky thiet b chuyen oi nao e chuyen oi ien ap tn hieu, do vay,
chuan I2C thch hp nhat cho cac giao tiep trong pham vi ngan (di 1m)
gia cac vi ieu khien.
Robot di ong theo dau tng PHU LUC B
39
PHU LUC B
MA NGUON CAC CHNG TRNH

Chng trnh cho master module:
/ * * * * * * * * * * * * * * * * * * * * * * * *
Descr ipt ion: This sour cecode is f or t he mast er module of t he
t hesis
"St udy on Cont r ol of Wall-Following Mobile Robot "
by Doan Minh Dang - P9900012
St ar t dat e: 2004.05.27
End dat e: 2004.07.03
* * * * * * * * * * * * * * * * * * * * * * * * /
# include <16f 877.h>
# device PI C16F877 * =16 ADC=10
# include <st r ing.h>
# include <st dlib.h>
# include <mat h.h>
# use delay(clock=20000000)
# use r s232(baud=9600,par it y=N,xmit =PI N_C6, r cv=PI N_C7)
# use I 2C(mast er , sda=PI N_C4, scl=PI N_C3)
# use f ast _I O(E)
# pr ior it y ccp1,ccp2,r b,t imer 1
/ / addr ess
# def ine addr ess_lef t 0xa0
# def ine addr ess_r ight 0xc0
/ / const ant
f loat vr =0.05,d0=0,k1=2.5,k2=250;/ / unit : [ vr ] =m/ s,[ d0] =m
40
/ / var iables
int 1 vr _ok=0;
char s[ 5] ;
int 1 st op1=0;
int 1 st op2=0;
int 1 online=0;/ / oper at ing f lag
/ / int 16 t emp=0; f or t est ing
signed int 16 encoder _1=0;/ / supplement al encoder in pr imit ive
unit , [ encoder _1] =pulse
signed int 16 encoder _2=0;/ / main encoder in pr imit ive unit ,
[ encoder _2] =pulse
/ / signed int 16 encoder _1_old,encoder _2_old;f or f ilt er ing
signed int 16 t emp_int 16=0;
/ / f loat encoder _1_m=0;/ / d2, [ encoder _1_m] =m
/ / f loat encoder _2_m=0;/ / d1, [ encoder _2_m] =m
f loat d1,d2;/ / [ d1] =m,[ d2] =m
f loat e1,e2,omega,delt a_omega_mu,par t 1,omega_lef t ,omega_r ight ;
f loat omega_mu=0;
/ / f loat omega_mu_t emp=0;
/ / [ e1] =mm
/ / [ e2] =r ad
/ / [ omega] =r ad
/ / [ omega_mu] =r ad
/ / [ delt a_omega_mu] =r ad/ (T* ms) (T=150)
/ / [ omega_lef t ] =[ omega_r ight ] =r ound/ minut e (r pm)
signed int 16 omega_lef t _new,omega_r ight _new;
/ / [ omega_lef t _new] =[ omega_r ight _new] =r ound/ minut e
byt e last _b;
int 1 count =0;
41
int cycle_count =0;
int 1 new_cycle=0;
int number _cycle=2;
signed int 16 omega_lef t _last =0;
/ / [ omega_lef t _last ] =r ound/ minut e
signed int 16 omega_r ight _last =0;
/ / [ omega_r ight _last ] =r ound/ minut e

void t ilt _c5(int 16 speak_t ime)
{/ / mast er boar d will speak in specif ied t ime(ms)
out put _high(PI N_C5);
delay_ms(speak_t ime);
out put _low(PI N_C5);
}
# SEPARATE
void send_commands_via_i2c(int f ir st _add,signed int 16
f ir st _dat a,int second_add,signed int 16 second_dat a)
{
int hi,lo;
/ / send command t o f ir st slave
hi=make8(f ir st _dat a,1);
lo=make8(f ir st _dat a,0);
i2c_st ar t ();
i2c_wr it e(f ir st _add);
delay_ms(1);
i2c_wr it e(lo);/ / send low byt e of velocit y
delay_ms(1);
i2c_wr it e(hi);/ / t hen send high byt e
i2c_st op();
42
/ / send command t o second slave
hi=make8(second_dat a,1);
lo=make8(second_dat a,0);
delay_ms(1);
i2c_st ar t ();
i2c_wr it e(second_add);
delay_ms(1);
i2c_wr it e(lo);
delay_ms(1);
i2c_wr it e(hi);
i2c_st op();
}

void calculat e_slave_velocit ies()
{
/ / input : omega_lef t , omega_r ight - f loat , global var iables
/ / out put : omega_lef t _new,omega_r ight _new - signed int 16,
global var iables
/ / This par t is moved t o a f unct ion t o r educe memor y cost
/ / calculat e new lef t and r ight velocit ies
omega_lef t =(vr -omega* 0.095)/ 0.033;
omega_r ight =(vr +omega* 0.095)/ 0.033;
/ / send new omega t o lef t and r ight modules
omega_lef t _new=(signed int 16)(omega_lef t * 9.55);
/ / conver t f r om r ad/ s t o r pm: (r ad/ s)* 60/ 2pi=r pm
omega_r ight _new=(signed int 16)(omega_r ight * 9.55);
}

# I NT_RB
43
void r b_isr ()
{
byt e changes,new_b;
new_b=input _b();
changes = last _b ^ new_b;
last _b = new_b;
if (bit _t est (changes,5))/ / RB5: st ar t but t on
{
/ / b5 went low
st op1=!st op1;/ / if it is set , it will be clear , and vice ver sa
}
if (bit _t est (changes,4))/ / RB4: st op but t on
{
/ / b4 went low
if (st op1) st op2=1;
}
delay_ms(200); / / debounce
}
# I NT_CCP1
void vant oc1()
{
if (input (PI N_E1)) encoder _1++;
else encoder _1--;
}
# I NT_CCP2
void vant oc2()
{
if (input (PI N_E2)) encoder _2--;
else encoder _2++;
44
}
# I NT_TI MER1
void t imer 1_isr ()
{
set _t imer 1(34286);
cycle_count ++;
if (cycle_count ==number _cycle)
{
cycle_count =0;
new_cycle=1;
}
}
void main()
{
/ / I NI T
t ilt _c5(500);
set _t r is_e(0x07);
set _t r is_b(0b00110000);
last _b=input _b();
set up_t imer _1(T1_I NTERNAL| T1_DI V_BY_8);
set up_CCP1(CCP_CAPTURE_FE);
set up_CCP2(CCP_CAPTURE_FE);
enable_int er r upt s(I NT_CCP1);
enable_int er r upt s(I NT_CCP2);
enable_int er r upt s(I NT_RB);
enable_int er r upt s(GLOBAL);
/ / RECEI VE PARAMETERS
/ / vr
get s(s);
45
vr =at of (s);
pr int f ("Receive velocit y desir ed: %f \ n",vr );
get s(s);
number _cycle=at oi(s);
pr int f ("Sampling t ime=%u* 0.05s \ n",number _cycle);
encoder _1=0;
encoder _2=0;
t ilt _c5(1000);
get s(s);
k1=at of (s);
pr int f ("k1=%f ",k1);
get s(s);
k2=at of (s);
pr int f ("k2=%f ",k2);
/ / f ir st move: move st r aight f or war d wit h at speed=vr
omega=0;
calculat e_slave_velocit ies();
pr int f ("Go!\ n");
send_commands_via_i2c
(addr ess_lef t ,omega_lef t _new,addr ess_r ight ,omega_r ight _new);
omega_lef t _last =omega_lef t _new;
omega_r ight _last =omega_r ight _new;
pr int f ("l:%ld\ nr :%ld\ n",omega_lef t _new,omega_r ight _new);
new_cycle=0;
/ / up t o her e, it cost 5146 t imer 1 count s=8233us
/ / it will r ing 500-8=492ms
t ilt _c5(492);
set _t imer 1(34286);/ / over f low af t er 3250 machine
cycles=50ms
46
enable_int er r upt s(I NT_TI MER1);

/ / MAI N LOOP
while ((!st op1)&&(!st op2))
{
if (new_cycle)
{
/ / st ar t new cycle
/ / GET d1 AND d2
d2=(f loat )encoder _1/ 51000.0;/ / scale: 51000 pulse/ m
d1=(f loat )encoder _2/ 51000.0;
/ / SEND ENCODERS' VALUES TO PC
pr int f ("1:%ld\ n2:%ld\ n",encoder _1,encoder _2);
/ / CALCULATE e1 & e2
e2=at an((d1-d2)/ 0.030);
e1=-d1* cos(e2);
/ / calculat e omega
par t 1=vr -(e1+d0)* omega_mu/ cos(e2);
omega=k2* par t 1* e1-k1* sin(e2)+omega_mu;
/ / calculat e new omega_mu
delt a_omega_mu=e1* (e1+d0)* t an(e2)-sin(e2)/ k2;
delt a_omega_mu=delt a_omega_mu* 0.05* number _cycle;
omega_mu +=delt a_omega_mu;
/ / calculat e dat a t o send t o slave
calculat e_slave_velocit ies();
/ / CHECK FOR EXCEEDED VELOCI TI ES
if (omega_lef t _new>90)/ / maximum limit
{
pr int f ("ERR1");/ / er r 1: Lef t velocit y exceed t he r ange
47
omega_lef t _new=90;
}
if (omega_lef t _new<-90)/ / maximum limit
{
pr int f ("ERR2");/ / er r 1: Lef t velocit y exceed t he r ange
omega_lef t _new=-90;
}
if (omega_r ight _new>90)/ / maximum limit
{
pr int f ("ERR3");/ / er r 2: Right velocit y exceed t he r ange
omega_r ight _new=90;
}
if (omega_r ight _new<-90)/ / maximum limit
{
pr int f ("ERR4");/ / er r 2: Right velocit y exceed t he r ange
omega_r ight _new=-90;
}
/ / SEND NEW VELOCI TI ES
send_commands_via_i2c
(addr ess_lef t ,omega_lef t _new,addr ess_r ight ,omega_r ight _new);

pr int f ("l:%ld\ nr :%ld\ n",omega_lef t _new,omega_r ight _new);
omega_lef t _last =omega_lef t _new;
omega_r ight _last =omega_r ight _new;
/ / wait unt il next cycle
new_cycle=0;
}
}
/ / SEND STOP COMMAND
48

send_commands_via_i2c(addr ess_lef t ,0xf f f f ,addr ess_r ight ,0xf f f f
);
t ilt _c5(200);/ / r inging f or st op declar ing
delay_ms(200);
t ilt _c5(200);
}

Chng trnh cho slave module:
/ * * * * * * * * * * * * * *
Descr ipt ion: This pr ogr am is used f or cont r olling mot or DC
velocit y wit h PI D met hod.
Ther e ar e slight dif f er ences bet ween t he pr ogr ams f or lef t
and r ight modules.
Doan Minh Dang - P9900012
St ar t dat e: 2004.04.27
End dat e: 2004.06.20
* * * * * * * * * * * * * * /
# include <16f 877.h>
# device PI C16F877 * =16 ADC=10
# f uses hs, nowdt , nopr ot ect , nolvp, put , br ownout
# use delay(clock=20000000)
# use r s232(baud=9600,par it y=N,xmit =PI N_C6, r cv=PI N_C7)
# use
I 2C(slave,sda=PI N_C4,scl=PI N_C3,addr ess=0xa0,FORCE_HW)
# include <st dlib.h>
# include <st r ing.h>
# include <cont r ol_mot or .c>
/ / t he f unct ion r un_mot or (value,condit ion,dir ect ,f r equency) is
used t o cont r ol mot or
49
/ / const ant
f loat kp=8.2;
f loat ki=1;
f loat kd=0.8;
/ / var iables
int 1 i2c_f lag=0;
int 1 st ar t _cycle=0;
int 1 st op=0;
int i2c_er r or =0;
int velocit y_er r or =0;
int 16 t imes=0;
int er r or _command;
signed int count =0;
signed int count _last =0;
signed int delt a_count =0;
char s[ 7] ;
byt e command_list en[ 3] ;
int i=0;
signed int 16 command_velocit y=0; / / [ r pm]
int 1 dir ect ion;
f loat r eal_velocit y=0;
signed int 16 r eal_velocit y_pr int ;
signed int 16 dut y=0;
int 16 PWMdut y;
f loat er r or _r pm_now=0;
f loat er r or _r pm_last =0;
f loat er r or _r pm_last _last =0;
f loat p_value;
f loat i_value;
50
f loat d_value;
f loat cont r ol_value;
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
void t ilt _a0(int 16 speak_t ime)
{/ / slave boar d will speak in specif ied t ime(ms)
out put _high(PI N_A0);
delay_ms(speak_t ime);
out put _low(PI N_A0);
}

void conver t _dut y()
{
if (dut y<0)
{
PWMdut y=-dut y;
dir ect ion=0;/ / negat ive speed: r ot at e backwar d
}
else
{
PWMdut y=dut y;
dir ect ion=1;/ / posit ive speed: r ot at e f or war d
}
/ / cont r ol_mot or (dir =0): backwar d
/ / cont r ol_mot or (dir =1): f or war d
if (PWMdut y>1023)
{
PWMdut y=1023;
velocit y_er r or ++;
51
}
if (64>PWMdut y)
{
PWMdut y=64;
}
}

void PI D_calculat ion()
{
p_value=kp* (er r or _r pm_now-er r or _r pm_last );
i_value=ki* er r or _r pm_now;
d_value=kd* (er r or _r pm_now-2* er r or _r pm_last
+er r or _r pm_last _last );
cont r ol_value=p_value+i_value+d_value;
dut y+=(signed int 16)cont r ol_value;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / / / / / / / / /
# I NT_SSP
void ssp_isr ()
{
if (i2c_poll()==FALSE)
{
i=0;
}
else
{
command_list en[ i++] =i2c_r ead();
52
/ / i=1;
if (i==3)
{
t imes++;
if
((command_list en[ 1] !=0xa0)&&(command_list en[ 2] !=0xa0))
/ / condit ion: dat a must be r eceived cor r ect ly
{
i2c_f lag=1;
i=0;
command_velocit y=make16(command_list en[ 2] ,command_list en[ 1] );
if (command_velocit y==0xf f f f ) / / st op command
{
st op=1;
}
}
else i2c_er r or ++;
}
}
}
# I NT_EXT
void count _encoder ()
{
if (input (PI N_B7)) count --;/ / backwar d r ot at e - lef t
else count ++;/ / f or war d - lef t
/ / r ever se f or r ight module
}
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / / / /
53
# I NT_TI MER1
void updat e_t ime()
{
st ar t _cycle=1;
set _t imer 1(15536); / / updat e t ime = 10ms
delt a_count =count -count _last ;
count _last =count ;
/ / mult iplier : 4ms - 7.5; 5ms - 6; 10ms - 3
r eal_velocit y=(f loat )delt a_count * 3.0;
er r or _r pm_last _last =er r or _r pm_last ;
er r or _r pm_last =er r or _r pm_now;
er r or _r pm_now=(f loat )command_velocit y-r eal_velocit y;
}
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / / / / /
void main()
{
t ilt _a0(400);
set _t r is_b(0x81);
ext _int _edge(0,L_TO_H);
cont r ol_mot or (0,0,0,2);
command_list en[ 0] =0;
set up_t imer _1(T1_I NTERNAL);
enable_int er r upt s(GLOBAL);
enable_int er r upt s(I NT_SSP);
while (!i2c_f lag) {};
delay_us(3385);
54
t ilt _a0(500);
set _t imer 1(15535);
enable_int er r upt s(I NT_EXT);
enable_int er r upt s(I NT_TI MER1);
st ar t _cycle=0;
while (!st op)
{
if (st ar t _cycle)
{
if (command_velocit y!=0)
{
PI D_calculat ion();
conver t _dut y();
cont r ol_mot or (PWMdut y,1,dir ect ion,2);
}
else {cont r ol_mot or (0,1,0,2);}/ / st op
st ar t _cycle=0;
}
}
cont r ol_mot or (0,0,0,2);
disable_int er r upt s(GLOBAL);
disable_int er r upt s(I NT_SSP);
disable_int er r upt s(I NT_EXT);
disable_int er r upt s(I NT_TI MER1);
t ilt _a0(200);
delay_ms(200);
t ilt _a0(200);
delay_ms(200);
if ((i2c_er r or >0)| | (velocit y_er r or >0))
55
{
t ilt _a0(1000);
}
while (1)
{
get s(s);
er r or _command=at oi(s);
swit ch (er r or _command)
{
case 1: pr int f ("i2c_er r or :%u ",i2c_er r or );
br eak;
case 2: pr int f ("velocit y_er r or :%u",velocit y_er r or );
br eak;
case 3: pr int f ("t imes:%lu",t imes);
br eak;
}
}
}

ROBOT DI Ä Á NG THEO DAU TUONG

Uploaded by

Copyright:

Available Formats

You might also like

ROBOT DI Ä Á NG THEO DAU TUONG

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

ROBOT DI Ä Á NG THEO DAU TUONG

Uploaded by

Copyright:

Available Formats

I HC QUC GIA TP.

: goc nh hng cua Mobile Robot.

: goc nghieng cua tng.

: van toc goc cua Robot.

vi van toc goc cua hai banh xe nh sau:

: c lng van toc goc cua tng

You might also like