Pozicování pomocí dvou serv

Šimon Mik, UREL, FEEC, VUT Brno
xmiksi00 stud.feec.vutbr.cz

Miloš Juhás, UREL, FEEC, VUT Brno
xjuhas01 stud.feec.vutbr.cz

Obsah:

1. Úvod
2. Hardware
3. Software
4. Záver
5. Literatúra

Úvod

Servo je zařízení které umožňuje otáčet hřídelí v rozsahu zpravidla od -90° do +90°. Na vstupu serva jsou tři vodiče: zem, napájení a řídící napětí, které nastavuje polohu serva. Servo obsahuje stejnosměrný motor, převodovku a výstupní hřídel spraženou s potenciometrem pro detekci úhlu natočení [3].

Cílem projektu je ovládání dvou modelářských mikroserv Hitec HS-422 (obrázek 1) pomocí mikrokontroléru. První servo se bude natáčet v rozsahu -90° až +90°, druhé servo v rozsahu 0° až +90°. Serva se ovládají pomocí pulzně šířkové modulace (PWM) s periodou 20ms a šířkou pulzů přibližně 1 až 2ms. Šířka pulzu přímo úměrně odpovídá natočení výstupní hřídele. Pulzy s šířkou 1,5ms nastaví servo do střední polohy.

Hardware

V laboratoři je k dispozici vývojová deska s mikrokontrolérem Atmel ATmega 16, rovněž nám byla zapůjčena konstrukce se dvěma servy, která je plánovaná k využití natáčení web kamery. Bylo nutno tedy ještě vyrobit napájecí obvody. Proudový odběr serva HS-422 udává výrobce na 10mA v klidu a 280mA při pohybu při napětí 5V. Ve skutečnosti, však podle [4] může proudový odběr při rychlém střídání krajních poloh, kdy servo ještě nedosáhlo požadované krajní polohy být až 700mA. Pro napájení dvou serv je potřeba hodnotu vynásobit dvěmi, tedy maximální proudový odběr může dosáhnout hodnoty až 1,4A.

Bylo navrženo zapojení (obrázek 2) Do napájecího konektoru J1 je přivedeno nestabilizované napětí (max 30V) a na výstupu stabilizátoru 78S05 dostáváme stabilizované napětí +5V. Kondenzátory C2 a C4 slouží jako filtrační. Na svorkovnici X2 budou přivedeny z portu mikrokontroléru řídící signály pro obě serva. Odpory R4 a R5 nastavují proud do optočlenů na 20mA. Odpory R6 a R7 jsou trvale připojeny k napájecímu napětí (pull-up rezistory). Aby na řídícím výstupu SV2 a SV3 bylo nulové napětí, musí být optron zapnutý (na výstupu mikrokontroléru log. 1). Impulsy na řídícím výstupu SV2 a SV3 jsou generovány stažením výstupních signálů mikrokontroléru na log. 1.

Obr.2: Schéma zapojenia	Obr.3: Predloha DPS
Obr.4: Osadzovací plán TOP	Obr.5: Osadzovací plán BOTTOM

Software

Na ovládanie horizontálnej a vertikálnej polohy serv bola napísaná knižnica ServoHV.h, obsahujúca všetky potrebné funkcie. Táto knižnica je určená pre mikroprocesor ATmega16 pracujúcim na kmitočte 16 MHz. Využíva 16 bitový Timer/Counter1 v režime CTC, ktorý umožňuje presné nastavenie polohy. Tá je nastaviteľná s presnosťou na jeden stupeň, keďže funkcie používajú celočíselné premenné. Polohu je možné nastaviť buď absolútne alebo relatívne. Absolútna poloha sa zadáva v kladných stupňoch, zatiaľ čo relatívna v stupňoch so znamienkom. Pre oba spôsoby platí, že pokiaľ je požadovaná hodnota mimo rozsah obmädzený krajnými polohami serva, funkcia vráti 1 a polohu nebude nastavovať. V opačnom prípade sa nastaví poloha a návratová hodnota bude 0. Vzhľadom na dlhšiu dobu nastavenia požadovanej polohy okolo 800 ms, je vhodné relatívnu polohu meniť s hrubším krokom a až potom prípadne doladiť jemnejšie.

Nastavenie parametrov serva

Popis jednotlivých funkcií

void _servo_init( void );

Inicializuje porty, premenné, timer/counter1 a povolí globálne prerušenie.
Poznámka:V prípade, že sa využíva kalibrácia, je možne zrušiť komentár pred funkciou _get_cal() a povoliť tak automatické načítanie nastavenych parametrov z EEPROM. V tomto prípade je ale dôležité aby boli premenné v EEPROM na daných adresách skutočne uložené.

void _set_cal( char write);

