Luan Van Robot Can Bang

n tt nghip-2013

LI CAM OAN

Knh gi: Hi ng bo v n tt nghip Khoa in T - Vin Thng, Trng i Hc Dn Lp Duy Tn.

Em tn l: Nguyn Phm Cng c.

Lp: K15EVT, Khoa in T - Vin Thng, Trng i Hc Dn Lp Duy Tn.

Em xin cam oan ni dung ca n ny khng phi l bn sao chp ca bt c n hoc cng trnh c t trc. y l n do em thc hin di s hng dn trc tip ca TS. H c Bnh. Mi tham kho dng trong n u c trch dn r rng tn tcgi, thi gian, a im cng b. Mi sao chp khng hp l, vi phm quy ch o to, hay gian tr, em xin chu hon ton trch nhim.

Nng, ngy 9 thng 12 nm 2013

Sinh vin thc hin

Nguyn Phm Cng c

LI CM N

Qua n ny em xin chn thnh cm n thy gio hng dn TS. H c Bnh tn tnh gip em thc hin n, ng thi em cng xin cm n Thy TS. ng Vit Hng v cc thy c trong khoa in t - Vin thng lun to iu kin thun li cho em hc tp thc hin n ny mt cch tt nht.MC LC

LI CAM OANLI CM NMC LCDANH MC CC HNH VDANH MC CC BNG BIUDANH MC CC T VIT TT1LI M U

3T VN

CHNG 1: 8L THUYT TIP CN

81.1 Tng quan v Robot cn bng.

91.2 Th no l xe 2 bnh t cn bng

111.3 Phn tch lc

121.4 Thut ton iu khin PID.

211.5. Lc nhiu tn hiu t cm bin

CHNG 2: 23THIT K PHN CNG

232.1 Thit k c kh.

242.2 Mch in t

242.2.1 Ngun in

252.2.2 Mch driver iu khin ng c

252.2.2.1 iu khin ng c vi IC L6203

262.2.2.2 Kt ni chn cho IC L6203

272.2.2.3 Mch nguyn l iu khin ng c

272.2.2.4 iu khin tc .

282.2.3 Cm bin gia tc gc ADXL335

2.2.4 B x l trung tm Kit LM3S1968312.2.4.1 Tnh nng ca vi iu khin LM3S1968312.2.4.2 Mch iu khin trung tm35

372.2.5 Hnh chp cc mch in t

CHNG 3: 43LP TRNH V TH NGHIM

433.1 S khi iu khin chnh.

443.2.S khi iu khin bng thut ton PID

463.3 S khi Read ADC.

463.3.1 S khi c d liu t ADXL33

463.3.2 Cu hnh hot ng ADC0

493.4 S khi iu khin ng c

523.5 Qu trnh thc nghim

543.6. Kt qu th nghim

543.6.1. Dng sng o c ca cm bin gia tc

563.6.2. Dng sng o c ca xung PWM

583.6.3 Kt lun

59KT LUN V HNG PHT TRIN

591.Nhng kt qu t c

592.Nhng kt qu cha t c

593.Nhng vn cha gii quyt

594.Hng pht trin.

61TI LIU THAM KHO

DANH MC CC HNH V

8Hnh 1.1 Cc module c thm vo xe t cn bng

9Hnh 1.2 Cc module c thm vo xe t cn bng.

9Hnh 1.3 M t nguyn l gi thng bng.

10Hnh 1.4 M t cch bt u di chuyn.

11Hnh 1.5 Phn tch lc

Hnh 1.6 S khi thut ton PID12

14Hnh 1.7 th hm truyn p ng khi cha c Kp, Ki, Kd.

15Hnh 1.8 th hm truyn theo thi gian ng vi gi tr Kp

16Hnh 1.9 th hm truyn theo thi gian ng vi gi tr Kp, Kd.

17Hnh 1.10 th hm truyn theo thi gian ng vi gi tr Kp, Ki.

18Hnh 1.11 th hm truyn theo thi gian ng vi gi tr Kp, Ki, Kd.

18Hnh 1.12 M t v tr ca xe cn bng

22Hnh 1.13 B lc trung bnh

24Hnh 2.1 M hnh khung n gin

25Hnh 2.2 S khi ngun in cung cp ton mch

26Hnh 2.3 Mch cu H L6203

26Hnh 2.4 S chn IC L6203

27Hnh 2.5 S khi mch Driver

28Hnh 2.6 iu ch xung PWM

Hnh 2.7 Cm bin gia tc ADXL33522929Hnh 2.8 S khi ca cm bin gia tc gc ADXL335

30Hnh 2.9 Phn tch lc

30Hnh 2.10 H trc t ca cm bin

31Hnh 2.11 Gi tr gc nghing tnh theo hm arcsin ca gia tc

32Hnh 2.12 Giao din Kit Stellaris LM3S1968

35Hnh 2.13 S nguyn l mch iu khin trung tm

37Hnh 2.14 Khi iu khin trung tm s dng Kit Stellaris LM3S1968

38Hnh 2.15 Mch Driver s dng IC L6203

38Hnh 2.16 Pin Li-ion 4.2V

39Hnh 2.17 ng c DC

40Hnh 2.18 Bnh xe.

40Hnh 2.19 Cm bin gia tc ADXL335

41Hnh 2.20 Khung xe.

42Hnh 2.21 M hnh xe t cn bng hon chnh

43Hnh 3.1 S khi chnh

45Hnh 3.2 S khi iu khin bng thut ton PID

46Hnh 3.3 S khi c d liu t ADXL33.

46Hnh 3.4 S khi Cu hnh hot ng ADC0

49Hnh 3.5 S khi iu khin ng c.

55Hnh 3.6 Xung trn chn X_OUT ca cm bin gia tc khi robot v tr cn bng.

55Hnh 3.7 Xung trn chn X_OUT ca cm bin gia tc khi robot nghing v trc

56Hnh 3.8 Xung trn chn X_OUT ca cm bin gia tc khi robot nghing v sau

56Hnh 3.9 rng xung PWM khi robot v tr cn bng

57Hnh 3.10 rng xung PWM khi robot ng v pha sau

57Hnh 3.11 rng xung PWM khi robot ng v pha trc

DANH MC CC BNG BIU

13Bng 1.1. c tnh ca lut iu khin P, I v D.

26Bng 2.1 Kim sot logic cho IC L6203

54Bng 3.1 c tnh ca lut iu khin P, I v D khi th nghim.

DANH MC CC T VIT TT

PPropotionalT l

IItergralTch phn

DDerivativeo hm

PWMPulse Width Modulationiu ch rng xung

RAM Ramdom Access MemoryB nh truy xut ngu nhinROM Read-OnlyMemoryB nh trongIC Ingrated CircuitMch in tch hp

ADC Analog Digital Converter B chuyn i tn hiu tng t

sang tn hiu s

GPIO General Purpose Input OutputNg vo ra giao tip ca vi iu khinLI M U

Vi s pht trin ca khoa hc cng ngh hin nay, cc m hnh Robot ang dn thay th con ngi trong cuc sng hng ngy hay trong cc qu trnh sn xut.Vic ln tng v thit k thnh cng mt m hnh xe t cn bng trn 2 bnh ng trc cng c nhiu tc gi nghin cu v ch to thnh cng.

ti xe hai bnh t cn bng c th xem l mt cu ni kinh nghim t m hnh thng bng con lc ngc n vic nghin cu v ch to cc loi robot di chuyn bng 2 bnh trong tng lai. Mc tiu ca ti l thit k v ch to mt xe hai bnh t cn bng. ti ny c quan tm t vic tnh ton cc thng s u vo v ra, da trn xy dng cc m phng, n vic thit k m hnh, thc hin phn in t v iu khin, vit cc chng trnh iu khin vi mc ch cui cng l to ra mt m hnh xe di chuyn cn bng trn hai bnh xe ng trc c lp trn hai ng c da theo cc nh lut c hc Newton v lc qun tnh.

M hnh l mt chic xe c hai bnh c t dc trc vi nhau (khc vi xe p l trc ca hai bnh xe song song). Trn m hnh s dng cc cm bin o gia tc gc ca thn xe. Nh cm bin ny, xe s c th t gi thng bng v di chuyn.

Trong h thng cm bin, loi tr cc tn hiu nhiu t h thng v nhiu t tn hiu o, sai s ca ng ra, ng thi c th c lng chnh xc gi tr o trong tng lai ca cm bin cng nh kt hp cc tn hiu, b lc tnh trung bnh c s dng nhm cho mt kt qu tt hn v gia tc gc ca xe. T cc tn hiu o, thng qua mt s i lng c trng ca m hnh (khi lng, chiu di, chiu cao vt, ng knh bnh) ta s tnh c momen qun tnh nghing (lt ca m hnh), t a ra cc gi tr iu khin ph hp cho cc bnh xe gi cho m hnh lun ng vng hoc di chuyn vi mt vn tc n nh.

Ton b m hnh c iu khin bng mt vi iu khin LM3S1968. y l th h tng i cao cp ca h Cortex-M3 based Microcontroller c th x l v thc thi chng trnh tc cao (t n 50MHz) trong vic tnh ton cc gi tr cm bin v a ra b truyn ng (ng c in). B vi iu khin ng vai tr th nht trong ti nh mt b lc trung bnh vi tn hiu vo t gyro. Vi cc d liu v gc x l v tn hiu to xung cho ng c DC, vai tr th hai ca vi iu khin trong ti s tnh ton da vo thut ton PID v a ra tn hiu iu khin b truyn ng, n bnh xe gi thng bng/di chuyn.

