Kha lun tt nghip

http://www.ebook.edu.vn

I HC QUC GIA H NI TRNG I HC CNG NGH

Bch Hong Giang

XY DNG ROBOT DI NG TRNH VT CN DA TRN CC SENSOR SIU M V SENSOR A BN

KHO LUN TT NGHIP I HC H CHNH QUY Ngnh: in t - Vin thng

Cn b hng dn: TS. Trn Quang Vinh ng hng dn: CN. Phm Duy Hng

H NI 2005

BCH HONG GIANG 1

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Li cm n
Trc tin ti xin chn thnh cm n Thy gio TS.Trn Quang Vinh, cng Ngi ng hng dn c nhn Phm Duy Hng thuc Khoa in TVin Thng tn tnh hng dn ti tm hiu v nghin cu kha lun ny. Ti cng t lng bit n ti cc Thy, c gio trong Trng i Hc Cng Ngh - i Hc Quc Gia H Ni c cng o to, quan tm, gip ti trong sut qu trnh hc tp v hon thnh kha lun ny. ng thi ti cng chn cm n cc cn b trong phng Robot to mi iu kin cho ti lm tt phn thc nghim ca mnh. Cui cng, ti mun cm n ti tt c bn b, gia nh v ngi thn ng vin v gip ti trong qu trnh hc tp v hon thnh kha lun ny. H ni,ngy 1 thng 6 nm 2005 Sinh vin

BCH HONG GIANG 2

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

TM TT NI DUNG

Kha lun ny tp trung nghin cu cc vn lin quan n xy dng mt robot di ng trnh vt cn da trn cc sensor siu m v sensor a bn.Trn c s thc hin nhim v chnh l xy dng mt robot di ng gii quyt 3 bi ton iu khin c bn. Robot c xy dng trong kha lun ny s dng vi iu khin Basic Stamp 2sx, vi mt h sensor bao gm: Sensor siu m v sensor a bn. Robot s dng motor mt chiu c iu khin thng qua mch cng sut.

BCH HONG GIANG 3

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Mc lc
Gii thiu 1 Phn 1: Tng quan v mt s vn lin quan n xy dng mt robot di ng thng minh t ng trnh vt...2 Chng 1: S lc v lch s ra i v pht trin ca robot.3 1.1. Lch s ra i v pht trin3 1.2. Cc th h robot.4 1.3. Xu hng pht trin ca robot hin i 5 Chng 2: H sensor trong robot..7 2.1. Phn loi sensor.7 2.1.1. Sensor ni ..7 2.1.2. Sensor ngoi ..7 2.1.3. Sensor kho 7 2.2. Mt s loi sensor thng gp...7 2.2.1. Sensor xc nh khong cch .8 2.2.1.1. Phng php xc nh khong cch bng lng gic.8 2.2.1.2. Phng php xc nh khong cch bng o khong cch thi gian truyn song8 2.2.2. Sensor pht hin vt th gn.10 2.2.2.1. Sensor siu m...10 Chng 3: C cu chuyn ng trong robot di ng12 3.1. Phn loi motor in12 3.1.1. Motor mt chiu...12 3.1.2. Motor bc...12 3.2. Cc c tnh ca motor13 3.2.1. in p s dng v dng tiu th ca motor...13 3.2.2. Tc cc i ca motor....13
BCH HONG GIANG 4 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

3.2.3. Lc xon camotor13 3.3. iu khin chuyn ng ca motor .14 3.3.1. iu khin motor DC.14 3.3.2. iu khin motor bc15 Chng 4: Kho st mt s loi sensor s dng trong robot di ng.. 16 4.1. Sensor hng ngoi..16 4.1.1. Sensor hng ngoi s dng pht hin mu en trng..16 4.1.2. Nghin cu cc sensor hng ngoi c mt trn th trng tch hp c b thu v pht trn cng mt v...17 4.2. Sensor siu m18 4.2.1. Sensor siu m MUST01d...18 4.2.2. Sensor siu m Devantech SRF04...20 4.3. Gii thiu v sensor a bn CMPS03 ...22 Phn 2: Thc nghim thit k, xy dng mt robot di ng t ng trnh vt...25 Chng 5: Kho st thit k b iu khin robot..26 5.1. Chp vi iu khin Basic Stamp 2sx...26 5.2. Gii thiu v ngn ng BASIC STAMP28 5.2.1 Mt s cu lnh chnh trong Basic Stamp30 5.3. Mch iu khin cng sut ng c...34 5.4. Kt cu c kh ca robot36 5.4.1. S thit k...36 5.4.2 Cc thnh phn khc ca Robot37 Chng 6: Robot di ng trnh vt cn.38 6.1. Bi ton iu khin robot trnh vt cn.38 6.2. Cc trng hp ca vt cn trn ng i ca robot38

BCH HONG GIANG 5

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

6.2.1 C mt vt cn nm trn ng i ca robot38 6.2.2. Trng hp t 2 vt so le nhau..40 6.2.3. Trng hp robot lch gia 2 vt cn.41

BCH HONG GIANG 6

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

GII THIU

Ngnh khoa hc cng ngh mi, to ra cc sn phm robot v nghin cu ng dng chnh hnh thnh trong nhng thp k gn y c gi l Robotics. Trong Robotics c hu ht cc vn ca c-in t. Thut ng c - in t ( mechatronics) th hin s kt hp gia c hc my v iu khin in t. ng thi s pht trin ca mechatronics cng c phn nh trong khoa hc cng ngh robot. Mt trong nhng tiu ch c trng cho robot l kh nng lp trnh c (programable). Mun c kh nng robot phi dng n my tnh hoc cc thit b khc c chc nng tng t. My tnh c vai tr nh b no ca robot. Ngy nay s ci thin ca tc my tnh to ra nhng bc trng thnh ng k cho robot trong tng giai on pht trin v ang m ra nhng trin vng tr thnh nhng robot thng minh nh kh nng x l rt nhanh ca b iu khin dng my tnh.Tuy nhin kh nng hin thc ca robot ny khng ch ph thuc vo b iu khin m cn kh nng nhn bit ca cc c cu cm nhn v kh nng phn ng kp thi ca c cu chn tay chp hnh. Robot ngy cng ng vai tr quan trng trong cng nghip v cuc sng hng ngy. Chng s thc hin nhng cng vic rt nhm chn hoc nguy him, nhng cng vic m tc v chnh xc vt qu kh nng ca con ngi. Bn kho lun ny cp n vn xy dng mt robot di ng trnh vt cn da trn cc sensor siu m v sensor a bn.

BCH HONG GIANG 7

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

PHN MT

TNG QUAN MT S VN LIN QUAN N XY DNGMT ROBOT DI NG T NG TRNH VT

BCH HONG GIANG 8

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

CHNG 1 S LC V LCH S RA I V XU HNG PHT TRIN CA ROBOT Robot l loi my c th lp trnh iu khin t ng bng my tnh (my tnh y c th c hiu l mt b vi iu khin hoc mt my tnh PC) thc hin cc di chuyn, cm nm cc vt, hon thnh cc cng vic di tc ng ca mi trng. Thng robot c s dng thc hin cc cng vic lp i lp li, cc cng vic d gy nhm chn, n cho cc kt qu chnh xc, nhanh hn, r hn nu c thc hin bi con ngi. 1.1 Lch s ra i v pht trin Thut ng Robot ln u tin xut hin nm 1921 Tip Khc trong tc phm R.U.R (Rossums Universal Robot) ca nh son kch Karel Capek mang ngha ngi lm tp dch. K t thut ng ny c s dng rng ri. Khi nim my t ng xut hin t lu vi nhng vin tng v ngi my trong cuc sng. Ngay sau chin tranh th gii ln th hai, nhiu cng trnh c bt u ti cc phng th nghim OakRidge v Argome pht trin cc my c kh c iu khin t xa nhm phc v trong cc phng th nghim v vt liu phng x. Cc cnh tay ny c thit kt m phng mt cch chnh xc s chuyn ng ca bn tay v cnh tay con ngi. Trong gia nhng nm 1950, bn cnh cc cnh tay c kh xut hin cc cnh tay thu lc v in t. Cng trong nhng nm ny, George C.Devol thit k mt thit b c tn l thit b vn chuyn c khp ni c lp trnh (programmed artculated transfer device). y l mt cnh tay my m hot ng ca n c th c lp trnh thc hin mt chui cc bc chuyn ng c xc nh trong cc cu lnh trong chng trnh. Pht trin xa hn tng trn, Devol v Joseph F.Engelberger dn ng cho cc robot cng nghip u tin c gii thiu nm 1959 cng ty Unimation. Thit b ny s dng my tnh lin kt vi tay my nhm dy cho n thc hin cc cng vic khc nhau mt cch t ng . Khi robot c lp trnh to mt s k l v to ra sc mnh trong sn xut, vo nm 1960 nh mt s tt yu, s linh hot ca h thng robot c nng cao ng k thng qua h thng phn hi t cc sensor. Tip
BCH HONG GIANG 9 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