Uloží do internej pamäte EEPROM tieto premenné: uiTopOcrV, uiBottomOcrV, ucTopDegV, ucBottomDegV, uiLeftOcrH, uiRightOcrH, ucLeftDegH, ucRightDegH,uiBaseOcr, ucSetCycles. Funkciu je možné použiť na kalibráciu alebo pri zmene kmitočtu oscilátora.
Parametre:
write = 0 // uloží tiež aktuálnu polohu, tj. premenné uiLastPosV, uiLastPosH
write != 0 // neukladá premenné uiLastV, uiLastH.

void _get_cal( char read);

Načíta uložené kalibračné premenné z internej EEPROM, viz. funkcia _set_cal().
Parametre:
read = 0 // načíta tiež aktuálnu polohu, tj. premenné uiLastPosV, uiLastPosH
read != 0 // nenačíta premenné uiLastV, uiLastH.

unsigned char _abs_pos( unsigned char setPosH, unsigned char setPosV);

Nastaví absolútne zadanú polohu v oboch osiach.
Parametre:
setPosH // celočíselná hodnota horizontálneho natočenia v stupňoch, z nastaveného rozsahu,
SetPosV // celočíselná hodnota vertikálneho natočenia v stupňoch, z nastaveného rozsahu.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe (uhoľ mimo rozsahu)

unsigned char _rel_pos( char setOffsetH, char setOffsetV);

Nastaví relatívne zadanú polohu serva v oboch osiach.
Parametre:
setOffsetH // celočíselná znamienková hodnota uhľa, o ktorý sa natočí horizontálne servo voči aktuálnej polohe, pričom nesmie byť prekročená medzná hodnota
SetOffsetV // celočíselná znamienková hodnota uhľa, o ktorý sa natočí vertikálne servo vočí aktuálnej polohe, pričom nesmie byť prekročená medzná hodnota.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe (uhoľ mimo rozsahu)

unsigned char _center_pos( void );

Nastaví stredovú polohu oboch serv. Tá sa počíta ako aritmetický priemer krajných polôh, čo by pre vertikálne servo znamenalo v defaultnom nastavení 45°. Funkcia obsahuje niekoľko zakomentovaných riadkov, a prepínaním komentárov je tak možné určiť, čo bude brané ako stred.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe

void _start_intro_1( void )

Spustí sa prednastavená sekvencia príkazov na zmenu polohy serva: h_center_pos(); h_right_pos(); h_left_pos(); h_center_pos(); v_center_pos(); v_top_pos(); v_bottom_pos(); v_center_pos(); Tá môže slúžiť buď na otestovanie funkcií alebo ako intro po zapnutí.

void _start_intro_2( void )

Spustí sa prednastavená sekvencia príkazov na zmenu polohy serva: _abs_pos(90,0); h_abs_pos(180); h_abs_pos(0); h_abs_pos(135); h_abs_pos(45); h_abs_pos(90); v_abs_pos(90); v_abs_pos(45); v_abs_pos(0); v_abs_pos(90); v_abs_pos(0); Tá môže slúžiť buď na otestovanie funkcií alebo ako intro po zapnutí.

void v_set_const( void );

Aktualizuje hodnotu premennej uiConstV, ktorá sa používa pri prepočte stupňov na šírku pulzu pre vertikálne servo. Volá ju tiež _servo_init(), takže ju nie je nutné volať, pokiaľ nedôjde k zmenám parametrov počas behu programu (napr. pri kalibrácii).

unsigned char v_abs_pos( unsigned char setPosV);

Nastaví absolútne zadanú polohu vertikálneho serva.
Parametre:
SetPosV // celočíselná hodnota vertikálneho natočenia v stupňoch, z nastaveného rozsahu.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe (uhoľ mimo rozsahu)

unsigned char v_top_pos( void );

Nastaví vrchnú krajnú polohu vertikálneho serva.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe

unsigned char v_center_pos( void );

Nastaví stredovú polohu vertikálneho serva. Tá sa počíta ako aritmetický priemer krajných polôh, čo by pre vertikálne servo znamenalo v defaultnom nastavení 45°. Funkcia obsahuje niekoľko zakomentovaných riadkov, a prepínaním komentárov je tak možné určiť, čo bude brané ako stred.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe

unsigned char v_bottom_pos( void );

Nastaví spodnú krajnú polohu vertikálneho serva.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe

unsigned char v_rel_pos( char setOffsetV);

Nastaví relatívne zadanú polohu vertikálneho serva.
Parametre:
SetOffsetV // celočíselná znamienková hodnota uhľa, o ktorý sa natočí vertikálne servo vočí aktuálnej polohe, pričom nesmie byť prekročená medzná hodnota.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe (uhoľ mimo rozsahu)

unsigned int v_get_deg( void );

