PID Servo Avr

8/3/2019 PID Servo Avr

1/22

I. Gii thiu

iu khin ng c DC (DC Motor) l mt ng dng thuc dng c bnnht ca iu khin t ng v DC Motor l c cu chp hnh (actuator)c dng nhiu nht trong cc h thng t ng (v d robot). iu khinc DC Motor l bn c th t xy dng c cho mnh rt nhiu hthng t ng. Khi nim Servo m ti dng trong bi hc ny ch mt hthng hi tip. DC servo motor l ng c DC c b iu khin hi tip.

Bi ny l mt bi tng hp nhiu vn ng dng AVR bao gm nhnd liu t ngi dng, iu khin motor, c encoder, hin th LCD, c giithut iu khin PID v mch cng sut cho MotorDo , t nht bn phinm c cc vn c bn nh Timer-Counter, TexLCD, mch cu H.Phn cn li ti s gii thch trong lc hc bi ny. C 2 phng php iukhin ng c DC l analog v digital. Mc ch chnh ca chng ta l dng

AVR iu khin ng c DC nn phng php s m c th l phng phpiu rng xung (PWM) s c gii thiu. Ngoi ra, khi ni n iu khinng c DC c 2 i lng iu khin chnh l v tr (s vng quay) v vntc. Trong phn gii thch v b iu khin PID ti s iu khin v tr lmv d, tuy nhin trong phn v d lp trnh cho AVR chng ta s thc hiniu khin vn tc cho DC Motor. Bng cch ny, bn c th t tin mrng v d iu khin cho c 2 i lng. V l iu khin mt cch tng nn chng ta cn c v i lng iu khin (c th l v tr hoc vntc motor) v hi tip (feedback) v hiu chnh PWM cp cho ng c.Chng ta s dng incremental optical encoder c s vng quay v hitip v cho AVR. B iu khin PID s c dng v vn hnh bi AVR.Tng qut, bi hc ny bao gm:

- AVR pht PWM iu chnh vn tc ng c: phn ny bn xem li bi4 v Timer-Counter. iu c bn cn nm l bng cch thay i rng caxung PWM chng ta s thay i c vn tc Motor.

- Xung PWM khng trc tip lm quay ng c m thng qua mtmch cng sut gi l dirver. Driver cho DC Motor chnh l mch cu H mchng ta tm hiu trong bi Mch cu H. Trong bi hc ny, ti giithiu mt chip c tch hp sn mch cu H, chip L298D.

- vic iu khin chip driver L298D d dng, chng ta s to mtmch logic dng cc cng NOT v AND.- ng c DC m chng ta s dng c tch hp sn mt encoder 3 ng

ra, chng ta s dng AVR c s xung (hay s vng quay) v tnh ra vntc ca Motor. Vic c encoder s c thc hin bng ngt ngoi.

- Mt gii thut PID c xy dng trong AVR hiu chnh vn tcng c.


2/22

- Ngi dng s nhp vn tc cn iu khin vo AVR thng qua ccswitches. Vn tc mong mun v vn tc thc ca ng c c hin thtrn Text LCD.

Mch in v d c trnh by trong hnh 1.

Hnh 1. H thng iu khin ng c DC servo.


3/22

Trong mch in hnh 1, ti chia h thng thnh 3 nhm: nhmCONTROL bao gm AVR vn hnh gii thut iu khin PID v vic nhp,xut. Nhm LOGIC thc hin vic bin i cc tn hiu iu khin to racc tn hiu ph hp cho chip driver. Nhm POWER bao gm chip driverL298D v DC Motor. Ngoi ra cn c mt Encoder c tch hp sn trnDC Motor.

Phn tip theo chng ta s tm hiu ring tng nhm, cui cng l vitchng trnh cho AVR iu khin h thng DC Servo Motor

II. Incremental Optical Encoder

iu khin s vng quay hay vn tc ng c th chng ta nht thitphi c c gc quay ca motor. Mt s phng php c th c dng xc nh gc quay ca motor bao gm tachometer (tht ra tachometer o

vn tc quay), dng bin tr xoay, hoc dng encoder. Trong 2 phngphp u tin l phng php analog v dng optiacal encoder (encoderquang) thuc nhm phng php digital. H thng optical encoder bao gmmt ngun pht quang (thng l hng ngoi infrared), mt cm binquang v mt a c chia rnh. Optical encoder li c chia thnh 2 loi:encoder tuyt i (absolute optical encoder) v encoder tng i(incremental optical encoder). Trong a s cc DC Motor, incrementaloptical encoder c dng v m hnh ng c servo trong bi ny cngkhng ngoi l. T by gi khi ti ni encoder tc l incremental encoder.Hnh 2 l m hnh ca encoder loi ny.


