Motor Control. 5. Control. Motor Control. Motor Control

Transcription

1 5. Conrol In his chaper we will do: Feedback Conrol On/Off Conroller PID Conroller Moor Conrol Why use conrol a all? Correc or wrong? Supplying a cerain volage / pulsewidh will make he moor spin a a cerain speed. Wrong! This will only work in idle condiion, no under load (e.g. driving up a slope) Bräunl Bräunl Moor Conrol Using a feedback conrol will make sure he desired speed is mainained under load. If moor goes oo slow accelerae If moor goes oo fas brake How is his done? (1) Read curren speed (2) Compare wih desired speed (3) Change moor oupu accordingly Bräunl Moor Conrol Noe: Time delay of conroller (for measuremen and calculaion) Time delay of moor (physical reacion ime of acuaor in real world) Inaccuracy of encoder daa Inaccuracy of acuaor movemen Bräunl

2 5.1 OnOff Conroller Simples case: desired speed ac.< des.? Only on/off of conrol value BangBang Conroller R() acual speed *K c Moor 0 or 1 0 or K c feedback encoder measuremen Bräunl OnOff Conroller BangBangConroller (onoff conroller) K C if v ac () < v des () R() = 0 else v acual R() Conrol signal acceleraes acuaor Wihou conrol signal, acuaor slows down May add hyseresis o avoid quick oscillaion Bräunl OnOff Conroller Noe: Updae is no coninuous Updae is e.g. every 10ms OnOff Conroller wih Hyseresis R( ) = K C if v ac () < v on () 0 if v ac () > v off () R() else v acual R() v acual R() v off v on Bräunl Bräunl

3 OnOff Conroller wih Hyseresis R( ) = R() K C if v ac () < v on () 0 if v ac () > v off () R() else OnOff Conroller BangBangConroller (onoff conroller) K C if v ac () < v des () R() = 0 else THEORY and PRACTICE v on v off v ac in main() { /* moor conrol */????? reurn 0; } Bräunl Bräunl OnOff Conroller So, we have a really simple formula, bu how can we ransform i ino a program??? desired speed ac.<des? *K c R() Moor acual speed 0 or 1 0 or K c encoder measuremen feedback 1. Wrie a conrol subrouine a. Read encoder daa (INPUT) b. Compue new oupu value R() c. Se moor speed (OUTPUT) 2. Call conrol subrouine periodically e.g. every 1/100 s Bräunl OnOff Conroller From THEORY o PRACTICE 1. Wrie a conrol subrouine a. Read encoder daa (INPUT) b. Compue new oupu value R() c. Se moor speed (OUTPUT) See library.hml: in QUADRead(QuadHandle handle); Inpu: (handle) ONE decoderhandle Oupu: 32bi counervalue (2^31.. 2^311) Semanics: Read acual QuadraureDecoder couner, iniially zero. Noe: A wrong handle will ALSO resul in a 0 couner value!! in MOTORDrive (MoorHandle handle,in speed); Inpu: (handle) logicalor of all MoorHandles which should be driven (speed) moor speed in percen Valid values: (full backward o full forward) 0 for full sop Oupu: (reurn code) 0 = ok 1 = error wrong handle Semanics: Se he given moors o he same given speed Bräunl

4 OnOff Conroller OnOff Conroller From THEORY o PRACTICE 1. Wrie a conrol subrouine a. Read encoder daa (INPUT) b. Compue new oupu value R() c. Se moor speed (OUTPUT) void onoff_conroller() { in enc_new, r_mo, err; enc_new = QUADRead(enc1);... err = MOTORDrive(mo1, r_mo); if (err) prinf( error: moor ); } K C 0 if v ac () < v des () else in v_des; /* user inpu icks/s */ #define Kc 75 /* cons. speed seing*/ void onoff_conroller() { in enc_new, v_ac, r_mo, err; saic in enc_old; enc_new = QUADRead(enc1); v_ac = (enc_newenc_old)*100; if (v_ac < v_des) r_mo = Kc; else r_mo = 0; err = MOTORDrive(mo1, r_mo); if (err) prinf( error: moor ); enc_old = enc_new; } speed in encodericks per s assuming 100 calls per s variable needs o keep value for nex call of rouine he whole conroller Bräunl Bräunl OnOff Conroller OnOff Conroller So far, so good! However, is here a problem wih overflow/underflow?? Bräunl Subsequen encoder readings (example): FFF8 FFFE (6) 0002 (4) Encoder Overflow our program compues: FFFC which equals (2 s complemen): 0004 similar for underflow Bräunl