H.A.Ernst cng b s ra i v pht trin ca bn tay c kh c iu khin bng my tnh s dng cc sensor xc gic. y l s xut hin u tin v robot c kh nng thch ng vi mi trng. Vo cui nhng nm 1960, Mc Carthy cng bn ng nghip cng b s pht trin ca my tnh cng vi camera v tuyn v microphone. Nm 1968 Pieper nghin cu nhng vn ng hc trong iu khin robot bng my tnh, trong khi nm 1971 Kanh v Roth phn tch v ng lc hc v gii hn iu khin tay my. Trong sut nhng nm 1970, mt s lng ln cc cng trnh nghin cu tp trung vo vic s dng cc sensor ngoi tng s tin li v linh hot cho robot. Vo thi gian ny cng ty my tnh IBM ch to ra loi robot c cc sensor xc gic v sensor lc lp rp cc my in gm 20 cm chi tit . Mt lnh vc c nhiu phng th nghim quan tm l robot t hnh, robot di ng. Nhiu cng trnh nghin cu thit k, xy dng to ra cc robot t hnh bt chc chn ngi hoc sc vt. Trong nhng thp k 80 - 90 do s pht trin mnh m ca khoa hc k thut, c bit trong lnh vc k thut vi x l v cng ngh thng tin, s lng robot gia tng, gi thnh gim i r rt, tnh nng c nhiu bc tin vt bc. Ngy nay, chuyn ngnh khoa hc robot (robotics) tr thnh mt lnh vc rng trong khoa hc, bao gm vic gii quyt cc vn v cu trc c cu ng hc, ng lc hc, lp trnh qu o, cm bin tn hiu, iu khin chuyn ng.v.v. 1.2 Cc th h robot. K t khi khi nim robot ra i, vic thit k v ch to robot tri qua nhiu giai on vi nhiu th h khc nhau. C 5 th h robot ra i k t nm 1960 . Th h th nht: Bao gm cc loi robot hot ng lp li theo mt chu trnh khng thay i. Chng trnh iu khin c hai dng: - Chng trnh cng, ngha l khng thay i hoc khng sa c tr khi thay i phn cng.

BCH HONG GIANG 10

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

- Chng trnh c th thay i c thng qua cc panel iu khin hoc thng qua my tnh. Cc robot th h ny s dng c cu iu khin servo vng h (openloop nonservo controlled system ). y l h thng khng s dng thng tin phn hi t mi trng v iu khin robot. Th h th hai: Robot c trang b cc sensor cho php robot giao tip vi mi trng bn ngoi. Cc thit b ny thc cht l cc b bin i nng lng. N chuyn cc i lng khng in thnh i lng in m qua b iu khin robot c th bit c trng thi ca mi trng xung quanh n. Nh cc sensor ny robot c th chn cc phng n khc nhau mt cch linh hot nhm thch nghi vi mi trng bn ngoi. Dng robot vi trnh iu khin ny cn c gi l robot iu khin thch nghi cp thp. y gi l c cu iu khin servo vng kn (closed-loop servo controller system). Th h th ba: cc b iu khin logic kh trnh PLC (Programmable Logic Controller) c s dng trong robot vi nhiu chc nng chuyn bit . Th h th bn: Khc vi PLC b gii hn trong chng trnh ca chng, th h robot ny s dng cc my tnh c trang b cc ngn ng lp trnh c bit hoc ngn ng chun nh Basic, C, C++...., to ra nhiu ng dng CAD/CAM v CIM hoc chng trnh khng trc tuyn. Th h th nm: Cc b iu khin ca robot s dng tr tu nhn to (artificial intelligence). Robot c trang b cc k thut nh nhn dng ting ni, hnh nh, xc nh khong cch, cm nhn i tng tip xc (da nhn to) x l, ra nhng quyt nh hp l. Ngoi ra robot c trang b mng Neuron gip n c kh nng t hc, t xy dng kin thc . 1.3 Nhng xu hng pht trin ca robot hin i Cc robot hin i s c xu hng tng trong tng lai gip con ngi c th to ra cc sn phm mi, bo v c s h tng ca th gii, chm sc nh ca, mua bn. Mt xu hng quan trng trong nghin cu v pht trin robot l pht trin cc h thng my vi c in t (MEMS) c kch thc nh t vi cm ti mm thm ch nh hn m. Cc robot rt nh ny c th di chuyn vo trong mch mu phn phi thuc hoc lm v sinh mch mu; chng c th di chuyn trong mt my tnh ln chun on cc vn xy ra vi my tnh.
BCH HONG GIANG 11 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Mt xu hng pht trin hin nay l vic nghin cu pht trin tr tu nhn to, mng neuron vo trong robot nhm to ra cc robot thng minh, c kh nng thch nghi vi mi trng xung quanh nh con ngi.

BCH HONG GIANG 12

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

CHNG 2 H SENSOR TRONG ROBOT Khi c trang b mt h thng sensor robot s c kh nng thch ng vi s thay i ca mt trng xung quanh. y cng l mt ch tiu quan trng nh gi mt robot c thng minh hay khng. Cc sensor u l b bin i nng lng, n bin nng lng t dng ny sang dng khc, thng l bin i tn hiu t cc i lng khng in thnh cc i lng in. Tn hiu ny c phn hi v b iu khin, thng qua n b iu khin c cc quyt nh cho ph hp. 2.1. Phn loi sensor Cc sensor s dng trong robot c chia thnh 3 loi: Sensor ni (internal sensor), sensor ngoi (external sensor), sensor kho (interlock sensor). 2.1.1. Sensor ni c t trong bn thn robot, n s dng cc thit b v c kh, in, in t hoc thu lc nhn cc thng tin phn hi v v tr ca cc b phn trn robot. Sensor ni c th n gin l cc cng tc gii hn nhng cng c th phc tp nh a lp m quang dng iu khin motor. 2.1.1 Sensor ngoi Sensor ngoi l loi sensor gip robot giao tip vi mi trng bn ngoi nh sensor th gic gip robot quan st, sensor xc nh khong cch gip robot c lng c khong cch ti i tng, sensor xc gic gip robot c cm gic khi cm nm cc vt. Cc sensor ny s phn nh cc thng tin mi trng xung quanh ti robot. 2.1.3 Sensor kho y l loi sensor c dng trong cc tnh hung khn cp bo v robot . Sau y xin c cp n mt s loi sensor thng c s dng . 2.2. Mt s sensor thng gp Trong thc t nghin cu robot, c rt nhiu loi sensor c s dng cho cc mc ch khc nhau. Sau y, xin gii thiu mt s sensor thng c s dng trong k thut robot.
BCH HONG GIANG 13 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

2.2.1 Sensor xc nh khong cch (Range sensor) y l loi sensor c s dng gip robot c th c lng khong cch t v tr t sensor (nm trn robot ) ti i tng. C nhiu cch xc nh khong cch ti i tng. Sau y trnh by hai cch xc nh thng dng nht l: 2.2.1.1 Phng php xc nh khong cch bng lng gic Phng php ny s dng mt ngun pht sng pht ra mt chm tia hp ti p vo b mt vt th. Do phn x, tia sng chuyn hng ti b thu c t cch b pht mt khong B, gc pht tia sng so vi phng ngang l
( hnh 2.1).
i tng

Ngun thu

Ngun pht