T VN

+Mc tiu v phm vi nghin cu

Mc tiu ca ti l xy dng xe hai bnh cn bng di chuyn trn a hnh phng. Kh nng di chuyn cn bng trn hai bnh lm phng tin di chuyn hiu qu v linh ng hn, d dng xoay tr trong iu kin khng gian cht hp. Trong khun kh 8 tun thc hin lun vn tt nghip i hc, nhng mc tiu ca ti c ra nh sau:

Tm hiu v cc loi xe t cn bng, nguyn l c bn v cn bng.

Tnh ton cc tham s khi phn tch lc ca m hnh.

Thit k bn v, ch to m hnh theo kch thc thc.

Thit k mch iu khin trung tm, lm nhim v x l tn hiu o v a ra cc quyt nh iu khin.

Thit k mch in t kt hp cc cm bin thc hin chc nng o gia tc gc.

Thit k mch iu khin cng sut cho hai ng c DC.

Kt hp gii thut PID cho vi iu khin kt hp v b tr cm bin c c gi tr gia tc gc chnh xc.

Xy dng thut ton iu khin cho ng c, gi thng bng v ngn nga qu ti ca cc bnh xe.

Lp trnh iu khin.

+Phng php nghin cu

Tham kho m hnh l thuyt gm c:

Tip cn t m hnh con lc ngc n m hnh tht ca ti.

M phng m hnh bng MatLAB t m hnh con lc ngc.

Tip cn m hnh thc, gm c:

Thit k khung sn c kh ca m hnh.

Mch iu khin bnh xe.

Mch cm bin gia tc gc.

B iu khin trung tm.

Lp trnh vi iu khin.

M hnh bao gm mt thn mang hai ng c DC, bo mch s dng b iu khin trung tm ca Kit LM3S1968 ca hng TI iu khin mch driver cho nhng ng c, iu khin nhng cm bin cn thit o cc gi tr gc. Cc tn hiu o gia tc gc t cm bin gyro c thng qua mt b lc tnh trung bnh c lp trnh trn vi iu khin LM3S1968 c cc thng s o gc chnh xc. Bng OLED trn Kit hin th cc thng s hot ng ca xe. Pin c lp 2 bn thn xe bng nha cung cp ton b nng lng cho xe hot ng.

+ ngha khoa hc v thc tin

- ngha khoa hc

Xy dng c mt phng tin vn chuyn mi trong khu vc cht hp. Lm phng tin vn chuyn hng ho n nhng ni c lp trnh sn trong cc ta nh, phng lm vic, nhng khng gian cht hp, kh xoay tr. Thm ch kt hp trn cc humanoid robot, nu c kt hp vi cc robot camera, robot d ng, robot li mt ng th hiu qu cc cng dng c th cc k linh hot. Tuy vy, cn phi tin hnh gii quyt thm v phn ln, xung dc cu thang (khng th leo ln cc bc thang cao).

-Tnh hnh nghin cu trong v ngoi nc

Hin nay cha c thng tin c th no v vic ch to xe hai bnh t cn bng dng trn robot cng nh xe hai bnh t cn bng Vit Nam. Nhng trn th gii, mt vi nc, cc k thut vin v mt s sinh vin nghin cu v cho ra i cc dng xe hai bnh nh th. Di y l mt s thng tin v chng.

+ nBot[16]

nBot do David P. Anderson sng ch. nBot c ly tng cn bng nh sau: cc bnh xe s phi chy xe theo hng m phn trn robot sp ng. Nu bnh xe c th c li theo cch ng vng theo trng tm robot, robot s vn c gi cn bng. Trong thc t, iu ny i hi hai cm bin thng tin phn hi: cm bin gc nghing o gc nghing ca robot vi trng lc, v encoder trn bnh xe o v tr c bn ca robot. Bn thng s ng vo xc nh hot ng v v tr ca xe con lc ngc cn bng l:

1) gc nghing.

2) o hm ca gc nghing, vn tc gc.

3) v tr bnh xe.

4) o hm v tr bnh, vn tc bnh xe.

Bn gi tr o lng c cng li v phn hi ti in p ng c, tng ng vi momen quay, cn bng, v b phn li robot.

+ Balance bot I

Balance-bot I (do Sanghyuk, Hn Quc thc hin) l mt robot hai bnh t cn bng bng cch kim sot thng tin phn hi. H thng cao 50cm. Khung chnh c lm bng nhm. N c hai trc bnh xe ni vi hp gim tc v ng c DC cho s pht ng. Tng cng c ba b vi x l Atmel c s dng. Vi iu khin chnh (master) thi hnh nhng nguyn l kim sot v thut ton c lng. Mt vi iu khin khc kim sot tt c cm bin analog. Vi iu khin th ba iu khin ng c DC. Linear quadratic regulator (LQR) c thit k v thc thi mch iu khin. N c bn gi tr khc nhau gc nghing, vn tc gc nghing, gc quay bnh xe, v vn tc gc quay, sau n to lnh cho ng c DC iu chnh tc bnh xe.

+BaliBot

Balibot, mt robot hai bnh cn bng, l mt trong cc mu u tin v robot hai bnh c trng tm pha trn cc bnh xe. Khng c h thng iu khin hot ng, robot s b ng. Khi robot c nhn bit hng m n sp ng, cc bnh xe s di chuyn v ph ng v thng gc vi chnh n.

Cm bin gc nghing o gc nghing ca robot, gia tc k Motorola MMA2260 c s dng, thit b c cu trc MEMS.

PIC16F7876 ca hng Microchip c chn lm trung tm iu khin cho robot.PIC tch hp mt b bin i A/D nhiu knh o cm bin gc nghing v cc ng I/O kim sot hai servo c m t cho s quay vng tip theo. in c cung cp bng 4 cc pin AA v c n p dropout. Ngun in 6V khng qua n p c phn phi n ng c servo qua t in 3300F qua b nng lng cho vi mch iu khin khi cng sut ng ra t cc servo c hot ng. Mch in t c xy dng trn bng project board Radio Shack RS 276-150 v lp rp ph trn cc motor servo, trn khung bng nhm. Ngun in c t gn nh v hoat ng nh trng lng ca con lc ngc. Mt phin bn khc ca BaliBot s dng cc cm bin hng ngoi o khong cch thay v dng cc cm bin o gc.

+ Equibot

Equibot l robot cn bng do Dan Piponi thc hin. C bn n da vo vi iu khin ATMega32 RISC. C hai servo Hitec HS-311 chun c sa i cho xoay vng 360o v ngun in vo c ni trc tip vi cc ng c PWM kim sot chng. Mt trong hai servo c gn vi b iu khin t cc LQR, l phn phc tp nht trong cu trc robot, bnh cn li bt chc tc ca bnh th nht. Equibot ch c mt loi cm bin hng ngoi Sharp thay cho cm bin v gc. N c t thp o khong cch vi sn. Ng ra t thit b c dng xc nh hng robot di chuyn.

-Tnh hnh thc t:

Hin ti, trong iu kin ng x giao thng ngy cng cht hp, khng kh ngy cng nhim, vic nghin cu v ch to mt m hnh xe in gn nh, d xoay x, khng tn nhin liu t l mt nhu cu thc s cn thit.

V kha cnh khoa hc v cng ngh, m hnh xe 2 bnh t cn bng thc s l mt bc m quan trng c kinh nghim trong vic tnh ton m hnh v ch to cc robot hai chn.Ngoi ra m hnh cn l s b sung cn thit v cc gii php cng ngh di chuyn ca cc robot di ng 3 bnh, 4 bnhlm phong ph nhng la chn gii php chuyn dng trong khng gian cho cc robot.

V yu t tm l con ngi, m hnh xe hai bnh t cn bng thc s l mt du chm hi ln cho nhng ngi cha tng thy hay dng n: ti sao xe c th di chuyn v thng bng c? iu ny cun ht nhu cu c s dng mt chic xe hai bnh t cn bng.

Vi nhng l do khch quan nh nu, ti c l c mt nhu cu nht nh trong tnh hnh hin nay ca Vit Nam cng nh ton th gii.

CHNG 1

L THUYT TIP CN 1.1 Tng quan v Robot cn bng.

Hnh 1.1 Cc module c thm vo xe t cn bng

Khi cm bin

B phn khng th thiu ca xe cn bng l khi cm bin. Khi ny c chc nng nhn bit (phn bit) u l gc ng ca thn robot khi di chuyn. Thng thng l nhng loi cm bin gia tc, cm bin vn tc gc v cm bin gc.

B iu khin

B iu khin l trung tm x l ca h thng xe. B iu khin c nhim v nhn tn hiu t khi cm bin v pht tn hiu iu khin 2 ng c bnh xe di chuyn xe ng theo thit k.

Khi cng sut

Ni ng hn y l khi driver. Driver l mt mch in c kh nng nhn xung iu khin (thng l xung PWM) t b iu khin v xut ra in p iu khin tc 2 ng c bnh xe. Driver l khi rt quan trng ca h thng, bi n l mt trong nhng yu t quan trng nh hng ti chnh xc ca vic iu khin.Khi ngun

