WallFollowingRobot Report

I HC QUC GIA TP.
H CH MINH
TRNG I HC BCH KHOA
KHOA IN IN T
Gio vin hng dn :

Sinh vin thc hin :
Nguyn Phc Lc
Nguyn Thin Thnh

40901457
TP.HCM 12/2012
PH LC
Tm tt ti
CHNG 1: TNG QUAN V T VN
1. Gii thiu chung v robot:
2. Tng quan v cc bi ton ca robot di ng
3. Bi ton di chuyn theo tng
3.1
Gii thiu bi ton
3.2
M hnh ton hc
3.3
Mc tiu iu khin
11
3.4
Gii quyt bi ton
11
CHNG 2: GII THIU V CC PHN T TRONG MCH
12
1. ng c DC
12
2. iu ch rng xung
12
3. Mch cu H
13
4. Cm bin siu m
14
4.1
Gii thiu
14
4.2
Cm bin siu m v nguyn tc TOF:
16
4.3
Cm bin siu m SRF05
17
5. Vi iu khin MSP430
21
5.1
Gii thiu s lc
21
5.2
MSP430 G2553
22
CHNG 3: GII THUT V PHN CNG
30
CHNG 4: KT QU V HNG PHT TRIN
33
TI LIU THAM KHO
33
PH LC CODE
Tm tt ti
Trong thi i cng nghip ngy nay, Robot ngy cng c s dng ph
bin trong sn xut cng nh trong cuc sng ca con ngi. Robot c mt v tr
quan trng kh c th thay th c, n gip con ngi lm vic trong cc iu kin
nguy him, kh khn. Ngoi ra, Robot cn c dng vo cc lnh vc thm
him khng gian, qun s, gii tr
Lnh vc Robot di ng ang ngy cng chim c s quan tm ca cc
nh nghin cu v x hi. T tnh hnh thc t , vic xy dng cc chng
trnh hot ng cho cc Robot l iu thit yu c bit i vi cc Robot di
ng. Bi ton Robot di ng bm tng (wall-following problem) l mt trong cc
bi ton thng gp ca Robot kiu phn x (reactive paradigm), n c gii
bng nhiu cch khc nhau.
Trong phm vi ti ny em s tm hiu thut ton v thi cng Robot di chuyn
theo mp tng (Wall Following Robot) da vo cm bin siu m.
n iu khin t ng 1
Chng 1: Tng quan v t vn
CHNG 1: TNG QUAN V T VN

Gii thiu chung v robot:
Robot hay ngi my l mt loi my c th thc hin nhng cng vic mt
cch t ng bng s iu khin ca my tnh hoc cc vi mch in t c lp trnh.
1.
Robot l mt tc nhn c kh, nhn to, o, thng l mt h thng c kh-in t.

Vi s xut hin v chuyn ng ca mnh, robot gy cho ngi ta cm gic rng n gic
quan ging nh con ngi. T "robot" (ngi my) thng c hiu vi hai ngha:
robot c kh v phn mm t hot ng. V lnh vc ngi my Nht Bn l nc i u
th gii v lnh vc ny.
Khi nim Robot xut hin ln u tin NewYork vo ngy 9/10/1922 trong v
kch Rossums Universal Robot ca nh son kch ngi Tip Khc l Karen Chapek,
cn t Robot l mt cch gi khc ca t Robota-theo ting Tip c ngha l cng vic
lao dch. Khi , Karen Chapek cho rng Robot l nhng ngi my c kh nng lm
vic nhng khng c kh nng suy ngh.
Gn mt th k tip theo, khi nim robot lin tc c pht trin, ng gp
thm bi nhiu nh nghin cu, nhiu cng ty chuyn v lnh vc robot.
Trc nhng nm 1970, ngi ta ch tp trung vo vic pht trin nhng robot tay
my hot ng trong cc nh my cng nghip. Sau mi xut hin nhng khi nim v
robot thng minh, v cc nghin cu bt u tp trung hn vo robot di ng. Mt trong
nhng chuyn gia u ngnh v robot di ng l Hans P. Moravec (bt u nghin cu t
nm 1964), v hin nay, chuyn nghin cu v robot di ng l Sebastien Thruns.
Cc robot di ng c ngi iu khin c dng cho cc mc ch qun s,
cc nhim v nguy him nh ph mn, thm d y i dng, hm m, kim tra cc
ng ng ngm, hay thm d sao Ho
Sn phm robot di ng c sn xut i tr v a vo th trng ln u tin l
robot ht bi Roomba v Trilobite ca hng Electrolux nm 2003.
GVHD: Nguyn Thin Thnh
Page 4
n iu khin t ng 1
Mt s hnh nh v cc Robot v ng dng ca n:
Robot t hnh Rover thm him sao Ho
Robot d mn
Page 5
n iu khin t ng 1
Robot thm him i dng
Robot ct c
Robot ht bi Trilobite
Page 6
n iu khin t ng 1
2.
Tng quan v cc bi ton ca robot di ng
Ngy nay, Robot hc t c nhng thnh tu to ln trong nn sn xut cng