4/22

Hnh 2. Optical Encoder (trch t [1]).

Encoder thng c 3 knh (3 ng ra) bao gm knh A, knh B v knh I(Index). Trong hnh 2 bn thy hy ch mt l nh bn pha trong ca aquay v mt cp phat-thu dnh ring cho l nh ny. l knh I caencoder. C mi ln motor quay c mt vng, l nh xut hin ti v trca cp pht-thu, hng ngoi t ngun pht s xuyn qua l nh n cm

bin quang, mt tn hiu xut hin trn cm bin. Nh th knh I xut hinmt xung mi vng quay ca motor. Bn ngoi a quay c chia thnhcc rnh nh v mt cp thu-pht khc dnh cho cc rnh ny. y l knhA ca encoder, hot ng ca knh A cng tng t knh I, im khc nhaul trong 1 vng quay ca motor, c N xung xut hin trn knh A. N l srnh trn a v c gi l phn gii (resolution) ca encoder. Mi loiencoder c phn gii khc nhau, c khi trn mi a ch c vi rnh

nhng cng c trng hp n hng nghn rnh c chia. iu khinng c, bn phi bit phn gii ca encoder ang dng. phn giinh hng n chnh xc iu khin v c phng php iu khin.Khng c v trong hnh 2, tuy nhin trn cc encoder cn c mt cp thu

pht khc c t trn cng ng trn vi knh A nhng lch mt cht(lch M+0,5 rnh), y l knh B ca encoder. Tn hiu xung t knh B ccng tn s vi knh A nhng lch pha 90o. Bng cch phi hp knh A vB ngi c s bit chiu quay ca ng c. Hy quan st hnh 3.


5/22

Hnh 3. Hai knh A v B lch pha trong encoder (trch t [1])

Hnh trn cng trong hnh 3 th hin s b tr ca 2 cm bin knh A vB lch pha nhau. Khi cm bin A bt u b che th cm bin B hon tonnhn c hng ngoi xuyn qua, v ngc li. Hnh thp l dng xung ngra trn 2 knh. Xt trng hp motor quay cng chiu kim ng h, tn hiui t tri sang phi. Bn hy quan st lc tn hiu A chuyn t mc caoxung thp (cnh xung) th knh B ang mc thp. Ngc li, nu ngc quay ngc chiu kim ng h, tn hiu i t phi qua tri. Lc ny, ticnh xung ca knh A th knh B ang mc cao. Nh vy, bng cch

phi hp 2 knh A v B chng ta khng nhng xc nh c gc quay(thng qua s xung) m cn bit c chiu quay ca ng c (thng quamc ca knh B cnh xung ca knh A).

Cu hi by gi l lm th no c encoder bng AVR?

Ty theo i lng iu khin (v tr hay vn tc) v c im encoder( phn gii) chng ta c cc gii php sau c encoder bng AVR- Dng input capture: mt s b timer-counter trn AVR c chc nng

Input capture, hiu nm na nh sau. C mi ln c mt tn hiu (cnh lnhoc cnh xung) trn chn ICP (Input Capture Pin), gi tr thi gian catimer c t ng gn cho thanh ghi ICR (Input capture Register). So snhgi tr thanh ghi ICR trong 2 ln lin tip s c c chu k ca tn hiukch chn ICP. T suy ra tn s tn hiu. Nu mt knh ca encoder cni vi chn ICP th chng ta c th o c tn s tn hiu ca knh ny.

Ni cch khc, chng ta s tnh c vn tc ca ng c. Chng ta c thdng ngt Input capture v khi ngt xy ra, c th m s thm s xung

bit c gc quay motor, cng c th xc nh c hng quay thng quaxc nh mc knh B trong trnh phc v ngt input capture. y l mt

phng php hay, nhng c nhc im l kh phc tp khi s dng chcnng input capture ca AVR. Mc khc trn cc chip AVR t mega32 trxung, Input capture ch c timer 1, trong khi Timer ny thng dng to PWM iu khin ng c.

- Dng chc nng counter: t cc knh ca encoder vo cc chn m(T0, T1) ca cc b timer chng ta s m c s lng xung ca cc