Hnh 2.1 Sensor o khong cch s dng phng php lng gic. Nu D l khong cch t i tng ti detector th khong cch ny c tnh nh sau: D = B. tg 2.2.1.2. Phng php xc nh khong cch bng o khong thi gian truyn sng Trong phng php ny s dng ngun pht lazer xung pht ra xung sng, sau thu li nh sng phn x ca n vi iu kin chm sng i v chm sng v phi ng trc. Khi khong cch t sensor ti i tng s c tnh nh sau: D = C . T/2 Trong :
BCH HONG GIANG 14

D l khong cch t sensor ti i tng .

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

C l vn tc truyn nh sng T l thi gian k t khi pht xung sng cho ti khi nhn c xung sng. Vi cng thc ny, o khong cch D s gp phi kh khn l khong thi gian T thng rt nh c picrogiy. Thc t, ngi ta thng s dng o thi gian tr gia sng pht ra v sng phn x. Trong cch ny, mt ngun laser s pht lin tc, b o s thc hin o pha gia hai sng ti v sng phn x. S thc hin c th c ch ra trn hnh 2.2. Trong s , chm laser pht c ti mt gng bn m, gng ny s chia chm laser thnh hai chm sng nh nhau. Mt truyn ti i tng, mt truyn ti b o pha. Chm laser truyn ti i tng s phn x v i ti b o pha. B o pha y ng vai tr l mt detector. Gi L l khong cch t tia chun ti Detector, D l khong cch t i tng ti gng bn m. Khi tng qung ng i ca laser ti vt th v quay tr v Detector l : D = 2D + L
Gng bn m Laser vt

b o pha

Hnh 2.2 o khong cch bng o pha Nu i tng gn (D = 0) khi pha ca hai tia sng nh nhau. Khi D tng th lch pha tng dn dch pha gia hai tia trong trng hp ny s l: D= L + . /3600 do D = . / (2. 3600 ).

BCH HONG GIANG 15

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Cng cn lu : khi = 3600 b o pha s khng phn bit c D = L + n (vi n l s nguyn ) hay D = L. Nh vy cch ny ch o vi cc gi tr lch pha < 3600 hay vi khong cch 2D < . Bn cnh vic s dng o khong cch bng ngun pht lazer, trn thc t ngi ta cn s dng ngun thu pht siu m. Chm xung siu m pht ra p ti b mt phn x ca i tng, xung phn x c thu u thu siu m c thi gian ko di xung t l vi khong cch t sensor ti vt. Nu o c khong thi gian ko di xung phn x ta hon ton c th o c khong cch ti vt. 2.2.2 Sensor pht hin vt th gn (Proximity sensor) y l loi sensor c s dng xc nh xem c hay khng c vt th nm trong vng hot ng ca sensor. Cc loi sensor pht hin vt th gn thng cho li ra tn hiu s. 2.2.2.1 Sensor siu m Loi sensor ny pht ra xung siu m. Nu tn ti vt th trong vng lm vic ca sensor s xut hin sng phn x v u thu. Cu trc mt sensor siu m thng gm nhng phn c miu t trong hnh 2.3. Phn t c bn ca sensor ny l b bin m thng l gm p in c bo v bng mt cht nha tng hp nhm chng li s m t, bi bn v cc nhn t khc ca mi trng. B thu m thanh s hng m a ti b bin i m thanh thnh tn hiu in. Tt c cc thnh phn ny c trong mt v. Mt dng sng in hnh c trnh by trong hnh 2.4 m t hot ng ca sensor siu m. Dng sng A l tn hiu iu khin tn hiu truyn. Dng sng B l tn hiu li ra khi c c tn hiu ting vng (echo) trong B1 l tn hiu pht ra cn B2 l tn hiu phn x li. Cc xung C tch bit tn hiu truyn v tn hiu nhn. phn bit s khc nhau gia cc xung tng ng vi tn hiu mang v tn hiu phn x li ta xem xt tn hiu D. Trong t1 l khong thi gian pht hin nh nht v t1 + t2 l khong thi gian pht hin ln nht. Cc khong thi gian ny tng ng vi khong thi gian truyn sng trong mi trng khi nhn c tn hiu phn x li (lc tn hiu D c gi tr ln nht) s hnh thnh tn hiu E, n s bng 0 khi kt thc xung tn hiu A. Cui cng tn hiu F c hnh thnh khi xut hin xung tn hiu E v s l tn hiu ra ca sensor siu m hot ng theo ch nh phn .

BCH HONG GIANG 16

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

V bc bn ngoi ca sensor Nha

V bc kim loi

Cp in Phn t p in

Hnh 2.3 : Cc thnh phn cu to sensor siu m

BCH HONG GIANG 17

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

CHNG 3 C CU CHUYN NG TRONG ROBOT DI NG Khc vi cnh tay my ti cc robot tnh, robot di ng c c cu chuyn ng nhm a b my ca ton h thng robot ti mt v tr mong mun trong khng gian hot ng. C cu ny c th l cc my thu lc, kh nn, motor in nhng trong ti ny ch s dng loi motor in. V vy chng ny cng ch trnh by tm tt cc tm hiu v motor in v h thng lin quan n iu khin motor. 3.1.Phn loi motor in Motor in thng l loi my chuyn i nng lng in thnh nng lng c, thng l chuyn ng trn. Motor c cu trc t hai phn chnh: Rotor v Stator. Rotor l phn chuyn ng nm li motor bao gm cun dy c cun quanh mt li thp. Li ny c gn vo trc ca motor. Stator l phn khng chuyn ng, n c cu to t nam chm vnh cu. Khi c dng in i qua motor s sinh ra mt t trng chng li t trng ca nam chm vnh cu lm cho motor quay. C hai loi motor thng c s dng trong cc robot di ng l: - Motor mt chiu (DC motor) - Motor bc (Step motor) 3.1.1. Motor mt chiu Motor mt chiu c cu to t mt cun dy v h thng nam chm vnh cu. Loi motor ny c th dng trc tip ngun mt chiu hoc c th iu khin thng qua rng xung. Tc motor s c quy nh bi in p mt chiu hay rng xung t vo. Mt nhc im khi s dng motor mt chiu l km v chnh xc trong chuyn ng. iu ny c khc phc khi s dng motor bc. 3.1.2. Motor bc Khc vi motor mt chiu, motor bc c cu to t nhiu cun dy. Mi cp cun dy tng ng vi mt pha trong chuyn ng. Vic iu khin motor bc c thc hin qua cc xung c lch pha so vi nhau ln lt i qua cc cp cun dy. Chnh nh vic cho cc tn hiu ln lt chy qua cc
BCH HONG GIANG 18 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

cp cun dy to ra cc bc trong chuyn ng. Tu thuc mi loi motor bc m tng ng vi n l cc gc ca bc. Gc ca bc cng nh th chnh xc ca motor cng ln. y l iu khc hn vi motor mt chiu. Vic iu khin bc ca motor s gip thc hin cc chuyn ng mt cch chnh xc ti cc v tr yu cu. 3.2. Cc c tnh ca motor in Mt motor khi s dng cn quan tm n cc thng s sau: - in p s dng, dng in tiu th. - Tc ti a c th t c. - Lc xon ca motor. Sau y s cp n tng phn . 3.2.1 in p s dng v dng tiu th ca motor Trn v mi motor thng ghi cc thng s v gii hn in p s dng v dng in tiu th cc i khi motor hot ng. Thng s ny rt quan trng i vi ngi s dng. N quyt nh vic thit k cc mch cng sut iu khin motor. Vic thit k ny hon ton khc nhau khi s dng cc motor c dng tiu th cc i khc nhau. 3.2.2 Tc cc i ca motor Tc cc i ca motor c quan tm khi c ti v khng ti. Vic o tc cc i c th thc hin bng cch m xung thu c t mt sensor. Tc cc i khi cp in p cho php cc i vo motor hoc khi rng xung cp ti motor l cc i. Tc ny l mt hm ph thuc vo ti ca motor. N quyt nh ti linh ng ca robot di ng v l mt thng s quan trng cn tnh n khi lp trnh iu khin motor v lin quan n qun tnh trong qu trnh hm hoc dng motor. 3.2.3 Lc xon ca motor Lc xon ca motor l thng s dng nh gi kh nng chu ti ca motor. Motor c lc xon cng ln th cng kho. Lc xon c o bng t s gia trng lng lc t ln motor m motor vn ko c vt lm cho dy treo vt vung gc vi bn knh i qua tm motor chia cho bn knh khong cch t tm trc motor ti dy treo nh hnh 3.1. Lc xon s gim khi tng tc ca motor. iu ny chng t khi motor chy vi tc cao thi ti dng cho
BCH HONG GIANG 19 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