5 OnOff Conroller From THEORY o PRACTICE 2. Call conrol subrouine periodically e.g. every 1/100 s See library.hml: TimerHandle OSAachTimer(in scale, TimerFnc funcion); Inpu: (scale) prescale value for 100Hz Timer (1 o...) (TimerFnc) funcion o be called periodically Oupu: (TimerHandle) handle o reference he IRQslo A value of 0 indicaes an error due o a full lis(max. 16). Semanics: Aach a irqrouine (void funcion(void)) o he irqlis. The scale parameer adjuss he call frequency (100/scale Hz) of his rouine o allow many differen applicaions. in OSDeachTimer(TimerHandle handle) Inpu: (handle) handle of a previous insalled imer irq Oupu: 0 = handle no valid 1 = funcion successfully removed from imer irq lis Semanics: Deach a previously insalled irqrouine from he irqlis. Bräunl OnOff Conroller From THEORY o PRACTICE 2. Call conrol subrouine periodically e.g. every 1/100 s in main() { TimerHandle 1; 1 = OSAachTimer(1, onoff_conroller); while (1) /* endless loop never reurns */ { /* oher asks or idle */ } OSDeachTimer(1); /* no used */ reurn 0; /* no used */ } sar conroller 100 imes per s ypical for embedded sysems Bräunl OnOff Conroller From THEORY o PRACTICE 2. Call conrol subrouine periodically e.g. every 1/100 s OnOff Conroller in main() { TimerHandle 1; 1 = OSAachTimer(1, onoff_conroller); while (KEYRead()!= KEY4) /* check end key */ { /* se desired speed wih inpu keys */ } OSDeachTimer(1); reurn 0; } beer for our purpose velociy [icks/sec] ime [sec] V desired V moor Bräunl Bräunl

6 5.2 Proporional Conroller Pconroller (proporional conroller) R() = K p * (v des () v ac ()) Linear change of conrol value o reduce error beer conroller Proporional Conroller R() = K p * e() wih e() = v des () v ac () error funcion K p is a consan value, conroller gain K p mus be seleced o achieve fas goal speed wihou overshoo Noe: No equilibrium when desired velociy is reached: if v des () = v ac () hen R() = 0 desired speed acual speed * K P Moor subrac encoder measuremen feedback Bräunl v moor varying K p Bräunl Proporional Conroller R() = K p * e() wih e() = v des () v ac () error funcion Implemenaion in C: Proporional Conroller 6000 V desired V moor K p = 0.85 V moor K p = 0.45 V moor K p = 0.20 V moor K p = 0.15 e_func = v_des v_ac; r_mo = Kp*e_func; velociy [icks/sec] Bräunl Bräunl ime [sec]

7 v moor 5.3 Inegral Conroller Adding IConroller (Inegral Conroller) Par Problem: PConroller may reach equilibrium wihou reaching he arge velociy seady sae error Soluion: Inegral par is used o eliminae seady sae error R() = K p * [ e() 1/T I * 0 e()d] v moor Inegral Conroller R() = K p * [ e() 1/T I * 0 e()d] or equivalen wih K I = K p /T I : R() = K p * e() K I * 0 e()d Implemenaion Naïve approach: inegral corresponds o SUM use ARRAY o sore e.g. 10 las values and sum up P PI Bräunl Bräunl Inegral Conroller R() = K p * [ e() 1/T I * 0 e()d] or equivalen wih K I =K p /T I : R() = K p * e() K I * 0 e()d Inegral Conroller R() = K p * e() K I * 0 e(τ)dτ Implemenaion Proper soluion: Implemenaion in C: Use rapezoid rule f ( (x i1 x i )/2 ) Area (or inegral) is: (x i1 x i ) * f ( (x i1 x i )/2 ) R n R n1 = K P * (e n e n1 ) Q I * dela * (e n e n1 )/2 R n = R n1 K P * (e n e n1 ) K I * (e n e n1 )/2 x i1 x i Bräunl (x i1 x i )/2 Bräunl

8 Inegral Conroller R() = K p * e() K I * 0 e(τ)dτ saic in r_old=0, e_old=0;... e_func = v_des v_ac; r_mo = r_old Kp*(e_funce_old) Ki*(e_funce_old)/2; r_mo = min(r_mo, 100); /* limi oupu */ r_mo = max(r_mo, 100); /* limi oupu */ r_old = r_mo; e_old = e_func; Bräunl Inegral Conroller velociy [icks/sec] V desired P conrol only (K p =0.2) PI conrol (K p =0.2 K I =0.05) Bräunl ime [sec] v moor 5.4 Derivaive Conroller Adding DConroller (Derivaive Conroller) Par Problems: PConroller responds slow o change in inpu PConroller wih high gain ends o oscillae Soluion: Add a derivaive erm for response/damping R() = K p * [ e() 1/T I * 0 e(τ)dτ T D * de()/d] v moor Derivaive Conroller R() = K p * [ e() 1/T I * 0 e(τ)dτ T D * de()/d ] or equivalen wih K I =K p /T I and K D =K p * T D : R() = K p * e() K I * 0 e(τ)dτ K D * de()/d Implemenaion in C: Derivaive corresponds o DIFFERENCE PI PID Bräunl Bräunl