knh. y l phng php s dng t ti nguyn nht (t tn thi gian choencoder). Nhc im ln nht ca phng php ny l khng xc nhc chiu quay, mc khc phng php ny khng n nh khi vn tcng c c s thay i ln.

- Cui cng l s dng ngt ngoi: y l phng php d nhng chnhxc c encoder v cng l phng php c dng trong bi hc ny. tng ca phng php rt n gin, chng ta ni knh A ca encoder vi 1


6/22

ngt ngoi (INT2 chng hn) v knh B vi mt chn no bt k (khngphi chn ngt). C mi ln ngt ngoi xy ra, tc c 1 xung xut hin trn knh A th trnh phc v ngt ngoi t ng c gi. Trong trnh phc vngt ny chng ta kim tra mc ca knh B, ty theo mc ca knh B chngta s tng bin m xung ln 1 hoc gim i 1. Tuy nhin, bn cn phi tnhton rt cn thn khi s dng phng php ny. V d trng hp encoderc phn gii 2000 xung/vng, motor bn quay vi vn tc 100 vng/s thtn s xung trn knh A ca encode l 2000x100=200KHz, ngha l c mi5 us ngt ngoi xy ra mt ln. Tn s ngt nh th l qu cao cho AVR,iu ny c ngha l AVR ch tp trung cho mi vic m xung, khng c thi gian thc thi cc vic khc. Trong bi ny, chng ta chn phngii ca encoder l 112 (112 xung trn mi vng quay). Vn tc ti a cang c c chn vo khong 30 vng/s nn tn s xung ln nht tencoder l 112x30=3.36KHz. Gi tr ny hp l v tn s cho AVR trong bi

ny c chn 8MHz. Knh A ca encoder c ni vi ngt INT2 ca chipatmega32, knh B c ni vi chn PB0, chng ta khng s dng knh I(xem hnh 1). Ch : cc ng ra trn a s (gn nh tt c) cc encoder c dng ccgp h (Open collector), mun s dng chng cn mc in tr ko lnVCC (5V).

III. Chip driver L298D

L298D l mt chip tch hp 2 mch cu H trong gi 15 chn. Tt c cc

mch kch, mch cu u c tch hp sn. L298D c in p danh nghacao (ln nht 50V) v dng in danh ngha ln hn 2A nn rt thch hpcho cc cc ng dng cng sut nh nh cc ng c DC loi nh v va.V l loi all in one nn l la chn hon ho cho nhng ngi cha cnhiu kinh nghim lm mch in t. Trong bi hc ny ti dng chipL298D lm driver cho motor. Hnh 4 th hin m hnh tht ca chip vcu trc bn trong chip.


7/22

Hnh 4. Chip L298D

Hnh pha trn l hnh dng bn ngoi v tn gi cc chn ca L298D.

Hnh pha di l cu trc bn trong chip. C 2 mch cu H trn mi chipL298D nn c th iu khin 2 i tng ch vi 1 chip ny. Mi mch cubao gm 1 ng ngun Vs (tht ra l ng chung cho 2 mch cu), mtng current sensing (cm bin dng), phn cui ca mch cu H khngc ni vi GND m b trng cho ngi dng ni mt in tr nh gi lsensing resistor. Bng cch o in p ri trn in tr ny chng ta c thtnh c dng qua in tr, cng l dng qua ng c (xem hnh 4). Mc


8/22

ch chnh ca vic o dng in qua ng c l xc nh cc trng hpnguy him xy ra trong mch, v d qu ti. Nu vic o dng ng ckhng tht s cn thit bn c th ni ng current sensing ny vi GND(trong mch in ca bi ny, ti ni chn current sensing vi GND). ngc s c ni vi 2 ng OUT1, OUT2 (hoc OUT3, OUT4 nu dngmch cu bn phi). Mt chn En (EnA v EnB cho 2 mch cu) cho phpmch cu hot ng, khi chn En c ko ln mc cao, mch cu sn sanghot ng. Cc ng kch mi bn ca mch cu c kt hp vi nhau vnhng mc in p ngc nhau do mt cng Logic NOT. Bng cch nychng ta c th trnh c trng hp 2 transistor cng mt bn c kchcng lc (ngn mch). Nh vy, s c 2 ng kch cho mi cu H gi lIn1 v In2 (hoc In3, In4). motor hot ng chng ta phi ko 1 trong 2ng kch ny ln cao trong khi ng kia gi mc thp, v d In1=1,In2=0. Khi o mc kch ca 2 ng In, ng c s o chiu quay. Tuy