nghip. Nhng cnh tay robot c kh nng lm vic vi tc cao, chnh xc v lin
tc lm nng sut lao ng tng nhiu ln. Chng c th lm vic trong cc mi
trng c hi nh hn, phun sn, cc nh my ht nhn, hay lp rp cc linh kin in
t to ra in thoi, my tnhmt cng vic i hi s t m, chnh xc cao. Tuy nhin
nhng robot ny c mt hn ch chung l hn ch v khng gian lm vic. Khng
gian lm vic ca chng b gii hn bi s bc t do tay my v v tr gn chng. Ngc
li, cc Robot t hnh li c kh nng hot ng mt cch linh hot trong cc mi trng
khc nhau.
Robot t hnh l loi mobile robot c kh nng t hot ng, thc thi nhim v m
khng cn s can thip ca con ngi. Vi nhng cm bin, chng c kh nng nhn bit
v mi trng xung quanh. Robot t hnh ngy cng c nhiu ngha trong cc ngnh
cng nghip, thng mi, y t, cc ng dng khoa hc v phc v i sng ca con
ngi. Vi s pht trin ca ngnh Robot hc, robot t hnh ngy cng c kh nng
hot ng trong cc mi trng khc nhau, ty mi lnh vc p dng m chng c nhiu
loi khc nhau nh robot sn, robot hn, robot ct c, robot thm him i dng, robot
lm vic ngoi v tr. Cng vi s pht trin ca yu cu trong thc t, robot t hnh tip
tc a ra nhng thch thc mi cho cc nh nghin cu. Vn ca robot t hnh l
lm th no robot t hnh c th hot ng, nhn bit mi trng v thc thi cc
nhim v ra.
Vn u tin l "navigation" (tm dch l "di chuyn"), Robot t hnh nn di
chuyn nh th no v c cu di chuyn no l s la chn tt nht. iu hng l vn
c bn trong nghin cu v ch to Robot t hnh. Trong hip hi nghin cu v
Robot t hnh c 2 hng nghin cu khc nhau:
- Hng th nht l nghin cu v Robot t hnh c kh nng iu hng tc
cao nh thng tin thu c t cm bin, y l loi robot c kh nng hot ng mi
trng trong phng cng nh mi trng bn ngoi. Loi robot ny yu cu kh nng
tnh ton s v c trang b cm bin c nhy cao, di o ln c th iu khin
robot di chuyn tc cao, trong nhng mi trng c a hnh phc tp.
- Hng th hai nhm gii quyt cc vn v cc loi robot t hnh ch dng
hot ng trong mi trng trong phng. Loi robot t hnh ny c kt cu n gin hn
loi trn, thc hin nhng nhim v n gin.
Page 7
n iu khin t ng 1
Vn di chuyn l vn trng tm ca robot di ng. di chuyn c, robot

phi thc hin mt lot cc tc v, mi tc v gn vi mt bi ton nh trong bi ton
"navigation". Cc bi ton gm:
Mapping: l cng vic lp bn mi trng hot ng ca robot. Nu khng c
cung cp d liu trc th robot phi c kh nng lp bn .
Positioning: l vic nh v, robot phi c kh nng bit c mnh ang u
trong bn ton cc hoc a phng.
Path planning: l vic hoch nh ng i sp ti ca robot, sau khi n bit c
bn v bit mnh ang v tr no.
Motion control: l vic iu khin cho robot di ng, tc l iu khin cc c cu
robot i theo con ng thu c t bi ton "path planning".
Obstacle avoidance: l nhim v trnh chng ngi vt khi robot ang di chuyn.
3.
Bi ton di chuyn theo tng
Vic di chuyn theo tng (wall following) l mt tc v thng thy robot di

ng, trong cc mi trng bit trc hoc khng bit trc. Tc v ny c dng vi
cc nhim v: trnh chng ngi vt, i theo tng bit trc, i theo tng khng bit
trc.
thc hin mc tiu trn, trong n ny em tp trung vo cc vn sau:
- Nghin cu cc b iu khin cho robot bm tng.
- Thit k v ch to mt Mobile Robot (mobile platform).
- Thit k v thc hin cc mch iu khin cho robot.
- Lp trnh cho robot hin thc cc b iu khin.
- Nhn xt kt qu v kt lun.
3.1
Gii thiu bi ton
Thit k v thc hin b iu khin dng iu khin Mobile Robot di chuyn dc

theo tng vi vn tc v khong cch t Robot n tng cho trc.
Gi thit ca bi ton:
- o c khong cch d t tng n Robot.
- o c gc lch gia Robot v tng.
- Tng l ng cong trn bt k c bn knh cong khng nh hn khong cch
d.
Page 8
n iu khin t ng 1
3.2
M hnh ton hc
M hnh robot di ng bm tng c cho nh hnh
M hnh ton hc ca Robot

M hnh bnh xe c l tng ha: bnh xe hnh trn, ln khng trt. Bnh xe
quay quanh trc ca n (trc Y) v chuyn ng theo phng X (trc X). Ta xy dng
phng trnh ng hc ca robot, bao gm mi quan h gia thng s iu khin v
thng s trng thi ca h thng trong khng gian.
Cc trc x, y xc nh ta ca im bt k trong h ta xOy. im P coi l
tm dch chuyn, dng xc nh v tr robot. H ta xm Pym l h ta tham chiu
ca robot. Nh vy im P c xc nh bi x, y v gc lch gia hai h ta ton
cc v cc b.
Cc tham s ca robot l:
vr (t ) : vn tc di bnh phi.
vl (t ) : vn tc di bnh tri.
r (t ) : vn tc gc bnh phi.
l (t ) : vn tc gc bnh tri.
r: bn knh mi bnh robot
L: khong cch 2 bnh
R: khong cch t tm robot ti tm vn tc tc thi.
ICC: tm vn tc tc thi.
Page 9
n iu khin t ng 1
R-L/2: bn knh m t chuyn ng cong ca bnh tri.

R+L/2: bn knh m t chuyn ng cong ca bnh phi.
Vn tc gc ca robot:
vr (t )
R L/2
v (t )
(t ) l
RL/2
v (t ) vl (t )
(t ) r
L
1
Vn tc di: v(t ) (t ).R vr (t ) vl (t )
2
(t )
Phng trnh ton hc trong khng gian trng thi:

x(t ) v(t ) cos (t )
y(t ) v(t ) sin (t )
(t ) (t )
Tch phn hai v:
t
x(t ) v(t ) cos (t )

0
t
y (t ) v(t ) sin (t )
0
t
(t ) (t )
0
Phng trnh trn c th vit di dng ma trn:

vx (t ) cos 0
v(t ) cos
v (t ) sin 0 v(t ) v(t ) sin vl (t )
y
(t )
v (t )
(t ) 0
(t ) r
1
1

2 (vr vl ) cos
(v v ) sin
2 r l