ng nguyn tc iu khin, h thng s c 2 ngun ring bit gi l ngun iu khin v ngun cng sut. Ngun iu khin l ngun cp in cho b iu khin, khi cm bin thng thng c mc in p khong 5V. Trong khi ngun cng sut l ngun cp cho Driver, cc ng c, in p ca ngun ny ph thuc vo in p ca ng c hot ng. Thit k mt b ngun tt cng l mt thnh cng trong vic lm xe t cn bng.

Thit b ngoi vi

Ty thuc vo c th cng vic thc hin m trn xe c trang b nhiu thit b ngoi vi khc nhau nh cc module giao tip, camera, c cu nng (dng trong vic sp xp hng ha nh my, cng bin, nh hng)

Cc module thng thng c dng trong xe t cn bng c th xem hnh sau:

Hnh 1.2 Cc module c thm vo xe t cn bng.

1.2 Th no l xe 2 bnh t cn bng

Cn bng B nghing

Hnh 1.3 M t nguyn l gi thng bng.

i vi cc xe ba hay bn bnh, vic thng bng v n nh ca chng l nh trng tm ca chng nm trong b mt chn do cc bnh xe to ra. i vi cc xe 2 bnh c cu trc nh xe p, vic thng bng khi khng di chuyn l hon ton khng th, v vic thng bng ca xe da trn tnh cht con quay hi chuyn hai bnh xe khi ang quay. Cn i vi xe hai bnh t cn bng, l loi xe ch c hai bnh vi trc ca hai bnh xe trng nhau, cho xe cn bng, trng tm ca xe (bao gm cc vt t trn xe) cn c gi nm ngay gia cc bnh xe. iu ny ging nh ta gi mt cy gy dng thng ng cn bng trong lng bn tay.

Thc ra, trng tm ca ton b xe khng c bit nm v tr no, cng khng c cch no tm ra n, v c th khng c kh nng di chuyn bnh xe nhanh gi n lun di ton b trng tm.

V mt k thut, gc gia sn xe v chiu trng lc c th bit c. Do vy, thay v tm cch xc nh trng tm nm gia cc bnh xe, xe cn c gi thng ng, vung gc vi sn xe (gc cn bng khi y l zero).

Hnh 1.4 M t cch bt u di chuyn.

Khi robot ng cn bng th gc nghing ca thn robot v trc thng ng (trc gia tc trng trng) bng 0.

Khi robot nghing v pha trc > 0, nu khng c iu khin th theo qun tnh, hai bnh xe s chy v pha sau (pha ngc li ca thn robot ang nghing), dn n robot s b ng. Nn trong trng hp ny, chng ta s iu khin cho 2 bnh xe chy v pha trc (pha m robot ang nghing ) nhm cho gc lch = 0, robot s thng bng tr li. Ngc li khi robot nghing v pha sau < 0, th chng ta iu khin cho 2 bnh xe chy v pha trc.

1.3 Phn tch lc

H thng xe hai bnh t cn bng c c trng bi cc bin gia tc trng trng . Gc lch ca xe so vi mt phng nm ngang l gc . Gia tc do bnh xe gy ra c chiu ngc vi bnh xe. Gia tc g1 tng hp t gia tc gia tc trng trng v gia tc a ca bnh xe c iu khin.

Hnh 1.5 Phn tch lc

Khi cha c tc ng ca gia tc a, xe ch chu s tc ng ca gia tc trng trng g = 9.8 m/s2:

gx = g* cos () ;

gy = g* sin () ;

Tc ng thm gia tc a:

ax = a* sin () ;

ax = a* cos () ;

g1x = gx + ax ;

g1 y = gy + ay;

g1 =

1.4 Thut ton iu khin PID.

PID l cch vit tc ca cc t Propotional (t l), Integral (tch phn) v Derivative (o hm). Tuy xut hin rt lu nhng n nay PID vn l gii thut iu khin c dng nhiu nht trong cc ng dng iu khin t ng.

Hnh 1.6 S khi thut ton PID

Hm truyn ca phng php iu khin PID c dng nh sau:

U = Kp*e + Ki*edt +Kd* Kp = H s t l

Ki = H s tch phn

Kd = H s vi phn

Trc tin, ta tm hiu xem b iu khin PID lm vic nh th no, ta ly s h thng trn. Bin e th hin cho sai s gia u vo mong mun Setpoint v u ra Output. Tn hiu lch ny s c gi n b iu khin PID v b iu khin PID s tnh c tch phn v vi phn ca sai s. Tn hiu iu khin U c gi tr bng Kp ln bin ca sai s cng vi Ki ln tch phn ca sai s cng vi Kd ln vi phn ca sai s.

U = Kp*e + Ki*edt +Kd*

Tn hiu U s c gi ti c cu chp hnh, v cho ra u ra Output. u ra Output s c gi n sensor o v ta li c gi tr sai s e mi. B iu khin s li ly gi tr sai s mi, tnh ton tch phn, vi phn v li a ra tn hiu iu khin. Qu trnh ny s c lp i lp li.

c tnh ca lut iu khin P, I v D

Lut iu khin P c tc dng lm gim thi gian qu nhng khng th kh c steady-state error. Lut iu khin I c tc dng kh c steady-state error, nhng n lm gim kh nng p ng tc thi ca h thng. Lut iu khin D c tc dng tng n nh ca h thng gim overshoot, v ci thin kh nng p ng tc thi. Hiu qu ca tng lut iu khin P, I, v D ln h thng vng kn c tng kt qua bng sau:

Thng sThi gian qu vt lThi gian p ngSteady-state error

KpGimTngt thay iGim

KiGimTngTngKh c

Kdt thay iGimGimt thay i

Bng 1.1. c tnh ca lut iu khin P, I v D.Sau y l mt minh ha bng Matlab cho vic chn Kp, Ki v Kd sao cho h thng p ng c 3 tiu ch sau: Qu nhanh.

Overshoot nh nht.

Khng c steady-state error.

Trc ht ta kho st p ng bc thang n v ca h thng h. To mt m-file c cc lnh nh sau:

num=1;

den=[1 10 20];

plant=tf(num, den);

step(plant)

Chy m-file ny ta s c th nh sau:

Hnh 1.7 th hm truyn p ng khi cha c Kp, Ki, Kd.

Gi tr DC gain ca hm truyn l 1/20, do vy 0.05 l gi tr cui ca output p ng u vo bc thang n v. Nh vy steady-state error l 0.95, thc s l kh ln. Hn na, thi gian qu khong 1 giy, thi gian n nh khong 1, 5 giy. By gi nhim v ca ta l thit k b iu khin sao cho gim thi gian qu , thi gian n n nh ngn, v kh c steady-state error.

Lut iu khin P

Chng ta bit lut iu khin P lm gim thi gian qu , tng overshoot, v gim steady-state error.

Chn Kp = 300 v thm vo m-file on m lnh sau:

Kp=300;

contr=Kp;

sys_cl=feedback(contr*plant, 1);

t=0:0.01:2;

step(sys_cl, t)

Chy m-file trong MATLAB ta c kt qu nh sau.

Hnh 1.8 th hm truyn theo thi gian ng vi gi tr Kp

Ch : Hm feedback dng to mt hm truyn vng kn t mt hm truyn vng h cho trc (thay cho vic thit lp li bng tay).

T th trn ta thy rng lut iu khin P lm gim thi gian qu v steady-state error, nhng tng qu iu chnh, v lm ko di thi gian n n nh i 1 cht.

iu khin t l-vi phn

By gi chng ta xt n lut iu khin PD. Nh bng nu phn trc, ta thy rng lut iu khin D lm gim c qu iu chnh v lm h thng nhanh n n nh.

Nh trc ta vn ly Kp = 300, ly Kd = 10 v ta thm vo m-file on m lnh sau v chy cho ra th.

Kp=300;

Kd=10;

contr=tf([Kd Kp], 1);


t=0:0.01:2;

step(sys_cl, t)

Hnh 1.9 th hm truyn theo thi gian ng vi gi tr Kp, Kd.

th ny cho ta thy ngay lut iu khin D gim c qu iu chnh v lm h thng nhanh n n nh so vi trc, v tc ng khng ng k ti thi gian qu cng nh steady-state error.

iu khin t l-tch phn

Trc khi xy dng b iu khin PID, chng ta xem xt ti lut iu khin PI . Nh bng tng kt, ta thy rng lut iu khin I lm gim thi gian qu , nhng lm tng qu iu chnh v ko di thi gian n n nh ca h thng, nhng li kh c steady-state error.

By gi ta chn li Kp = 30, Ki = 70. To m-file vi cc dng lnh sau.

Kp=30;

Ki=70;

contr=tf([Kp Ki], [1 0]);


t=0:0.01:2;

step(sys_cl, t)

Chy chng trnh trong MATLAB ta c th sau:

Hnh 1.10 th hm truyn theo thi gian ng vi gi tr Kp, Ki.

L do chng ta gim Kp i l v lut iu khin I cng c tc dng lm gim thi gian qu v tng qu iu chnh nh lut iu khin P. th trn cho ta thy lut iu khin I kh c sai s xc lp.

iu khin t l-tch phn-vi phn(PID)

By gi ta xt n b iu khin PID.