nhin, do L298D khng ch c dng o chiu ng c m cn iukhin vn tc ng c bng PWM, cc ng In cn c t hp li bngcc cng Logic (xem phn tip theo). Ngoi ra, trn chip L298D cn c ccng Vss cp in p cho phn logic (5V) v GND chung cho c logic vmotor.

Trong thc t, cng sut thc m L298D c th ti nh hn so vi gi trdanh ngha ca n (V=50V, I=2A). tng dng in ti ca chip ln gpi, chng ta c th ni 2 mch cu H song song vi nhau (cc chn c chcnng nh nhau ca 2 mch cu c ni chung).

II. Mch logic cho L298D

Thng thng, khi thit k mt mch driver cho motor ngi ta thngdnh 3 ng iu khin l PWM dng iu khin vn tc, DIR iukhin hng v En cho php mch hot ng. Chip L298D c sn ngEn nhng 2 ng iu khin In1 v In2 khng tht s chc nng nh chngta mong mun. V th, chng ta s thit k mt mch logic ph vi 2 ngvo l PWM v DIR trong khi 2 ng ra l 2 ng iu khin In1 v In2.Bng chn tr ca mch logic cn thit k c trnh by trong bng 1.

Bng 1. bng chn tr ca mch logic cho driver L298D.PWM DIR In1 In2

0 0 0 00 1 0 01 0 1 01 1 0 1


9/22

T bng chn tr ny, chng ta c th vit hm bool cho 2 ng In1 vIn2:

In1=PWM.NOT(DIR)In2=PWM.DIRMch logic v th s c dng nh trong hnh 5.

Hnh 5. Mch logic cho L239

Ti s khng gii thch chi tit phn ny, tuy nhin iu bn cn nm lvi mch logic ny, ng DIR c chc nng o chiu ng c trong khing PWM iu khin vn tc ng c bng tn hiu PWM.

V. Gii thut iu khin PID

PID l cch vit tc ca cc t Propotional (t l), Integral (tch phn) vDerivative (o hm). Tuy xut hin rt lu nhng n nay PID vn l giithut iu khin c dng nhiu nht trong cc ng dng iu khin tng. gip bn c ci hiu r hn bn cht ca gii thut PID ti s dng

mt v d iu khin v tr ca mt car (xe) trn ng thng. Gi s bn cmt xe ( chi...) c gn mt ng c DC. ng c sinh ra mt lc yxe chy ti hoc lui trn mt ng thng nh trong hnh 6.


10/22

Hnh 6. V d iu khin v tr xe trn ng thng

Gi F l lc do ng c to ra iu khin xe. Ban u xe v tr A, nhimv t ra l iu khin lc F (mt cch t ng) y xe n ng v tr Ovi cc yu cu: chnh xc (accurate), nhanh (fast response), n nh (smallovershot).

Mt iu rt t nhin, nu v tr hin ti ca xe rt xa v tr mong mun(im O), hay ni cch khc sai s(error) ln, chng ta cn tc ng lc Fln nhanh chng a xe v O. Mt cch n gin cng thc ha tng ny l dng quan h tuyn tnh:F=Kp*e

(1)Trong Kp l mt hng s dng no m chng ta gi l h s P

(Propotional gain), e l sai s cn iu khin tc khong cch t im O nv tr hin ti ca xe. Mc tiu iu khin l a e tin v 0 cng nhanh cngtt. R rng nu Kp ln th F cng s ln v xe rt nhanh chng tin v v trO. Tuy nhin, lc F qu ln s gia tc cho xe rt nhanh (nh lut II ca

Newton: F=ma). Khi xe n v tr O (tc e=0), th tuy lc F=0 (vF=Kp*e =Kp*0 =0) nhng do qun tnh xe vn tip tc tin v bn phi vlch im O v bn phi, sai s e li tr nn khc 0, gi tr sai s lc ny

c gi l overshot (vt qu). Lc ny, sai s e l s m, lc F li xuthin nhng vi chiu ngc li ko xe v li im O. Nhng mt ln na,do Kp ln nn gi tr lc F cng ln v c th ko xe lch v bn tri imO. Qu trnh c tip din, xe c mi dao ng quanh im O. C trng hpxe dao ng cng ngy xng xa im O. B iu khin lc ny c ni lkhng n nh. Mt xut nhm gim overshot ca xe l s dngmt thnh phn thng trong b iu khin. S rt l tng nu khi xe ang