9 5.5 PID Conroller Full PID Soluion: R() = K p * e() K I * 0 e()d K D * de()/d R n R n1 = K P * (e n e n1 ) Q I * dela * (e n e n1 )/2 Q D / dela * (e n 2*e n1 e n2 ) R n = R n1 K P * (e n e n1 ) K I * (e n e n1 )/2 K D * (e n 2*e n1 e n2 ) PID Conroller R() = K p * e() K I * 0 e()d K D * de()/d (full PID) saic in r_old=0, e_old=0, e_old2=0;... e_func = v_des v_ac; r_mo = r_old Kp*(e_funce_old) Ki*(e_funce_old)/2 Kd*(e_func 2* e_old e_old2); r_mo = min(r_mo, 100); /* limi oupu */ r_mo = max(r_mo, 100); /* limi oupu */ e_old2 = e_old; e_old = e_func; r_old = r_mo; Bräunl Bräunl PID Conroller PID Conroller PIDConroller is combinaion of P, I, and D conroller Simple, universal conroller Tradeoffs: Response ime vs. sabiliy Noise limis max. proporional gain Noe physical limis of acuaor e.g. max acceleraion, velociy Bräunl Bräunl

10 PID Conroller PID Conroller Tuning: Find suiable parameer seings for PID conroller 1. Selec ypical operaing seing for desired speed, urn off inegral and derivaive par, hen increase K P o max. or unil oscillaion occurs. 2. If sysem oscillaes, divide K P by Increase K D and observe behavior when changing desired speed by abou 5%. Choose a value of K D which gives a fas damped response. 4. Slowly increase K I unil oscillaion sars. Then divide K I by 2 or 3. Bräunl Problems: Wha happens if desired speed > physical max. speed? Error always > 0 inegral erm accumulaes If he desired speed is reached for a possible speed, he inegral residue will make he conroller overshoo Bräunl PID Conroller Soluions: Use max. and min. values for inegral Sop Isummaion upon sauraion Use more sophisicaed velociy algorihms or analyical mehods Acknowledgemens Bales: PID formulas and uning Kasper: PID formulas Braunschädel: PID measuremens Bräunl Velociy Conrol vs. Posiion Conrol Velociy Conrol is wha we have achieved so far We can make a moor drive a a cerain speed and make i mainain ha speed Posiion Conrol also incorporaes a sar/sop phase and makes he moor sop a a cerain locaion E.g. exacly afer one full revoluion This is imporan for many applicaions, e.g. driving a vehicle Bräunl

11 Velociy Conrol vs. Posiion Conrol Example for posiion conrol: move objec from s 0 o s 1 Velociy Conrol vs. Posiion Conrol a v v max s s 1 Bräunl s 0 Bräunl Velociy Conrol vs. Posiion Conrol Simple assumpion: Consan acceleraion / deceleraion Linear velociy in sar/sop phase consan oherwise Quadraic disance in sar/sop phase linear oherwise Velociy Conrol vs. Posiion Conrol A pracical way of achieving posiion conrol Use velociy conrol in sar phase and coninuous phase Monior curren posiion and calculae begin of end phase Coninuously updae speed while deceleraing a v s s 1 Bräunl s 0 Bräunl 2006 s 44

12 5.7 Driving a Vehicle Driving a Vehicle PID LMo PID RMo Two independenly driven wheels Task 1: Sraigh line How can you synchronize he moors? 1. Use one PID conroller per wheel pros / cons? how can you avoid irregulariies? (each wheel is conrolled bu sligh disurbances will resul in curve) Bräunl Bräunl Driving a Vehicle Driving a Vehicle PID PID LMo I RMo desired speed PID PID LMo I RMo curve offse How can you synchronize he moors? 1. Use one PID conroller per wheel pros / cons? how can you avoid irregulariies? (each wheel is conrolled bu sligh disurbances will resul in curve) ake difference of wheel posiion, inegrae, and feed back o boh wheels Bräunl Task 2: Curve Adap srucure for sraigh line wih offse for boh wheels o se pah curvaure Bräunl

13 Driving a Vehicle Oher soluions are possible as well! E.g. Use one PID conroller for speed and one PID conroller for sraighness pros / cons? Bräunl