Vráti celočíselnú hodnotu naposledy nastavenej polohy vertikálneho serva v stuňoch. Hodnota nie je uložená v žiadnej premennej a dopočítava sa. Keďže presnosť funkcie je nižšia ako presnosť funkcie, ktorá nastavuje natočenie serva, je možná odchýlka o 1°.
Návratová hodnota:
Hodnota registra uiLastPosV prepočítaná na stupňe.

void h_set_const( void );

Aktualizuje hodnotu premennej uiConstH, ktorá sa používa pri prepočte stupňov na šírku pulzu pre horizontálne servo. Volá ju tiež _servo_init(), takže ju nie je nutné volať, pokiaľ nedôjde k zmenám parametrov počas behu programu (napr. pri kalibrácii).

unsigned char h_abs_pos( unsigned char setPosH);

Nastaví absolútne zadanú polohu horizontálneho serva.
Parametre:
SetPosH // celočíselná hodnota horizontálneho natočenia v stupňoch, z nastaveného rozsahu.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe (uhoľ mimo rozsahu)

unsigned char h_right_pos( void );

Nastaví pravú krajnú polohu horizontálneho serva.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe

unsigned char h_center_pos( void );

Nastaví stredovú polohu horizontálneho serva, ktorá je aritmetickým priemerom pravej a lavej krajnej polohy.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe

unsigned char h_left_pos( void );

Nastaví lavú krajnú polohu horizontálneho serva.
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe

unsigned char h_rel_pos( char setOffsetH);

Nastaví relatívne zadanú polohu horizontálneho serva.
Parametre:
setOffsetH // celočíselná znamienková hodnota uhľa, o ktorý sa natočí horizontálne servo voči aktuálnej polohe, pričom nesmie byť prekročená medzná hodnota
Návratová hodnota:
0 - funkcia bola vykonaná správne,
1 - pri vykonávaní funkcie došlo k chybe (uhoľ mimo rozsahu)

unsigned int h_get_deg( void );

Vráti celočíselnú hodnotu naposledy nastavenej polohy horizontálneho serva v stuňoch. Hodnota nie je uložená v žiadnej premennej a dopočítava sa. Keďže presnosť funkcie je nižšia ako presnosť funkcie, ktorá nastavuje natočenie serva, je možná odchýlka o 1°.
Návratová hodnota:
Hodnota registra uiLastPosH prepočítaná na stupňe.

 
void _set_uiBaseOcr(unsigned int val); 
void _set_ucSetCycles(unsigned char val); 
void _set_uiTopOcrV(unsigned int val); 
void _set_uiBottomOcrV(unsigned int val); 
void _set_uiLastPosV(unsigned int val); 
void _set_uiLeftOcrH(unsigned int val); 
void _set_uiRightOcrH(unsigned int val); 
void _set_uiLastPosH(unsigned int val); 
void _set_ucTopDegV(unsigned char val); 
void _set_ucBottomDegV(unsigned char val); 
void _set_ucLeftDegH(unsigned char val); 
void _set_ucRightDegH(unsigned char val); 
unsigned int _get_uiBaseOcr(void); 
unsigned char _get_ucSetCycles(void); 
unsigned int _get_uiTopOcrV(void); 
unsigned int _get_uiBottomOcrV(void); 
unsigned int _get_uiLastPosV(void); 
unsigned int _get_uiLeftOcrH(void); 
unsigned int _get_uiRightOcrH(void); 
unsigned int _get_uiLastPosH(void); 
unsigned char _get_ucTopDegV(void); 
unsigned char _get_ucBottomDegV(void); 
unsigned char _get_ucLeftDegH(void); 
unsigned char _get_ucRightDegH(void); 
 

Sada funkcií slúžiacich na prácu s globálnymi parametrami. Funkcie s prefixom _set_ nastavujú hodnotu premennej a funkcie s prefixom _get_ naopak hodnotu premennej vracajú. Názov funkcie za prefixom je totožný s názvom premennej, s ktorou sa pracuje. Tieto funkcie sú určené na kalibráciu a je vhodné ich použiť s funkciami _get_cal() a _set_cal().

Inicializácia serva

   
int main(void)
{                
    _servo_init();                               
    if(!_abs_pos(90,0))   
        error();                
    while(1);                
    return 0;
}             
   

Kalibrácia

             
// Uloženie parametrov           
int ulozenie(void)
{             
    _set_LastPosV(3000);             
    _set_cal(0);             
    sei();             
    while(1);             
return 0;
}             
             

                  
// Načítanie uložených parametrov                
int nacitanie(void)
{                  
    _get_cal(0);                  
    _servo_init();                  
    while(1);                  
return 0;
}                  
                  

                  
// Obnovenie pôvodných parametrov                
int obnovenie(void)
{                 
    _set_cal(0);                  
    _servo_init();                  
    while(1);                  
return 0;
}                  
                  

Záver

Download

Literatúra