11/22

xa im O, b iu khin sinh ra lc F ln nhng khi xe tin gn nim O th thnh phn thng s gim tc xe li. Chng ta iu bit khimt vt dao ng quanh 1 im th vt c vn tc cao nht tm daong (im O). Ni mt cch khc, gn im O sai s e ca xe thay inhanh 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 theothi gian. Nh vy, khi xe t A tin v gn O, o hm ca sai s e tng gitr nhng ngc chiu ca lc F (v e ang gim nhanh dn). Nu s dngo hm lm thnh phn thng th c th gim c overshot caxe. Thnh phn thng ny chnh l thnh phn D (Derivative) trong biu khin PID m chng ta ang kho st. Thm thnh phn D ny vo biu khin P hin ti, chng ta thu c b iu khin PD nhu sau:

F=Kp*e +Kd*(de/dt)

(2)Trong (de/dt) l vn tc ca sai s e v Kd l mt hng s khng m

gi l h s D (Derivative gain).S hin din ca thnh phn D lm gim overshot ca xe, khi xe tin gn

v O, lc F gm 2 thnh phn Kp*e > =0 (P) v Kd*(de/dt)


12/22

(du c ti dng thay cho du tch phn, edt l tch phn ca bin etheo t)

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 thc lt bt tr thnh b iu khin P, PI hoc PD. Cng vic chnh cangi thit k b iu khin PID l chn cc h s Kp, Kd v Ki sao cho biu 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. Ti tmtt mt kinh nghim c bn khi chn cc h s cho PID nh sau:

- Chn Kp trc: th b iu khin P vi i tng tht (hoc mphng), iu chnh Kp sao cho thi gian p ng nhanh, chp nhnovershot nh.

- Thm thnh phn D loi overshot, tng Kd 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 tng Ki t b n ln gim steady state error ng thi khng cho overshot xut hin tr li.

C mt phng php rt ph bin dng chn cc h s cho b iukhin PID gi l ZieglerNichols, bn quan tm c th t tm hiu thm.

iu khin PID sCng thc ca b iu khin PID trnh by trong (3) l dng hm lin tc

ca bin e, trong c c thnh phn tuyn tnh, o hm v tch phn. Tuynhin, h thng my tnh v vi iu khin li l h thng s. Mun xy dng

b iu khin PID trn my tnh hay trn vi iu khin chng ta phi bitcch xp x phng trnh lin tc thnh dng ri rc. thc hin s ha

b iu khin PID trc ht ti ni s qua th no l h thng s (digital) sovi h thng lin tc hay h thng tng t (analog). Hy quan st h thngiu chnh nhit n gin nh trong hnh 7.


13/22

Hnh 7. T ng iu chnh nhit

Gi s chng ta cn iu chnh nhit trong phng mt mc no (ty theo gi tr tham chiu) bng qut. Cm bin o nhit v hi tip v

b khuych i vi sai (so snh v khuych i). Nu c sai s gia gi trtham chiu v gi tr o t cm bim, b khuych i vi sai s t ngkhuych i sai s ny v lm tng hay gim vn tc ca qut iu chnhnhit . Qu trnh ny xy ra mt cch lin tc. B khuych i vi sai trong

trng hp ny chnh l b iu khin tng t (analog controller). Bkhuych i ny l mt mch in t thng thng nh Opamp chng hn.Nu chng ta thay b khuych i ny bng mt vi iu khin AVR th qutrnh hiu chnh khng cn xy ra lin tc na m theo mt chu k no .V d c mi 10 ms chng ta c gi tr t cm bin mt ln tnh ton sais v xut gi tr iu khin qut. B iu khin do AVR thc hin gi l biu khin s (digital controller) v khong thi gian 10ms ny gi l thigian ly mu (sampling time), l khong cch gia 2 ln iu khin lintip. R rng thi gian ly mu cng nh (tn s cao) th vic hiu chnhcng tin gn n s lin tc v cht lng iu khin s tt hn. Trong

cc b iu khin s, thi gian ly mu l mt yu t rt quan trng. Cntnh ton thi gian ny khng qu ln nhng cng ng qu nh, v nhth s hao ph thi gian thc thi.

V b iu khin PID xy dng trong AVR s l b iu khin s, chngta cn xp x cng thc ca b iu khin ny theo cc khong thi gian rirc. Trc ht, thnh phn P tng i n gin v l quan h tuyn tnhKp*e, chng ta ch cn p dng trc tip cng thc ny m khng cn bt k


