mgr inĪ. àukasz Baádyga dr inĪ. Marian Gilewski Politechnika Biaáostocka
UKàAD STERUJĄCY ROBOTEM AL5A
W artykule opisano strukturĊ ukáadu sterującego ramieniem robota AL5A. Ukáad zawiera obwód zasilający, moduá izolacji galwanicznej oraz moduá wykonawczy z ukáadem FPGA. Matryca FPGA zawiera algorytm sterujący zaimplementowany w jĊzyku VHDL. Rozwiązanie charakteryzuje siĊ zwartą konstrukcją oraz stosunkowo maáym poborem mocy.
A CONTROL CIRCUITS OF AL5A ROBOT ARM
This paper describes an idea of AL5A robot’s control circuit. It includes of power supply circuit, optically coupled isolators and FPGA control chip. A control algorithm was implemented into the FPGA chip using VHDL language. The solution of control circuit has compacted structure and it is characterized by low power consumption.
1. WSTĉP
Obiektem badanym w analizowanym przypadku byáo ramiĊ robota AL5A [1, 2] (rys. 1) sterowane z ukáadu programowalnego FPGA. Ze wzglĊdu na elastycznoĞü struktury programowalnej zrezygnowano z firmowego rozwiązania sterownika opartego na mikrokontrolerze. Model AL5A zawiera 5 niezaleĪnie kontrolowanych serwomechanizmów.
Rys. 1. Widok ramienia robota AL5A firmy Lynxmotion
KaĪdy z nich poáączony jest trójprzewodową magistralą zawierającą dwa przewody zasilające oraz liniĊ sygnaáową. Konfiguracja wewnĊtrzna serwomechanizmu zawiera (rys. 2): komutatorowy silnik prądu staáego, przekáadniĊ mechaniczną, potencjometr sprzĊĪony z silnikiem oraz wyspecjalizowany scalony ukáad sterujący. UĪytkownik
ustawiając parametry impulsu w linii sygnaáowej wyznacza pozycjĊ, w której waá serwomechanizmu powinien znaleĨü siĊ, zaĞ serwomechanizm próbuje tĊ pozycjĊ osiągnąü i utrzymaü.
Rys. 2. Poglądowy widok ukáadu serwomechanizmu
Zakres zmian obrotu waáu badanych serwomechanizmów zawiera siĊ w przedziale od 0° do 180°. Sygnaá linii sterującej jest okresowym, prostokątnym przebiegiem sterującym o okresie 20 ms i zmiennym wspóáczynniku wypeánienia. WartoĞü czasu trwania impulsu z przedziaáu od 0,6 ms do 2,4 ms jednoznacznie okreĞla poáoĪenie waáu serwomechanizmu. WewnĊtrzny ukáad sterujący serwomechanizmu (rys. 3) stara siĊ minimalizowaü uchyb báĊdu przemieszczenia na podstawie wejĞciowego sygnaáu sterującego oraz sygnaáu zwrotnego z potencjometru sprzĊĪonego z waáem.
Rys. 3. Schemat blokowy serwomechanizmu
Model ramienia robota zawiera piĊü niezaleĪnie zasilanych i sterowanych serwomechanizmów. Stąd naturalnym rozwiązaniem centralnego ukáadu sterującego jest zastosowanie pracujących wspóábieĪnie, niezaleĪnych algorytmów dla poszczególnych serwomechanizmów. Implementacja takiego rozwiązania wydaje siĊ naturalna w strukturze FPGA. Synteza elastycznego algorytmu w strukturze jednoprocesorowej jest zbyt skomplikowana, z tego powodu zrezygnowano z firmowej aplikacji ukáadu sterującego opartej na mikrokontrolerze ATMEGA168-20PU.
2. SCHEMAT FUNKCJONALNY UKàADU STERUJĄCEGO
Elektroniczny ukáad sterujący pracą poszczególnych serwomechanizmów (rys. 4) zawiera: moduá prototypowy DE2 [3] z ukáadem FPGA, moduá izolacji galwanicznej záoĪony z piĊciu transoptorów oraz ukáad zasilający. W pracy wykorzystano moduá uruchomieniowy [4] z ukáadem FPGA EP2C70F896C6N, którego zasoby są wystarczające do implemetacji algorytmów sterujących. Ukáad FPGA dostarcza sygnaáy sterujące poszczególne serwomechanizmy poprzez záącze GPIO_1 moduáu DE2, który dodatkowo zawiera min. Ĩródáo sygnaáu synchronizującego o czĊstotliwoĞci 50 MHz oraz elementy zasilające strukturĊ. Sygnaá sterujący z wyjĞcia DE2 zasila obwód wejĞciowy transoptora CNY-17 izolujący galwanicznie FPGA z wybranym serwomechanizmem.
Rys. 4. Schemat ukáadu sterującego ramienia AL5A
Ukáad zasilający wytwarza napiĊcia staáe zasilające poszczególne serwomechanizmy oraz obwody wyjĞciowe transoptorów. W celu unikniĊcia sprzĊĪeĔ poprzez obwód zasilania kaĪdy z serwomechanizmów ma oddzielne stabilizowane Ĩródáo zasilania o napiĊciu 6 V i wydajnoĞci prądowej 1 A. W zaleĪnoĞci od wartoĞci obciąĪenia serwomechanizmu oraz amplitudy zmiany kąta obrotu waáu natĊĪenie prądu zasilającego moĪe zmieniaü siĊ w zakresie od kilkudziesiĊciu mA do 1 A. Istotna jest zatem maáa wartoĞü rezystancji wyjĞciowej oraz duĪa pojemnoĞü wyjĞciowa ukáadu zasilającego w celu zapewnienia wymaganej dynamiki zmian prądu zasilającego.
Sygnaáy sterujące poszczególne serwomechanizmy w strukturze FPGA moĪna wytwarzaü implementując niezaleĪne algorytmy lub korzystaü ze wspólnej tablicy (pamiĊci), której elementy zawierają wartoĞci nastaw poáoĪeĔ waáów w kolejnych krokach poáoĪenia chwytaka. Autonomiczne algorytmy efektywniej wykorzystują zasoby struktury programowalnej, eliminując nadmiarowoĞü informacji przechowywanej w ukáadzie tablicowym. Ukáad tablicowy jest prostszy w implementacji, gdyĪ przechowuje jedynie wartoĞci wspóáczynników wyznaczonych wczeĞniej w sposób analityczny. NiezaleĪnie od zastosowanej metody uzyskiwania nastaw poáoĪeĔ serwomechanizmów, koĔcowymi elementami ukáadu sterującego pozostają bloki wytwarzające sygnaá prostokątny 50 Hz o wymaganej wartoĞci wspóáczynnika wypeánienia. StrukturĊ przykáadowego bloku przedstawiono na rys. 5; jego projekt przygotowano z uĪyciem pakietu projektowego Quartus II Web Edition. Zawiera on trzy elementy funkcjonalne: generator sygnaáów sterujących (gen_tor), pamiĊü wspóáczynników (ROM) oraz generator impulsów wyjĞciowych (pwm). KaĪdy z elementów zdefiniowany zostaá z uĪyciem bibliotek
Ĩródáowych pakietu projektowego lub kodów Ĩródáowych napisanych w jĊzyku VHDL. Generator sygnaáów sterujących, na podstawie sygnaáu odniesienia 50 MHz z portu iCLK_50, wytwarza stabilny sygnaá 50 Hz (wyjĞcie out50Hz) oraz sygnaá 1 MHz (wyjĞcie out1MHz) i krótki impuls zerujący 50 Hz (wyjĞcie zer50Hz). Do wejĞcia ukáadu doprowadzony jest równieĪ sygnaá zerujący z portu iSW[4]. PamiĊü wspóáczynników przechowuje 12-bitowe dane okreĞlające poáoĪenia waáu serwomechanizmu w kolejnych krokach. WejĞciowa magistrala iSW[3..0] wybiera wartoĞü skoku poáoĪenia waáu – amplitudĊ zmiany kąta poáoĪenia. W badanym przypadku zakodowano cykliczną zmianĊ poáoĪenia waáu od 0° do 180° i z powrotem z moĪliwoĞcią zadawania skoku: 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 2, 5, 10, 30, 60 i 90 stopni. Generator impulsów wyjĞciowych wytwarza okresowy sygnaá prostokątny o wspóáczynniku wypeánienia zaleĪnym od wartoĞci 12-bitowych danych z wyjĞcia pamiĊci. Sygnaá z generatora wyprowadzany jest przez port wyjĞciowy GPIO_1[1]. Przykáadowy kod opisujący funkcjonowanie generatora impulsów przedstawiono w nastĊpnym rozdziale.
Rys. 5. Struktura bloku sterującego serwomechanizmem
3. OPROGRAMOWANIE WYBRANEGO MODUàU
Algorytm pracy generatora impulsów wyjĞciowych zakodowano w jĊzyku VHDL stosując technikĊ maszyny stanów. Sygnaáami wejĞciowymi są: przebieg okresowy 1 MHz (port
clk1MHz), 20 ns sygnaá zerujący o czĊstotliwoĞci 50 Hz (port reset50Hz) oraz 12-bitowa
magistrala wartoĞci czasu trwania impulsu wyjĞciowego (port szer_imp).
W stanie s0 odmierzana jest minimalna wartoĞü czasu trwania impulsu wyjĞciowego 0,6 ms, która odpowiada skrajnemu lewemu poáoĪeniu waáu serwomechanizmu. Po upáywie wymaganego czasu ukáad przechodzi do stanu s_low uzupeániającego czas trwania impulsu o wartoĞü odczytaną z pamiĊci ROM. Po wygenerowaniu impulsu o Īądanym czasie trwania moduá przechodzi w stan zawieszenia s_high. Po upáywie okresu 20 ms ukáad wyzerowany impulsem reset50Hz przechodzi ponownie do stanu s0. PoniĪej zawarto kompletny kod definiujący pracĊ moduáu. Sygnaá wyjĞciowy output zapisano w notacji odwrotnej (czasowi trwania impulsu przypisano stan niski) gdyĪ transoptor odwraca fazĊ.
library ieee; use ieee.std_logic_1164.all; use ieee.numeric_std.all; entity pwm is
port(clk1MHz, reset50Hz : in std_logic;
szer_imp : in unsigned(11 downto 0); -- nastawa szerokoĞci impulsu output : out std_logic);
end entity;
architecture rtl of pwm is
type state_type is (s0, s_low, s_high); signal state : state_type;
begin
variable tmp : unsigned(11 downto 0) := "000000000000"; begin
if reset50Hz = '1' then
output <= '1'; tmp := "000000000000"; state <= s0; elsif (rising_edge(clk1MHz)) then
case state is
when s0 => -- pomiar 0,6 ms = 600 okresów tmp := tmp + 1; output <= '0';
if tmp = "001001011000" then tmp := "000000000000"; state <= s_low; else state <= s0;
end if; when s_low =>
output <= '0'; tmp := tmp + 1;
if tmp = "011100001000" then -- 1,8 ms - przekroczenie nastaw state <= s_high; -- 1,8 + 0,6 = 2,4 ms
elsif tmp = szer_imp then state <= s_high; else state <= s_low;
end if;
when s_high => output <= '1'; state <= s_high; when others => state <= s0;
end case; end if; end process; end rtl;
Rys. 6. Przebiegi czasowe generatora po wyzerowaniu
Na rys. 6 przedstawiono wyniki symulacji zachowania ukáadu tuĪ po jego wyzerowaniu (iSW[4] = ‘1’). Widoczny jest sygnaá synchronizujący Icek_50 o czĊstotliwoĞci 50 MHz oraz wytworzony na jego podstawie sygnaá GPIO_1[3] o czĊstotliwoĞci 1 MHz. Narastające zbocze sygnaáu GPIO_1[3] rozpoczyna fazĊ s0 trwającą 0,6 ms. Z kolei na poniĪszym rysunku przedstawiono wyniki symulacji w dáuĪszej skali czasowej. Widoczne są na nim okresowe impulsy wyjĞciowe generatora (GPIO_1[1]) o czasie trwania poziomu niskiego proporcjonalnym do wartoĞci sáowa wyjĞciowego ROM i skoku zadanego nastawami iSW[0] – iSW[3]. Sygnaá GPIO_1[5] odzwierciedla okresowe impulsy zerujące 50 Hz.
4. PODSUMOWANIE
W pracy wykazano, iĪ moĪna zaimplementowaü algorytmy sterujące pracą piĊciu serwomechanizmów ramienia robota w pojedyĔczym ukáadzie FPGA Ğredniej mocy obliczeniowej. W analizowanym przypadku, jak wynika z zestawienia przedstawionego na rys. 8, do ponad 1,5 minutowej pracy serwomechanizmu ze zmianą kąta obrotu waáu, co 0,1° wykorzystano poniĪej 1 % dostĊpnych makrokomórek (591 total logic elements) oraz poniĪej 15 % pamiĊci wbudowanej (172416 total memory bits). Zaletą powyĪszego rozwiązania jest zwarta konstrukcja umoĪliwiająca syntezĊ jednoukáadowego cyfrowego systemu sterującego typu on-chip. DáuĪsze cykle pracy ramienia robota wymagaáyby zastosowania wiĊkszych struktur programowalnych FPGA. MoĪna w tym celu zastosowaü ukáady rodziny Stratix (Altera) lub Virtex (Xilinx), mające matryce logiczne i wbudowane pamiĊci ponad 10-krotnie wiĊksze niĪ zastosowana rodzina Cyclone II. Rozwiązaniem problemu byáoby równieĪ zastosowanie pamiĊci zewnĊtrznych, których pojemnoĞü jest elementem decydującym o czasie pracy algorytmu, rozdzielczoĞci zmiany poáoĪenia waáu serwomechanizmu oraz jej dynamice. Istotny jest teĪ rodzaj algorytmu sterującego, jest to jednak odrĊbne zagadnienie wykraczające poza zakres niniejszej publikacji.
Rys. 8. Raport wykorzystania zasobów struktury EP2C70
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. Baádyga à.: Implementacja algorytmu sterującego ramieniem robota w ukáadzie FPGA -
praca magisterska, Politechnika Biaáostocka, Wydziaá Elektryczny, 2010.
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: Cyclone II Device Family Data Sheet, 101 Innovation Drive, San