(v v ) / L
r
l
1
cos
2
1
sin
2
1/ L
1
cos
2
1
sin
2
1/ L
vr
vl
Page 10
n iu khin t ng 1
3.3
Mc tiu iu khin
Yu cu ca bi ton t ra l phi iu khin sao cho Mobile Robot chy song

song vi tng theo mt khong cch d cho trc. Robot s o khong cch bng cm
bin siu m, t cc khong cch ny n s tnh ton bng logic m iu khin vn tc
hai bnh ch ng.
3.4
Gii quyt bi ton
gii quyt bi ton ny, trong phm vi ca n 1, em thit k m hnh robot

gm 2 bnh xe ch ng pha sau, 1 bnh xe pha trc chuyn ng t do, 2 cm bin
siu m SRF05 thu khong cch v trnh vt cn, mch iu khin c s dng y
l MSP430 G2553, mch ngun acquy 12V.
Page 11
n iu khin t ng 1
Chng 2: Gii thiu v cc phn t trong mch
CHNG 2: GII THIU V CC PHN T TRONG MCH

1.
ng c DC
DC ng c l ng c in hot ng vi
dng mt chiu. ng c DC s dng rng ri
trong cc ng dng dn c cng nh cc ngnh
cng nghip. Thng thng ng c DC ch chy
mt tc khi kt ni vi ngun in, nhng
chng ti vn c th kim sot tc v hng ca
ng c vi s h tr ca cc mch in t vi
phng php PWM.
ng c DC trong cc hnh thc ca ng c dn s thng hot ng vi in p
thp, c s dng vi ti trng nh. Trong cng nghip, ng c DC c s dng
trong cc lnh vc i hi phi c m-men xon ln hn trn my tnh hoc yu cu thay
i trong phm vi tc .
2.
iu ch rng xung
iu khin ng c PWM phng php da trn nguyn tc hot ng cho ng

c chy bng chui xung nhanh chng m v ng. in DC c chuyn i thnh tn
hiu sng vung (ch c hai cp v khong 0 in p hot ng volt). Tn hiu sng
vung ny cho ng c.
Nu tn s chuyn i l ln ng c s chy tc n nh ph thuc vo
trc m-men xon.
Vi phng php PWM, iu chnh tc ng c thng qua iu ch rng
xung, tc l thi gian "xung y " ("on") ca chui xung vung cho ng c. iu
chnh ny s nh hng n cng sut trung bnh cho ng c v do s thay i cn
iu khin tc ng c.
Page 12
n iu khin t ng 1
ng c s chy chm khi

cp xung PWM c rng
nh.
Nu rng xung ln hn
(chng hn nh xung 2 v
3) ng c DC chy cng
nhanh.
3.
Mch cu H
Cu H l mt mch in t cho php in p c t ln ti theo 2 hng , mch

ny thng c s dng thay i in p trung bnh cp cho ng c nh vo xung
PWM, ngoi ra cu H cn c dng o chiu quay ng c.
Page 13
n iu khin t ng 1
iu khin ng c ta
iu khin cng lc 2 cng
tc (FET) 2 pha ngc
nhau.
Nh hnh ta thy nu ta bt S2,S4 th dng in s c chiu t (+ sang -) s lm motor

quay theo chiu thun, motor s quay theo chiu nghch nu ta bt S1,S3.
Bng di y tm tt hot ng, vi S1-S4 tng ng vi s trn:
High Side
Left
High
Side
Right
Lower
Left
Lower
Right
Quadrant Description
On
Off
Off
On
Motor goes Clockwise
Off
On
On
Off
Motor goes Counterclockwise
On
On
Off
Off
Motor "brakes" and

decelerates
Off
Off
On
On
Motor "brakes" and

decelerates
4.
Cm bin siu m
4.1
Gii thiu
Siu m l sng c hc c tn s ln hn tn s m nghe thy (trn 20kHz). Thnh

gic ca con ngi rt nhy cm vi di tn s t m trm (vi chc Hz) n cc m
thanh rt cao (gn 20kHz). Mt s loi vt nh di, ong c th cm nhn c siu m.
Page 14
n iu khin t ng 1
Cm bin siu m l thit b dng xc nh v tr ca cc vt thng qua pht sng

siu m.
Cm bin siu m c th pht hin ra hu ht cc i tng l kim loi hoc khng
phi kim loi,cht lng hoc cht rn,vt trong hoc m c (nhng vt c h s phn x
sng m thanh ln).
Mt s hnh nh v cm bin siu m:
Cm bin siu m tim cn loi thu-pht
Cm bin siu m Endress Hauser-Ultrasonic Level Measurement
Cm bin siu m Omron E4PA-LS600-M1-N

Page 15
n iu khin t ng 1
4.2
Cm bin siu m v nguyn tc TOF:
Sng siu m c truyn i trong khng kh vi vn tc khong 343m/s. Nu mt

cm bin pht ra sng siu m v thu v cc sng phn x ng thi, o c khong
thi gian t lc pht i ti lc thu v, th my tnh c th xc nh c qung ng m
sng di chuyn trong khng gian. Qung ng di chuyn ca sng s bng 2 ln
khong cch t cm bin ti chng ngoi vt, theo hng pht ca sng siu m. Hay
khong cch t cm bin ti chng ngi vt s c tnh theo nguyn l TOF:
Trong :
d l khong cch cn o,
v l vn tc sng siu m trong mi trng truyn sng,
t l thi gian t lc sng c pht i n lc sng c ghi nhn li.
u im v nhc im ca cm bin siu m:
u im:
Khong cch m cm bin c th pht hin ln ti 15m.
Sng phn hi ca cm bin khng ph thuc vo mu sc ca b mt i
tng hay tnh cht phn x nh sng ca i tng.
V d:b mt knh trong sut ,b mt gm mu nu,b mt plastic
mu trng hay b mt cht liu nhm sng trng l nh nhau.
Tn hiu p ng ca cm bin tim cn analog l t l tuyn tnh vi
khong cch.iu ny c bit l tng cho cc ng dng nh theo di cc
mc vt cht,mc chuyn ng cu i tng.
Page 16
n iu khin t ng 1
Nhc im:
Cm bin siu m yu cu i tng c mt din tch b mt ti thiu (gi
tr ny ty thuc vo tng loi cm bin).
Sng phn hi cm bin nhn c c th chu nh hng ca sng m
thanh tp m.
Cm bin tim cn siu m yu cu mt khong thi gian sau mi ln sng
pht i sn sng nhn sng phn hi. Kt qu cm bin tim cn siu m
nhn chung chm hn cc cm bin khc.
Vi cc i tng c mt vt cht thp nh bt hay vu qun o rt kh
pht hin vi khong cch ln.
Cm bin tim cn siu m gii hn bi khong cch pht hin nh nht.
4.3
Cm bin siu m SRF05
SRF05 l bn nng cp ca SRF04, c thit k tng chnh xc, phm vi