14/22

xp x no. Tip n l xp x cho o hm ca bin e. V thi gian ly mucho cc b iu khin thng rt b nn c th xp x o hm bng s thayi ca e trong 2 ln ly mu lin tip:

de/dt =(e(k) e(k-1))/h.Trong e(k) l gi tr hin ti ca e, e(k-1) l gi tr ca e trong ln ly

mu trc v h l khong thi gian ly mu (h l hng s).

Hnh 8. Xp x o hm ca bin sai s e

Thnh phn tch phn c xp x bng din tch vng gii hn bi hmng biu din ca e v trc thi gian. Do vic tnh ton tch phn khngcn qu chnh xc, chng ta c th dng phng php xp x n gin nhtl xp x hnh ch nht (sai s ca phng php ny cng ln nht). tngc trnh by trong hnh 9.


15/22

Hnh 9. Xp x tch phn ca bin sai s e

Tch phn ca bin e c tnh bng tng din tch cc hnh ch nht timi thi im ang xt. Mi hnh ch nht c chiu rng bng thi gian lymu h v chiu cao l gi tr sai s e ti thi im ang xt. Tng qut:

(4)Tng hp cc xp x, cng thc ca b iu khin PID s c trnh by

trong (5)

(5)

Trong u l i lng output t b iu khin. n gin ha victnh thnh phn tch phn, chng ta nn dng phng php cng dn (hay quy):

(6)

Vi I(k) l thnh phn tch phn hin ti v I(k-1) l thnh phn tch phntrc .


16/22

Cc cng thc (5) v (6) rt d dng thc hin bng AVR. Do , nlc ny chng ta sn sng a tng vo lp trnh cho chip.

VI. iu khin DC Motor bng AVR

Phn ny chng ta s vn dng tt c phn l thuyt gii thiu trn vit chng trnh cho AVR. Mc ch l iu khin vn tc ca DC Motor

bng gii thut PID. Mch in m phng c trnh by trong hnh 1. Mhnh Motor dng trong v d l loi 12V c vn tc khng ti ti a l720rpm (revolute per minute) tc 20 vng/s. Encoder dng cho motor cchn c phn gii 112 pulse/vng. Knh A ca encoder c ni vi ngtngoi INT2 m xung, knh B ni vi chn PB0 (chn 1) ca chip

Atmega32 xt hng quay. Bn switches c ni vi 4 bit cao caPORTB ci t vn tc mong mun cn iu khin. Mt Text LCD dnghin th vn tc thc ca motor c t Encoder (Actual speed) v vn tc cit (Desired speed). Do Text LCD c ni vi PORTC nn nu bn mundng chng trnh ny cho ng dng tht th phi np li fuses v hiuha JTAG. Gii thut PID s c vn hnh bi AVR trong thi gian lymu l 25ms. Timer 2 c dng to khong thi gian 25ms. Timer 1 (16

bit) l b to PWM iu khin vn tc ng c. Ton b ni dung chngtrnh c trnh by trong list 1.

List 1. iu khin vn tc ng c DC


17/22


18/22


19/22

Cc dng t 14 n 17 chng ta nh ngha cc chn iu khin DC Motor,chn DIR iu khin hng v EN kch hot hoc dng Motor (thc ra ldng L298D). Do mc ch ca chng ta l iu khin vn tc ng c, 2chn ny ch c kch mt ln duy nht trong chng trnh chnh (khngcn i hng quay ca Motor). Dng 18 nh ngha thi gian ly mu,Sampling_time l 25 ms (.025s). Bin inv_Sampling_time dng 19 lnghch o ca Sampling_time, 1/0.025 = 40, v y cng l hng s, chngta nh ngha trc sau ny khng cn thc hin php nghch o trongchng trnh chnh (tit kim thi gian thc thi). PWM dng iu khin

ng c c chn c tn s 1KHz nn chu k la 1ms. Do chng ta dngngun xung gi nhp 8MHz, to thi gian 1ms cn 8000 xung, gi tr nyc nh ngha trong dng 20 v s c gn cho thanh ghi ICR1 (TOP caPWM, xem li bi Timer-Counter, Timer1, Fast PWM) trong chng trnhchnh (dng 81). Cc dng code t 22 n 27 khai bo mt s bin ton ccdng trong chng trnh chnh. Do cc bin ny s c dng c trong trnh

phc v ngt v chng trnh chnh nn cn khai bo c tnh volatile, kiu


20/22

