Dieu Khien Robot Omni Tranh Vat Can

7/22/2019 Dieu Khien Robot Omni Tranh Vat Can

1/52

A M H: ieu khi en hethong Bomon: C i en t--------------------------------------------------------------------------------------------------------------------------

pg. 1

AI HOC QU OC GI A TP.HCM

TRNG H BA CH K HOA

--------------

KHOA C KH

BOM ON C IEN T

CONG HOA X AHOI CH UNGH A V IET NA M

OC L A P - TDO HA NH PHUC

NHIEM V U OA N M ON HOC

Ten etai : ieu khi en robot omni tranh vat can

Giao vien hng dan: Nguyen Thai A nh Tuan

Sinh vien thc hien: Bui Quoc V u M SSV : 20503575

LeNhat Toan M SSV: 20502985

Ngay nhan oan:........................................................................................................

Ngay hoan thanh oan: ............................................................................................

NHA N X ET CUA GIA O V IEN HNG DA N:

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................


2/52


3/52


pg. 3

NHA N X ET CUA GIA O V IEN DUY ET

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

......................................................................................................................................

Giao vien duyet


4/52


5/52


pg. 5

TNG QUAN

!n %i&u khi(n th*ng 1 l %!n %,u tin c.a ch12ng trnh h3c, gip sinh vin lmquen v5i mi tr17ng lm vi9c %:c l;p hay theo nhm v5i s


6/52


7/52


8/52


9/52


pg. 9

NGu nh1ta cIp xung c th7i gian mHc tch cnguyn l dng 3 chn c?a Port 1 89C51 1;c@p xung cho 3 1+ng c.


10/52


11/52


12/52


pg. 12

2. Hng d6ng vi 1i8u khi;n cho 1+ng c.:

S4d6ng cc Port Xu@t nhIp v m+t ngn ngJlIp trnh ph h3p vDi lo/i vi

1i8u khi;n ta sL1i8u khi;n 123c 1+ng c.Cc thng sc$n quan tm khi 'i(u khi*n ',ng c.:-i9n p sZdDng- Cng suIt lm vi9c- Dng khng tCi, dng lm vi9c- T*c %:c.a %:ng c2Cc ph/ong php 'i(u khi*n ',ng c.:a- i&u khi(n ON/OFFb- i&u khi(n t*c %:%:ng c2- MJch c,u H:

- MJch sZdDng 1 FET + 1 Relay^%y, ta quan tm %Gn cc thng s*kNthu;t c.a RC Servo sZdDng %1Mc nuVph,n tr15c. Dng TIMER %(%i&u r:ng xung, %i&u khi(n %:ng c2quay nh1mong mu*n.


13/52


pg. 13

CH#$NG 3 : TM HI%U VBCMM BION HQNG NGORI

1 GiDi thiFu:

Cch %y vi nWm ,nhi&u loJi cCm biGn h!ng ngoJi %o khoCng cch % %1McSharp gi5i thi9u .Cc cCm biGn ny %1Mc %ng gp nhQg3n ,tiu thDdng rIt t,v nhi&u s


14/52


pg. 14

2.Nguyn l ho/t 1+ng


15/52


16/52


17/52


pg. 17

Gc c.a chm tia h2i nhQ,v khoCng cch giEa con pht v thu nhQchXc 16cm. NGu bJn mu*n tWng gc mV, bJn c th(sZdDng kGt hMp 2 cCm biGn h!ngngoJi nh1v dDVhnh d15i .

Vi9c sZdDng GP2D12 %1Mc lin kGt n*i t5i 3 dy .NhEng kGt n*i ny l :Ground , Vcc v Output . CCm biGn sP%o khoCng cch v xuIt ra output d15idJng mHc volt analog . BVi v vi9c thu nh sng xCy ra lin tDc , khng c,n xungclock . Nn t*n nhi&u nWng l1Mng ,v c th(Cnh h1Vng %Gn cc ngoJi vi khc,

v b:xZl nGu trn robot c nhi&u cCm biGn ny .

Tn hi9u Output c nhEng xung gai nhQnn c,n c thm tD l3c giEa Output vGround . Th17ng lIy tD4,7aF


18/52


pg. 18


19/52


pg. 19

Ch2.ng 4 : Phn tch 1+ng hSc v 1+ng lTc hSc

Robot Omni1. Phn tch 1+ng hSc Robot Omni 3 Bnh

M hnh ha robot omni nh1hnh vPtrn.

3 bnh xe robot %1Mc b*tr tiGp tuyGn v5i 1 %17ng trn bn knh oR v l9ch nhau 1

gc 120 %:.

3 CCm biGn h!ng ngoJi %1Mc %Ot tJi 3 dy cung c.a %17ng trn trn cRng %1Mc b*trl9ch nhau 1 gc 120 %:.


20/52


pg. 20

Cc chuy(n %:ng c.a robot bao g!m

Chuy;n 1+ng tUnh tiWn :

Khi cho 2 bnh xebIt kAquay cng lc v5inhau theo 1 ph12ng no% ( 2 bnh quay ng1Mcchi&u nhau )th sPlm chorobot chuy(n %:ng t?nhtiGn .

V d6: cho 2 bnh xe W1 v W2 cng quay theo ph12ng nh1hnh vPthrobot sPchuy(n %:ng t?nh tiGn nh1trn .V0i R l bn knh c2a bnh xeOmni.

GiCsZ2 bnh xe cng quay v5i v;n t*c gc l c( 2 bnh quay cng

ph12ng V%y nghda l 2 bnh phCi quay ng1Mc chi&u nhau ) c vecto v;nt*c di

2WV v 1WV nh1hnh vP.

TTng hMp 2 chuy(n %:ng trn ta c vecto chuy(n %:ng t?nh tiGnttV c.a

robot


21/52


pg. 21

Quan h9hnh h3c cho ta %1Mc : === 30cos2cos2cos2 21 RVVV WWtt (1)

Chuy;n 1+ng xoay :Khi cho 3bnh xe chuy(n %:ng cng chi&unhau th sPlm cho robot chuy(n%:ng xoay


22/52


pg. 22

2. Phn Tch'+ng lTc hSc Robot Omni


23/52


pg. 23

Gi s#&o gc c'a m*i tr,c motor so v-i tr,c x trong h0ta &2c'a robot nh3hnh v4l6n

l3.t l1 2, ,..., n ,lc ny h3-ng chuy8n &2ng c'a bnh th:i l 2 / 3+i .

Khi n motor cng ho;t &2ng s4t;o ra n lt t;i tm hnh hc c'a robot ta c gia t#c tJnh tiFn a v gia

t#c gc.

l

1 2

1( ... )na F F F

M= + + +

.

1 2( ... )nR

f f fI

= + + +

V-i: m l kh#i l3Bng robot

R l bn knh robot

i if F=

I l momen qun tnh

DLu c'a1 2, ,..., nf f f c th8l m ho>c d3.ng ph,thu2c vo chi=u quay c'a motor

Phn tch gia t#c c'a robot trn hai tr,c Ox v Oy, ta &3Bc:

1 1 2 2. .sin .sin ... .sinx n nM a f f f =


24/52


pg. 24

1 1 2 2. . os . os ... . os

y n nM a f c f c f c = + + +

Chng ta biFt rGng momen qun tnh i v-i m2t kh#i tr, &Mng chLt l: 21

. .2

I M R= ,

momen qun tnh i v-i m2t hnh trn &Mng chLt l: 2.I M R= .

Do & i v-i robot, ta xt momen qun l: 2. .I M R=

Chng ta c th8bi8u diNn ph3.ng trnh gia t#c d3-i d;ng ma trCn nh3sau:

1

1 2

2

1 2

.

sin sin ... sin1

. os os ... os

. . .

...

x n

y n

n

fa

fa c c c

MM R M R M R

fI I I

=

M

V-i 2. .I M R= , ta &3Bc:

1

1 2

2

1 2

.

sin sin ... sin1

. os os ... os

1 1 1. ...

x n

y n

n

fa

fa c c c

M

R f

=

M

Ta gi ma trCn 3.n trn l ma trCn C

=1 2

1 2

sin sin ... sin1

. os os ... os

1 1 1...

n

nc c cM

3.ROBOT TRONG KHNG GIAN PH%NG:

Chng ta c th8tnh &3Bc vCn t#c cu#i cng c'a cc bnh robot, v vCn t#c c'a robot trn

m>t ph@ng bGng cch m tHcc ph3.ng trnh chuy8n &2ng c'a robot theo thDi gian. Khi

xt robot trong khng gian ph@ng, chng ta c th8tnh &3Bc quO&;o c'a robot v chng

ta c th8&;t &3Bc &i=u ny bGng cch phn tch vCn t#c c'a cc bnh xe robot.


25/52


pg. 25

Chng ta xt vCn t#c c'a cc bnh xe d3-i d;ng vector ( )1 2, ,..., T

nv v v , vCn t#c trong

khng gian ph@ng v vCn t#c gc c'a robot l ( ), , .T

x yv v R .

Xt robot chuy8n &2ng v-i vCn t#c ( )1,0,0 T

, &i=u ny c nghQa l robot &ang chuy8n

&2ng theo ph3.ng ngang m khng quay trong m>t ph@ng. Khi robot &ang chuy8n &2ng

theo ph3.ng ngang v-i vCn t#c l 1, th bnh xe th:i quay v-i vCn t#c l sin i v vCn

t#c c'a cc bnh nhSgTn trn bnh l-n c ph3.ng tr


26/52


pg. 26

Gi:.

.

x

y

a

a a

R

=

,

1

2

n

v

vm

v

=

M,

1 1

1 1

1 1

sin os 1

sin os 1

sin os 1

c

cD

c

=

L L L,

R.

x

y

v

v v

=

, f =

1

2

n

f

f

f

M

Ta c: .a C f=

V m = D.v (*)

Nh3ta & biFt: .v t a =

Nn: . . .m t D C f =

ThDi gian t robot c th8&Fm &3Bc nhDvo cc chip thDi gian th


27/52


pg. 27

Khi cho ma trCn l


28/52


pg. 28

KWt LuIn: Chng ta c th;1i8u khi;n chnh xc robot bXng ph2.ng php 123c giDithiFu Zph[n trn nWu nh2ng khng k;1Wn =nh h2Zng c?a ma st tr23t . NWu c ma sttr23t sLx=y ra nhJng sai lFch tSa 1+ngoi muGn v ta c th;trnh 123c hiFn tr23ngny bXng cch theo di vIn tGc c?a m\i bnh 1;c th;ki;m sot 123c sTtr23t c x=yra hay khng v hiFu ch^nh h3p l


29/52


pg. 29

CH#$NG 5 : 'i8u khi;n chuy;n 1+ng c?a Robot Omni

'i8u khi;n Servo:

^%y loJi servo sZdDng l loJi RC servo do % m*i quan h9giEa %Ji l1Mng vo (Xung PWM) v %Ji l1Mng ra ( Servo Speed) khng quan h9tuyGn tnh nn sPgIy khkhWn cho qu trnh %i&u khi(n %Oc bi9t l khi %i&u khi(n robot chuy(n %:ng t?nh tiGn , khi% 2 %:ng c2quay ng1Mc chi&u nhau v PWM c.a 2 servo lJi khc nhau v&h9s*dutytrn 1 chu kA20ms v %Oc bi9t l vi9c ch3n h9s*duty cho 2 servo phCi hi9u chXnh saocho thQa mn :

- CC2 %:ng c2xoay ng1Mc chi&u

- T*c %:xoay g,n bKng nhau %(tJo thnh vecto t?nh tiGn thfng , khng cchuy(n %:ng quay.

Theo th


30/52


pg. 30

Nh1chng ta % biGt ty theo %i9n p phn c


31/52


pg. 31

^%y chng ta sZdDng ng vo V_in c.a transistor l ng ra c.a CCm biGn H!ngngoJi , ng ra c.a transistor sPn*i %Gn ng vo c.a VDK Slave .

Tn hi9u vo c.a h!ng ngoJi l tn hi9u Analog biGn %Ti lin tDc nh1ng tn hi9u ra sPlDigital VmHc tch c


32/52


pg. 32

i9n tr7vo BR bVi biGn trV%(c th(tinh chXnh ph hMp v5i %iu ki9n mi tr17ng.

V5i ph12ng n sZdDng chG%:kha c.a transistor nh1trn th VDK Master c nhi9mvDl phCi n trnh v;t cCn trn %17ng %i.

Nh1v;y khi robot gOp v;t cCn chng ta sP%i&u khi(n robot xoay 1 gc isau % t?nhtiGn t5i %&trnh khQi v;tcCn


33/52


pg. 33

S.1>m/ch nguyn l thTc hiFn chdc neng trn c?a robot


34/52


35/52


pg. 35

S.1>khGi ho/t 1+ng c?a VDK Master


36/52


pg. 36

Ph2.ng n 2:'i8u khi;n Robot omni trn 12ang 1i n trnh vIt c=n 1>ng thaixc 1Unh chnh xc kho=ng cch cc vIt c=n 1Wn n cfng nh21Unh vUtr c?aRobot trong mgt phhng

`Ph2.ng n ny 1i hii chng ta ph=i xc 1Unh chnh xc kho=ng cch tjvItc=n 1Wn robot dTa vo tn hiFu c?a c=m biWn h>ng ngo/i do 1 chng ta ph=i s4

d6ng b+chuy;n 1ki ADC ( Analog to Digital Converter) v tn hiFu ra c?a h>ngngo/i l tn hiFu biWn thin lin t6c .

V c 3 c=m biWn h>ng ngo/i c[n 1Sc nn Z1y ta s4d6ng IC ADC 809 vDi ngvo l 8 knh lTa chSn cho php 1Sc 123c 8 tn hiFu ngoi.


37/52


pg. 37

GiDi thiFu v8ADC 0809 :

ADC 0809 l b:chuy(n %Ti AD 8bit ,

%,u ra dEli9u ra bao g!m 8 bit v c 8knh %,u vo .

Cc chn IN0 IN 7 l chn %,u vo

Cc chn D0 D7 l 8bit %,u ra.

Chn REF + v REF- l chn cIp %i9n ptham chiGu , thng th17ng ta cho REF -=0V

Chn CLK l chn cIp xung clock tSbnngoi gip cho ADC hoJt %:ng . xung clockny Cnh h1Vng %Gn t*c %:c chuy(n dTinh1ng khng %1Mc nhanh h2n 100ms

Chn ALE dng %(ch*t %?a chX%,u vo

Chn OEC dng %(bo cho biGt qu trnhchuy(n %Ti kGt thc .

Chn OE %1Mc dng %(kch hoJt %3c dE

li9u ng ra.

Chn START dng %(kch hoJt qu trnhchuy(n %Ti

Cc chn A0 , A1 v A2 dng %(ch3n knh ng vo .

Ch3n knh t12ng t


38/52


pg. 38

IN7 1 1 1

Hng d6ng :

^%y ta sZdDng s2%!nguyn l sau

3 ng ra c.a cCm biGn h!ng ngoJi ta n*i %Gn chn IN0 , IN1 v IN2 .

Ng ra D0-D7 n*i vo Port 0 c.a VDK Slave .


39/52


pg. 39

Ta sZdDng PORT 2 c.a VDK Slave %(%i&u khi(n ADC 0809

PORT 3 v PORT 1 c.a VDK Slave dng %(xuIt tn hi9u cho VDK Master

^%y ta mu*n %:phn giCi %Jt %1Mc l 10mv th ta cho %i9n p tham chiGu REF- l 0V

v REF + l 2.5 6V . ( %:phn giCi = 2.56V/256 )

S2%!kh*i vi9c %3c gi tr?CCm biGn tSADC :

S2%!kh*i hoJt %:ng c.a VDK Slave


40/52


pg. 40


41/52


pg. 41

S.1>khGi ho/t 1+ng c?a VDK Master

KWt LuIn :

- C=2 ph2.ng n trn 18u c th;thTc hiFn 123c tuy nhin ph2.ng n 2 ctnh tGi 2u cao h.n.

- C=2 ph2.ng n 18u c nhJng 2u 1i;m :o Tnh chnh xc kh caoo Cng su@t th@p ph h3p vDi nhJng dng d6ng nhi

- Trong qu trnh ho/t 1+ng c=2 ph2.ng n c nhJng h/n chW:o C=2 VDK Master va Slave sLc th;ho/t 1+ng khng 1>ng b+, gy

nn sTchIm trltrong hFthGng

o Cc nhilu c?a mi tr2ang xung quanh tc 1+ng ln h>ng ngo/i sL

lm sai lch tn hiFu vo dmn 1Wn robot sLkhng di chuy;n nh2muGn

o HFthGng ho/t 1+ng chIm do tGc 1+quay c?a Servo th@p


42/52


pg. 42

PHjLjC:

Ch12ng trnh %i&u khi(n robot theo ph12ng n 1 viGt bKng ngn ngEASM

#y ta s$d'ng cc thanh ghi

- R0 v R1 #*cho bi.t timer 0 v timer 1 #ang #%0c s$d'ng #2ng c1servono. Vd : R0=1 ngh4a l timer0 #ang #%0c s$d'ng #*#i7u khi*n #2ng c1

servo 1 , R1=2 ngh4a l timer1 #ang #%0c s$d'ng #*#i7u khi*n #2ng c1

Servo 2

- R2 v R3 cho bi.t ng ra c8a Servo timer0 v timer 1 #ang #%0c s$d'ng#*#i7u khi*n #ang 9m:c tch c


43/52


pg. 43

;-------------------TESTING----------------------------------------------

MOV R6,#0FFH

ACALL TINHTIEN_12 ;Tinh tien bang 2 banh 1 2

CJNE R6,#00,$

MOV R6,#0FFH

ACALL LUI_12 ;Lui bang 2 banh 1 2CJNE R6,#00,$

MOV R6,#0FFH

ACALL TINHTIEN_23 ; Tinh tien bang 2 banh 2 3

CJNE R6,#00,$

MOV R6,#0FFH

ACALL LUI_23 ;Lui bang 2 banh 2 3

CJNE R6,#00,$

MOV R6,#0FFH

ACALL TINHTIEN_13 ;Tinh tien bang 2 banh 1 3

CJNE R6,#00,$

MOV R6,#0FFH

ACALL LUI_13 ;Lui bang 2 banh 1 3CJNE R6,#00,$

MOV R6,#0FFH

ACALL XOAY_THUAN ;Quay cng chi7u kim dong ho

CJNE R6,#00,$

MOV R6,#0FFH

ACALL XOAY_NGHICH ;Quay ng%0c chi7u kim #Mng hM

CJNE R6,#00,$

;---------------------- GO by WHELL 1 2--------------

TT12: ACALL TINHTIEN_12

HERE1: MOV R7,P0

CJNE R7,#04,HERE1 ;CO VAT CAN DANG TRUOC

ACALL NE12_1 ; LUI LAI VA NEMOV R7,P0

CJNE R7,#04,TT12 ; SAU KHI NE MA VAN CON BI CAN

ACALL NE12_2 ; XOAY NGUOC LAI 2 ALPHA

MOV R7,P0

CJNE R7,#04,TT12

LJMP TT23 ; DOI CHE DO DI CHUYEN

;--------------------------------

NE12_1: ACALL SHORT_BACK12

ACALL CCW_ALPHA

RET


ACALL CW_2ALPHA

RET

;---------------------------

SHORT_BACK12: ACALL INIT

MOV R6,#50H

ACALL LUI_12

CJNE R6,#00,$

RET


44/52


pg. 44

;---------------------- GO by W 2 3--------------

TT23: ACALL TINHTIEN_23

HERE2: MOV R7,P0


ACALL NE23_1 ; LUI LAI VA NE

MOV R7,P0CJNE R7,#01,TT23 ; SAU KHI NE MA VAN CON BI CAN

ACALL NE23_2 ; XOAY NGUOC LAI 180

MOV R7,P0

CJNE R7,#01,TT23 ; VAN CON VATCAN


;--------------------------------


ACALL CCW_ALPHA

RET


ACALL CW_2ALPHA

RET;---------------------------


MOV R6,#50H

ACALL LUI_23

CJNE R6,#00,$

RET

;---------------------- GO by W 1 3--------------

TT13: ACALL LUI_13

HERE3: MOV R7,P0


ACALL NE13_1 ; LUI LAI VA NE

MOV R7,P0CJNE R7,#02,TT13 ; SAU KHI NE MA VAN CON BI CAN

ACALL NE13_2 ; XOAY NGUOC LAI 2ALPHA

MOV R7,P0

CJNE R7,#02,TT13


;--------------------------------


ACALL CCW_ALPHA

RET


ACALL CW_2ALPHA

RET

;---------------------------


MOV R6,#50H

ACALL TINHTIEN_13

CJNE R6,#00,$

RET


45/52


pg. 45

CCW_ALPHA: ACALL INIT ;XOAY 1 GOC ALPHA

MOV R6,#64H

MOV R7,#05

ACALL XOAY_THUAN

CJNE R6,#00,$

RETCW_2ALPHA: ACALL INIT ;XOAY NGUOC 1 GOC 2ALPHA

MOV R6,#7DH

MOV R7,#04

ACALL XOAY_NGHICH

CJNE R6,#00,$

RET

;-------------MOVEMENT TYPE--------------------------------------------------

-----

TINHTIEN_12:ACALL INIT

MOV P1,#00000011B

MOV R0,#01 ; TIMER 0 DANG DIEU KHIEN BANH 1

MOV R1,#02 ; TIMER 1 DANG DIEU KHIEN BANH 2MOV R2,#01 ; NGO RA TM0 DANG DK O MUC TICH CUC CAO

MOV R3,#01 ; NGO RA TM1 DANG DK O MUC TICH CUC CAO

MOV R4,#00 ; TIMER 0 DANG DIEU KHIEN QUAY THUAN

MOV R5,#01 ; TIMER 1 DANG DIEU KHIEN QUAY NGICH

ACALL DELAY1MS_T0

ACALL DELAY2MS_T1

RET

LUI_12: ACALL INIT

MOV P1,#00000011B

MOV R0,#01

MOV R1,#02

MOV R2,#01MOV R3,#01

MOV R4,#01

MOV R5,#00

ACALL DELAY1MS_T0

ACALL DELAY2MS_T1

RET


MOV P1,#00000110B

MOV R0,#02

MOV R1,#03

MOV R2,#01

MOV R3,#01

MOV R4,#00

MOV R5,#01

ACALL DELAY1MS_T0

ACALL DELAY2MS_T1

RET

LUI_23: ACALL INIT

MOV P1,#00000011B


46/52


pg. 46

MOV R0,#02

MOV R1,#03

MOV R2,#01

MOV R3,#01

MOV R4,#01

MOV R5,#00ACALL DELAY1MS_T0

ACALL DELAY2MS_T1

RET


MOV P1,#00000101B

MOV R0,#01

MOV R1,#03

MOV R2,#01

MOV R3,#01

MOV R4,#00

MOV R5,#01

ACALL DELAY1MS_T0ACALL DELAY2MS_T1

RET

LUI_13: ACALL INIT

MOV P1,#00000101B

MOV R0,#01

MOV R1,#03

MOV R2,#01

MOV R3,#01

MOV R4,#01

MOV R5,#00

ACALL DELAY1MS_T0

ACALL DELAY2MS_T1RET

XOAY_THUAN: MOV P1,#07H

MOV A,#01

MOV R2,#01

MOV R4,#00

ACALL DELAY1MS_T0

RET

XOAY_NGHICH:MOV P1,#07H

MOV A,#01

MOV R2,#01

MOV R4,#01

ACALL DELAY2MS_T0

RET

INIT: MOV A,#00

CLR TR0

CLR TF0

CLR TR1

CLR TF1

RET


47/52


pg. 47

;---------------TIMER INTERRUPT----------------------------------------------

--------------

TIMER0_INT: CLR TF0

JNZ ROTATE_CK ; NHAY NEU DANG XOAY

CJNE R0,#01,BANH_0_23 ; TIMER 0 DANG DIEU KHIEN BANH 1 ?

CJNE R2,#00,LOW_0_1 ; DANG DK BANH 1 , TICH CUC THAP ?SETB P1.0 ; DANG O TC THAP THI SET HIGH

LJMP REVERT_CK0 ; NHAY DEN KIEM TRA CHE DO CHAY THUAN HAY

NGHICH

LOW_0_1: CLR P1.0 ; DANG O TC THAP THI SET LOW

LJMP REVERT_CK0 ; NHAY DEN KIEM TRA CHE DO CHAY

BANH_0_23: CJNE R0,#02,BANH_0_3 ; TIMER 0 DANG DIEU KHIEN BANH 2 ?

CJNE R2,#00,LOW_0_2 ; DANG DK BANH 2 VA CO O TICH CUC THAP ?

SETB P1.1 ; DANG O TC THAP THI SET HIGH


LOW_0_2: CLR P1.1 ; DANG O TC CAO THI SET lOW


BANH_0_3: CJNE R2,#00,LOW_0_3 ; DANG DK BANH 3 VA CO TICH CUC THAP ?SETB P1.2 ; DANG O TICH CUC THAP THI SET HIGH

LJMP REVERT_CK0 ; NHAY DEN KIEM

LOW_0_3: CLR P1.2 ; DANG O TICH CUC THAP THI SET LOW

REVERT_CK0: CJNE R4,#00,REVERT_0 ; KIEM TRA CO PHAI DANG O CHIEU THUAN

CJNE R2,#01,HIGH_NRV_0 ; NEU DANG O CHIEU THUAN THI O MUC HIGH ?

MOV R2,#00 ; DANG O CHIEU THUAN O MUC 2 THI R2=0

ACALL DELAY19MS_T0 ; GOI CT DELAY19MS CUA CHIEU THUAN

LJMP EXIT_T0 ; THOAT CT NGAT T0

HIGH_NRV_0:MOV R2,#01 ; DANG O CHIEU THUAN VA TICH CUC THAP THI

CHO R2=1

ACALL DELAY1MS_T0 ; GOI CT DELAY 1MS CUA CHIEU THUAN

LJMP EXIT_T0 ; THOAT CT NGAT T0REVERT_0: CJNE R2,#01,HIGH_RV_0 ; DANG O CHIEU THUAN VA TICH CUC CAO ?

MOV R2,#00 ; DANG TICH CUC CAO THI XUONG THAP

ACALL DELAY18MS_T0 ; GOI CT DELAY18MS

LJMP EXIT_T0 ; THOAT CT NGAT ?

HIGH_RV_0: MOV R2,#01 ; DANG O CHIEU NGHICH VA TICH CUC THAP

THI SET HIGH

ACALL DELAY2MS_T0 ; DELAY 2MS

LJMP EXIT_T0 ; THOAT NGAT TIMER0

ROTATE_CK: CJNE R4,#00,RT_CCW ;KIEM TRA DANG XOAY THEO CHIEU NAO

CJNE R2,#01,RT_CW_LOW ;KIEM TRA DANG O MUC LOW HAY HIGH

MOV P1,#00 ;NEU DANG O MUC HIGH THI SET LOW

MOV R2,#00

ACALL DELAY19MS_T0

LJMP EXIT_T0

RT_CW_LOW: MOV P1,#07H

MOV R2,#01

ACALL DELAY1MS_T0

LJMP EXIT_T0

RT_CCW: CJNE R2,#01,RT_CCW_LOW


48/52


pg. 48

MOV P1,#00

MOV R2,#00

ACALL DELAY18MS_T0

LJMP EXIT_T0

RT_CCW_LOW: MOV P1,#07H


EXIT_T0: RETI

;--------------TIMER 0 INTERRUPT---------------------------------------------

---------------

TIMER1_INT: CLR TF1

CJNE R1,#01,BANH_1_23

CJNE R3,#00,LOW_1_1

SETB P1.0

LJMP REVERT_CK1

LOW_1_1: CLR P1.0

LJMP REVERT_CK1

BANH_1_23: CJNE R1,#02,BANH_1_3CJNE R3,#00,LOW_1_2

SETB P1.1

LJMP REVERT_CK1

LOW_1_2: CLR P1.1

LJMP REVERT_CK1

BANH_1_3: CJNE R3,#00,LOW_1_3

SETB P1.2

LJMP REVERT_CK1

LOW_1_3: CLR P1.2

REVERT_CK1: CJNE R5,#00,REVERT_1

CJNE R3,#01,HIGH_NRV_1


LJMP EXIT_T1

HIGH_NRV_1:MOV R3,#01

ACALL DELAY1MS_T1

LJMP EXIT_T1

REVERT_1: CJNE R3,#01,HIGH_RV_1

MOV R3,#00

ACALL DELAY18MS_T1

LJMP EXIT_T1

HIGH_RV_1: MOV R3,#01

ACALL DELAY2MS_T1

EXIT_T1: RETI

; CAC CHUONG TRINH DELAY DAT O DAY-------------------------------------------

-----

DELAY1MS_T0: MOV TL0,#50H

MOV TH0,#0FBH

SETB TR0

RET

DELAY19MS_T0: MOV TL0,#0ADH


49/52


pg. 49

MOV TH0,#0BCH

SETB TR0

RET


MOV TH1,#0FBH

SETB TR1RET

DELAY19MS_T1: MOV TL1,#0ADH

MOV TH1,#0BCH

SETB TR1

RET

DELAY2MS_T0: MOV TL0,#0CEH

MOV TH0,#0F8H

SETB TR0

RET


MOV TH0,#0BFHSETB TR0

RET

DELAY2MS_T1: MOV TL1,#0CEH

MOV TH1,#0F8H

SETB TR1

RET


MOV TH1,#0BFH

SETB TR1

RET

END

/--------------------------------------------------------------------------------------/

;Ch12ng Trnh cho VK SlaveORG 0000HMAIN: MOV P2,#0FFH ;CHON PORT 2 LA NGO VAO

LOOP: MOV P1,#00MOV A,P2 ;3c gi tr?Port 2CPL A ;Co BitMOV P1,A ;XuIt ra Port 1JNZ OUTPUT ; NGu c tn hi9u cCn th nhCy %Gn bo ngUt cho VK MasterSETB P3.2

SJMP LOOPOUTPUT: CLR P3.2DELAY: MOV R1,#0FFH

DJNZ R1,$SJMP LOOP


50/52


pg. 50

Ti li9u tham khCo :

- Vi %i&u khi(n 8051- T*ng VWn On

- Raul Rojas,Holonomic control of a robot with an omni directional drive,

Kunstliche Intelligenz, 2006

- Jeremy Wiemer, Omni-Directional Control, Eli McAngus, 2004.

- M2t s#ti li0u tham khHo tUtrang web www.acroname.com

- Cc ngu!n t1li9u trn cc website :o www.dientuvietnam.neto www.picvietnam.como www.Datasheet4u.como Cng m:t s*web site khc
http://www.acroname.com/http://www.dientuvietnam.net/http://www.picvietnam.com/http://www.datasheet4u.com/http://www.datasheet4u.com/http://www.picvietnam.com/http://www.dientuvietnam.net/http://www.acroname.com/


51/52


pg. 51

MUC LUC

NHI EM V UCUA ETA I ....... ........ ........ ........ ........ ........ ....... ........ ........ ........ ........ ........ ... 1

L I CA M N ....... ........ ........ ........ ........ ........ ........ ........ ....... ........ ........ ........ ........ ........ ..... 4

TONG QUAN ................................................................................................................... 5

Phan 1 L Y THUYET

Chng 1 T M HIEU V EONG C RC SERV O

1.1 ong c RC Servo lag? ........ ........ ........ ........ ........ ........ ........ ........ ....... ........ ........ ....... 7

1.2ong c RC Servo c sdung ........ ........ ........ ....... ........ ........ ........ ........ ........ ........ ... 7

1.3'i8u khi;n 1+ng c.Servo .......................................................................................7

Chng 2: S LC V EHC V I IEU K HIEN 8051

2.1 Gii thieu veV DK 89C51 vas okhoi. .... .... .... .... .... .... .... .... .... .... .... .... .... .... .... .... .... ... 8

2.2 ng dung 8051 vao ieu khien ong c.. ..... ..... ... ..... ... ..... ..... ... ..... ... ..... ... ..... ..... ... ..... . 10

Chng 3 :TM HI%U V%CMM BION HQNG NGORI

3.1 Gii thieu ......... ........ ........ ........ ....... ........ ........ ........ ........ ........ ........ ........ ....... ........ .. 13

3.2 Nguy en tac hoat ong ........ ........ ........ ........ ........ ........ ........ ....... ........ ........ ........ ........ . 14

Phan 2 THC H IEN THIET KE

Ch2.ng 4: Phn Tch '+ng hSc v 1+ng lTc hSc robot

4.1 Phn tch 1+ng hSc robot Omni 3 bnh...............................................................194.2 Phn Tch'+ng lTc hSc Robot Omni 3 bnh......................................................22

4.3 Robot trong khng gian phhng ............................................................................24

Ch2.ng 5:'i8u Khi;n chuy;n 1+ng c?a Robot Omni

Ph2.ng n 1 ................................................................................................................29


52/52

A M H: ieu khi en hethong Bomon: C i en t--------------------------------------------------------------------------------------------------------------------------Ph2.ng n 2 ................................................................................................................37

K ET L UA N .................................................................................................................... 42

PHULUC...........................................................................................................................

Code ASM (i)u khi,n Robot..................................................................................................43

TA I L IEU TH A M KHA O ........ ........ ........ ........ ........ ........ ........ ....... ........ ........ ........ ........ . 49

Documents

Dieu Khien Robot Omni Tranh Vat Can