motor se b gim xung. tng lc xon cho motor ngi ta thng dng cho motor qua cc c ch gim tc. y thc cht ch l cc c cu bnh rng. T s gim tc cng ln thi lc xon motor s cng ln tuy nhin tc motor s b hn ch.

bn knh

Trc motor Trng lng t ln trc motor

Lc xon = trng lc /bn knh Hnh 3.1: M t cch o lc xon ca motor 3.3 iu khin chuyn ng ca motor Vic thit k mch iu khin motor phi cn c vo cng sut tiu th ca motor v loi motor c thit k hp l. Trong bo co ny trnh by 2 loi mch iu khin motor p dng cho 2 loi motor thng gp l motor DC v motor bc. 3.3.1 iu khin motor DC Thng thng, c th iu khin tc ca motor mt chiu bng thay i mc in p t vo motor. Nhng vic ny gp phi kh khn khi mc in p tr nn qu thp lm vic iu chnh tc thp khng chnh xc. Cn mt cch khc l s dng cc xung in c bin bng mc in p cc i nhng c rng xung khc nhau iu khin tc motor. rng xung cng ln, nng lng trung bnh cp cho motor cng ln v tc quay ca motor ng vi mt ti nht nh cng cao. Vic ny c thc hin bng cc chuyn mch in t cng sut lp trn cc tranzitor c sn hin nay l kh thun li.

BCH HONG GIANG 20

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

3.3.2 iu khin motor bc Khc vi motor DC ch c hai ng iu khin, motor bc c nhiu pha khc nhau. ng vi s pha s tng ng vi s ng tn hiu iu khin. S hnh 3.2 l th d trnh by mch iu khin motor bc 4 pha. Mch cu ti 4 ng iu khin. Vi yu cu dng iu khin c gi tr cc i phi ln, trong s ny, mi ng tn hiu iu khin t vi x l c khuych i qua mt tng Darlingtn tng dng cho tn hiu iu khin.

Hnh 3.2 : S khuych i tn hiu iu khin motor bc Ta thy rng 4 cun dy ca motor bc c sp xp thnh tng cp (cun 1 vi cun 2), (cun 3 vi cun 4). S mch to ra hai cp tn hiu iu khin (bt 1 v bit 2), (bt 3 v bt 4).

BCH HONG GIANG 21

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

CHNG 4 KHO ST MT S LOI SENSOR S DNG TRONG ROBOT DI NG Nh c nhc n, sensor l yu t gip robot thch nghi kp thi vi mi trng xung quanh. Trong phn ny s a ra mt s loi sensor c th s dng dn ng cng nh pht hin vt cn, o khong cch ca robot ti vt trnh vt. 4.1. Sensor hng ngoi y l loi sensor quang s dng thu pht nh sng gn vng hng ngoi. Bc sng nh sng s dng trong cc sensor ny c 880nm. Loi sensor ny c th s dng pht hin mu en/trng hoc pht hin vt cn. Kh nng thc hin c tu thuc vo vic thit k mch. 4.1.1. Sensor hng ngoi s dng pht hin mu en trng Mch in thu pht hng ngoi dng pht hin vch en trng c gii thiu trong hnh 4.1

Hnh 4.1 S mch thu pht hng ngoi pht hin en /trng Trong s ny s dng mt diode pht hng ngoi pht ra nh sng gn vng hng ngoi. Diode ny c mc ni tip vi mt in tr nhm hn ch dng. u thu s dng y l mt phototransistor vi in p cc base thay i theo lng nh sng hng ngoi thu nhn c. Tn hiu t collector ca phototransistor c a ti u o ca b so snh vi u vo cng t ngng. Li ra ca b so snh dng vi mch khuych i thut ton LM324 s

BCH HONG GIANG 22

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

c hai mc hoc cao, hoc thp tu thuc vo in p u vo o so vi ngng. n LED c mc u ra b so snh s ch th nh sau. Khi gp vch trng cc base ca phototransistor thu c nhiu nh sng phn x nht lm cho tn hiu collector thp, khi li vo o ny s c mc in p nh hn ngng lm cho li ra c mc cao lm LED ch th ti. Ngc li khi gp vch en, phototransistor nhn yu lm co li vo o c mc in p cao hn li vo t ngng dn ti li ra c gi tr cao lm n LED ch th sng. Mch thit k nh hnh trn s dng thu pht hng ngoi mt chiu. Trong thc t gp phi vn kh khn l nhiu ca cc ngun nh sng c tim n ngun hng ngoi tc ng. C li s dng pht hin vch en trng khong 3 cm. Loi mch ny ch nn s dng trong cc iu kin mi trng l tng khng can nhiu. 4.1.2. Nghin cu cc sensor hng ngoi c mt trn th trng c tch hp c b thu v pht trong mt v. Hin nay, trn th trng c bn cc sensor hng ngoi c tch hp sn cc u thu pht trn cng mt v. Loi sensor ny thng ch c 3 dy ni trong hai dy ngun nui v mt dy tn hiu. Tn hiu li ra thng c mc logic 0 hoc 1. Sau y xin gii thiu mt photo sensor mang k hiu E3SDS30E4 ca hng OMRON.

Hnh 4.2 Photo sensor E3S-DS30E4 Cc c tnh ca sensor c th k ra nh sau: - Sensor hng ngoi s dng nh sng c bc sng 880nm. - nh sng hng ngoi c iu ch. - S dng pht hin vt cn trong c ly 70cm, c kh nng phn bit c vch trng/en.
BCH HONG GIANG 23 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

- nhy ca sensor c th hiu chnh c thng qua mt bin tr. - Trn sensor c b tr hai n ch th: n ch th ngun v n ch th trng thi tn hiu ra. 4.2 Sensor siu m Trn th trng hin nay ph bin hai loi sensor siu m trong loi th nht b thu pht siu m c tch hp vi nhau trn cng mt . Loi th hai b thu pht c ch to c lp (c mt u thu v mt u pht ). Trong kho lun ny nghin cu kho st c hai loi sensor k trn thng qua hai loi mang k hiu MUST01a v Devantech SRF04. Sensor siu m c th s dng o khong cch thng qua o thi gian tn hiu phn x hoc pht hin vt cn. 4.2.1 Sensor siu m MUST01d y l loi sensor c b thu pht c tch hp trn cng mt ( khng phn bit u thu v u pht ). c tnh ca n c th k ra nh sau: - S dng ngun nui mt chiu 8-16V, dng cc i cho php l 30mA. - Sensor cho tn hiu li ra tng t t 0-5 V tuy nhin gi tr cc i trong thc t ch t c 4,096V - Khong cch sensor pht hin c vt t 1cm-3m vi sai s 2% - Gc m chm siu m pht ra l 150 Cu trc ca sensor v cc chn c m t trong hnh 4.1. Khi c xung kch, siu m pht ra s p vo vt v phn x tr li, tn hiu in thu c li ra ca sensor c mc in p t l vi khong cch. C th, nu khng c sng phn x in p li ra mc cao. Ngc li, c tn hiu phn x tu thuc vo sng phn x mnh hay yu (vt gn hay xa) m in p gim li ra s gim dn.

BCH HONG GIANG 24

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Hnh 4.1 S m t sensor siu m MUST01d

Bng 4.1 Cu trc chn ca sensor siu m MUST01d nh sau Chn 1 2 3 4 5 6 7 Chc nng Ngun nui c n p 8-16V(30mA) t Xung kch ngoi mc TTL (khng s dng) Cho php xung kch (khng s dng) Xung ng h li ra mc TTL(Khng s dng) LI ra tng t t l vi khong cch 0-5VDC Khng s dng