bin l long int tc s nguyn 32 bit ( trnh b trn khi tnh ton sau ny).Bin Pulse v pre_Pulse l s xung hin ti v ln ly mu trc c tencoder. Cc bin trong dng 23 v 24 dng cho b iu khin PID, binCtrl_Speed l vn tc mong mun (set point) ton cc v bin Output chagi tr tnh c t b iu khin PID.

Trc khi i tm hiu chng trnh con cha gii thut PID, chng ta skho st ni dung chng trnh main v cc trnh phc v ngt trc hiutng quan cch thc thc hin. Chng trnh chnh bt u t dng 45 v ktthc dng 103. Phn u ca chng trnh chnh (ngoi vng lp while)khai bo v khi to cc module c s dng. 2 dng 49 v 50 ci thng cho PORTB, do PORT ny dng c encoder v cc switches chngta cn set n l input v c in tr ko ln. Hai dng 52 v 53 set hngcho ng c v s gi hng ny khng i trong sut qu trnh iu khinsau ny. Hai dng 55 v 56 khai bo ngt ngoi INT2 dng m xung knh

A ca encoder. Ch l INT2 ch c 2 mode l cnh xung v cnh ln nnch c 1 bit sense ISC2 chn mode. Bit ISC2 khng nm trong thanh ghiiu khin MCUCR nh cc ngt khc m nm trong thanh ghi iu khin-trng thi MCUCSR. Khi ISC2=0 th ch ngt cnh xung ca INT2c chn (xem dng 55). Sau INT2 c cho php hot ng dng56. Hy tm thi di chuyn n dng 109 xem trnh phc v ngt INT2.Chc nng ca INT2 trong bi ny l m xung encoder v th trnh phcv s lm vic ny. Khi c mt ngt INT2 xy ra tc c 1 xung t encodervo th trnh phc v ngt ISR(INT2 vect) t ng c gi ra, dng 110trong trnh phc v ngt kim tra trng thi chn PB0, tc kenh B caencoder. Nu PB0=1 th tng bin xung m c Pulse ln 1, ngc li nuPB0=0 th gim Pulse i 1 trong dng 111. Quay v gii thch chng trnhchnh dng 59, y l cc khai bo cho timer 2. Chng ta s dng timer 2to ra mt khong thi gian ly mu 25 ms, c sau 25 ms th s c ngt trntimer2 mt ln v trong trnh phc v ngt trn ca timer2 chng ta thchin tnh ton PID. Dng 59 chng ta set cc bit CS chn b chia tn s,

b chia Prescaler=1024 c chn v 25 ms kh ln so vi thi gian 1 chuk xung gi nhp (1/8 micro giy). Prescaler = 1024 ngha l sau 1024 nhpca xung gi nhp, tc sau 128 micro giy (1024 *1/8=128 us) th thanh ghi

gi tr TCNT2 mi tng 1 n v. Do chng ta mun to khong thi gian 25ms tng ng 25000/128=195 n v m ca thanh ghi TCNT2, chngta s gn gi tr khi to cho TCNT2 l 255-195=60 (timer 2 s trn mt lnkhi TCNT2 m n 255, xem li bi Timer-Counter). iu ny thc hin dng 60 TCNT2=60. Dng 61 cho php ngt trn timer2. Hai dng 64 v 65khi ng Timer 1 dng nh mt b to xung Fast PWM, mode 14, trong thanh ghi ICR1 cha chu k PWM v 2 thanh ghi OCR1A, OCR1B cha


21/22

duty cycle (khong ON) ca PWM. Cc dng t 68 n 70 ghi texts lnLCD. Cc dng t 80 n 83 khi ng PWM cho DC Motor v cho phpngt ton cc sei();. Trong vng lp while ch yu l cng vic kim tra vhin th, bin sample_count m s ln ngt trn timer2 xy ra, n c tng1 n v khi c mt ngt trn (xem dng 106) tc sau 25ms. Dng 86, chngta kim tra bin sample_count, vic hin th ch cthc hin mi 250 msmt ln (sample_count=10) v vic ny tn kh nhiu thi gian. Trong dng87 chng ta kim tra cc swiches xem ngi dng cho mun thay i vntc tham chiu cho iu khin. Cc dng tip theo in bin rSpeed l s lngxung m c t encoder trong vng 25 ms (cho ti hin ti) dong 1 caLCD v in bin Ctrl_Speed l s xung/25ms m ngi dng mong munmotor t c. Ni dung quan trng nht ca list 1, tuy nhin, khng nmtrong chng trnh chnh m nm cc trnh phc v ngt v chng trnhcon Motor_Speed_PID(long int des_Speed).