Sau mt s ln chy th vi cc tham s khc nhau, ta chn c Kp=350, Ki=300, v Kd=50 h thng s t c c tnh nh mong mun.

Kp=350;

Ki=300;

Kd=50;

contr=tf([Kd Kp Ki], [1 0]);


t=0:0.01:2;

step(sys_cl, t)

Hnh 1.11 th hm truyn theo thi gian ng vi gi tr Kp, Ki, Kd.

By gi ta c mt h thng iu khin vng kn khng c qu iu chnh, qu nhanh, v khng c sai s xc lp.

p dng vo m hnh xa t cn bng

c ci nhn r hn bn cht ca gii thut PID trong vic gip xe t cn bng, Lun vn s cp mt v d iu khin tc ca mt xe trn ng thng. Gi s c mt xe 2 bnh ng trc c gn 2 ng c DC. ng c sinh ra mt lc y xe chy ti hoc lui trn mt ng thng gip xe n c v tr cn bng.

B nghing Cn bng B nghing

Hnh 1.12 M t v tr ca xe cn bng

Gi F l lc do ng c to ra iu khin xe. Ban u xe v tr b nghing nhim v t ra l iu khin lc F (mt cch t ng) y xe n ng v tr cn bng vi cc yu cu: chnh xc (accurate), nhanh (fast response), n nh (small overshot).Mt iu rt t nhin, nu v tr hin ti ca xe b nghing c gc nghing ln so vi v tr cn bng, hay ni cch khc sai s(error) ln, chng ta cn tc ng lc F ln nhanh chng a xe v v tr cn bng. Mt cch n gin cng thc ha tng ny l dng quan h tuyn tnh:

F=Kp*e

Trong :

+ Kp l mt hng s dng no m chng ta gi l h s P

+e l sai s cn iu khin tc l chnh lch gia gc nghing v v tr cn bng ca xe.

Mc tiu iu khin l a e tin v 0 cng nhanh cng tt. R rng nu Kp ln th F cng s ln v xe rt nhanh chng tin v v tr cn bng. Tuy nhin, lc F qu ln s lm cho gia tc ca xe rt nhanh (nh lut II ca Newton: F=ma). Khi xe n v tr cn bng (tc e=0), th tuy lc F=0 (v F=Kp*e=F=Kp*0) nhng do qun tnh xe vn tip tc tin v bn phi v lch v tr cn bng v bn phi, sai s e li tr nn khc 0, gi tr sai s lc ny c gi l overshot ( vt l). Lc ny, sai s e l s m, lc F li xut hin nhng vi chiu ngc li ko xe v li v tr cn bng. Nhng mt ln na, do Kp ln nn gi tr lc F cng ln v c th ko xe lch v bn tri v tr cn bng. Qu trnh c tip din, xe c mi dao ng quanh v tr cn bng. C trng hp xe dao ng cng ngy cng xa v tr cn bng. B iu khin lc ny c coi l khng n nh. Mt xut nhm gim vt ca xe l s dng mtthnh phnthng trong b iu khin. S rt l tng nu khi xe ang xa v tr cn bng, b iu khin sinh ra lc F ln nhng khi xe tin gn n v tr cn bng ththnh phnthng s gim tc xe li. Chng ta u bit khi mt vt dao ng quanh 1 im th vt c vn tc cao nht tm dao ng (quanh v tr cn bng). Ni mt cch khc, gn v tr cn bng sai s e ca xe thay i nhanh nht (cn phn bit: e thay i nhanh nht khng phi e ln nht). Mt khc, tc thay i ca e c th tnh bng o hm ca bin ny theo thi gian. Nh vy, khi xe t v tr b nghing tin v gn v tr cn bng, o hm ca sai s e tng gi tr nhng ngc chiu ca lc F (v e ang gim nhanh dn). Nu s dng o hmlm thnh phnthng th c th gim c vt l ca xe.Thnh phnthng ny chnh l thnh phn D (Derivative) trong b iu khin PID m chng ta ang kho st. Thm thnh phn D ny vo b iu khin P hin ti, chng ta thu c b iu khin PD nh sau:

F = Kp*e+Kd*

Trong :

+ (de/dt) l vn tc thay i ca sai s e.

+Kd l mt hng s khng m gi l h s D (Derivative gain).

S hin din ca thnh phn D lm gim vt ca xe, khi xe tin gn v v tr cn bng, lc F gm 2 thnh phn Kp*e > =0 (P) v Kd*(de/dt) < = 0 (D). Trong mt s trng hp thnh phn D c gi tr ln hn thnh phn P v lc F i chiu, thng xe li, vn tc ca xe v th gim mnh gn v tr cn bng. Mt vn ny sinh l nu thnh phn D qu lnso vithnh phn P hoc bn thn thnh phn P nh th khi xe tin gn v tr cn bng (cha tht s n v tr cn bng), xe c th dng hn, thnh phn D bng 0 (v sai s e khng thay i na), lc F = Kp*e. Trong khi Kp v e lc ny u nh nn lc F cng nh v c th khng thng c lc ma st tnh. Hy tng tng tnh hung bn dng sc ca mnh y mt xe ti nng vi chc tn, tuy lc y tn ti nhng xe khng th di chuyn. Nh th, xe s ng yn mi d sai s e vn cha bng 0. Sai s e trong tnh hung ny gi l steady state error (sai s trng thi tnh). trnh steady state error, ngi ta thm vo b iu khin mt thnh phn c chc nng cng dn sai s. Khi steady state error xy ra, 2 thnh phn P v D mt tc dng, thnh phn iu khin mi s cng dn sai s theo thi gian v lm tng lc F theo thi gian. n mt lc no , lc F ln thng ma st tnh v y xe tin tip v v tr cn bng. Thnh phn cng dn ny chnh l thnh phn I (Integral -tch phn) trong b iu khin PID. V chng ta iu bit, tch phn mt i lng theo thi gian chnh l tng ca i lng theo thi gian. B iu khin n thi im ny y l PID:

F= Kp*e + Ki*edt +Kd*

(ch :edt l tch phn ca bin e theo t)

Trong :

+ edt l tch phn ca bin e theo t.

+Ki l mt hng s khng m gi l h s I (Integral gain).

Nh vy, chc nng ca tng thnh phn trong b iu khin PID gi r. Ty vo mc ch v i tng iu khin m b iu khin PID c th c lt bt tr thnh b iu khin P, PI hoc PD. Cng vic chnh ca tc gi thit k b iu khin PID p dng cho robot cn bng l chn cc h s Kp, Kd v Ki sao cho b iu khin hot ng tt v n nh (qu trnh ny gi l PID gain tuning). y khng phi l vic d dng v n ph thuc vo nhiu yu t. Vy ta chn cc h s cho PID nh sau:

-Chn Kp trc: th b iu khin P vi m hnh xe cn bng tht (hoc m phng), iu chnh Kp sao cho thi gian p ng nhanh, chp nhn vt nh.

-Thm thnh phn D loi b vt l, tngKd t t, th nghim v chn gi tr thch hp. Steady state error c th s xut hin.

-Thm thnh phn I gim steady-state error. Nn tngKi t b n ln gim steady state error ng thi khng cho overshot xut hin tr li.

1.5. Lc nhiu tn hiu t cm bin

x l nhiu t cm bin, lun vn ny s dng b lc trung bnh. y l k thut lc tuyn tnh, hot ng nh 1 b lc thng thp. B lc ny c s dng lc cc tn hiu t cm bin gia tc gc c v. y lun vn s dng mt ca s lc(ma trn 1 x n) qut qua ln lt cc gi tr ca cm bin nhn c. Vy gi tr u ra ca cm bin sau khi qua b lc l gi tr trung bnh ca n gi tr lin tip m vi iu khin c v. Vic tnh ton ny kh n gin vi hai bc gm tnh tng cc thnh phn trong ca s lc v sau chia tng ny cho n phn t ca ca s lc.

Mc ch ca vic s dng b lc trung bnh ny gip cho vi iu khin nhn c nhng gi tr tr v ca cm bin c mn hn v hn ch c nhiu tn hiu.

S d hnh dung hn bng m t cc gi tr cm bin c v nh hnh di y:

Hnh 1.13 B lc trung bnh

CHNG 2

THIT K PHN CNG

2.1 Thit k c kh.

Cu trc c kh ca ti c chia lm hai phn: khung v hai ng c.

- Khung c lm bng Nha (20 cm x 30 cm x 4 cm).

- Hai ng c t trn hai bnh xe bng hp kim nhm c ng knh 10cm.

Thit k phn robot

Mt Robot ng vai tr v cng quan trng trong vic lm tng kh nng hot ng n nh ca Robot, c bit l i vi cc loi Robot cn bng. Yu cu c bn nht cho phn Robot l c vng v ng phng tng i trn a hnh di chuyn, tt nhin khng phi a hnh di chuyn no cng ging nhau v nhiu yu t nh phng, ma st trn bnh xe ( ph thuc vo kh nng tip xc ca bnh xe ch ng so vi mt sn ).

+ cng vng

Phn ny lin quan ti kh nng thit k ca tng ngi, nu c kinh nghim trong thit k c kh phn ny tng i n gin, quan trng l lm sao cho nhng phn kt ni gia trc ng c, khung vi phn thn ca xe c chc chn.

+ ng phng

