dr inĪ. Marian Gilewski Politechnika Biaáostocka
ALGORYTM STERUJĄCY SERWOMECHANIZMAMI
RAMIENIA ROBOTA AL5A
W artykule przedstawiono koncepcjĊ sterowania ramieniem AL5A z zastosowaniem ukáadu FPGA. Serwomechanizmy AL5A posiadają autonomiczne algorytmy zaimplementowane w jĊzyku VHDL. Opracowany jednoukáadowy system cyfrowy moĪe obsáuĪyü pracĊ systemu przez kilka minut.
THE AL5A ROBOT’S CONTROL ALGORITHM
This paper presents an idea of AL5A robot’s control algorithm. Servo motors of AL5A have autonomous algorithms. Each algorithm was implemented into VHDL code. The FPGA on-chip control system can maintain AL5A arm for a few minutes.
1. WSTĉP
RamiĊ robota AL5A [1] zawiera piĊü niezaleĪnie sterowanych serwomechanizmów. KaĪdy z serwomechanizmów áączy siĊ z systemem sterującym za poĞrednictwem trójprzewodowej magistrali zasilająco-sterującej. Magistrala zawiera przewody doprowadzające: staáe napiĊcie zasilające, sygnaá sterujący oraz wspólny przewód masy. Sygnaá sterujący jest cyfrowym przebiegiem prostokątnym w standardzie LVTTL, o czĊstotliwoĞci 50 Hz i zmiennym wspóáczynniku wypeánienia. Czas trwania impulsu sterującego zawiera siĊ w przedziale od 0,6 ms do 2,4 ms, co zapewnia obrót waáu serwomechanizmu o kąt od –90° do +90° w stosunku do poáoĪenia neutralnego. PoáoĪeniu neutralnemu odpowiada impuls o czasie trwania 1,5 ms. MoĪliwe są róĪne implementacje systemu sterującego pracą omawianego ramienia. W publikacji przedstawiono koncepcjĊ z uĪyciem struktury programowalnej FPGA. Do badaĔ wybrano ukáad EP2C70F896C6 [2] wystĊpujący w module prototypowym DE2-70 [3] firmy Altera (rys. 1).
Schemat blokowy ukáadu sterującego ramieniem robota, bĊdący jednoczeĞnie zapisem algorytmu sterującego na platformie Quartus II [4], przedstawiono na rys. 2.
Rys. 2. Schemat ukáadu sterującego
Zawiera on nadrzĊdny moduá sterujący Ster_caly, który koordynuje pracĊ poszczególnych serwomechanizmów. Jego rolą jest ustawienie pozycji początkowych serwomechanizmów a nastĊpnie podawanie sygnaáów sterujących ich pracą za poĞrednictwem 13 bitowych magistral, zawierających sygnaáy: F[1..0], Limit_L[3..0], Limit_H[3..0], KEY i PW[1..0]. Do moduáu nadrzĊdnego doprowadzone są dwa sygnaáy sterujące: sygnaá zegarowy o czĊstoliwoĞci 50 MHz Clk50MHz i sygnaá zerujący Reset. Moduá ten moĪna zrealizowaü jako pamiĊü staáą, której komórki przechowują nastawy zmiany poáoĪenia waáów serwomechanizmów w kolejnych krokach pracy ramienia. W takim przypadku obsáuga moduáu nadrzĊdnego sprowadza siĊ do cyklicznego, z czĊstotliwoĞcią 50 Hz, generowania adresów pamiĊci.
Alternatywą moĪe byü implementacja w kodzie VHDL równaĔ opisujących trajektorie zmiany poáoĪenia ramienia. Biorąc pod uwagĊ, iĪ ukáad EP2C70F896C6 zawiera okoáo 1 200 000 bitów pamiĊci wewnĊtrznej, zastosowanie w omawianej realizacji 65-bitowych sáów pozwala na zapamiĊtanie ponad 18 000 kroków, czyli umoĪliwia ponad 6-minutową pracĊ ramienia. Jest to niewątpliwie rozwiązanie i oszacowanie nadmiarowe jeĪeli chodzi o wykorzystanie pamiĊci, gdyĪ nie wszystkie serwomechanizmy muszą pracowaü w tym samym czasie oraz zmieniaü swoje poáoĪenie w maksymalnym zakresie. Wspominany ukáad FPGA naleĪy do rozwiązaĔ Ğredniej wielkoĞci, najwiĊksze matryce zawierają pamiĊci wewnĊtrzne nawet 10-krotnie wiĊksze. Istnieje równieĪ moĪliwoĞü zastosowania dodatkowych pamiĊci zewnĊtrznych, wówczas dysponowany czas pracy niepowtarzalnego algorytmu wydáuĪa siĊ do kilkudziesiĊciu godzin (pamiĊci SDRAM o pojemnoĞci gigabitów).
W proponowanej aplikacji zastosowano 5 identycznych moduáów Serwom nadzorujących pracĊ poszczególnych serwomechanizmów, które sterowane są podobnymi zunifikowanymi sygnaáami. Implementacja pojedynczego moduáu absorbuje ponad 1000 makrokomórek struktury FPGA, co stanowi okoáo 1 % zasobów logicznych uĪytego ukáadu.
2. MODUà STERUJĄCY POJEDYēCZYM SERWOMECHANIZMEM
StrukturĊ moduáu obsáugującego pojedynczy serwomechanizm przedstawiono na rysunku trzecim. Znaczenie sygnaáów zewnĊtrznych jest nastĊpujące:
- dwubitowa magistrala F[1..0] umoĪliwia ustawienie wartoĞci zmiany poáoĪenia waáu w kolejnym kroku (o 1°, 2°, 5° lub 10°),
- czterobitowe magistrale Limit_L[3..0] i Limit_H[3..0] okreĞlają skrajne lewe i prawe poáoĪenie waáu wzglĊdem poáoĪenia neutralnego (–90°, –85°, ...; 90°, 85°, ...),
- dwubitowa magistrala PW[1..0] wymusza liczbĊ powtórzeĔ bieĪącego poáoĪenia waáu (brak powtórzenia, 5 powtórzeĔ, 10 powtórzeĔ, dowolna liczba powtórzeĔ zakoĔczona rĊcznym wyzwoleniem),
- KEY sygnaá rĊcznego wyzwalania,
- Wy_PWM jest sygnaáem wyjĞciowym, podawanym bezpoĞrednio na liniĊ sterującą serwomechanizmu,
- pozostaáe sygnaáy (oHEX0_D[6..0]...oHEX7_D[6..0], Szer_Imp[7..0], Zer50Hz, Gen50Hz) peániáy pomocnicze punkty pomiarowe w czasie uruchamiania moduáu.
Rys. 3. Schemat moduáu sterującego serwomechanizmem AL5A
Generator sygnaáów Gen_tor na podstawie przebiegu okresowego 50 MHz wytwarza trzy sygnaáy: F100kHz o czĊstotliwoĞci 100 kHz, Gen50Hz o czĊstotliwoĞci bazowej 50 Hz oraz Zer50Hz krótkie impulsy zerujące o czĊstotliwoĞci 50 Hz. Blok Cykl synchronizowany sygnaáem Gen50Hz na podstawie nastaw KEY i PW[1..0] wytwarza sygnaá Key_out inicjujący lub wstrzymujący zmianĊ poáoĪenia waáu w nastĊpnym kroku. Automat stanów Licznik_sterow na podstawie nastawy F[1..0] oraz sygnaáów Key_out, Skok_2 (z bloku Limit_kata) i Gen50Hz wytwarza kod poáoĪenia waáu Adr_ROM[7..0] w nastĊpnym kroku. Kod ten niesie informacjĊ o przyroĞcie kąta oraz kierunku (w prawo lub lewo) poáoĪenia. DáugoĞü kodu okreĞla minimalną rozdzielczoĞü zmiany poáoĪenia w kolejnym kroku; w badanym przypadku zastosowano kod 8-bitowy, co pozwoliáo uzyskaü 1-stopniową rozdzielczoĞü. Limit_kata porównuje wartoĞü kodu poáoĪenia z wartoĞciami nastaw Limit_L[3..0] i Limit_H[3..0], jeĪeli zostaáo osiągniĊte skrajne poáoĪenie blok generuje sygnaá Skok_2 zmiany kierunku obrotu waáu. Dla potrzeb diagnostycznych podczas badaĔ
blok wytwarzaá dodatkowe sygnaáy diagnostyczne oraz obsáugujące wyĞwietlacze DE2. PWM jest sterowanym przerzutnikiem monostabilnym, który na podstawie kodu poáoĪenia Adr_ROM[7..0] wytwarza impuls prostokątny o wymaganym czasie trwania z rozdzielczoĞcią sygnaáu F100kHz a nastĊpnie przechodzi w stan zawieszenia. Ze stanu zawieszenia przerzutnik jest wyprowadzany sygnaáem Zer50Hz po upáywie 20 ms, generując kolejny impuls na narastającym zboczu sygnaáu Gen50Hz.
3. WYBRANE WYNIKI BADAē MODUàU SERWOMECHANIZMU
W tej czĊĞci pracy przedstawiono wyniki badaĔ symulacyjnych oraz laboratoryjnych w wybranych punktach moduáu sterującego serwomechanizmem. Na rys. 4 przedstawiono zachowanie ukáadu po jego wyzerowaniu. Zerowanie ukáadu nastĊpuje w czasie wysokiego poziomu na wejĞciu Reset, w tym stanie serwomechanizm ustawia siĊ w poáoĪenie neutralne, tj. 0°. Po zaniku sygnaáu zerującego pojawia siĊ narastające zbocze sygnaáu Gen50Hz oraz sygnaá F100kHz, po upáywie 0,6 ms rozpoczyna siĊ impuls sterujący serwomechanizmem (poziom niski sygnaáu Wy_PWM). WartoĞü 0,6 ms jest minimalnym czasem trwania impulsu akceptowanym przez serwomechanizm, odpowiadającym poáoĪeniu –90°.
Rys. 4. Zerowanie moduáu
Rys. 5. Inne sygnaáy sterujące
Symulacja w dáuĪszym przedziale czasowym (rys. 5) pokazuje synchronizacjĊ wytwarzania impulsów sterujących Wy_PWM narastającymi zboczami przebiegu Gen50Hz. Impulsy zerujące Zer50Hz wyprowadzają przerzutnik sterowany PWM ze stanu zawieszenia tuĪ przed pojawieniem siĊ kolejnego narastającego zbocza Gen50Hz. Przedstawione wyniki symulacji dotyczyáy sytuacji zatrzymania serwomechanizmu w tym samym poáoĪeniu, czas trwania impulsów Wy_PWM nie ulegaá zmianie. Lewe i prawe skrajne poáoĪenia byáy ustawione w wartoĞci maksymalne, tzn. –90° i +90° (Lim_L = "0000”, Lim_H = "0000”). Kolejne dwie charakterystyki (rys. 6 i rys. 7) przedstawiają wyniki badaĔ laboratoryjnych, odpowiadające poáoĪeniu waáu w pozycji –90° oraz +90°. Ujemna notacja sygnaáów
Wy_PWM (aktywny sygnaá niski) wynika z faktu iĪ w ukáadzie elektronicznym elementy izolacji galwanicznej odwracają fazĊ sygnaáu.
Rys. 6. Oscylogramy sygnaáu Wy_PWM (przebieg 1) oraz Gen50Hz (przebieg 3) w pozycji waáu –90°
Rys. 7. Oscylogramy sygnaáu Wy_PWM (przebieg 1) oraz Gen50Hz (przebieg 3) w pozycji waáu +90°
Dalsze wyniki badaĔ symulacyjnych dotyczą odcinków czasu rzĊdu setek sekund. Z tego wzglĊdu nie bĊdzie prezentowany sygnaá wyjĞciowy Wy_PWM lecz jego odpowiedniki w postaci dziesiĊtnej kodów poáoĪenia Szer_Imp[7..0]. Takie podejĞcie nie wymaga symulacji caáego ukáadu w tak dáugim czasie. Na rys. 8 przedstawiono wynik symulacji
realizacji algorytmu cyklicznej zmiany poáoĪenia serwomechanizmu ze skokiem 10° miĊdzy skrajnym lewym poáoĪeniem okreĞlonym kodem Lim_L = "1100” (–50°, Szer_Imp = 40) a skrajnym prawym poáoĪeniem okreĞlonym kodem Lim_H = "1100” (+50°, Szer_Imp = 140).
Rys. 8. Kolejne zmiany poáoĪenia waáu ze skokiem 10°
Zmiana wartoĞci magistrali F[1..0] pozwala wielokrotnie powtórzyü to samo poáoĪenie waáu w kolejnych krokach (rys. 9). Ustawienie F[1..0] = "11” powoduje przejĞcie w tryb rĊcznego wyzwalania. W tym trybie ukáad po wygenerowaniu impulsu powiela go w kolejnych cyklach (rys. 10) aĪ do pojawienia siĊ niskiego poziomu sygnaáu na wejĞciu KEY.
Rys. 9. Zatrzymanie zmiany poáoĪenia waáu serwomechanizmu przez 5 cykli
Rys. 10. RĊczne wyzwalanie zmiany poáoĪenia waáu
Sygnaáy aktywujące, o róĪnych czasach trwania, mogą pojawiaü siĊ z róĪnymi interwaáami czasowymi co jest cechą rĊcznego wyzwalania. W publikacji nie przedstawiono kodów Ĩródáowych opisujących w VHDL funkcjonowanie poszczególnych bloków ze wzglĊdu na ich obszernoĞü. KaĪdy z elementów struktury hierarchicznej (rys. 3) zostaá zdefiniowany w wersji VHDL’93 [5] i uruchomiony z zastosowaniem platformy projektowej Quartus II Web Edition dedykowanej do syntezy systemów w ukáadach PLD firmy Altera.
4. PODSUMOWANIE
Celem pracy byáo sprawdzenie moĪliwoĞci implementacji algorytmu sterującego ramieniem robota AL5A w strukturze programowalnej FPGA. Jako docelowy ukáad wybrano matrycĊ rodziny Cyclone II typu EP2C70F896C6 firmy Altera. Byá to Ğredniej wielkoĞci ukáad programowalny. Algorytm kontrolujący pracĊ jednego serwomechanizmu zawieraá kod obejmujący okoáo 2500 wierszy w jĊzyku VHDL. W wyniku jego implementacji wykorzystano tylko 1 % dostĊpnych zasobów matrycy logicznej FPGA. Oznacza to, iĪ w badanej strukturze moĪna zaimplementowaü zarówno 5 algorytmów kontrolujących pracĊ wszystkich serwomechanizmów jak teĪ moduá nadrzĊdny koordynujący pracĊ caáego ukáadu. Badania przeprowadzono w stosunku do rozwiązania "oszczĊdnego” funkcjonalnie, tzn. zastosowano rozdzielczoĞü zmiany poáoĪenia waáu na poziomie 1°, moĪliwoĞü zmiany przyrostu kąta zmiany poáoĪenia waáu w zakresie 4 wartoĞci oraz realizacjĊ nastaw skrajnych poáoĪeĔ z 5-stopniowym krokiem. Badania laboratoryjne wykazaáy, iĪ w celu uzyskania wiĊkszej páynnoĞci ruchu serwomechanizmu naleĪaáoby zastosowaü wiĊkszą rozdzielczoĞü, np. rzĊdu 0,1 stopnia. Do algorytmu sterującego nie wprowadzono mechanizmów linearyzacji charakterystyki odpowiedzi ukáadu na szerokoĞü impulsów wyzwalających. PrzybliĪone pomiary wykazaáy zasadnoĞü zastosowania linearyzacji ukáadu, jednak ze wzglĊdu na brak odpowiedniej jakoĞci aparatury pomiarowej w tym przypadku nie uwzglĊdniono tego aspektu.
OdrĊbnym zagadnieniem pozostaje moĪliwy do osiągniĊcia czas pracy algorytmu. Zastosowany ukáad FPGA pozwalaá na kilkuminutową pracĊ niepowtarzalnej trajektorii ruchu. Rozwiązaniem problemu mogáoby byü zastosowanie wiĊkszego ukáadu lub zewnĊtrznych pamiĊci, co nie stanowiáoby ograniczeĔ przy tak maáych prĊdkoĞciach zmiany poáoĪenia. JĊzyk VHDL dziĊki rozbudowanym bibliotekom umoĪliwia syntezĊ zaawansowanych funkcji sterujących. Ze wzglĊdu na wiĊkszą przewidywalnoĞü poáoĪenia ramienia wybrana zostaáa wersja oparta na pamiĊciach i automatach stanu, gdyĪ nie wszystkie konstrukcje opisu behawioralnego w tym jĊzyku są w peáni syntezowalne.
PublikacjĊ przygotowano w ramach realizacji pracy statutowej S/WE/1/2006 finansowanej przez Ministerstwo Nauki i Szkolnictwa WyĪszego.
5. BIBLIOGRAFIA
1. Hitec Robotics, http://www.lunxmotion.com/c-124-al5a.aspx [opis robota, dostĊp online: 1 paĨdziernika 2010].
2. Altera Corporation: Cyclone II Device Family Data Sheet, 101 Innovation Drive, San Jose, California, 95134 USA, 2004.
3. Terasic Technologies Inc.: DE2-70 Development and Education Board - User Manual, ver. 1.08, huBei City, HsinChu County, Taiwan 302, http://www.terasic.com, 2009. 4. Altera Corporation: Introduction to the Quartus II Software, 101 Innovation Drive, San
Jose, California, 95134 USA, www.altera.com, 2010.
5. IEEE Computer Society: IEEE Standard VHDL Language Reference Manual, New York, NY 100016-5997, USA, 2002.