hot ng v gim gi thnh. Nh vy, vic s dng SRF05 hon ton tng t SRF04
ngoi nhng ci tin trn. Phm vi hot ng c tng t 3m ln 4m.
C hai mode lm vic: mode 1 tng t SRF04, cn 1 chn trigger v 1 chn echo;
mode 2 ch cn 1 chn cho c trigger v echo. Nhng vy tit kim c s chn s dng
trn vi iu khin, vi x l SRF05 c thm mt khong thi gian tr nh trc khi tr
v xung echo cho nhng b iu khin c tc x l chm c thi gian thc hin
cc lnh.
Mode 1 - Nh SRF04 - Hai chn Trigger v Echo ring
mode ny, SRF05 s dng hai chn ring bit: chn trigger v chn echo, l mode n
gin nht. Tt c hon ton ging nh SRF04, ch cn khng ni chn mode ( nh hnh
v).
Page 17
n iu khin t ng 1
Mode 2 Mt chn cho c Trigger v Echo

Mode ny s dng mt chn duy nht cho c hai tn hiu trigger v echo, c thit k
tit kim chn s dng cho cc b iu khin nhng. s dng ch ny, ta kt ni
chn Mode vi mass. Khi sau khi gi tn hiu trigger, th tn hiu echo s xut hin
trn chn trigger tr v b iu khin. SRF05 s khng lp tc phn hi tn hiu echo
ngay sau khi nhn c tn hiu trigger m sau thi gian 700uS k t kt thc tn hiu
trigger mi gi tn hiu echo. Trong thi gian c th kch hot li chn s dng t
output sang input nhn tn hiu phn hi.
Page 18
n iu khin t ng 1
Tnh ton khong cch:

Theo s thi gian ca SRF05 nh trn, ta ch cn cung cp mt xung ngn ti
thiu 10uS lm tn hiu trigger. Sau cm bin s gi mt khi 8 chu k sng siu m
ti 40kHz v xut xung echo. Sau SRF05 i sng siu m phn hi sau khi gp vt
cn, khi nhn c sng phn hi th xung echo b ngt. Nh vy, xung echo c chiu
rng t l thun vi khong cch ti vt cn. Thi gian xung echo tn ti c th cho ta bit
c khong cch gia cm bin vi vt. Nu trng hp khng pht hin c vt cn,
hoc vt cn ngoi tm xc nh ca SRF05 (4m) th sau 30ms xung echo cng c a
v mc thp. SRF05 c th c kch hot nhanh nht mi 50ms (20 ln mi giy). Nn
ch mi 50ms ri thc hin ln o tip theo. iu ny m bo cho cc nhiu siu m
phn x ca ln o trc khng gy sai lch kt qu thu c.
Page 19
n iu khin t ng 1
Cng thc tnh khong cch t cm bin ti vt cn:

(
Page 20
n iu khin t ng 1
5.
Vi iu khin MSP430
5.1
Gii thiu s lc
MSP 430 l h vi iu khin cu trc RISC 16-bit c sn xut bi cng ty
Texas Instruments.
MSP l ch vit tt ca MIXED SIGNAL MICROCONTROLLER. L dng vi
iu khin siu tit kim nng lng, s dng ngun thp, khong in p ngun cp t
1.8V 3.6V.
MSP 430 kt hp cc c tnh ca mt CPU hin i v tch hp sn cc module
ngoi vi. c bit Chp MSP 430 l gii php thch hp cho nhng ng dng yu cu trn
tn hiu.
Nhng c tnh ca dng MSP 430 bao gm:
in p ngun: 1.8V 3.6 V.
Mc tiu th nng lng cc thp:
- Ch hot ng: 270 A ti 1MHz, 2,2 V.
- Ch ch: 0.7 A.
- Ch tt (RAM vn c duy tr): 0.1 A.
Thi gian nh thc t ch Standby nh hn 1s.
Cu trc RISC-16 bit, Thi gian mt chu k lnh l 62.5 ns
Cu hnh cc module Clock c bn:
- Tn s ni ln ti 16 MHz vi 4 hiu chnh tn s +- 1%.
- Thch anh 32 KHz.
- Tn s lm vic ln ti 16 MHz.
- B cng hng.
- Ngun to xung nhp bn ngoi.
- in tr bn ngoi.
Timer_A 16 bit vi 3 thanh ghi hnh, 3 thanh ghi so snh rng 16 bit
Timer_B 16 bit vi 3 thanh ghi hnh, 3 thanh ghi so snh rng 16 bit
Giao din truyn thng ni tip:
- H tr truyn thng ni tip nng cao UART, t ng d tm tc
Baud.
- B m ha v gii m IrDA (Infrared Data Associatio).
- Chun giao tip ng b SPI.
- Chun giao tip I2C.
B chuyn i ADC 10 bit, 200 ksps vi in p tham chiu ni, Ly mu
v cht. T ng qut knh, iu khin chuyn i d liu.
Page 21
n iu khin t ng 1
Hai b khuch i thut ton (hot ng) c th nh cu hnh (i vi

MSP 430x22x4).
Bo v st p.
B np chng trnh.
Module m phng trn chip.
Cc thnh vin ca dng MSP 430 bao gm:
- MSP430F2232: 8KB + 256B Flash Memory 512B RAM.
- MSP430F2272: 32KB + 256B Flash Memory 1KB RAM.
- MSP430F2274: 32KB + 256B Flash Memory 1KB RAM.
MSP430 c s dng v bit n c bit trong nhng ng dng v thit b o c
s dng hoc khng s dng LCD vi ch ngun nui rt thp. Vi ch ngun nui
t khong 1,8 n 3,6v v 5 ch bo v ngun.
Vi s tiu th dng rt thp trong ch tch cc th dng tiu th l 200uA,
1Mhz, 2.2v; vi ch standby th dng tiu th l 0.7uA. V ch tt ch duy tr b
nh Ram th dng tiu th rt nh 0.1uA.
MSP430 c u th v ch ngun nui. Thi gian chuyn ch t ch
standby sang ch tch cc rt nh (< 6us). V c tch hp 96 kiu hnh cho hin th
LCD. 16 bit thanh ghi, 16 bit RISC CPU.
C mt c im ca h nh MSP l khi MCU khng c tn hiu dao ng ngoi,
th MSP s t ng chuyn sang hot ng ch dao ng ni.
5.2
MSP430 G2553
Vi iu khin c s dng thc hin iu khin robot l MSP430 G2553.

S chn ca chip MSP430 G2553:
Page 22
n iu khin t ng 1
S khi ca MSP430 G2553:
Cu trc xut nhp:

Chp MSP 430G2553 c 2 port xut nhp l port 1 v port 2. Port1 c 8 chn v
port 2 c 6 chn. Mi chn u c nhng chc nng c bit c th nh hng lm u
vo hoc u ra, c th c truy sut hoc iu khin.
Ports P1 v P2 c kh nng ngt. Mi ngt c cu hnh ring theo cnh ln hoc
cnh xung ca sn xung hoc theo tn hiu vo tng ng vi cc vector ngt.
c tnh xut nhp bao gm:
+ Chc nng c bit ca cc chn khng ph thuc vo vic lp trnh.
+ C th la chn lm OUTPUT hoc INPUT.
+ P1 v P2 c cu hnh ring cho tn hiu ngt.
+ Khng ph thuc thanh ghi output hay input.
Hot ng xut nhp:
Hot ng xut nhp d liu c lp trnh bng phn mm.
Thanh ghi Input PxIN:
Khi c la chn lm u vo. Gi tr mi bt ca thanh ghi tng ng vi tn
hiu.
Bit = 0 u vo mc thp.
Bit = 1 tn hiu mc cao.
Page 23
n iu khin t ng 1
Thanh ghi u ra PxOUT:

Khi khng s dng in tr ko, v c nh hng lm u gia. Gi tr cc bt
ca thanh ghi tng ng:
Bit =0 u ra mc thp.
Bit = 1 u ra mc cao.
Thanh ghi nh hng PxDIR:
Cc chn ca mi port c nh hng lm u ra hoc u vo phc thuc vo
gi tr cc bt ca thanh ghi PxDIR:
Nu bit = 0 chn c nh hng lm u vo.
Nu bit = 1 chn c nh hng lm u ra.
Thanh ghi la chn chc nng PxSEL v PxSEL2:
Cc chn ca mi port u c nhiu chc nng tng ng vi cc module ngoi vi
khc nhau. Mi bt ca thanh ghi PxSEL v PxSEL2 c s dng la chn chc
nng l cc port xut nhp hoc chc nng c bit.
PxSEL2
0
0
1
1
PXSEL
0
1
0
1
Chc nng ca chn

Chc nng xut nhp
Module u tin c la chn
ng k trc
Module ngoi vi th 2 c la chn
Ch : Khi PxSEL =1 cc ngt ca P1, P2 b cm. Khi bt k bit no ca thanh ghi

P1SELx hoc P2SELx c set, cc ngt tng ng vi cc chn ny s b cm.
Page 24
n iu khin t ng 1
Cc ngt P1 v P2:
Mi chn ca port P1 v P2 u c kh nng ngt. Cc ngt c cu hnh bi cc
thanh ghi PxIFG, PxIE v PxIES. Tt c cc chn ca port P1 mt nguyn nhn tng
ng vi mt vector ngt. Tt c cc chn ca port 2 c nhiu hn mt nguyn nhn khc
nhau tng ng vi 1 vector. C th s dng thanh ghi PxIFG xc nh nguyn nhn
cc ngt port P1 v P2.
a) Thanh ghi c ngt P1IFG v P2IFG:
- Mi bit ca thanh ghi PxIFGx l mt c ngt tng ng vi chn xut/ nhp v
khi c cnh xung tn hiu ti cc chn. Tt c cc c ngt ca thanh ghi PxIFGx i hi
mt ngt tng ng vi cc bit PxIE v GIE c set. Mi c ngt i hi phi reset
bng phn mm. Phn mm cng c th set c ngt:
+ Bit =0 Khng c ngt no ang ch.
+ Bit =1 C 1 ngt ang ch c phc v.
Ch : Khi thay i thanh ghi PxOUT v PxDIP c ngha l bn ang set c ngt
tng ng.
b) Thanh ghi la chn ngt theo sn xung P1IES, P2IES:
Mi bit ca thanh ghi PxIES la chn ngt theo cnh ln hay cnh xung
tng ng vi mi chn xut/ nhp:
+ Bit = 0 C ngt c set khi c cnh ln ca xung.
+ Bit = 1 C ngt c set khi c cnh xung ca xung.
c) Thanh ghi cho php ngt P1IE v P2IE:
Mi bt PxIE cho php hay cm ngt v lin quan n c ngt tng ng:
+ Bit = 0: Cm ngt.
+ Bit = 1: Cho php ngt.
Page 25
n iu khin t ng 1
B nh thi
MSP340G2553 c hai b nh thi 16 Bit l Timer_A0 v Timer_A1.
L mt b Timer/Counter 16 bit. Vi 3 thanh ghi lu tr v 3 thanh ghi so snh.
L mt Timer a chc nng. m thi gian, so snh, PWM . Timer_A cng c kh nng
ngt khi counter m trn hoc mi thanh ghi m trn. Cc c tnh chnh ca Timer_A
bao gm:
+ L mt Timer/counter 16 Bit khng ng b vi 4 ch hot ng.
+ C th la chn v cu hnh ngun xung
+ Hai thanh ghi c th cu hnh capture/compare
+ Cu hnh u ra vi ch PWM
+ Cht ng vo v ng ra khng ng b
S khi ca TimerA:
Cc chc nng c th ca Timer A nh sau:

1. Ngun xung Clock:
Timer A c th hot ng vi 1 trong 4 ngun xung clock sau, ty thuc vo yu cu ca
ng dng m ta cn thc hin.
TACLK (Timer A clock): L mt ngun xung clock ngoi c a vo chn

P1.0 ca MSP430G2231
ACLK (Auxiliary clock): L mt ngun clock b sung cho Timer A, ch c cp

Page 26
n iu khin t ng 1
ngun t LFXTCL, thng dng cho h thng ph vi clock hot ng thp tit kim
nng lng.
SMCLK (Submaster clock): thng dng cho ngoi vi, ly ngun t DCO hoc
XT2.
2. Timer/Counter:
Cc ch hot ng:
Stop : Tt Timer A.
Up : Ch m ln ca Timer A. Gi tr ca thanh ghi TAR (Timer A register)

s tng lin tc t 0 cho n mt gi tr c nh sn trong thanh ghi TACCR0 ri reset
v gi tr 0, qu trnh hot ng c tip tc nh vy.
Continous: Ch m ln lin tc ca Timer A.Trong mt chu k hot ng gi

tr ca thanh ghi TAR s tng lin tc t 0 cho n 0FFFFh ri reset v gi tr 0.
Up/Down: Ch m ln/xung ca Timer A. Trong mt chu k hot ng gi

tr ca thanh ghi TAR s tng lin tc t 0 cho n TACCR0 ri gim dn v gi tr 0.
Page 27
n iu khin t ng 1
3. Capture/Compare:
Chc nng Capture dng ghi li mt s kin theo thi gian. C th hot ng
vi c ngun tn hiu ni v tn hiu ngoi. Ty thuc vo gi tr config cho Timer A m
b Capture c th xt cnh ln, cnh xung hoc c hai cnh ca tn hiu. Khi s kin
Capture xy ra th:
Gi tr ca TAR s c sao chp vo thanh ghi TACCR0 (Timer A

Capture/Compare register 0).
C ngt CCIFG (Capture/Compare interrupt flag) c bt ln.

Chc nng Compare dng to tn hiu ra cho PWM hoc to ngt cho mt
khong thi gian nh trc.Khi gi tr ca TAR bng vi gi tr ca TACCRx th:
C ngt CCIFG c bt ln.

Tn hiu ni EQUx=1.
Tn hiu vo CCI s c cht vo SCCI
Cc ch ng ra ca b Capture/Compare: Ta c th thc hin config cho gi tr

OUTMODx to ra cc dng tn hiu ng ra khc nhau v dng tn hiu ra ny cn ph
thuc vo ch hot ng ca Timer A.
Ch Set: ng ra ca tn hiu c bt ln khi gi tr ca TAR bng vi

gi tr ca TACCRx,v gi tr ny c gi cho ti khi Timer b reset hoc mt
ch hot ng khc ca Timer c chn v nh hng n tn hiu.
Ch Toggle/Reset: ng ra ca tn hiu i trng thi khi gi tr ca TAR

bng vi gi tr ca TACCRx,v gi tr ny c gi cho ti khi Timer b reset
hoc mt ch hot ng khc ca Timer c chn v nh hng n tn hiu.
Ch Set/Reset: ng ra ca tn hiu c bt ln khi gi tr ca TAR

bng vi gi tr ca TACCRx,v sau reset khi thanh ghi TAR ca b m t
gi tr ca TACCRx.
Ch Toggle: ng ra ca tn hiu i trng thi khi gi tr ca TAR bng

vi gi tr ca TACCRx,chu k ca tn hiu ng ra gp i chu k ca Timer.
Page 28
n iu khin t ng 1
Ch Reset: ng ra ca tn hiu c reset v 0 khi gi tr ca TAR bng

vi gi tr ca TACCRx,v gi tr ny c gi cho ti khi mt ch hot ng
khc ca Timer c chn v nh hng n tn hiu.
Ch Toggle/Set: ng ra ca tn hiu i trng thi khi gi tr ca TAR

bng vi gi tr ca TACCRx,v c bt ln khi gi tri thanh ghi ca Timer t
gitr TACCR0.
Ch Reset/Set: tn hiu ng ra c reset khi gi tr ca TAR bng vi

gi tr ca TACCRx, v c bt ln khi gi tri thanh ghi ca Timer t gi tr
TACCR0.
Page 29
n iu khin t ng 1
Chng 3: Gii thut v phn cng
CHNG 3: GII THUT V PHN CNG

3.1
Thit k phn cng:
S khi phn cng Wall Following Robot:

KHI
NGUN
VI IU
KHIN
MCH CNG
SUT
NG C
SRF05 (v tr)
M hnh phn cng:
Page 30
n iu khin t ng 1
3.2
Lu gii thut:
Khi mi khi ng, robot s pht sng siu m thu v v tr ca n, nu xm < xf robot
s r tri ( trnh vt cn ), nu xm > xf robot s thc hin bm theo mp tng bng lut
iu khin logic m.
Cc thng s:
x_ref:
xf:
x_right:
xm:
khong cch tham chiu vi tng