y gn nh l mt trong nhng vn quan trng nht khi tin hnh lp rp 1 khung Robot, nh bn bit, nu lm mt Robot vi 3 bnh xe th chng cn phi quan tm n iu ny nhiu, v 3 bnh s lun nm trn mt mt phng, nhng nu mun ch to 1 khung ch c 2 bnh th vn t ra l lm sao cho 2 bnh phi nm trn 1 mt phng. khng ch n gin l vic canh u trn 1 tm phng cho u m ci chnh l lm sao gi c cho n lun phng, v nh bn bit a hnh khi di chuyn l rt khc nhau v th khng phi khi ch to tht phng th di chuyn trn sn cng phng.

Hnh 2.1 M hnh khung n gin

2.2 Mch in t 2.2.1 Ngun in

H thng in t c ly t ngun chnh 12V(4 Pin li-on 4.2V ni tip nhau) c a qua IC n p LM7805 chuyn thnh in p 5V cung cp cho vi iu khin, cc bng mch ni vi nhau bng cp in v kt ni vi nhng ngun in khc nhau cch ly xung nhiu, trong 2 bng mch ng vai tr chnh l b li cng sut ca mi bnh xe vi cng sut 2A, in p 12V, v mt bng mch kt hp gia cm bin gia tc gc vi kit iu khin trung tm LM3S1968.

S khi lm vic ca ton b cc thnh phn in in t ca m hnh c th c m t theo hnh di y:

Hnh 2.2 S khi ngun in cung cp ton mch

Tn hiu t cm bin gyro c thu nhn vo vi iu khin LM3S1968, c x l cng vi thut ton PID c c cc lnh iu khin tc bnh xe ph hp. Cc lnh ny c m ho thng qua mch driver, a ti b MOSFET cng sut cung cp nng lng ph hp (PWM duty cycle) cho hai ng c hot ng.

2.2.2 Mch driver iu khin ng c

2.2.2.1 iu khin ng c vi IC L6203

IC L6203 l mch iu khin cu y cho cc ng dng trong vic iu khin ng c DC th hin trong k thut MultiPower-BCD.

Bng cch s dng k thut tch hp ti u ha cc mch logic v giai on tng hp t c hiu sut tt nht c th.Cc stransistor DMOS u ra c th hot ng mc in p ln n 42V t hiu qu tc chuyn i cao. Tt c cc thng s u vo l TL, CMOS vi cc cng logic tng thch. Mi knh(na cu) ca IC c iu khin bi 1 cng logic ring bit.

Mch tch hp L6203 l mch iu khin cu y cho ng c DC, n c th iu khin ng c mc in p t 12V n 42V v c tn s hot ng hiu qu cao ln n 100KHz.

Hnh 2.3 Mch cu H L6203

2.2.2.2 Kt ni chn cho IC L6203

Hnh 2.4 S chn IC L6203

PWMI/OEnableFunction

HLHng c quay nghch

LHHng c quay thun

LLHng c dng

HHHng c dng

XXLng c dng

L=low, H=hight, X=khng xc nh

Bng 2.1 Kim sot logic cho IC L6203

2.2.2.3 Mch nguyn l iu khin ng c

Hnh 2.5 S khi mch Driver

2.2.2.4 iu khin tc .

Trong ti ny s dng 2 ng c DC gip robot chuyn ng ti, lui. Tc ca ng c c iu khin bi rng xung(PWM).

iu ch rng xung l mt k thut tng gim lng in nng cung cp cho ng c. Bi v khng phi lc no ng c cng phi vn hnh mc cng sut ti a hoc ti thiu m n phi c tuyn tnh theo mi trng thi nht nh.

iu khin 1 ng c, cn dng 3 tn hiu t vi iu khin LM3S1968. Trn Pin 11 ca IC L6203 lun t mc in p cao(5V) cho php mch lun hot ng. Trn Pin 5(PWM) ca L6203 t cc tn hiu PWM t vi iu khin vo.in p ca tn hiu s quyt nh ng c quay nhanh hay chm. Trn Pin 7(I/O) s do vi iu khin quy c l 0 hoc 1(quay ti hoc lui).

Hnh di y s minh ha cho khi nim ny, Ton v Toff ln lt l khong thi gian kch(trng thi HIGHT) v ngt tn hiu(trng thi LOW) trong mt chu k.

Hnh 2.6 iu ch xung PWM

Chu k ca mt tn hiu ra PWM l khong thi gian m sau mu tn hiu c lp li.

Tn s PWM = 1/chu k.

H s duty cyle l t l b rng ca mc T ON(mc 1) so vi b rng chu k.

2.2.3 Cm bin gia tc gc ADXL335

Cm bin gia tc gc ADXL335 l mt module nh, mng, in nng thp, a ra thng s trn 3 trc ta X, Y, Z, gi tr c v ca cm bin s l hnh chiu ca gia tc trng trng trn tng trc ta tng ng. N c th o gia tc tnh, tc l gia tc trng trng trong cc ng dng cm bin nghing, cng nh trong cc chuyn ng, sc, hoc rung ng

Ta bit rng, gia tc trng trng ti mt im l khng i (g = 9, 8 m/s2). Khi cm bin quay 1 gc no , h trc ta gn vi cm bin cng quay theo, do hnh chiu ca gia tc trng trng ln cc trc ta s thay i.

T cc gi tr , ta xc nh c gc nghing hin ti ca cm bin, cng nh gc m cm bin quay i so vi v tr trc.

Gi tr c v ca cc cm bin gia tc thng c tnh theo n v "g", g tc lgia tc trng trng(m/s2). Do kt qu tnh ton gc nghing, sau khi chia cho nhau s mt i thnh phn "g". Suy ra, kt qu o gc khng ph thuc gia tc trng trng, tc khng ph thuc vo v tr a l.

V cm bin gia tc ADXL335:

+Gi tr c v t cm bin l hnh chiu cag trn 3 trc to X, Y, Z.

+Kt qu ng ra dng tn hiu tng t, c th la chn kt qu dng 10-bit hoc 8-bit.

+Nhit hot ng: -40 ~ 85 .

+ nhy +/-3g.

+Giao tip theo chun ADC.

+Bng thng: 50Hz.

+in p hot ng: 3 ~ 5V.

+Tiu th in nng: 400uA. Hnh 2.7 Cm bin gia tc ADXL335

Hnh 2.8 S khi ca cm bin gia tc gc ADXL335

Cm bin gia tc ba chiu kiu t cho php xc nh mt cch c lp cc gia tc theo cc phng trc to X, Y v Z. Theo s khi ta thy cm bin ny c ba u trn cc knh Xout, Yout v Zout ( rng xung t l vi gia tc). Cm bin c th c s dng o c gia tc tnh v gia tc ng.

Xc nh gc nghing bng gi tr gia tc trn 1 trc:

Gc nghing c xc nh da vo gi tr gia tc tnh. Lun vn ny ch s dng trc X tnh ton gc nghing.

Gi chnh l gc nghing ca trc x so vi mt phng ngang. Ta gi trc nm ngang l (), vi chiu dng ca gc nghing l chiu kim ng h (chiu hng v gia tc trng trng g).

Hnh 2.9 Phn tch lc

Ta c: gx=g*sin x

Gi tr gia tc tnh c c t cc ng ra ca cm bin gia tc chnh l hnh chiu ca g ln mi trc tng ng. Nh vy ta s xc nh c:

x = arcsin(gx/g)

Vi nm trong khong -90 +90.

H trc ta :

Hnh 2.10 H trc t ca cm bin

Ng ra ca cc loi accelerometer l tuyn tnh dng analog, vi ADXL335 nhy danh nh mode 3g l 49 count/g, mod 10-bit d liu.

Gi tr gc tnh theo phng php ny cng chnh xc khi cng gn 0, v sai s rt ln khi cng gn 90.

Hnh 2.11 Gi tr gc nghing tnh theo hm arcsin ca gia tc

Theo th trn, ta thy, khi gi tr gc ln cn 0 dc p ng nh hn nhiu so vi khi gc theta tin ti 90. Chnh v dc ln vng gn 90 lm gi tr gc nghing c vng ny thiu chnh xc.

Vi nhy l 49 count/g. Vi gi tr gia tc c vo c l 0x01 v 0x02, tng ng l 1 v 2 trong h thp phn, ta s tnh c gc nghing chnh lch bao nhiu.

Ta c: (acc=1) = arcsin(1/49) = 1, 1o

(acc=2) = arcsin(2/49) = 2, 2o

Trong khi , vi cc gi tr gia tc rt gn vi 3g (tc l gi tr output gn vi 49) th chnh lch li rt ln.

(acc=48) = arcsin(48/49) = 87o.

(acc=49) = arcsin(49/49) = 90o.

2.2.4 B x l trung tm Kit LM3S19682.2.4.1 Tnh nng ca vi iu khin LM3S1968

Mch iu khin ng c l KIT LM3S1968 giao tip t Gyro ti IC L2603. Bng cch gi cc bit chnh hng v tn hiu PWM n IC L2603, Kit ny kim sot ng tc v hng quay ng c.

Hnh 2.12 Giao din Kit Stellaris LM3S1968

Cc tnh nng c bn ca Kit Stellaris LM3S1968:

Vi iu khin Stellaris LM3S1968 c nhiu Port xut nhp a dng: Port A, Port B, Port C, Port D, Port E, Port F, Port G, Port H.