Trc ht, trnh phc v ngt ISR(TIMER2_OVF_vect) c t nggi sau mi 25ms, trong trnh ny chng ta cn set li gi tr khi ng chothanh ghi gi tr TCNT2 (xem li bi Timer-counter) dng 105. Sau tng bin m sample_count ln 1 (cng cho vic m thi gian hin th, ni trn). Cui cng l gi chng trnh con tnh ton gii thut PIDMotor_Speed_PID(long int des_Speed). y l on chng trnh tnhton gii thut PID v xut gi tr iu khin Motor. Hy quay li dng 30 tm hiu chng trnh con ny. Do bin Pulse cha tng s xung c tencode (trong ISR(INT2_vect) ), chng ta ly gi tr ny tr i gi tr

pre_Pulse, tc s lng xung thi im 25 ms trc , thu c tngs xung thu c trong 25 ms qua. y chnh l vn tc motor tnh trn 25ms: rSpeed=Pulse-pre_Pulse. Sau khi tnh c vn tc rSpeed chng tagn li gi tr Pulse cho pre_Pulse ln ly mu sau dng n (dng 32).Sai s vn tc c t tn l Err, bin ny c tnh bng bng cch lyvn tc mong mun tr vn tc hin ti: Err=des_Speed-abs(rSpeed) dng 33. Dng 34 tnh thnh phn P ca b iu khin pPart=Kp*Err.Dng 35 tnh thnh phn D ca b iu khin, nh chng ta tho luntrong cng thc (2) th thnh phn D c tnh l: dPart=Kd*(Err-

pre_Err)/Sampling_time, trong pre_Err l gi tr sai s ln ly mu

trc c lu li. Do 1/Sampling_time = inv_Sampling_time nn chng tac th thay dng tnh dPart bng cng thc trong dng 35: dPart=Kd*(Err-pre_Err)*inv_Sampling_time. Dng 36 tnh thnh phn I (iPart), s dng

phng php cng dn ( quy) chng ta thu c iPart bng iPart trc cng vi din tch hnh ch nht sai s hin ti:iPart+=Ki*Sampling_time*(Err+pre_Err)/1000. Chng ta phi chia iPartcho 1000 v Sampling_time c tnh theo ms trong khi n v tnh ton


22/22

chun trong l s. Cng cc thnh phn ny li chng ta c gi tr Outputtng hp trong dng 37. Tuy nhin, theo l thng th cng thc dng 37

phi l Output=pPart+dPart+iPart nhng y li l :Output+=pPart+dPart+iPart ( du + trc du =), ngha l Outputc cng dn thay v l tng tc thi nh chng ta tho lun trong phngii thut PID. Tht ra vic ny cng d hiu. Trong bi ton iu khin vtr, khi sai s bng 0 chng ta c th dng b iu khin (u=0) nhng trong

bi ton iu khin vn tc, khi sai s bng 0 th gi tr u vn phi c gil gi tr trc .V vy, trong bi ton iu khin vn tc gi tr Outputc cng dn thay v gn trc tip, bn phi ghi nh iu ny trong ccng dng iu khin ca mnh. Hai dng 40 v 41 xt trng hp bo ha(saturation) khi Output vt qu gii hn cho php ca PWM (xn 2 u).Cui cng l gn gi tr tnh ton c t PID cho thanh ghi OCR1A tnghoc gim duty cycle ca PWM trn chn OC1A (ni vi PWM ca Motor)

v gn gi tr sai s Err cho bin pre_Err cho ln ly mu sau dng n.Chy m phng: ton b chng trnh v c mch in m phng

c ti to sn. Ngi c ch cn c hiu v chy m phng mch in.Ch khi chy m phng hy thay i cc switches thay i vn tc cniu khin. Gi tr vn tc thc cht l s xung encoder trong 25 ms, ngic hy t tnh ra s vng /s. Do m hnh motor trong phn mm m phngkhng hon ho lm nn p ng b iu khin hi chm, bn c th phich mt khong thi gian thy vn tc Motor t n vn tc yu cu.Hay thay gi tr Kd trong dng 23 thnh 1 hoc 0, bin dch li chng trnhv m phng quan st v so snh ovetshot (s vt qu) ca h thng.

PID Servo Avr

Documents

Transcript of PID Servo Avr