khong cch tham chiu pha trc
khong cch robot vi tng bn phi
khong cch gia robot vi vt cn pha trc
STAR
T
SRF05
xm > xf
Logic m
Y
Turn Left
Motor
Page 31
n iu khin t ng 1
Khi iu khin logic m:

Gi tr khong cch thu c t cm bin t bn tri s c tnh sai s so vi
khong cch tham chiu. B iu khin t l s da vo sai s iu khin tc ng
c.
SRF05
N
Turn Left
Y
x_right > x_ref
Turn Right
Tc
ng c
Page 32
n iu khin t ng 1
Chng 4: Kt qu v hng pht trin
CHNG 4: KT QU V HNG PHT TRIN

4.1 Kt qu:
4.1.1
Kt qu t c:
Phn cng:
- Kt cu chc chn, linh hot do c bnh Omni pha trc, chu c va chm.
- ng c mnh, tc ti a khi cp ngun 24V.
- Cm bin siu m chnh xc ti 1cm.
Gii thut:
- o c khong cch t robot ti tng, t xc nh v tr ca Robot.
- Lut iu khin n gin nhng kh hiu qu.
4.1.2
Hn ch v cch khc phc:
Robot hot ng cha linh hot.
Tn s hot ng ca SRF05 b hn ch 20Hz, nn dng cc cm bin c tm o
ngn v tn s cao.
4.2 Hng pht trin ca ti:
Robot c th gip cc cng vic cng vic nh sn dc mp tng, di chuyn
hng ha
TI LIU THAM KHO

[1] Lun vn tt nghip: Robot di ng theo du tng, Don Minh ng.
[2] Cc din n: dientuvietnam.net, ti.com.vn, webdien.com
Page 33
PH LC CODE:
#include "MSP430G2553.h"
#include <stdbool.h>
////////////////Defines////////////////
#define MOTOR_LEFT
BIT1 // P2.1
#define MOTOR_RIGHT
BIT4 // P2.4
#define MOTOR_LEFT_DIR BIT6 //P2.0
#define MOTOR_RIGHT_DIR BIT7 //P2.2
#define
#define
#define
#define
SRF05_LEFT
SRF05_FRONT
TRIGER
TRIGER2
BIT1
BIT2
BIT0
BIT4
//P1.1
//P1.2
//P1.0
//P1.4
//input signal port

//input signal port
// ---------- KHAI BAO BIEN ---------------------------------------------------float x_current, front_distance;

// insigned int pre_x_current, pre_front_distance;
int x_ref = 15, current_x; // khoang cach tham chieu
int Output_left, Output_right;
// ---------- VUNG CHUONG TRINH CON -------------------------------------------void initport ()
{
P2SEL |=
P2DIR |=
P2OUT &=
// TA1.1
BIT1 + BIT4;
BIT0 + BIT2 + BIT1 + BIT4;
~(BIT1|BIT4|BIT0|BIT2); //clear P2.1 and P2.4
and TA1.2 are outputs for PWM on P2.1 and P2.4
// SRF05
P1DIR = 0x00;
P1DIR |= TRIGER + TRIGER2 + MOTOR_LEFT_DIR + MOTOR_RIGHT_DIR ; // OUT
TRIGER
P1OUT &= ~(TRIGER | TRIGER2| MOTOR_LEFT_DIR | MOTOR_RIGHT_DIR) ; // OUT
TRIGER
P1IE |= SRF05_LEFT + SRF05_FRONT ;
P1IFG &= ~ (SRF05_LEFT | SRF05_FRONT );
P1SEL = SRF05_LEFT + SRF05_FRONT;
//
// ---------------------------------------------TA1CCR0 = 8000 - 1;
// Tan so PWM 2000Hz
TA1CCR1 = 0; // right motor
TA1CCR2 = 0; // left nmotor
TA1CCTL1 = OUTMOD_7;
// Reset/Set mode
TA1CCTL2 = OUTMOD_7;
//( 0-> TA1CCR1 set, TA1CCR1 -> TA1CCR0 reset )
TA1CTL |= TASSEL_2 + MC_1; // Start timerA1 in up mode , SMCLK= 16MHz
// cai dat cam bien SRFF05
TACCTL0 = CM_3 + SCS + CCIS_0 + CAP +CCIE; // both edge + CCI0A (P1.1)//
+ Capture Mode + Interrupt
TACCTL1 = CM_3 + SCS + CCIS_0 + CAP +CCIE; // both edge + CCI1A (P1.2)//
+ Capture Mode + Interrupt
TA0CTL |= TASSEL_2 + MC_2 + ID_3;//+ ID_3; // Start timerA0 in CONT mode
// CAPTURE , SMCLK
TA0CCR0 = 0;
}
void ultrasonic (void)
{
// Ultrasonic sensor 1 // phat xung do khoang cach
P1OUT |= TRIGER + TRIGER2;
__delay_cycles(10); // tao xung 10us de do khoang cach, CLOCK = 8Mhz
P1OUT &= ~(TRIGER | TRIGER2);
}
void go_left(int valuepwm)
{
TA1CCR1 = 1000;
TA1CCR2 = valuepwm;
}
void go_right(int valuepwm)
{
TA1CCR1 = valuepwm;
TA1CCR2 = 1000;
}
void go_forward()
{
TA1CCR1 = 7000 ;
TA1CCR2 = 7000 ;
}
/* void go_resever()
{
P1OUT |= MOTOR_LEFT_DIR + MOTOR_RIGHT_DIR
TA1CCR1 = 8000-1 ;
TA1CCR2 = 8000-1 ;
} */
void stop()
{
TA1CCR1 = 0 ;
TA1CCR2 = 0 ;
}
void go_right_90()
{
TA1CCR1 = 8000;
TA1CCR2 = 0;
// delay 1s;
}
void go_left_90(int valuepwm)
{
TA1CCR1 = 0;
TA1CCR2 = valuepwm;
}
; // OUT TRIGER
// ---------- CHUONG TRINH DIEU KHIEN CHINH -----------------------------------void main(void)