Thit lp n gin, cp USB giao tip ni tip, g li v cung cp ngun in(5V) cho kit hot ng.

ha OLED hin th vi phn gii 128 x 96 im nh.

B pin ni 3 V v h tr cho trn m-un khi chip hot ng ch ng ng.

Dy in p cung cp: 4, 37 - 5, 25 V t kt ni USB.

42 knh ngt vi 8 cp u tin khc nhau.

256 KB cho b nh flash.

64-KB cho b nh SRAM.

Chn UARTs.

8 knh ADC chuyn i tn hiu tng t sang tn hiu s, phn gii 10-bit.

2 m-un I2C.

Port iu ch rng xung PWM.

- 1 Port 16-bit truy cp.

- 2 Port so snh.

- To ra hai tn hiu PWM c lp.

5-52 GPIO l cc chn tn hiu vo ra.

GPIO

Cc m-un GPIO gm 8 khi GPIO vt l, mi tng ng vi mt cng GPIO ring bit (Port A, Port B, Port C, Port D, Port E, Port F, Port G, Port H). GPIO m-un h tr 5-52 chn lp trnh u vo / u ra, ty thuc vo thit b ngoi vi c s dng.

Cc m-un GPIO c cc tnh nng sau y :

5-52 GPIO, ty thuc vo cu hnh.

Chu c mc in p 5V trong cu hnh u vo.

Lp trnh iu khin ngt cho GPIO:

To mt n ngt.

Edge-kch hot ln, xung hoc c hai.

Level-nhy vi gi tr cao hoc thp.

Mt n che bit trong c c v ghi cc hot ng thng qua cc dng a ch.

C th c s dng bt u mt chui mu ADC.

m 2 mA, 4 mA v 8 mA cho truyn thng k thut s, ln n bn ming m c th iu khin 18 mA cho cc ng dng cao hin ti.

iu khin tc quay cho m 8 mA.

ADC

B chuyn i tng t sang s (ADC) l mt thit b ngoi vi c th chuyn i lin tc mt in p tng t vi mt s tn hiu s ring bit.

Cc m-un ADC h tr 8 knh u vo cng vi mt b cm bin nhit bn trong. Bn b m mu tun t cho php ly mu nhanh chng ln n 8 ngun u vo tng t m khng cn can thip iu khin. Mi mu tun t cung cp chng trnh linh hot vi ngun y cu hnh u vo, kch hot s kin v to ngt. Mi module ADC c mt chc nng so snh k thut s cho php cc gi tr c chuyn i chuyn sang mt n v so snh cung cp tm b so snh mu k thut s.

Cc m-un Stellaris ADC cung cp cc tnh nng sau :

Tm knh u vo tng t.

Cm bin nhit bn trong chip.

Tc ly mu l mt triu mu / giy.

Linh hot, cu hnh chuyn i tng t sang s.

iu khin linh ng cc kch hot:

- B iu khin ( phn mm ).

- Timers.

- Analog Comparators.

- PWM.

- GPIO.

Phn cng trung bnh ln n 64 mu ci thin chnh xc.

Chuyn i s dng mt ti liu tham kho 3 -V ni b.

Ngun v t cho cc mch tng t l ring bit vi ngun v t ca cc tn hiu s.

I2C

Bus giao tip gi cc IC vi nhau (I2C) cung cp truyn d liu hai chiu thng qua hai dy thit k (mt dng d liu ni tip SDA v mt dng xung ni tip SCL). Cc giao din bus I2C n I2C thit b ngoi vi nh l b nh (RAMs v ROM), thit b mng, mn hnh LCD, my pht nhc. Bus I2C c th c s dng kim tra h thng v chun on trong pht trin sn phm v sn xut.

Mi thit b trn bus I2C c th c cu hnh Master hoc Slave. M-un I2C h tr vic gi v nhn d liu, c th l mt Master hoc Slave hay c th hot ng ng thi nh Master v Slave. C hai ch Master v Slave c th to ra ngt.

Vi iu khin LM3S1968 bao gm bn m-un I2C vi cc tnh nng sau:

Thit b trn bus I2C c th c cu hnh l Master hoc Slave:

H tr c truyn v nhn d liu nh l mt Master hoc l mt Slave.

H tr hot ng ng thi Master v Slave.

Bn cu hnh I2C:

Master ch truyn.

Master ch nhn.

Slave truyn.

Slave nhn.

Hai tc truyn dn.

Tc tiu chun (100 Kbps).

Ch nhanh (400 Kbps).

Dao ng mc thp trong thi gian ch ngt.

Master v Slave to ra ngt thi gian ch:

Master to ra ngt khi truyn hoc nhn hot ng hon thnh (hoc b hy b do mt li).

Slave to ra ngt khi d liu c chuyn giao hay theo yu cu ca mt Master hay khi iu kin START hoc STOP pht hin.

2.2.4.2 Mch iu khin trung tm

Hnh 2.13 S nguyn l mch iu khin trung tm

Vi iu khin LM3S1968 ng vai tr rt quan trng trong mch. T chn ADC0 ca vi iu khin c ni vi chn X_OUT ca cm bin nhn tn hiu tr v. T nh b chuyn i ADC c tch hp sn trong chip chuyn i tn hiu tng t sang tn hiu s.

Cc chn PWM4, PWM1 c ni vi chn IN1 trong mch Driver xut tn hiu PWM iu khin ng c.

Cc chn PWM5, U1TX c ni vi chn IN2 trong mch Driver xut tn hiu 0, 1 gip mch driver phn bit c phi dng tn hiu PWM mc cao hoc thp. Nu mc cao th ng c s chy ti ngc li ng c s chy lui.

Cc chn OUT1, OUT2 c ni 2 ng c cung cp ngun cho 2 ng c hoat ng.

Cc chn ENABLE c ni vi ngun 5V.

Ci t PWM trong vi iu khin:

i vi iu khin PWM bng phn cng t vi iu khin LM3S1968 c tch hp b PWM sn trong kit. bt u PWM th phn mm ca chip phi thc hin cc nhim v sau:

t chu k ca counter iu ch xung vung(ly xung t thch anh).

Khi to b PWM hot ng.

Quy inh chn GPIO_PIN_2 ca PORT F v GPIO_PIN_2 ca PORT D 2 chn l chn xut tn hiu PWM.

Quy inh chn GPIO_PIN_3 ca PORT F v GPIO_PIN_3 ca PORT D 2 chn iu khin quay ti hoc lui ca 2 ng c.

Cu hnh b PWM.

Np duty cycle vo b iu khin PWM.

t trng thi iu xung mc cao hay thp.(Nu cao th s iu khin ng c quay ti, ngc li quay lui).

Cho php b iu khin PWM hot ng.

c tn hiu t cm bin gia tc tr v v xut ra OLED

Khai bo su ca kch thc mng lu gi tr tr v t cm bin.

Khi to xung clock 16Mhz cho h thng.

Kch hot module ADC0.

Thit lp tc ly mu 250k trong mi giy.

Trc khi thit lp ADC sequencer.Ta cn phi disable ADC sequencer 1.

Cu hnh ADC sequencer.

Cu hnh tt c 4 steps trong sequencer ADC.Trong lun vn ny s t trung bnh tt c 4 steps sequencer lm nhim v lu gi tr ca cm bin tr v.

-Lu : steps cui cng i hi phi cu hnh c ngt module ADC hiu rng y l vic chuyn i cui cng trn sequencer 1.

Cho php module ADC hot ng.

Sau , kch hot b chuyn i ADC bng phn mm c tch hp sn trong vi iu khin.

Sau khi chuyn i hon thnh, by gi c th c gi tr ca ADC t ADC sample Sequencer 1.Hm ny c chc nng sao lu mu d liu chuyn i ADC cha trong ADC Sample Sequencer 1 vo mt b m trong b nh.

2.2.5 Hnh chp cc mch in t

Mt s hnh nh v phn cng ca xe cn bng:

Hnh 2.14 Khi iu khin trung tm s dng Kit Stellaris LM3S1968

Hnh 2.15 Mch Driver s dng IC L6203

Hnh 2.16 Pin Li-ion 4.2V

Pin ca xe c trng lng kh nng cn phi c phn b u hai bn thnh xe, khng nn lch v nh th rt c kh nng lm hai ng c hoc ng tc khc nhau.

Hnh 2.17 ng c DC

Cc b phn phi c gn cht vo khung , sc nng chia u hai bnh xe ch ng c ngha l trng tm ca phn khung nm trn ng thng i xng dc theo khung , cc bnh xe phi tip xc u vi mt sn.

Gi t ng c, pulley cn ch cc mt phng g m bo song song v vung gc, m bo cho mt phng truyn ng khng b nghing cho (s nh hng n ng lc hc robot).

Hnh 2.18 Bnh xe.

Bnh xe khi g vo ng c phi m bo s ng tm khi quay khng b o, lc ( li khuyn nn dng my tin gia cng c kh khi g ng c vo trc).

Hnh 2.19 Cm bin gia tc ADXL335

Khi t cm bin gia tc ln thn xe. Nn lu t v tr gia thn xe v lc di chuyn cm bin s t b dao ng nht. T s cho ta tn hiu tt v t b nhiu hn.

Hnh 2.20 Khung xe.