Vi mt gii hn nht nh (ngng) sensor ny hon ton c th s dng pht hin vt cn. iu ny c ngha l vi khong cch vt gii hn th li ra c mc in p tng xng vi mc logic cao, trong trng hp nm trong gii hn cho php tn hiu li ra c gi tr logic thp. Sensor siu m MUST01d hon ton c th s dng o khong cch ti vt phn x. Vic o khong cch c thc hin thng qua tn hiu s t li ra ca sensor. Gi tr thu c t l vi khong cch t sensor ti vt. Tu thuc khong cch t sensor ti vt l bao nhiu m xung c rng khc nhau. Xc nh c rng xung ny ta s xc nh c khong cch t vt ti robot.

BCH HONG GIANG 25

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

4.2.2. Sensor siu m Devantech SRF04 Siu m Devantech SRF04 c s dng thu cc thng tin v khong cch mt cch chnh xc trong di t 3cm n 3m. Sensor ny gm b thu pht siu m tch bit, sens c 4 chn ni trong hai chn cho ngun nui, hai chn cn li mt cho xung tn hiu vo, mt cho tn hiu ra.

Hnh 4.2 Sensor siu m Devantech SRF04 SRF04 hot ng thng qua vic truyn mt xung m thanh c tn s nm ngoi di tn s nghe ca con ngi. Xung ny truyn tc m thanh khong 0.9ft/giy, di dng mt chm hnh nn, m thanh phn x tr li ti u thu t vt th bt k trn ng i ca sng siu m. Tn hiu in ti u thu c dng cc xung vi ko di xung ph thuc vo khong cch phn x k t v tr vt ti u thu. vic thu pht lm vic hiu qu, b pht tm ngng pht sng trong khong thi gian siu m c truyn sau i tn hiu phn x. Xung u ra ca sensor c a ti b x l. Vic xc nh thi gian ko di xung s gip cho robot xc nh c khong cch. Cc c trng ca SRF04 c th k ra nh sau: - in p s dng 5VDC, dng tiu th cc i 50mA, thng l 30mA. - Vng lm vic ca sensor: pht hin khong cch t 3cm ti 3m. - Tn s s dng 40Khz. - Xung kch pht 10us mc cao tng thch TTL. - Xung phn x mc cao c rng xung t l vi khong cch. - Gc m ca b pht l 300.

BCH HONG GIANG 26

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Hnh 4.3: Gc m ca b pht siu m SRF04 Mt s yu cu v khong thi gian thu pht siu m: Tn hiu xung kch pht siu m c gi mc thp (logic 0) v sau a ln mc cao (logic 1) trong 10us khi pht xung siu m. Xung siu m c to ra thng qua sn xung ca tn hiu li vo. Sau khi c kch pht, sng siu m s c pht ra trong 8 chu k burst. B nhn s gi khong trng trong khong thi gian chng 100us trnh nhiu, n t cc m sc nhn ca s khi pht v sau cho php nghe m thanh phn x. ng tn hiu di m mc thp cho n khi cho php nhn m thanh. Khi qu trnh nhn c cho php, b thu s pht hin sn xung ca tn hiu di m (nu c vt) hoc timeout (vt qu thi gian cho php). Vic o thi gian c tin hnh t sn xung ca tn hiu li vo v kt thc khi sn xung ca tn hiu di m. Nu khng c vt c pht hin xung phn x s vt qu thi gian c 36ms. Gin thi gian thu pht siu m c m t hnh 4.4. Vi tc truyn m thanh, c 73,746us m thanh s i c mt khong l 1 inch (2,56 cm). Nu s dng b nh thi nh m t di y vi h s chia trong l 12 v tn s dao ng 12MHz th tn s pht ca timer s l 1MHz tng ng vi khong thi gian ko di xung l 2us. Mi cung ca timer pht c gi l 1 tick. Khong cch ti a sensor c th pht hin c l 3m =118 inch =300cm, qung ng cc i m thanh phi i l 118*2=236 inch. Vi qung ng nh vy, cn khong thi gian di chuyn l 236*73,746 us. Do s tick cc i thu c l : ( 234 * 73,746 ) / 2 = 8702

BCH HONG GIANG 27

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

y a ra mt khi nim gi l h s chuyn i. H s ny c xc nh nh sau : H s chuyn i = 8702/118 = 74 (ng vi n v inch) H s chuyn i = 8702/300 = 74 (ng vi n v cm) Khong cch o c xc nh nh sau : Khong cch = thi gian ko di xung thu c/h s chuyn i.

xung nh nht 10 s Li vo

10ms tr gia 2 xung

m thanh

Li ra

8 chu k burst

Xung ph t hin vt th 100 s-18ms

Hnh 4.4: Gin xung thu pht siu m SRF04 Trong kho lun ny ngoi vic s dng sensor siu m o khong cch em cn s dng sensor la bn o gc lch khi robot di chuyn so vi hng i ban u ca robot. 4.3 Gii thiu v sensor a bn CMPS03 Module a bn trn tr gip cho robot. N s dng sensor trng Philips KMZ51, loi n y c nhy nhn bit t trng ca tri t. Hai li ra ca n lin kt theo hnh tam gic v mi trong s chng s dng tnh ton chiu ca cc thnh phn t trng tri t. Ta c mt s v d s dng m hnh rt gn ny cho cc h iu khin ln hn v nhng ng dng ph bin hn. S kt ni module:

BCH HONG GIANG 28

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Hnh 4.5: Sensor a bn M hnh ny yu cu ngun in 5V v dng khong 15mA.C hai cch kim tra thng s ca module ny: tn hiu PWM u s ra 4 hay giao tip l I2C c cung cp cc u s 2 v 3. Chn 1 2 3 4 5 6 7 8 9 Chc nng Ngun 5V SCL SDA Tn hiu PWM Khng dng nh c 50/60Hz Khng dung GND

Tn hiu PWM l xung c iu ch vi rng xung bin thin tam gic, rng xung bin thin t 1mS(0o) n 36.99mS(359.9o) theo cch gi khc l 100uS/1o vi mt +1mS offset.Tn hiu i khong 65mS gia xung, nn trong khong thi gian 65mS, rng xung bin thin t 66mS n 102mS. Xung c m ha bi 16 bit thi gian ca b vi x l, v kh nng cho php ca vi x l l 1uS, tuy nhin c mt li khuyn l khng nn o bt c ci g vt qu 0.1o(10uS). Hy chc chn l kt ni ti cc u I2C, SCL v SDA, ti ngun cp 5 V nu bn s dng PWM, cng nh l khng s dng in tr cao ti cc u d.
BCH HONG GIANG 29 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

u ra 2 v 3 l mt giao tip I2C v c th s dng c trc tip cc thng s. Nu giao tip I2C khng c s dng sau khi cc u c np in p cao(ti 5V) qua mt cp in tr. khong 47k l va, cc gi tr khng phi l nht thit chnh xc lm. Sensor a bn dng xc nh gc lch, trong kho lun ny dng sensor xc nh gc lch ca robot so vi hng i ban u. Cc thng s ca Sensor a bn - Ngun vo l 5V. - Dng l 20mA. - phn gii l 0.1o. - Sai s l 3o-4o. - Li ra 1: xung thi gian 1mS-37mS. - Li ra 2: giao din I2C, 0-255 v 0-3599, SCL tc 1MHz. - Kch thc: 32mm x 35mm. Sensor ny s dng 2 cun dy KMZ51, t vung gc vi nhau.

H nh 4.6 Giao thc truyn thng I2C s dng module rt gn trn c s dng ph bin trong EEPROOM nh l ca 24C04.Trong bit c truyn u tin,a ch ca module(0xC0) vi bit ghi/c c t mc thp, sau th ghi s m bn mun c.iu ny s lp li ti lc khi ng module a ch ln na vi cc bit c/ghi mc cao(0xC1).By gi bn c th c mt hay hai byte cho 8 bit hay 16 bit thanh ghi.Thanh ghi 16 bit c byte cao trc.M hnh ny c mng gm 16 byte thanh ghi.C mt s c nng ln thnh 16 bit thanh ghi nh hnh 4.7

BCH HONG GIANG 30

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

PHN 2

THC NGHIM THIT K

XY DNG MT ROBOT DI NG

