WallFollowingRobot Report

I HC QUC GIA TP. H CH MINH TRNG I HC BCH KHOA

KHOA IN IN T

Gio vin hng dn : Nguyn Thin Thnh

Sinh vin thc hin :

Nguyn Phc Lc 40901457

TP.HCM 12/2012

PH LC

Tm tt ti 3

CHNG 1: TNG QUAN V T VN 4

1. Gii thiu chung v robot: 4

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

3. Bi ton di chuyn theo tng 8

3.1 Gii thiu bi ton 8

3.2 M hnh ton hc 9

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

GVHD: Nguyn Thin Thnh Page 4

CHNG 1: TNG QUAN V T VN

1. 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.

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.



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

Robot t hnh Rover thm him sao Ho

Robot d mn



Robot thm him i dng

Robot ct c

Robot ht bi Trilobite



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.



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.



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 m mx Py 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:

( )rv t : vn tc di bnh phi.

( )lv 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.



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:

( )

( )/ 2

rv ttR L

( )( )

/ 2

lv ttR L

( ) ( )( ) r l

v t v tt

L

Vn tc di: 1

( ) ( ). ( ) ( )2

r lv t t R v t v t

Phng trnh ton hc trong khng gian trng thi: ( ) ( )cos ( )

( ) ( )sin ( )

( ) ( )

x t v t t

y t v t t

t t

Tch phn hai v:

0

0

0

( ) ( ) cos ( )

( ) ( )sin ( )

( ) ( )

t

t

t

x t v t t

y t v t t

t t

Phng trnh trn c th vit di dng ma trn:

( ) cos 0 ( )cos( )( )

( ) sin 0 ( )sin( )( )

( ) 0 1 ( )

1 1 1( )cos cos cos

2 2 2

1 1 1( )sin sin sin

2 2 2

( ) / 1/ 1/

x

l

y

r

r l

r l

r l

v t v tv tv t

v t v tv tt

t t

v v

v v

v v L L L

r

l

v

v



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.

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.



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.

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.



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 Counter-

clockwise

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.

iu khin ng c ta iu khin cng lc 2 cng tc (FET) 2 pha ngc nhau.



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 Endress Hauser-Ultrasonic Level Measurement

Cm bin siu m Omron E4PA-LS600-M1-N

Cm bin siu m tim cn loi thu-pht



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.



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).



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.



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.



Cng thc tnh khong cch t cm bin ti vt cn:

( )

( )



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.



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:



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.



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 PXSEL Chc nng ca chn

0 0 Chc nng xut nhp

0 1 Module u tin c la chn

1 0 ng k trc

1 1 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.



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.



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



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.



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.



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.

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:

M hnh phn cng:

VI IU KHIN

MCH CNG SUT

NG C

SRF05 (v tr)

KHI NGUN



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: khong cch tham chiu vi tng xf: khong cch tham chiu pha trc x_right: khong cch robot vi tng bn phi xm: khong cch gia robot vi vt cn pha trc

Y

N

STAR

T

SRF05

xm > xf Logic m

Turn Left

Motor



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.

Y N

SRF05

Tc ng c

Turn Left Turn Right x_right > x_ref

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

PH LC CODE: #include "MSP430G2553.h"

#include

////////////////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 SRF05_LEFT BIT1 //P1.1 //input signal port

#define SRF05_FRONT BIT2 //P1.2 //input signal port

#define TRIGER BIT0 //P1.0

#define TRIGER2 BIT4 //P1.4

// ---------- 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 |= BIT1 + BIT4;

P2DIR |= BIT0 + BIT2 + BIT1 + BIT4;

P2OUT &= ~(BIT1|BIT4|BIT0|BIT2); //clear P2.1 and P2.4

// TA1.1 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 ; // OUT TRIGER

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;

}

// ---------- 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 =8000) Output_left = 8000-1;

else if ( Output_left = x_ref -1) | (x_current

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

Documents

Transcript of WallFollowingRobot Report