Khung xe l phn kh quan trng, phi chn cho ph hp cc yu t nh cng vng, kch thc, trng lng v quan trng y l nhng vt liu khng c sn bn phi t gia cng cho ph hp vi thit k ca mnh. Phn khung cn lu v kch thc (mt chn ), hnh dng sao cho g t cc b phn nh ng c, bnh xe, pin, cc bng mch in v cm bin c d dng.

n gin, lun vn chn khung xe l mt tm meca dy 10mm hay nha tm dy 10mm cho r tin v d tm mua.

Do khung xe c lm bng nha nn rt gin d v khi va p mnh, v vy cn phi c mt si dy c ni vo u xe ngay trung tm gi li khi xe b ng.

Hnh 2.21 M hnh xe t cn bng hon chnh

CHNG 3

LP TRNH V TH NGHIM3.1 S khi iu khin chnh.

Hnh 3.1 S khi chnh

3.2.S khi iu khin bng thut ton PID

Hnh 3.2 S khi iu khin bng thut ton PID

3.3 S khi Read ADC.3.3.1 S khi c d liu t ADXL33

Hnh 3.3 S khi c d liu t ADXL33.

3.3.2 Cu hnh hot ng ADC0

Hnh 3.4 S khi Cu hnh hot ng ADC0

Chng trnh c sensor ADXL335

#include "inc/hw_memmap.h"

#include "inc/hw_types.h"

#include "driverlib/adc.h"

#include "driverlib/gpio.h"

#include "driverlib/sysctl.h"

#include "drivers/rit128x96x4.h"

#include "stdio.h"

////Bien ADC///

long data1;

unsigned long gt[4];

volatile unsigned long Data_ADC;

volatile unsigned long Data_ADC_1;

static unsigned long sensor_array[10];

/************************************/

void ADC_INT()

{

//ENABLE ADC

SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0);

SysCtlADCSpeedSet(SYSCTL_ADCSPEED_125KSPS);

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

//TRIGGER TO CONVERT

ADCSequenceConfigure(ADC0_BASE, 1, ADC_TRIGGER_PROCESSOR, 0);

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

ADCSequenceStepConfigure(ADC0_BASE, 1, 0, ADC_CTL_CH0);



ADCSequenceStepConfigure(ADC0_BASE, 1, 3, ADC_CTL_CH0 | ADC_CTL_IE | ADC_CTL_END);

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

//ENABLE SEQUENCE 3

ADCSequenceEnable(ADC0_BASE, 1);

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

}

/****Mang dung cho bo loc****/

static unsigned long sensor_array[10];

void Read_ADC(void)

{

ADCIntClear(ADC0_BASE, 1);

ADCProcessorTrigger(ADC0_BASE, 1);

while(!ADCIntStatus(ADC0_BASE, 1, false))

{

}

ADCSequenceDataGet(ADC0_BASE, 1, gt);

Data_ADC = (gt[0] + gt[1] + gt[2] + gt[3] )/4;

Data_ADC_1 = Data_ADC;

sensor_array[2]=sensor_array[1];

sensor_array[1]=sensor_array[0];

sensor_array[0]=Data_ADC;

Data_ADC_1 = sensor_array[0] + sensor_array[1] + sensor_array[2];

Data_ADC_1 = Data_ADC_1/3;

}

int main(void)

{

SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);

ADC_INT();

RIT128x96x4Init(1000000);

RIT128x96x4StringDraw("ADCX", 0, 0, 15);

while(1){

Read_ADC();

RIT128x96x4UDecOut3(Data_ADC_1, 30, 0, 15);

}

}3.4 S khi iu khin ng c

Hnh 3.5 S khi iu khin ng c.

-Bin flag suy ra t bin Vantoc chng trnh iu khin bng thut ton PID.

Nu bin Vantoc < 0 th flag=1 th ng c s quay ti.

Ngc lai, bin Vantoc > 0 th flag =0 th ng c s quay lui.

-Bin period c gi l rng xung(-10.000 < Period < 10.000) c trng cho tc nhanh chm ca ng c c tnh bng cng thc:

Period=flag*10.000 + Vantoc.

Chng trnh iu khin ng c

#include "inc/hw_ints.h"#include "inc/hw_memmap.h"


#include "driverlib/debug.h"


#include "driverlib/pwm.h"


#include "driverlib/interrupt.h"


#include "driverlib/timer.h"


#include "driverlib/flash.h"

#include "stdlib.h"

#include "stdio.h"

#include "float.h"

#include

//dinh nghia dc 1

#define MOTOR_PORT GPIO_PORTF_BASE

#define MOTOR_PORT_INT INT_GPIOF

#define MOTOR_PORT3 GPIO_PORTH_BASE

#define MOTOR_PORT_ENABLE SYSCTL_PERIPH_GPIOF

#define PIN_PWM

GPIO_PIN_2

#define TURN_LR

GPIO_PIN_3

#define LEFT

0

#define RIGHT

8

#define LED

GPIO_PIN_2

//dinh nghia dc 2

#define MOTOR_PORT1

GPIO_PORTD_BASE

#define MOTOR_PORT2

GPIO_PORTG_BASE

#define MOTOR_PORT_ENABLE1 SYSCTL_PERIPH_GPIOD

#define MOTOR_PORT_ENABLE2 SYSCTL_PERIPH_GPIOG

#define PIN_PWM1

GPIO_PIN_1

#define TURN_LR1

GPIO_PIN_3

#define ulPeriod

SysCtlClockGet()

#ifdef DEBUG

Void

__error__(char *pcFilename, unsigned long ulLine)

{

}

#endif

void update_motor_param(void)