T TRNH VT

BCH HONG GIANG 31

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

CHNG 5 KHO ST THIT K B IU KHIN ROBOT Bn cnh h sensor gip robot cm nhn vi mi trng bn ngoi. B iu khin l mt thnh phn quan trng nht quyt nh s thng minh ca robot. Cc b iu khin c kh nng tnh ton x l mnh, s gip robot thc hin cc bi ton phc tp. i vi cc ng dng trung bnh v nh, b iu khin robot thng c thit k trn c s cc vi iu khin. Kha lun ny nghin cu, thit k b iu khin da trn vi iu khin BASIC STAMP BS2SX. Trc khi trnh by thit k b iu khin, cn cp n nhng hiu bit v vi iu khin BASIC STAMP. 5.1 Chip vi iu khin BASIC Stamp 2sx B phn iu khin trung tm ca robot l mt vi iu khin BASIC Stamp 2sx. BASIC Stamp 2sx l mt mch tch hp vi IC c ln gm c 24 chn nh hnh 5.1. Chc nng ca cc chn c miu t trong bng 5.1 lp trnh cho vi iu khin BASIC Stamp, mt phn mm rt mnh c vit ring cho chp gi l Basic Stamp. Chi tit phn mm ny c gii thiu phn sau.

Hnh 5.1: Vi iu khin Basic Stamp 2sx

BCH HONG GIANG 32

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Bng 5-1
S chn 1 2 3 4 Tn chn SOUT SIN ATN VSS Miu t Li ra ni tip: Ni ti chn RX ca cng COM trong PC Li vo ni tip: Ni ti chn TX ca cng COM trong PC Ch : Ni ti chn DTR ca cng COM t h thng: (ging nh chn 23). Ni ti chn t ca cng COM Cc chn vo/ra chung: mi mt chn c th l u ra hoc u vo , dng ln nht c th t c l 30mA. Tuy nhin, dng tng ca tt c cc chn khng c vt qu 75 mA nu s dng b n p 5V ni. Dng tng trong cc nhm 8 chn (t P0-P7 hoc P8-P15) khng c vt qu 100mA nu s dng n p ngoi. Li vo/ra 5V: Nu cung cp ngun bt thng vo chn VIN, th in p li ra ca chn VIN l 5V. Nu khng c in p no t vo chn ny th chn ny s c in p nm trong khong 4,5V v 5,5V Reset li vo/ra: n s mc low khi ngun cung cp nh hn 4,2V, iu ny lm reset BASIC Stamp. C th a xung thp reset chip. Chn ny khng nht thit phi ni, khng c t n ln cao. t h thng (ging nh chn 4 )c ni vi chn t ca thit b u cui. Ngun mt chiu cha n nh c th chp nhn in p 5,5V-12VDC (Thng thng l 7,5V). Chn ny c th khng c s dng nu nh s dng b n p ngoi.

6-20

P0-P15

21

VDD

22

RES

23

VSS

24

VIN

kt ni BASIC Stamp vi PC v cc mch khc, ta dng mch c cu to nh hnh 5.2 Mun lp trnh cho mch trn ta ch cn kt ni n vi my tnh thng qua cng COM. Ch cn cm dy cp t cng ni tip ca PC vo b vi iu khin. Np chng trnh vo vi iu khin. Khi n phm Run th chng trnh c np vo vi iu khin v s t ng chy chng trnh. Sau c th b dy cp ni gia my tnh v vi iu khin ra. Do t m c lu trong EEFROM nn chng trnh c bo v khi mt ngun nui. Nu mun thay i chng trnh, phi np t m mi. Vi iu khin s dng li v chp nhn t m mi v sau s chy chng trnh vi nhng t m mi ny.
BCH HONG GIANG 33 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Hnh 5.2: Mch iu khin dng BSe2sx

S nguyn l ca mch vi iu khin.

Hnh 5.3: S nguyn l ca mch vi iu khin

5.2 Gii thiu v ngn ng BASIC STAMP

BCH HONG GIANG 34 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Hnh 5.4: Giao din ca ngn ng Basic Stamp 2 Mt chp vi iu khin cng c th coi l mt my tnh, cc h s dng vi iu khin thng yu cu cc b phn ca n phi gn nh, kh nng lm vic hiu qu v c bit l cng sut tiu th thp. Cc h nh vy thng c gi l cc my tnh nhng (Embedded PC) y l mt trong nhng b phn s dng cng ngh tch hp cao. BASIC l ngn ng lp trnh rt d s dng, BASIC STAMP c xy dng da trn ngn ng BASIC, n l b vi iu khin c gi tng i r (nh hn 50$). BASIC STAMP th h u tin c gii thiu vo nm 1993 (California-based Parallax, Inc). Vo ma h nm 2000 xut hin hai loi ngn ng Stamp l BASIC STAMP I (BS 1) v BASIC STAMP II (BS 2 ) c vit ring cho chp vi iu khin ny. Tt c Stamp u c cc c im sau : + Kch thc nh. + C sn chng trnh vit bng BASIC. + Ngun cung cp 9V.

BCH HONG GIANG 35

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Chng trnh c lu tr trong b nh vnh cu EEPROM (c th xo c ). Bt c khi no c cp ngun th chng trnh BASIC trong b nh s chy. Stamp c th c lp trnh li bng cch ni tm thi vi my tnh thng qua chng trnh ch. Cc li vo/ra c th c ni vi cc thit b s khc, nh chuyn mch sensor, thm ch iu khin trc tip cc loi ti nh nh LED. B x l PIC s dng cng ngh Microchip Inc, nn cho php hot ng vi tc 5 triu m my trn 1 giy. 5.2.1 Mt s cu lnh chnh trong Basic Stamp.
a) Nhn, lnh nhy gotos.

Tt c cc chng trnh va gii thiu u chy trc tip. Chng bt u chy t lnh u tin v chy tng dng mt. Nhng iu hay nht trong lp trnh l kh nng thay i trt t ca cc lnh trong chng trnh. Hy xem dng lnh sau debugReady debugReady debugSet.. BS2 debugSet.. debugGo! debugGo! By gi, c th on trc c kt qu ca chng trnh; Stamp s hin th. C th thay i chng trnh nh sau BS2 debug Ready... goto Start debug Set Start:
BCH HONG GIANG 36 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

debug Go!,cr Nhn c: Ready...Go! Stamp b qua cc lnh in Set.. do s dng lnh nhy goto Start, Stamp s thy n b qua cu lnh debug Set.. iu ny minh ha cch n gin nht thay i tin trnh hot ng ca chng trnh. Nhn bao gm mt tn v kt thc l du (:), nhn thng ng trc lnh s l ch ca lnh nhy goto. Thng thng, cp goto/label c s dng to vng lp. Chng ta hy xem v d di y xem c g khc vi cc v d trn khng. BS1: debug Ready... goto Start debug Set Start: debug Go!,cr goto Start Nu chy chng trnh s thy s lp li cc ch th, v Stamp s hin th: Ready...Set..Go! Go! Go! N s lp li lnh Go! n v tn. b) Vng lp For...Next Gi s mun lp li lnh Go! Ba ln hoc mt s ln. Cp lnh goto/label khng cho php t c s vng lp, nhng c mt cp lnh cho php iu khin chng. BS2: Symbol loops = b0 FOR loops=1 to 3
BCH HONG GIANG 37 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

debug Go!,cr NEXT debug Done Chng ta s c Go! Go! Go! Done. Cp ln For ...Nt s dng bin m s vng lp. Lnh For t gi tr ban u ca bin: FOR loops=1 ... Phn sau cu lnh For l ...to 3, thc cht l thng bo cho lnh Next. Lnh Next thm 1 vo bin (vng lp) v so snh kt qu vi gi tr cui cng c gi t ln For (trong trng hp ny bng 3). Nu gi tr bin nh hn hoc bng vi kt qu cui cng, lnh Nt lm cho vng lp ca chng trnh quay tr li lnh For. Nu gi tr bin ln hn gi tr cui cng th lnh Next kt thc vng lp v chng trnh chy tip cc cu lnh khc. For...Next s chy cc cu lnh nm gia chng v vn tip tc cho n khi gi tr bin vt qu gi tr cui cng. c) Cu iu kin If...Then Trong BASIC s dng cu iu kin If...Then. V d: BS2 x varbyte x=99 IF x<100 then saySo debug x is 100 or more End saySo debug x is less than 900 chng trnh a ra x=99 nh hn 100. Thay i x=99 gn gi tr x bng 100 hoc nhiu hn (nhng khng ln hn 255, gi tr ln nht ca byte) v tr li chng trnh. By gi chng trnh s hin th thng ip x is 100 or more. (x ln hn hoc bng 100).
BCH HONG GIANG 38 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