{
initport();
//-------------------------------------------//-------------------------------------------WDTCTL = WDTPW + WDTHOLD; // Stop WDT
BCSCTL1 = CALBC1_8MHZ;
// Set DCO 16MHz
DCOCTL = CALDCO_8MHZ;
__bis_SR_register(GIE);// enable global interrupt
__enable_interrupt();
while(front_distance ==0 )
{
ultrasonic();
__delay_cycles(10000);
}
while(x_current ==0 )
{
ultrasonic();
}
while(1)
{
// Fuzzy Control
if ( front_distance > 30 )
{
if (x_current > x_ref + 1 )
{
Output_right = (int)x_current*40;
Output_right = Output_right + 5000;
if (Output_right >=8000) Output_right = 8000-1;
else if ( Output_right <=0 ) Output_right = 0;
go_right(Output_right);
}
else if (x_current < 10 )
{
go_left(6000);
}
else if (x_current < x_ref -1 )
{
Output_left = (int)x_current*50;
Output_left = Output_left + 5000;
if (Output_left >=8000) Output_left = 8000-1;
else if ( Output_left <=0 ) Output_left = 0;
go_left(Output_left);
}
else if ( (x_current >= x_ref -1) | (x_current <= x_ref +1) )
{
go_forward();
}
}
else
{
stop();
}
ultrasonic();
}
}
// ---------------------- KHU VUC NGAT ----------------------------------------unsigned int RisingEdge[2];
unsigned int FallingEdge[2];
unsigned int PulseTimes[2];
unsigned short First;
unsigned short First2;
#pragma vector=TIMER0_A0_VECTOR
__interrupt void Timer0_A0 (void)
{
if
{
(First == 0)
RisingEdge[0] = TA0CCR0;
First = 1;
}
else
{
FallingEdge[0] = TA0CCR0;
PulseTimes[0] = FallingEdge[0] - RisingEdge[0];
First = 0;
TA0CCR0 = 0; //
x_current = (PulseTimes[0])/58; // left_front distance SMCLK = 2Mhz
}
}
#pragma vector=TIMER0_A1_VECTOR
__interrupt void Timer0_A1 (void)
{
volatile unsigned short ta0iv;
ta0iv = TA0IV; // Reading TA1IV clears it
if( ta0iv == TA0IV_TACCR1 )
{
if( First2 == 0)
{
RisingEdge[1] = TA0CCR1;
First2 =1;
}
else
{
FallingEdge[1] = TA0CCR1;
PulseTimes[1] = FallingEdge[1] - RisingEdge[1];
First2 = 0;
TA0CCR1 = 0; //
front_distance = (PulseTimes[1])/58;
}
}
}

WallFollowingRobot Report

Hochgeladen von

Dokumentinformationen

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

WallFollowingRobot Report

Hochgeladen von

Copyright:

Verfügbare Formate

I HC QUC GIA TP.

Gio vin hng dn :

Nguyn Thin Thnh

CHNG 1: TNG QUAN V T VN

1. Gii thiu chung v robot:

2. Tng quan v cc bi ton ca robot di ng

3. Bi ton di chuyn theo tng

Gii thiu bi ton

Gii quyt bi ton

CHNG 2: GII THIU V CC PHN T TRONG MCH

Cm bin siu m v nguyn tc TOF:

Cm bin siu m SRF05

CHNG 3: GII THUT V PHN CNG

CHNG 4: KT QU V HNG PHT TRIN

TI LIU THAM KHO

Chng 1: Tng quan v t vn

CHNG 1: TNG QUAN V T VN

Robot l mt tc nhn c kh, nhn to, o, thng l mt h thng c kh-in t.

GVHD: Nguyn Thin Thnh

Chng 1: Tng quan v t vn

Mt s hnh nh v cc Robot v ng dng ca n:

Robot t hnh Rover thm him sao Ho

GVHD: Nguyn Thin Thnh

Chng 1: Tng quan v t vn

Robot thm him i dng

Chng 1: Tng quan v t vn

Tng quan v cc bi ton ca robot di ng

Ngy nay, Robot hc t c nhng thnh tu to ln trong nn sn xut cng

GVHD: Nguyn Thin Thnh

Chng 1: Tng quan v t vn

Vn di chuyn l vn trng tm ca robot di ng. di chuyn c, robot

Bi ton di chuyn theo tng

Vic di chuyn theo tng (wall following) l mt tc v thng thy robot di

Gii thiu bi ton

Thit k v thc hin b iu khin dng iu khin Mobile Robot di chuyn dc

GVHD: Nguyn Thin Thnh

Chng 1: Tng quan v t vn

M hnh robot di ng bm tng c cho nh hnh

M hnh ton hc ca Robot

GVHD: Nguyn Thin Thnh

Chng 1: Tng quan v t vn

R-L/2: bn knh m t chuyn ng cong ca bnh tri.

Phng trnh ton hc trong khng gian trng thi:

x(t ) v(t ) cos (t )

Phng trnh trn c th vit di dng ma trn:

GVHD: Nguyn Thin Thnh

Chng 1: Tng quan v t vn

Yu cu ca bi ton t ra l phi iu khin sao cho Mobile Robot chy song

Gii quyt bi ton

gii quyt bi ton ny, trong phm vi ca n 1, em thit k m hnh robot

GVHD: Nguyn Thin Thnh

Chng 2: Gii thiu v cc phn t trong mch

CHNG 2: GII THIU V CC PHN T TRONG MCH

iu khin ng c PWM phng php da trn nguyn tc hot ng cho ng

GVHD: Nguyn Thin Thnh

Chng 2: Gii thiu v cc phn t trong mch

ng c s chy chm khi

Cu H l mt mch in t cho php in p c t ln ti theo 2 hng , mch

GVHD: Nguyn Thin Thnh

Chng 2: Gii thiu v cc phn t trong mch

Nh hnh ta thy nu ta bt S2,S4 th dng in s c chiu t (+ sang -) s lm motor

Motor goes Clockwise

Motor goes Counterclockwise

Motor "brakes" and