{

if(van_toc 0 : xe ang ng v pha trc.

+ Error = 0 : xe v tr cn bng.

T thng s lch ta iu khin ng c ca 2 bnh tuyn tnh theo gc nghing ca xe, khi xe c gc nghing ln so vi v tr cn bng, hay ni cch khc lch (error) ln, chng ta cn iu khin ng c quay nhanh hn (theo hng ng) nhanh chng a xe v v tr cn bng v khi lch (error) nh th ta phi iu khin ng c quay chm tr li. Mt cch n gin cng thc ha vic ny l dng quan h tuyn tnh P:

Vantoc = Kp* Error.

Sau nhiu ln th nghim ti chn Kp = 280. V gi tr vn tc c gii hn l t -10.000 < Vantoc < 10.000.

+ Vantoc < 0 : xe chy v pha sau.

+ Vantoc > 0 : xe chy v pha trc.

+ Vantoc = 0 : xe ng yn.

R rng nu Kp ln th gi tr Vantoc cng s ln v xe rt nhanh chng tin v v tr cn bng. Tuy nhin, Vantoc qu ln s lm cho gia tc cho xe rt nhanh. Khi xe n v tr cn bng (tc Error = 0), th tuy Vantoc = 0 (v Vantoc = Kp* Error) nhng do qun tnh xe vn tip tc tin v hng ang chy v lch v tr cn bng, sai s e li tr nn khc 0, gi tr sai s lc ny c gi l overshot ( vt l). Qu trnh ny c tip din cho n khi xe ng do gc nghing qu ln.

Tip theo, da vo thut ton PID ti thm thnh phn D ( vi phn) loi b vt l.

Vantoc = Kp*Error + Kd*D_value

D_value l gi tr vi phn thay i sau mi ln cp nht gi tr I_array[0] v thi gian c tnh bng cch da vo thng s I_value. I_value l gi tr tch phn n s ly tng cc gi tr trong mng I_array. Mng I_array[0] dng cha cc thng s Error.Ngha l, ban u n s cng dn cc gi tr gm I_array[0] vi 14 gi tr 0, ln th 2 sau khi dch n s cng I_array[0] + I_array[1] vi 13 gi tr 0 v c tip theo sau 15 ln mng I_array[tt] s c cp nht y . Cui cng n s lm php tnh cng t I_array[0] + . .+ I_array[15]. C sau mi ln dch l ta s c c 1 gi tr I_value. Nh vy n s dch v lun cp nht mng I_array[tt].

I_array[0] = Error;

for(int tt=15;tt>=0;tt--)

{

I_array[tt] = I_array[tt-1];

I_value = I_value + I_array[tt-1];

}

D_value = I_array[0]-I_array[1];

S hin din ca thnh phn D lm gim vt ca xe, khi xe tin gn v v tr cn bng nhng li ny sinh ra vn sai s trong trng thi tnh (steady state error) v s c nhng lc khi xe gn n v tr cn bng nhng do thnh phn D_value lm gim tc ca xe, c lc lm cho xe cha n v tr cn bng m Vantoc gn bng 0.

trnh vn sai s trong trng thi tnh lc ny ti thm vo b iu khin thnh phn I_Value c chc nng cng dn sai s theo thi gian v s lm tng gi tr Vantoc theo thi gian. n mt lc no , gi tr Vantoc ln thng ma st tnh v y xe tin tip v v tr cn bng B iu khin n thi im ny y l PID:

Vantoc = Vantoc = Kp*Error + Ki*I_Value + Kd*D_value

Sau qu trnh thc nghim ti chn Kp = 280, Ki = 20, Kd= -180 V rt ra c c tnh ca cc thng s nh sau:Thng sThi gian qu vt lThi gian p ngSteady-state error

Kp=280GimTngt thay iTng

Ki= 20GimTngTngKh c

Kd= -180t thay iGimGimt thay i

Bng 3.1 c tnh ca lut iu khin P, I v D khi th nghim.3.6. Kt qu th nghim

3.6.1. Dng sng o c ca cm bin gia tc

Sau khi hon thnh vic lp rp h thng ta tin hnh kim tra hot ng ca robot v o dng xung thc t ca h thng i chiu vi l thuyt.

Khi robot v tr cn bng mc in p do cm bin gia tc tr v dao ng trong khong 1.5436 V.

Hnh 3.6 Xung trn chn X_OUT ca cm bin gia tc khi robot v tr cn bng.

Khi robot nghing v pha trc mc in p do cm bin gia tc tr v dao ng trong khong 1.2276 V.

Hnh 3.7 Xung trn chn X_OUT ca cm bin gia tc khi robot nghing v trc

Khi robot nghing v pha sau mc in p do cm bin gia tc tr v dao ng trong khong 1.8684 V.

Hnh 3.8 Xung trn chn X_OUT ca cm bin gia tc khi robot nghing v sau

3.6.2. Dng sng o c ca xung PWM

Khi robtot v tr cn bng, lc ny gi tr ca cm bin gia tc tr v l ADXL = 558, v robot ang dao ng v tr cn bng nn rng xung PWM khi ny rt nh.

Hnh 3.9 rng xung PWM khi robot v tr cn bng

Khi robot ng v pha sau, lc cm bin gia tc tr v c gi tr ADXL = 571, v robot ang c xu hng ng v pha sau nn rng xung lc ny c vi iu khin ly mc m iu khin ng c quay theo chiu ngc gip robot tin v sau.

Hnh 3.10 rng xung PWM khi robot ng v pha sau

Khi robot ng v pha trc, lc cm bin gia tc tr v c gi tr ADXL = 546, v robot ang c xu hng ng v pha trc nn rng xung lc ny c vi iu khin ly mc dng iu khin ng c quay theo chiu thun gip robot tin v trc.

Hnh 3.11 rng xung PWM khi robot ng v pha trc

3.6.3 Kt lun

Sau nhiu ln th nghim, m hnh robot t c nhng kt qu sau:

-Theo cc xung o c trn cm bin gia tc ADXL335 ti thy cm bin hot ng tng i ng vi l thuyt.

-Xc nh c lch ca thn xe so vi v tr cn bng.

-Xc nh c cc thng s Kp,Ki,Kd.

-Thi gian p ng ca ng c nhanh a thn xe v v tr cn bng lc xe b nghing (nhng gc nh) nhng vn cn b tnh trng vt l. Ty thuc vo v tr ban u ca xe khi tip xc vi mt phng m vt l nhiu hay t.

-Robot vn cha tht s n nh, vn cn dao ng mnh khi v tr cn bng do gi tr cm bin tr v v tr ny c s sai lch qu ln.

KT LUN V HNG PHT TRIN

1.Nhng kt qu t c

Thit k v hon thin m hnh c kh xe t cn bng.

Thit k mch driver c th iu khin ng c vi cng sut 2A, in p 12V.

Thit k mch in phi hp gia cm bin gia tc v vi iu khin.

Thit k v ch to khung xe hon chnh gip xe c th chu c nhng va p mnh trong khi th nghim.

Tip cn v p dng tng i thnh cng thut ton PID vo m hnh.

Trong qu trnh thc hin lun vn gip ti tip cn vi chip ARM Cortex-M4 c th l vi x l LM3S1968 Stellaris ca TI, qua gip ti hiu c mt s tnh nng ca n nh: GPIO, PWM, ADC

Mc d cha khai thc ht nhng ng dng ca vi x l nhng qua cng gip ch cho ti rt nhiu v kh nng tm kim ti liu, b sung rt nhiu kin thc v vi iu khin m ti cn thiu v gip ti nh hng pht trin cng vic sau ny.

2.Nhng kt qu cha t c

Xe t cn bng vn cha gi c s n nh cn thit, vn cn rung v lc khi v tr cn bng.

ng c ca xe khng mnh (2 vng/s) to vt khi xe ng nhng gc ln.

3.Nhng vn cha gii quyt

Gi tr sensor tr v l gia tc gc ch khng phi l gc nghing ca xe. c bit khi v tr cn bng gi tr tr v thiu chnh xc. Ban u mi th nghim th gi tr tr v ca cm bin v tr cn bng l 555 nhng sau 1 thi gian th gi tr thay i l 557, 558.

Ngun ca xe cng khng tht s n nh.Khi Pin mi c sc th ng c vt mnh nhng sau 1 thi gian th ng c c v yu li.V vy rt kh khn trong vic xc nh cc thng s Kp, Ki, Kd trong thut ton PID.

4.Hng pht trin.

Cn thit k thm trong mch con cm bin vn tc gc Gyro MPU6050. T s cho ta bit c gc ng chnh xc ca thn xe so vi v tr cn bng hn.

u im ca Gyro:

+Quay vi gc chnh xc

+t ph thuc yu t bn ngoi

+Gi r (tm khong 200.000 ngn).

+Chun giao tip I2C

S dng ng c tt hn, mnh hn ngoi vic xe di chuyn c cn c kh mang ch vt nng.

S dng loi pin n nh hn c thi gian hot ng lu hn.

Phi hp hai thut ton PID v Q Learning.

TI LIU THAM KHO

Ti liu ngoi nc:

[1] Stellaris Peripheral Driver Library USERS GUIDE, Luminary Micro, September 29, 2008.

[2] Data Sheet ADXL335, Small, Low Power, 3-Axis 3 g Accelerometer.

[3] Data Sheet L6201, L6202, L6203.Ti liu wedsite:

[1] picvietnam.com/download/thuyetminh.pdf

[2] www.ti.com/litv/pdf/spmu052c[3] www.ti.com/product/lm3s1968. PH LC

Code chng trnh:






#include "stdio.h"

#include "inc/hw_ints.h"



#include "driverlib/debug.h"


#include "driverlib/pwm.h"


#include "driverlib/interrupt.h"


#include "driverlib/timer.h"

#include "math.h"

#include "stdlib.h"

#include

//dinh nghia dc 1

#define MOTOR_PORT GPIO_PORTF_BASE

#define MOTOR_PORT_INT INT_GPIOF

#define MOTOR_PORT3 GPIO_PORTH_BASE

//#define MOTOR_PORT_INT3 INT_GPIOH

#define MOTOR_PORT_ENABLE SYSCTL_PERIPH_GPIOF

//#define MOTOR_PORT_ENABLE3 SYSCTL_PERIPH_GPIOH

//#define PORTG SYSCTL_PERIPH_GPIOG

#define PIN_PWM GPIO_PIN_2

#define TURN_LR GPIO_PIN_3

//#define ENABLE GPIO_PIN_0

#define LEFT 0

#define RIGHT 8

#define LED GPIO_PIN_2

//dinh nghia dc 2

#define MOTOR_PORT1 GPIO_PORTD_BASE

#define MOTOR_PORT2 GPIO_PORTG_BASE

#define MOTOR_PORT_INT1 INT_GPIOD

#define MOTOR_PORT_INT2 INT_GPIOG

#define MOTOR_PORT_ENABLE1 SYSCTL_PERIPH_GPIOD

#define MOTOR_PORT_ENABLE2 SYSCTL_PERIPH_GPIOG

#define CHANNEL_A1 GPIO_PIN_0

#define CHANNEL_B1 GPIO_PIN_2

#define CHANNEL_C1 GPIO_PIN_1

#define PIN_PWM1 GPIO_PIN_1

#define TURN_LR1 GPIO_PIN_3

//#define ENABLE1 GPIO_PIN_1

/*****************************/

static int dc1_flag;

static int dc2_flag;

static signed long van_toc; // -ulPeriod Center

Center = Center + 5

Center = Center -5

1

Vantoc= ulPeriod-11

Vantoc > ulperiod - 11

D_value= I_array[0] - I_array[1]

Vantoc=Kp*Error + Ki* I_value + Kd* D_value

Vantoc = -ulPeriod+11

Update Vantoc

Hin th OLED

Vantoc< -ulperiod+11

1

D liu tng t t ADXL335

ADC chanel 0

Bin i in p c c thnh gi tr tng ng.

Hin th ln LCD v x l

Khi to xung clock 16MHz cho h thng

Xa c ngt ADC0

Kch hot ADC0

Cu hnh ADC0

Nhn d liu t cm bin

Kch hot qu trnh chuyn i

i hon thnh qu trnh chuyn i

X l gi tr c chuyn i

Vi iu khin nhn tn hiu iu khin ng c

Cu hnh chn PF3 v chn PD2 l chn iu khin quay ti, lui cho ng c thng qua bin flag(0, 1)

Begin

Flag=0

Xut tn hiu iu khin ng c quay nhanh hay chm n chn PF2 v chn PD1 thng qua bin Period

Cho php b PWM hot ng

a tn hiu rng xung n mch cu H L6203

ng c quay ti

Flag=1

ng c quay lui

Hnh 2.6.Cm bin Gyro MPU6050

_1448265651.unknown

_1448265655.unknown

_1448265656.unknown

_1448265654.unknown

_1448265653.unknown

_1448265646.unknown

_1448265649.unknown

_1448265645.unknown

Documents

Luan Van Robot Can Bang