C hai phn trong cu lnh If...Then. phn u l iu kin- cu lnh ny l mi quan h gia hai gi tr, nh l x<100. Nu cu lnh If ng th v th hai s c thc hin. Nu cu iu kin sai th cu lnh If...then s khng lam g c. n cho php chng trnh tip tc chy cc cu lnh khc.

Biu tng = < > <> <= >= V d: BS2 Loops var byte Loops=0 Repeat:

ngha Bng Nh hn Ln hn Khng bng Nh hn hoc bng Ln hn hoc bng

Debug looping:, DEC loops,cr Loops=loops +1 If loops <=5 then repeat Debug Done:, DEC loops d) Lnh Gosub Cc lnh trong ngn ng Stamp u s dng hm cng c xy dng chng trnh. V thng cng c ny nh, ch c 36 lnh trong BS2. trong mt s trng hp, nhm lnh s rt hu ch v n s s dng nhiu hn mt ln trong chng trnh n, ta s dng cu lnh Gosub...Return.

BCH HONG GIANG 39

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Gosub l mt bin th ca lnh Goto, n gi chng trnh con c bt u bng mt nhn v bao gi cng kt thc bng lnh Return. Sau y l mt v d c s dng chng trnh con: BS2 Loops var byte Debug Subroutine Example GOSUB mySub For loops=0 to 5 Debug DEC loops GOSUB mySub Next end mySub: pause 500 debug cls,> RETURN Chng trnh con mySub s lm tm dng chng trnh Stamp mt na giy (500/1000ths/sec), xa mn hnh Debug bng lnh cls, v ghi ra mn hnh k t >. Khi chy chng trnh, chng trnh con c a vo u chng trnh v c gi mt ln trong mi bc ca vng lp. 5.3 Mch iu khin cng sut ng c. Mch c thit k theo nguyn l mch cu H o chiu ca ng c DC. y em s dng cp transistor trng knh P l IRF9540 v knh N l IRF640. IRF9540: Nu th m VGS = -10V th dng in cc i i qua lin tc l 25 Ampe, dng in xung l 75 Ampe. Th nh thng VGS l 20V. IRF640: Nu th m VGS = +10V th dng in cc i i qua lin tc l 18 Ampe, dng in xung l 75 Ampe. Th nh thng VGS l 20V. Nguyn l hot ng ca mch. cu H th nht khi khng c tn hiu iu khin vo, hai transistor Q1 v Q2 ng, lc th vo cc Gate ca cc
BCH HONG GIANG 40 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

transistor cng sut l mc cao, do Q5, Q6 m, khng c dng in i qua ng c. Khi c tn hiu iu khin vo, chng hn chn 1, lc Q1 m, n s lm cho Q6 v Q9 cng m, Q5 v Q10 cng ng, dng in s i qua ng c theo chiu thun. Khi tn hiu vo chn s 2, Q2 m, dn n Q5, Q10 m cn Q6, Q9 ng. Dng in s i qua ng c theo chiu nghch.

Hnh 5.4: S nguyn l mch iu khin ng c Tng t khi khng c tn hiu iu khin vo, hai transistor Q3 v Q4 ng, lc th vo cc Gate ca cc transistor cng sut l mc cao, do Q7, Q8 m, khng c dng in i qua ng c. Khi c tn hiu iu khin vo, chng hn chn 3, lc Q3 m, n s lm cho Q8 v Q11 cng m, Q7 v Q12 cng ng, dng in s i qua ng c theo chiu thun. Khi tn hiu vo chn s 4, Q4 m, dn n Q7, Q12 m cn Q8, Q11 ng. Dng in s i qua ng c theo chiu nghch.

Hnh 5.5: Mch cng sut iu khin ng c

BCH HONG GIANG 41 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

im c bit ca mch cu ny l c ch phanh ng (c ngha l khi ta ngt khng iu khin ng c na th hai transistor na trn s cng m, tc l hai cc ca ng c s chp vo nhau, chng li qun tnh quay ca ng c ). Mch ny cng c c ch t bo v, khi ta cung cp tn hiu iu khin vo c hai chn chiu tin v li vn khng xy ra xung t, chp chy mch m vn l ch phanh ng. 5.4 Kt cu c kh ca robot 5.4.1 S thit k Robot c lin kt bi cc thanh nhm, c kch thc v kt cu c ch ra hnh 5.6

sensor siu m

39cm

cqui

Sensor a bn

45cm

Hnh 5.6: Hnh dng v kch thc ca robot Khung ca robot hnh ch nht, chiu di 36cm, chiu rng l 31cm, chiu cao tnh t mt sn ti sonar l 30cm. Bao gm 3 bnh xe, 2 bnh pht ng v mt bnh b ng.

BCH HONG GIANG 42

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Hnh 5.7: Hnh dng robot nhn t pha trn

Hnh 5.8: S v cc bnh ca robot 5.4.2 Cc thnh phn khc ca Robot Robot s dng mt ngun nui l acqui 12V-5Ah cung cp ngun in cho cc mch in. - ng c s dng cho robot l ng c 12V DC. - Sensor siu m dng o khong cch. - Compass Sensor
BCH HONG GIANG 43 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

CHNG 6 ROBOT DI NG TRNH VT CN 6.1 Yu cu bi ton t ra Kho lun ny nghin cu, xy dng chng trnh iu khin robot thc hin 3 bi ton : - iu khin robot i thng. - Nu gp vt cn th trnh. - iu khin robot tr v hng i trng vi hng i ban u. 6.2 Cc trng hp ca vt cn trn ng i ca robot C 3 trng hp chnh khi gp vt cn : 6.2.1 C mt vt cn nm trn ng i ca robot: bit c c vt cn trn ng i ca robot hay khng ta dng mt sensor siu m xc nh khong cch. Sensor siu m dng y l sensor must01d c nhc n trn. Nh vic chun ho khong cch ta c th xc nh c khong cch t robot ti vt cn l bao nhiu c th trnh vt. V robot c kch thc xc nh nn ta c th xc nh c khong cch ti thiu gia robot v vt cn m robot bt u trnh. n gin cho bi ton ta gi s vt cn l mt khi hp hnh ch nht nh hnh 6.1. Ta cn xc nh khong cch nh nht (Dmin) robot bt u trnh vt.

Robot

Vt cn

A O P H T

Hnh 6.1: Trng hp c 1 vt cn

BCH HONG GIANG 44 K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Do kch thc ca robot ta c th xc nh ngay t ban u nn vic xc nh Dmin s tr nn n gin hn. Khi robot quay th 1 bnh tin v 1 bnh li nn coi nh tm quay (O) gia 2 bnh, v khi robot quay th quay quanh ng trn tm O. Nn cho cnh ca robot khng chm vo vt cn th Dmin OA OH.. Ta c th ly Dmin = OA - OH + d ( d l mt sai s).

Robot

Vt cn

Hnh 6.2 Xc nh Dmin Sau khi xc nh c khong cch ngn nht robot bt u quay th iu khin cho robot quay trnh vt. Nhng cn 1 vn t ra l lm th no robot bit c n quay 1 gc l bao nhiu im A trn robot khng chm vo vt. Do sensor siu m dng y c gc m b nn khi robot quay c 1 gc no th sensor khng cn nhn thy vt cn na trong khi robot cha thc s thot hn khi vt cn. xc nh c im A trn robot thc s thot khi vt cn cha ta c th dng 1 sensor siu m hoc sensor hng ngoi pht hin mu en trng t ti im A. Nhng trong kho lun ny em gii quyt vn trn bng cch to tr. Tc l khi robot quay n 1 gc m sensor siu m khng cn nhn thy vt na th em cho robot quay thm mt gc nh na (qua thc nghim c th xc nh c gc ny). Sau khi robot quay 1 gc trnh vt th n cn phi o xem n quay gc l bao nhiu. Vic xc nh ln ca gc quay da vo sensor la bn. Xc nh ln ca gc quay ny l rt cn thit v sau khi trnh vt ta dng n iu khin robot quay tr v hng i ban u.

BCH HONG GIANG 45

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Tin trnh ca robot trnh vt l: - Ti im P quay bn phi 1 gc im A ca robot ra khi vt cn. - Tin 1 on L ti im Q im D ca robot ra khi hng chn ca vt cn. - Ti im D quay li bn tri 1 gc tr li hng i song song vi hng ban u. - Tin 1 on M n im R robot thc s thot khi vt. - Ti im R li quay bn tri 1 gc . - Tin 1 on L robot ti im T. Ti im T robot quay bn phi 1 gc robot tr li hng i ban u sau khi trnh c vt cn. 6.2.2 Trng hp t 2 vt so le nhau.
Vt cn 1 Y

Vt cn 2

Robot L Q R m

2L

L T

Hnh 6.3: 2 vt cn sp xp so le vi l < AB Trong trng hp ny khi robot trnh vt cn th nht ri li gp lun vt cn th 2 th n s trnh tip vt cn th 2, sau n quay tr v hng i ban u. Vn t ra y l iu kin khi no th n coi vt 2 l vt cn. Do kch thc ca robot ta xc nh c trc nn ta c th xc inh c khi no n coi vt 2 l vt cn.

BCH HONG GIANG 46

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Nh trn trnh ti im Q robot tin 1 on QR thot khi vt 1, nu khe h gia 2 vt khng robot lch qua th n s trnh vt th 2. Sau vi quay tr v hng i ban u. Gi s khi robot i ti im R m khong cch do sensor siu m o c so vi vt 2 l m < Dmin tc l khe h gia 2 vt khng robot c th lch qua c th robot s tip tc trnh vt 2. Vic x l trnh vt 2 cng tng t nh vic x l trnh vt 1. Sau khi trnh vt 2 v robot tin tip ti im Y th ti im Y n s quay tri 1 gc . Sau khi quay xong th khng ging vi trng hp 1 l n tin 1 on L, m n phi i 1 on l 2L v im T. Ti T n quay phi 1 gc v hng i ban u. 6.2.3 Trng hp robot lch gia 2 vt cn.
Vt cn 1 P T

Robot

Vt cn 2

Q M

R x

Hnh 6.4: Robot lch qua 2 vt Trong trng hp ny th ti im R sensor siu m o c khong cch t robot ti vt cn 2 l m > Dmin, tc l khong cch gia vt robot lch qua. Th n s khng coi vt 2 l vt cn v n s tip tc cng vic quay tr v hng i ban u ca n.

BCH HONG GIANG 47

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

START

I THNG S KC<Dmin R PHI

KC<Dmin

S
R TRI

Hnh 6.1: Lu thut ton chng trnh trnh vt cn

BCH HONG GIANG 48

K46C

Kha lun tt nghip

Chng trnh iu khin robot ' {$STAMP BS2sx} ' {$PBASIC 2.5} ' {$PORT COM1} '**************** '*KHAI BAO BIEN * '**************** gocquay VAR Word 'bien luu so xung cua gocbd VAR Word gocsau VAR Word acc VAR Word 'bien luu so xung khoang kc VAR Word 'bien luu khoang cach tu chuankc VAR Word Dmin VAR Word 'Bien luu khoang cach de loops VAR Word temp VAR Word delta VAR Word '********************* '* CHUONG TRINH CHINH* '********************* main: Batdau: HIGH 5

http://www.ebook.edu.vn

goc lech cach tu vat toi sensor vat toi sensor quay tranh vat

IF IN5 =0 THEN Tien 'Neu chan 5 dc an thi bat dau chay GOTO Batdau Tien: 'Tien thang cho toi khi gap vat GOSUB Dogoclech gocbd=gocquay PULSIN 0,1,kc IF kc=0 THEN tien kc=20+((kc-1583)/70) IF kc<30 THEN GOTO quaysangfai ELSEIF kc>50 THEN GOTO tienthang ENDIF tienthang: GOSUB banhtraitien GOSUB banhfaitien GOTO tien quaysangfai: DO GOSUB quayfai Tien1: 'Tien thang cho toi khi gap vat PULSIN 0,1,kc IF kc=0 THEN tien1 kc=(kc-1583)/70 +20 IF kc>50 THEN EXIT LOOP

BCH HONG GIANG 49

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

quaysangtrai: GOSUB quayfai khi quay nguoc lai PAUSE 500 GOSUB banhtraitien GOSUB banhfaitien PAUSE 3000 GOSUB Dogoclech gocsau=gocquay delta = gocsau-gocbd DO

'quay tiep mot doan de tranh khoi vat truoc 'tien thang sau khi quay trai

'do goc lech de quay nguoc lai 'goc lech khi tranh vat 'luu goc lech

GOSUB quaytrai 'quay sang trai de co huong // huong ban dau GOSUB dogoclech IF gocquay<gocbd+10 THEN EXIT 'sai so goc quay la 10 do LOOP GOSUB banhtraitien GOSUB banhfaitien PAUSE 4000 DO 'Tien 1 doan de thoat khoi vat

'quay tiep ben trai de ve duong di ban dau

GOSUB quaytrai GOSUB dogoclech IF gocquay<gocbd-delta THEN EXIT lech LOOP GOSUB banhtraitien GOSUB banhfaitien PAUSE 3000

'quay ve huong ban dau - goc

DO GOSUB quayfai 'quay ve huong ban dau GOSUB dogoclech IF gocquay>gocbd+1 THEN EXIT LOOP GOSUB banhtraitien GOSUB banhfaitien

GOTO tien GOTO main

'************************************ '* CHUONG TRINH CON DIEU KHIEN MOTOR* '************************************ banhtraitien:

BCH HONG GIANG 50

K46C

Kha lun tt nghip

LOW 15 HIGH 14 RETURN 'enable motor left 'derector motor left tien

http://www.ebook.edu.vn

banhfaitien: LOW 13 'enable motor right HIGH 12 'derector motor right tien RETURN banhtrailui: HIGH 15 'enable motor left LOW 14 'derector motor left lui RETURN banhfailui: HIGH 13 'enable motor right LOW 12 'derector motor right lui RETURN stopbanhtrai: HIGH 15 'Stop motor left HIGH 14 RETURN stopbanhfai: LOW 13 'Stop motor right LOW 12 RETURN stop2banh: HIGH 13 'Stop motor right HIGH 12 HIGH 15 'Stop motor left HIGH 14 RETURN

'************************************* '*CHUONG TRINH CON QUAY TRAI,QUAY FAI* '************************************* quaytrai: HIGH 15 'banh trai lui LOW 14 LOW 13 'banh fai tien HIGH 12 RETURN quayfai: HIGH 13 'banh fai lui LOW 12 LOW 15 'banh trai tien HIGH 14 RETURN

BCH HONG GIANG 51

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

'****************************** '*CHUONG TRINH CON DO GOC LECH* '****************************** Dogoclech: PULSIN 9,1,temp temp = (temp-1250)/125 IF temp<>514 THEN gocquay=temp RETURN

BCH HONG GIANG 52

K46C

Kha lun tt nghip

http://www.ebook.edu.vn

Ti liu tham kho

[1] Parallax Inc. www.parallaxinc.com. [2] Scott Edwards programming and Customizing the Basic Stamp omputer. [3]http://www.robot-electronics.co.uk/ [4] www.semiconductors.philips.com/ acrobat/datasheets/KMZ51_3.pdf [5]Trn Th Thy H Nghin cu cu trc v pht trin chng trnh iu khin thng minh ca cc robot nhiu bc t do. [6] Trn Duy Hng Nghin cu xy dng mt robot di ng thng minh hot ng t qun tr. [7]Phan Hu Ph Nghin cu th nghim mt m hnh th gic my tnh dng cho bm i tng v dn ng ca robot di ng thng minh.

BCH HONG GIANG 53

K46C