konstrukcja i oprogramowanie dwukołowego robota mobilnego...
Post on 03-Sep-2019
2 Views
Preview:
TRANSCRIPT
Naprawachrekopisu
INSTYTUT CYBERNETYKI TECHNICZNEJPOLITECHNIKI WROCŁAWSKIEJ
Raportserii SPRnr 20/2002
Konstrukcja i oprogramowaniedwukołowegorobotamobilnego
MarekKabałaMarekWnuk
Słowakluczowe: robotmobilny, napedelektryczny, mikrokontroler, sterownik
Wrocław 2002
Spistr esci
1 Wstep 4
2 Konstrukcja mechaniczna 4
3 Sterownik 83.1 Budowasterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.2 Jednostkacentralna . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.3 Układypomiarowe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.3.1 Pomiarpredkosci i połozen kół . . . . . . . . . . . . . . . . . . . . . . . . . 133.3.2 Pomiarpredkosci katowej korpusu. . . . . . . . . . . . . . . . . . . . . . . 133.3.3 Pomiarpołozeniakatowegokorpusu. . . . . . . . . . . . . . . . . . . . . . 133.3.4 Pomiarpradówzasilajacychsilniki . . . . . . . . . . . . . . . . . . . . . . 143.3.5 Pomiarpołozeniaosi joystick-a . . . . . . . . . . . . . . . . . . . . . . . . 153.3.6 Pomiartemperaturyradiatoraukładuwykonawczego . . . . . . . . . . . . . 15
3.4 Układywykonawcze . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.5 Układzasilania . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
4 Oprogramowanie 184.1 Strukturaoprogramowania . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
4.1.1 Strukturaplików oprogramowania . . . . . . . . . . . . . . . . . . . . . . . 184.2 Opisglobalnychstrukturdanych . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
5 Eksperymenty 215.1 Badanieukładupomiarupołozeniakatowegokorpusu. . . . . . . . . . . . . . . . . 225.2 Badanieukładuregulacji pradu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225.3 Sterowaniew petli otwartej . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
5.3.1 Sterowanieskokowe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 255.3.2 Sterowaniesinusoidalne . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
5.4 Przykładprostegosterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
Podsumowanie 28
Bibliografia 28
Dodatek 30
Spisrysunków
1 Pierwszawersjamodelu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 Pierwszawersjamodelu– konstrukcja . . . . . . . . . . . . . . . . . . . . . . . . . 53 Konstrukcjakorpusurobota. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 Drugawersjamodelu,widok z przodu . . . . . . . . . . . . . . . . . . . . . . . . . 75 Drugawersjamodelu,widok z tyłu . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 Drugawersjamodelu– konstrukcja . . . . . . . . . . . . . . . . . . . . . . . . . . 87 Schematblokowy układusterownika . . . . . . . . . . . . . . . . . . . . . . . . . . 98 Zmontowany układsterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 Zdemontowanakasetkasterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . 1010 Interfejsumozliwiajacy prace w trybieBDM podłaczony do robota. . . . . . . . . . 1211 Jednostkacentralnasterowanika . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1212 Pomiarkataodchyleniawahadła.. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1413 Schematukładupomiarupradu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1514 Schematukładupomiarowegopołozeniaosi joystick-a . . . . . . . . . . . . . . . . 1515 Schematukładupomiarutemperatury . . . . . . . . . . . . . . . . . . . . . . . . . 1516 Schematmetodysterowaniaprademsilnika . . . . . . . . . . . . . . . . . . . . . . 1717 Strukturaoprogramowaniasterownika . . . . . . . . . . . . . . . . . . . . . . . . . 1918 Przyspieszeniaa1, a2 zmierzoneakcelerometrem.. . . . . . . . . . . . . . . . . . . 2219 Zmierzonapredkosc α orazwyliczony kat α. . . . . . . . . . . . . . . . . . . . . . 2220 Wykrespradówzadanych(Id0 � 1) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2321 Pradysilników zmierzonew trakcieekperymentu(I0 � 1). . . . . . . . . . . . . . . . . 2322 Wykrespradówzadanych(Id0 � 1) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2423 Pradysilników zmierzonew trakcieekperymentu(I0 � 1). . . . . . . . . . . . . . . . . 2424 Zmierzonepredkosci ( f idot0 � 1) i połozenia( f i0 � 1) kół. . . . . . . . . . . . . . . . . 2525 Zmierzonapredkosc katowa(al f adot) i połozenie(al f a) wahadła.. . . . . . . . . . 2526 Zmierzonepredkosci ( f idot0 � 1) i połozenia( f i0 � 1) kół. . . . . . . . . . . . . . . . . 2627 Zmierzonapredkosc katowa(al f adot) i połozenie(al f a) wahadła.. . . . . . . . . . 26
Spistablic
2 ParametrytechniczneprzekładniplanetarnejGP32C . . . . . . . . . . . . . . . . . 61 Parametrytechnicznesilnika A-max 32 . . . . . . . . . . . . . . . . . . . . . . . . 63 Opispól strukturystate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214 Opispól strukturycontrol . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
1 Wstep
Ze wzgledunabadaniaprowadzonew ZakładziePodstaw Cybernetykii Robotykidotyczaceosobli-wosci układówrobotycznych powstałpomysłkonstrukcjirobotakulistego nazwanego RoBall nape-dzanego przezzmiane połozeniajego srodkaciezkosci [3]. Wiele problemówprzwidywanych przykonstrukcjirobotakulistego jestwspólnychz konstrukcjabedaca tematemniniejszego raportu.Kon-struowany robot jestprojektemwstepnym majacym nacelurozwiazaniewspomnianych problemówzarównotechnicznych jak i ideowych.Wewstepnychzałozeniachmiałapowstackonstrukcjamozliwie tania,prostai mozliwadowykonaniaskromnymi srodkamitechnicznymi.W celuzbadaniamozliwoscipraktycznejrealizacjimodelurobotakulistegozaprojektowanoi wyko-nanodwukołowy wózekwykorzystujacy analogiczny sposóbprzekazywanianapedu.Jego konstruk-cja, znacznieprostszapod wzgledemmechanicznym od RoBall, dała sie zrealizowac skromnymisrodkami.Wózekpozwalabadac elementynapedui sterowanianiezbednedo zbudowaniarobotaRoBall. Sta-nowi nie tylko model doswiadczalny poprzedzajacy wykonaniewłasciwego robotakulistego, alerówniez samodzielnestanowisko do badaniaalgorytmówsterowaniaobiektemnieholonomicznymz uwarunkowaniamidynamicznymi (przekazywanie napedu z wykorzystaniemgrawitacji wymagauwzglednianiaparametrówdynamicznychrobotaprzy rozwiazywaniuzadanianawigacji [4]).
2 Konstrukcja mechaniczna
W pierwszejwersji robota(rysunek2), opisanejdokładniew [5] zostałyuzyte silniki i przekład-nie slimakowe pochodzacez demontazu, pochodzacez mechanizmówprzesuwupapierudrukarekDZM180. Elementemnosnym dla akumulatorów, układusterowaniaorazzespołównapedowychbyłkorpuswykonany z kształtowników zestopówlekkich. Łaczeniakształtowników wykonanezostałynitami. Korpusbył zawieszony nastalowym preciełaczacym osiedwóch,typowych,20–calowe kółrowerowych. Momentnapedowy z silników przekazywany był zaposrednictwemprzekładnislima-kowych i łancuchowychnakoła. Osiesilników połaczonebyły z koderamiprzyrostowymi.Zastosowanesilniki miały niska sprawnosc i mały momentnapedwy. Ze wzgleduna samohamow-nosc przekładnislimakowychorazwspomnianewadysilników uzyskanewyniki były bardzodalekieodoczekiwan i rezultatówsymulacji.Dlategodokonanoprzekonstruowaniarobotaw oparciuo nowezespołynapedowe. Zmieniony zostałrówniez sposóbmocowaniakasetkisterownika. Sposóbmo-cowaniaakumulatorów, modułównapedowych i sterownika do korpusuw pierwszeji drugiejwersjikonstrukcjiprzedstawionezostałynarysunku3.W nowej wersji (rysunki 4, 5 i 6) uzyte zostaływysokosprawne silniki pradustałego z magnesemtrwałym A-max 32 o mocy mechanicznej20W zaopatrzonew ceramiczne,planetarneprzekładnieGP32Co przełozeniu1:66. Szczgółowe parametrytechnicznesilników i przekładnizawartezostaływ tablicach1 i 2
4
I wersja II wersja
Rysunek3: Konstrukcjakorpusurobota
maksymany wyjsciowy 6 Nmmomentobrotowy
maksymalny pradciagły 1.02Asprawnosc 70 %
sredniluz stopniawysciowego 1o
Tablica2: ParametrytechniczneprzekładniplanetarnejGP32C
mocmechaniczna 20Wznamionowenapieciezasilania 30V
predkosc obrotowabezobciazenia 6130obr/minmaksymalny pradciagły 1.02A
maksymalny ciagłymomentobrotowy 47.1mNmsprawnosc 77%
Tablica1: ParametrytechnicznesilnikaA-max 32
Podobniejak w pierwszejwersji zastosowanodwie przekładnieposredniemiedzysilnikiem a ko-łem. Momentnapedowy z silników jestprzekazywany zaposrednictwemprzekładniplanetarnejnaprzekładnie łancuchowa. Koła łancuchowe posiadaja 3 tryby umozliwiajacedopasowanieprzełoze-nia przekładniłancuchowej do aktualnych potrzeb. Istniejemozliwosc wybraniajednego z trzechprzełozen: 0.8:1,1:1 oraz1:1.25. Maksymalna,uzytecznawartosc momentunapedowego kazdej zosiwynosi1.5Nm(przyprzełozeniu1:1).Zasadnicza wada zastosowanych przekładniłancuchowych jest wystepujacy w nich luz wynikajacyz nieosiowego zamocowaniakół zebatychwzgledemosi obrotu. Wadata objawia sie zaleznoscianapiecia łancuchówod połozen katowych kół, co uniemozliwia likwidacje luzu łancuchówza po-srednictwemsrubregulacyjnych. Przekładniete zostaływybranezewzgledunaich dostepnosc orazniskacene.Rozwiazaniemalternatywnym mogłyby byc zebateprzekładniepasowe. Zastosowanietakichprze-kładninie wymagałobyzmianw konstrukcjikorpusu.Inna mozliwoscia jestzrezygnowaniez przekładniposrednichmiedzymodułaminapedowymi a ko-
6
Rysunek6: Drugawersjamodelu– konstrukcja
łami i umieszczenieosi modułównapedowych w osi kół. Rozwiazanietakie wymagałobyjednakcałkowitej zmiany konstrukcjikorpusu.
3 Sterownik
Układyelektronicznerobotamuszazapewnic wykonaniewszystkichniezbednychpomiarówparame-trów ruchuorazrealizacjesterowan wyliczonychprzezalgorytmsterowania.Ze wzgledunazałozona autonomie robotajadnostkacentralnapowinnamiec wystarczajaca mocob-liczeniowadorealizacjialgorytmusterownikaw krótkim czasie,aukładyzasilaniapowinny posiadacwystarczajacy zapasenergii by zapewnic wykonanieserii doswiadczen.Przyszłezastosowanierobotajako stanowiskabadawczego wymagaabyimplementacjaalgorytmówsterowaniabyłaprostai niewymagałaodeksperymentatoraszczegółowej wiedzydotyczacejbudowyrobota.Komunikacjaz robotemmusizapewnic mozliwosc zdalnego zadawaniaparametróworazpobieraniadanychpomiarowych.Wektorstanuw modelumatematycznym robotazawierapołozenie
�x � y � w układziewspółrzednych
zwiazanym z podłozem, połozeniakatowe kół�ϕ1 � ϕ2 � mierzonewzgledemkorpusu,wychylenia
katowekorpusuα mierzonewzgledempionuorazzmienne�η1 � η2 � zalezneliniowo odpredkoscikół
i zmiennaη3 zalezna liniowo od predkosci katowej korpusuα.Układy sterownika musza zapewnic pomiar zmiennych
�ϕ1 � ϕ2 � ϕ1 � ϕ2 � α � α � . Połozenieglobalne�
x � y� moze byc obliczoneprzezprocedure odometrii na podstawie dostepnych parametrówruchu.Tak wiecsterownik powinien pozwalac naodtworzeniepełnego wektorastanumodelumatematycz-nego robota.Algorytmy sterowaniaoparteo modelmatematyczny tj. uwzgledniajacekinematyke i dynamike ro-botageneruja sterowania,którychfizyczna interpretacja jestmomentnapedowy. Układy sterownikapowinny zapewnic fizyczna realizacje sterowan takiej postaci. Sterownik powinien równiez umoz-
8
Rysunek7: Schematblokowy układusterownika
liwi c napieciowe sterowanienapedami,by istniałamozliwosc implementacjialgorytmóww którychmodeldynamikisilników jestwłaczony domodeludynamikirobota.
3.1 Budowa sterownika
W celurealizacjipowyzszychzałozen zaprojektowano,zbudowano,uruchomionoi oprogramowanosterownik do konstruowanego robota.Schematblokowy układusterownika przedstawiony zostałnarysunku7. Szczegółowe schematyzostałyumieszczonew dodatku. Sterownik zostałzmontowanynatypowej płytce6TE/3U(rysunek8) i umieszczony w kasetcew sposóbumozliwiajacy jego łatwydemontaz (rysunek9).W dalszejczesci rozdziałuzamieszczony zostałszczegółowy opis metodi układówpomiarowych,układówwykonawczychi pozostałychelementówsterownika.Opiswykorzystaniatransmisjiradiowej do komunikacjizezmodyfikowanym protokołemMODBUSzamieszczonow [9].
9
3.2 Jednostkacentralna
Pozyskaniewszystkichpotrzebnych parametrówruchu oraz realizacjasterowan w krótkim czasiewymagazastosowania wydajnej jednostki centralnej. W sterowniku zastosowano 32–bitowy mi-krokontrolerMC68332[12, 10], posiadajacy półautonomiczneukładytransmisjiszeregowych QSM(QueuedSerialModule) [11] i autonomiczny układczasowy TPU(TimeProcessorUnit) [15].Moduł QSM mikrokontroleraumozliwia zaprogramowaniesekwencjiprzesłan synchronicznym ła-czemSPI.ZastosowanieukładówperyferyjnychposiadajacychłaczeSPIumozliwia wiecich obsługebezkoniecznosci interwencjizestrony jednostkicentralnejmikrokontrolera.Rozwiazanietakiepo-zwalanazwiekszeniewydajnoscicałegoukładu.W sterowniku wykorzystanote cechemikrokontro-lera.Zastosowaneprzetworniki CA i AC posiadaja łaczeSPIi ich obsługa,tj. odczytywaniewartoscinapiec z przetwornikaAC i zapisdoprzetwornikaCA, odbywasiewedługzaprogramowanejsekwen-cji.Moduł TPU umozliwia realizacje zaprogramowanych funkcji czasowych. Funkcjete sa programo-wanew mikrokodzie. Programistamoze skorzystac z zestawu gotowych funkcji [17] lub stworzycwłasne[13]. W sterowniku wykorzystanomodułTPUdo:� generacjisygnałuPWM sterowanianapieciowegosilnikami (standardowafunkcjaPWM),� dekodowaniasygnałukwadraturowegokoderów(standardowafunkcjaQDEC),� pomiaruwypełnieniasygnałówpochodzacychz akcelerometrów(standardowafunkcjaPPWA)� pomiarupredkoscikół napodstawie sygnałukwadraturowegokoderów(opracowanadlapotrzeb
projektufunkcjaQDVEL).
PozabogatymizasobamimikrokontroleraMC68332waznym argumentemprzemawiajacymi zajegowyboremprzy konstrukcji sterownika jest wygodny sposóbprogramowaniaorazmozliwosc pracyw specjalnym trybie BDM (BackgroundDebug Mode). Połaczeniezewnetrznego komputeraPCzaposrednictwemspecjalnego interfejsuz mikrokontroleremi uzycieodpowiedniegooprogramowania,umozliwia prace krokowa mikrokontrolera,zakładaniepułapekprogramowych,podgladorazmody-fikacje rejestrówpamieci mikrokotroleraitp. Dla wygody eksperymentatorazłaczeBDM zostałoumieszczonew sterowniku, w łatwo dostepnym miejscu. Na rysunku10 pokazany zostałinterfejsumozliwiajacy prace w trybie BDM podłaczony do robotai gotowy do podłaczeniado komputeraPC.Układ mikrokontrolerawraz z układamipamieci tworzy moduł (rysunek11). ZastosowanopamiecprogramutypuEEPROM (128kB)orazpamiec RAM (512kB).Konstrukcjaoprogramowaniaumozliwia przeprogramowaniemikrokontroleraw trybie BDM lub zaposrednictwemportuszeregowego. Zastosowaniemodemówradiowych umozliwia wiecnawet bez-przewodoweprzeprogramowaniesterownika.Poniewaz zastosowany mikrokontrolernie posiadabloku koprocesoraumozliwiajacego wykonywa-nie obliczen naliczbachzmiennoprzecinkowych, jego mocobliczeniowa pozwalanaimplementacjezłozonych algorytmówsterowaniatylko przy załozeniu, ze obliczeniawykonywanesa na liczbachstałoprzecinkowych. Rozwiazanietakie jestkłopotliwe z punktuwidzeniaeksperymentatora,ponie-waz wymagaodpowiedniego skalowaniaformuł w trakciewykonywaniaobliczen. Budowa wybra-nego mikrokontrolerapozwala na dołaczeniedodatkowego układupełniacego funkcje koprocesoramatematycznego.Mozliwym, obiecujacymrozwiazaniemwydajesiezastosowanieprocesorasygnałowegoseriiSHARC.Procesorytej serii posiadaja wbudowana,dwudostepnapamiec którapozwoliłaby nałatwa wymiane
11
Rysunek10: Interfejsumozliwiajacy pracew trybieBDM podłaczony do robota
Rysunek11: Jednostkacentralnasterowanika
danychzesterownikiemrobota.Zaletatakiegorozwiazaniajestbrakkoniecznosciprzebudowy czescisterownika odpowiedzialnejza wykonywaniepomiaróworazsterowaniesilnikami. Równiez opro-gramowaniesterownika nie uległoby znaczacym zmianom;modyfikacjepolegałyby na umieszcze-niu globalnych strukturdanych zawierajacych zebranepomiaryi wyliczonesterowaniaw obszarzepamieci dwudostepnej,orazumieszczenieproceduryuzykownika realizujacejalgorytmsterowania(rysunek17) w pamiecikoduprocesoraSHARC.Innym mozliwym rozwiazaniemnaprzyszłosc jestwymianajednostkicentralnejna inna, np. z mi-krokontroleremMPC555.Wspomniany mikrokontrolerposiadawszystkiezasobyukładuMC68332i dodatkowo posiadawydajny blok doobliczen zmennoprzecinkowych.
12
3.3 Układy pomiarowe
Jakjuz wspomniano,układysterownikazapewniajapomiarwartoscizmiennych�ϕ1 � ϕ2 � ϕ1 � ϕ2 � α � α � .
Mierzonesa równiez pradyzasilajacesilniki, temperaturastopnimocy orazpołozeniaosi joystick-a,który moze byc dołaczony do sterownika. Pomiartemperatury, pradów silników, predkosci kato-wej korpusuα, oraz połozen osi joystick-a odbywa sie przezprzekształceniemierzonych wielko-sci do napiec. Pomiarnapiec wykonywany jest12–bitowym przetwornikiemanalogowo–cyfrowymTLC2543C.Pozostałewielkosci sa mierzonenapodstawie zaleznosci czasowych w bloku TPU mi-krokontrolera.
3.3.1 Pomiar predkosci i połozen kół
Predkosci i połozeniaosi silników sa mierzonekoderamiprzyrostowymi. Poniewaz momentnape-dowy z silników jestpodawany nakoła zaposrednictwemdwóchprzekładni,luzy nanich wystepu-jacewpływaja negatywniena jakosc pomiaru. ZastosowanekoderyHEDS55generuja 500 impul-sów/obrótosi silnika. Kwadraturowy sygnałz koderówjest dekodowany w bloku TPU mikrokon-trolerastandardowa funkcja QDEC [15]. Poniewaz dekodowanieuwzgledniawszystkiezboczasy-gnału,pomiarpołozeniaosisilnikaodbywasiez rozdzielczoscia2000/obrótosisilnika. Zastosowanaprzekładniaplanetarnamaprzełozenie1:66,tak wiecprzy załozeniuzeprzekładniałancuchowa maprzełozenie1:1pomiarpołozeniakatowegokoła
�ϕ1 � ϕ2 � odbywasie z rozdzielczoscia 132000/obr.
Aktualnepredkosci osi�ϕ1 � ϕ2 � mierzonesa posrednioprzezpomiarokresusygnałówkwadraturo-
wych generowanych przezkodery. Pomiartenodbywa sie równiez w bloku TPU. FunkcjaQDVELdokonujacapomiaruzlicza ilosc impulsówzegarataktujacego blok TPU w trakcietrwaniajednegookresusygnałukwadraturowego. Na podstawie zliczonejilosci impulsówoprogramowaniesterow-nika obliczaaktualnepredkosci kół. Dokładnosc pomiarubazujacego naokresiesygnałukwadratu-rowegomalejezewzrostemmierzonejpredkosci. Z tegopowodu,gdymierzonapredkosc przekroczypewien ustalony próg,pomiarrealizowany jestnapodstawie róznicy aktualnegopołozeniakatowegokoła i połozeniaw poprzednimcyklu pomiarowym.
3.3.2 Pomiar predkosci katowej korpusu
Do pomiarupredkosci katowej korpusuwzgledempionu α zastosowany zostałpiezoceamiczny zy-roskop ENC-03J.Układ ten posiadawyjscienapieciowe, które po odfiltrowaniu składowej stałej iwzmocnieniuwe wzmacniaczujestpodawanenaprzetwornik AC. Zastosowanafiltracja górnoprze-pustowa,którejdolnaczestotliwosc granicznawynosi0.3Hz,jestkoniecznazewzgledunazaleznoscwartosci składowej stałejw sygnalewyjsciowym zyroskopuod temperatury. Zastosowany zyroskoppozwalanapomiarpredkosci katowej w zakresie� 300o � s. Bioracpoduwage znacza bezwładnoscwahadła,zakrestenmoznauznac zawystarczajacy.
3.3.3 Pomiar połozeniakatowegokorpusu
Dopomiarukatawychyleniakorpusuodpionuwykorzystany zostałdwuosiowy akcelerometrADXL202o rozdzelczosci5mgi zakresiepomiarowym � 2g. Kazdaz osiakcelerometrugenerujesygnałprosto-katny o wypełnieniumodulowanym aktualna wartoscia przespieszenia.Wypełnieniegenerowanegosygnałumierzonejest w bloku TPU mikrokontrolera. Przy pomiarzekatawychyleniakorpusuodpionu załozono, ze srodeklokalnego układuwspółrzednych (Xw � Yw rysunek12) lezy w osi obrotu
13
kół 1. Znajomosc składowych a1 � a2 orazprzyspieszeniaziemskiego g pozwalawyznaczyc wypad-
Rysunek12: Pomiarkataodchyleniawahadła.
kowe przyspieszenierobota,składowa pozioma przyspieszenialiniowego a oraz kat α, zgodniezzaleznoscia �
1a2 � �
cosα � sinαsinα cosα � �
ga � �
Wyznaczenieskładowej poziomeja odbywasie z dokładnosciadoznaku��� �� a ��� 12 � a22 � g2
α atan2� a1g� a2a
a21 � a2
2��� a1a� a2g
a21 � a2
2�
Korzystajacz równan kinematykirobota[4] znakprzyspieszeniaa moznawyrazic jako
sgn�a � sgn
�ϕ1
� ϕ2� 2α � �
Szacowanieznakupowyzszego wyrazeniaodbywa sie przezporównaniedwóchwartosci wyrazenia�ϕ1
� ϕ2� 2α � w kolejnych cyklach pomiarowych. Wyniki badaniaukładu pomiarowego kata α
zostałyprzedstawionew rozdziale5.
3.3.4 Pomiar pr adówzasilajacychsilniki
Pradpojedynczegosilnika Is (rysunek13) jestmierzony posrednioprzezpomiarnapiecianarezysto-rzepomiarowym.Napiecie to jest wzmacnianewe wzmacniaczuróznicowym, filtrowanedolnoprzepustowo, i poda-wanenawejscieprzetwornikaAC (sygnałUpom). SygnałUr wykorzystywany jestw układzieregula-cji pradu.Zastosowanametodapomiarunie dajeinformacji o znakumierzonegopradu.
1W modelurzeczywistymrozwiazanietakieniebyło mozliwe zewzgledówtechnicznych i srodekosiakcelerometrujestpołozony odległosci 20mmodosi kół.
14
Rysunek13: Schematukładupomiarupradu
3.3.5 Pomiar połozeniaosi joystick-a
Sterownik posiadamozliwosc podłaczeniaanalogowego joystick-a,którego działaniemozebyc do-wolnie oprogramowane.Układ pomiarowy połozeniaosi joystick-apokazany zostałnarysunku14.NapieciaU jx � U jy podłaczonesabezposredniodoprzetwornikaanalogowo – cyfrowego.
Rysunek14: Schematukładupomiarowegopołozeniaosi joystick-a
3.3.6 Pomiar temperatury radiatora układu wykonawczego
Schematukładuwykorzystanegodopomiarutemperaturyprzedstawiony zostałnarysunku15. Czujnikiem
Rysunek15: Schematukładupomiarutemperatury
15
temperaturyjest bipolarny tranzystorw obudowie przystosowanejdo montazu na radiatorze.Roz-wiazanietakie pozwoliło na uzyskanieszybkiejodpowiedzi czujnikana zmiany temperatury(czasopóznieniapomiarujest rzedukilku sekund). 2 Metodapomiaruwykorzystujezaleznosc napieciana złaczuP–N,spolaryzowanym w kierunkuprzewodzenia,od temperatury. Warunkiemuzyskanialiniowej zaleznosci napieciana złaczuP–N od temperaturyjest zapewnieniestałejwartosci pradupolaryzacji.Z tego wzgledudo zasilaniazłaczazastosowanezostałozródłopradowe. Układ wzmac-niaczanapiecia umozliwia ustawienie wartosci temperaturyodpowiadajacej wartosci 0V napieciaUtemp potecjometremP2,orazustaleniezakresupomiarowegopotencjometremP1.
3.4 Układy wykonawcze
Układy wykonawczepozwalaja nasterowanienapieciowe lub pradowe silnikami. Dzieki własnoscizastosowanychsilników pradustałegoz trwałymi magnesamipolegajacejnaliniowej zaleznoscimo-mentunapedowego generowanego przezsilnik od praduzasilajacego, sterowaniepradowe silnikówumozliwia kontrole momentunapedowego. Sterowanienapieciowe odbywa sie przezgeneracje sy-gnałuPWM w bloku TPUmikrokontrolera.SygnałtenkluczujewybranesygnałemDIR półramionaH-mostkaukładuwykonawczego (rysunek16). Jednoczesniemozliwe jest ograniczeniepradupły-nacegoprzezsilnik napieciemUic (rysunek16). Nepiecieto jestzadawaneprzetwornikiemcyfrowo–analogowym MAX529 [16]. Przysterowaniupradowym sygnałPWM jestustawiany nawypełnienie100%. Kierunekpradujest wybierany sygnałemDIR, a wartosc praduzadajesie napieciemUic zaposrednictwemprzetwornikaCA.Jako elementykluczuacew układachwykonawczychzastosowanotranzystoryMOSFET. Rozwiaza-nie takiepozwoliło nauzyskanieduzej sprawnosci układu. Wzmacniaczesterujacebramkamitran-zystoróworazukłady logicznezapewniajacezachowanieodpowiednichzaleznosci czasowych przysterowaniumostkamizbudowanezostaływ techniceSMD, dzieki czemuuzyskanoniewielkie gaba-ryty modułu.Układ logiczny sterujacy przełaczaniemramionH-mostkazapeweniawyłaczeniewszystkichtranzy-storówmostkanaczas1 � 8µs przedwłaczeniemprzeciwległych ramion. Rozwiazanietekieokazałosiekonieczne,poniewaz przełaczenieramionmostkabezopóznieniapowodowałochwilowezałacze-nie wszystkiechtranzystorówmostkai powstawanie impulsupradupłynacego przeztranzystoryoznacznejamplitudzie.Napiecianasyceniatranzystorówtypu N przy pradzie1.5A wynosza 40mV, atranzystorówtypuP– 145mV, tak wiecstratymocy w układziewykonawczymsabardzomałe.Sposóbregulacji praduw zastosowanym układziejestnastepujacy. ZałaczeniesygnałemPWM wy-branych sygnałemDIR półramionmostkapowoduje narastaniepradu w silniku ze stała czasowaτ L � R (L – indukcyjnosc uzwojen silnika, R – rezystancjauzwojen). Na rezystorzeRi propor-cjonalniedo pradurosnienapieciektóre jestwzmacnianewe wzmacniaczuróznicowym, filtrowanedolnoprzepustowo, i podawanena komparator. Drugie wejsciekomparatorajest podłaczonedo na-pieciaUic programujacego wartosc zadana pradu. Gdy prad silnika osiagniewartosc, przy którejodpowiadajacemu napieciena wyjsciu wzmacniaczaprzekroczywartosc napieciaprogramujacegoUic, wyjsciekomparatoraprzełaczysiew logiczny stanniski i zaposrednictwemukładulogicznegowyłaczyzasilaniesilnika. Ciagłosc pradusilnika zapewniaja diody Shottky-ego. Odłaczeniezasi-laniaspowodujezmniejszaniepradusilnika az do momentu,gdy napieciena wyjsciuwzmacniaczaspadnieponizej wartosciUic, co spowodujeponownewłaczeniezasilaniasilnika i cykl pracy układusie zamyka.
2Alternatywnym rozwiazaniembyło zastosowaniegotowego, scalonego układupomiarutemperatury, np. układuLM35, co znaczenieuprosciłobyukładpomiarowy. Niestety, układ ten dostepny jestw obudowie TO92wykonanejz tworzywa sztucznego. Z tego powodu czasopóznieniapomiarubyłby rzedukilkudziesieciusekund.
16
Rysunek16: Schematmetodysterowaniaprademsilnika
Praktycznesprawdzeniedziałaniaukładuprzedstawionezostałow rozdziale5.
3.5 Układ zasilania
Układ napedowy robotaprzy pełnym wysterowaniu i obciazeniupobieramoc rzedu60W. Elektro-niczneukładysterownikapobierajamocrzedu2.5W. Sredniamoctraconaw układachwykonawczychnieprzekracza5W. Przeprowadzenieseriieksperymentówwymagazatemodukładuzasilaniadostar-czeniaznacznych ilosci energii. Z tego wzgleduzastosowano trzy ołowiowe akumulatoryzelowe12V/4.5Ah połaczoneszeregowo. Napiecia � 5V niezbednedo zasilaniaukładówelektronicznychsterownika uzyskiwanesa z przetwotnicy impulsowej zbudowanejz wykorzystaniemukładuL4960[14]. Rozwiazanietakieumozliwiło uzyskaniewysokiejsprawnosciukładuzasilaniasterownika. Doładowaniaakumulatorówzbudowany zostałukładzasilany z sieciAC 220V. Układtenskonstruowanyzostałw taki sposób,by niebyło koniecznerozłaczanieanidemontowanieakumulatorów.
17
4 Oprogramowanie
Oprogramowaniesterownika powinno zapewnic eksperymentatorowi nie posiadajacemuszczegóło-wej wiedzy o konstrukcji robotamozliwosc wygodnego implementowaniaalgorytmówsterowania.W projekciewykorzystanokoncepcje swobodnieprogramowalnego sterownika [1] którapowalanaspełnieniepowyzszegozałozenia.
4.1 Struktura oprogramowania
Schematblokowy oprogramowaniasterownikaprzedstawiony zostałnarysunku17.Elementemposredniczacym miedzyoprogramowaniemuzytkownika a sprzetemi układamirobotajestoprogramowaniejadra,którezapewnia:� inicjalizacjeurzadzen sterownika i globalnychstrukturdanych,� zebraniepomiarówparametrówruchu,� realizacjewyliczonychprzezprocedureuzytkownikasterowan,� komunikacjez otoczeniemw standardzieMODBUS.
Komunikacjamiedzyoprogramowaniemuzytkownika i jadremodbywa sie za posrednictwemglo-balnych strukturdanych. Pouruchomieniusterownika oprogramowaniejadrainicjalizuje urzadzeniarobotaa nastepnieuruchamiaprocedure inicjalizacyjna uzytkownika user_init(). W petli głów-nej jadrajesturuchamianafunkcjaobsługikomunikacjiz otoczeniemw standardzieMODBUS orazprocedurapetli głównej uzytkownika user_main(). W trakcie inicjalizacji urzadzen uruchamianejestcykliczneprzerwaniePIT orazodblokowywanejestprzerwaniepochodzaceod interfejsuszere-gowego. W trakciepracy sterownika, co ustalony interwał czasuuruchamiasie proceduraobsługiprzerwaniacyklicznego, w której oprogramowanie jadrazbieradanepomiarowe i umieszczaje wglobalnejstrukturzedanych state. Nastepnieuruchamianajest jednokrokowa procedurasterow-nika uzytkownika user_control(), któranapodstawie danych pomiarowych obliczasterowania,iumieszczaje w globalnejstrukturzecontrol. Nowe sterowaniasa przepisywanedo układówwyko-nawczychprzezoprogramowaniejadrai obsługaprzerwaniacyklicznegosiekonczy. Zatemoprogra-mowanierobotaprzezuzytkownika polegananapisaniuw jezykuC proceduryinicjalizacyjnej orazjednokrokowejpocedurysterownika. Procedurainicjalizacyjnamazazadanieprzygotowaniestrukturdanych uzytkownika,a zadaniemprocedurysterownika jestwyliczenienowych sterowan napodsta-wie aktualnego stanurobota. Uzytkownik moze modyfikowac rejestryMODBUS, które nastepniemoga zostac odczytaneprzezzewnetrzny komputer. Istniejerówniez mozliwosc reakcjioprogramo-waniauzytkownikanazdalny wpisdo rejestrówMODBUS(procedurauser_MdBusWrtSvc()).Utworzeniekompletnego programupolegananapisaniuwspomnianych proceduri dokonaniukom-pilacji przy pomocy narzedzi skrosnych pracujacych na komputerzeklasy PC.Wynikowy programmozezostac załadowany do pamieci RAM lub EEPROM jednostkicentralnejsterownika zaposred-nictwem łaczaszeregowego lub łaczaBDM i uruchomiony. W rozdziale5 znajdujesie przykładprostegostrownikazrealizowanegozgodniez opisanakoncepcja.
4.1.1 Struktura plików oprogramowania
Oprogramowanierobotaprzygotowanezostałow jezykuC przywykorzystaniupakietuHiware.Struk-turakatalogówoprogramowaniaprzedstawia sie nastepujaco:
18
/Template/User/inc/src/obj/qdvel
KatalogUser zawiera plik z proceduramiuzytkownika. W kataloguobj przechowywanesa plikiobiektowe stworzoneprzezkompilatornapodstawie plików zródłowych umieszczonych w katalogusrc orazplików nagłówkowychprzechowywanychw katloguinc. Katalogqdvel zawierapliki zró-dłowe funkcji QDVEL uruchamianejw bloku TPU mikrokontrolera.Z punktuwidzeniauzytkownikanajistotniejszymielementamisa katalagiUser orazinc.W kataloguinc znajduja sie pliki nagłówkowe zawierajacedefinicje skal wartosci pomiarowych,stałekalibracyjne,deklaracjeglobalnychstrukturdanych. W kataloguUser znajdujesieplik user.c.W pliku tym uzytkownik mozezdefiniowac własnefunkcje,przy czymczteryfunkcjeo ustalonychnazwachmusza siew nim znalesc. Sa to funkcje:� void user_init() – funkcja inicjalizacji zmiennych uzytkownika uruchamianiaprzezjadro
sterownika jedenraz,po uruchomieniurobota.� void user_MdBusWrtSvc(unsigned int ind, unsigned int cnt) – funkcjauruchamianaprzezoprogramowanie jadrasterownika przy zdalnym wpisie do rejestrówMODBUS. Argu-mentcnt okreslailosc zapisanych rejestrów, a parametrind okreslaindeksrejestrupoczawszyodktóregozostałwykonany zapis.� void user_control() – funkcjawywoływanakazdorazowo przy wystapieniuprzerwaniacy-klicznego. Przedwywołaniemtej funkcji oprogramowanie jadrasterownika zapisujedo glo-balnejstrukturystate aktualneparametryruchu,a po jej zakonceniuprzepisujesterowaniazglobalnejstrukturycontrol do układówwykonawczych.� user_main() – funkcjawywoływanaz petli głównejoprogramowaniajadra.Na czaswykony-waniatej funkcji wstrzymywanajestobsługazdalnegozapisui odczyturejestrówMODBUS.
Połozenieposzczególnychprocedurw strukturzeoprogramowaniarobotazostałoprzedstawionesche-matycznienarysunku17.Deklaracjei definicje globalnych struktur danych state i control znaduja sie w pliku nagłów-kowym inc/globals.h. Korzystanieze wspomnianych strukturmozliwe jest po dołaczeniuplikuinc/globals.h dyrektywa
#include "inc\globals.h"
4.2 Opis globalnychstruktur danych
Globalnestruktury danych w oprogramowaniu robota(rysunek17) słuza do komunikacji miedzyproceduramijadrai proceduramiuzytkownika. Po uruchomieniuproceduryobsługiprzerwaniacy-klicznegofunkcjejadrakorzystajacz zasobówsprzetowychrobotazapisujadostrukturystate aktu-alnewartosci parametrówstanurobota. Nastepniesterowanieprzekazywanejest do jednokrokowejprocedurysterownikastworzonejprzezuzytkownika. Proceduratanapodstawie zgromadzonychpo-miarówobliczanowewartosci sterowan i umieszczaje w strukturzecontrol. Gdysterowaniewracado funkcji jadraobliczonesterowaniasa przekazywanedo układówwykonawczych. Polastruktury
20
pomiarowej state zostałyopisanew tablicy 3. Skaleposzczególnych pól zdefiniowanesa makramiw pliku common.h.
typ pola Nazwa pola Opis Skala Jednostkalong position[2] połozeniakatowekół WHEEL_SC [obr]long speed[2] prekosci katowekół SPEED_SC [1000rad/s]long angle połozeniekatowekorpusu ANGLE_SC [o]int current[2] zmierzonepradysilników CURRENT_SC [A]long anglespeed predkosc katowakorpusu SPEED_SC [1000rad/s]int joy_x połozenieosiX joystick-a JOY_SC [100%]int joy_y połozenieosiY joystick-a JOY_SC [100%]
Tablica3: Opispól strukturystate
Aby uzyskac wartosc okreslonego parametruw odpowiadajacejmu jednostce,nalezy podzielic goprzezskale. Takwiecwyrazenie
state.position[0]/WHEEL_SC
przyjmujewartosc pozycji lewego koła w [obr]. Poniewaz pole position[0] i stałaWHEEL_SC satypulong czesc ułamkowawyrazeniazostaniezaniedbana.Jezeli przedwykonanieminstrukcjidzie-lenia pole state.position[0] przemnozymy np. przez10, to wartosc wyrazeniabedziew skali[0.1 obr], i uzyskamywieksza dokładnosc. Zastosowanieskal typu zmiennoprzecinkowego byłobyznaczniewygodniejsznie,alew znacznym stopniuzwiekszyłobyczasobliczen. Połozeniasrodkowejoystick-aodpowiadajawartosciom50%.
Typ pola Nazwa pola Opis Skala Jednostkaint PWM[2] zadanewypełnienie PWM_PER [100%]
sygnałuPWMunsignedchar current[2] zadanepradysilników DCURRENT_SC [A]
Tablica4: Opispól strukturycontrol
Opispól strukturysterowan control zawiera tablica4. Pradzadany o wartosci DCURRENT_S odpo-wiadamomentowi napedowemu1.5Nm.
5 Eksperymenty
W celusprawdzeniapoprawnosci konstrukcjirobota,sterownika i oprogramowaniaprzeprowadzonoszereg eksperymentów, którychczesc przedstawionow dalszejczescirozdziału.Sato zarównoekspe-rymentyelementarnepolegajacenabadaniuposzczególnychwezłówrobotajak i eksperymentycało-sciowepokazujacezachowanierobotaprzysterowaniuw petli otwartej.W koncowej czesci rozdziałuzamieszczonoprzykładimplementacjiprostego sterownika umozliwiajacego sterowanierobotemzapomoca joystick-a. Przykładten zostałwykonany w przygotowanym srodowisku, z zachowaniemopisanejstrukturyoprogramowania.
21
-4
-2
0
2
4
6
8
10
12
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[m/s^2]
[s]
a1
-15
-10
-5
0
5
10
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[m/s^2]
[s]
a2
Rysunek18: Przyspieszeniaa1, a2 zmierzoneakcelerometrem.
-2.5
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[rad/s]
[s]
alfa_dot
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
[rad]
[s]
alfa
Rysunek19: Zmierzonapredkosc α orazwyliczony katα.
5.1 Badanieukładu pomiaru połozeniakatowegokorpusu
W celuzbadaniaukładupomiarupołozeniakatowego korpusuprzeprowadzony zostałeksperymentw trakciektórego zapamietanezostałyprzyspieszenia1 � 2 zmierzoneakcelerometramiorazkatowepołozeniekorpusui katowa predkosc korpusu. Eksperymentpolegał na zablokowaniu kół robota,uniesieniui zwolnieniu korpusu. Wyniki pomiarówprzedstawione zostałyna rysunkach18 i 19.Zaproponowanametodapomiarowa, mimo znacznego stopniaskomplikowania,działapoprawnie.
Obliczony kat moznaw pewnym stopniuzweryfikowac obserwujacwykresjego pochodnej,tj. pred-kosci α. Z zamieszczonychwykresówwidac, zezaleznosc ta jestodpowiednia.
5.2 Badanieukładu regulacji pr adu
Badanieukładuregulacji praduprzeprowadzonezostałodla dwóchróznych przebiegów pradówza-danych, tj. przebiegu skokowego i sinousoidalnego. Na rysunku20 zostałyprzedstawionewykresypradówzadanych w postaciskoku Id0 � 1, a narysunku21 wykreslonopradyzmierzonew trakcieeks-perymentuI0 � 1.Wykresypradówzadanych w postacijednego pełnego okresufunkcji sinusi zmierzonych w trakcieeksperymentuzostałyprzedstawioneodpowiednionarysunkach22 i 23.Zarównodla skokowego jak i dla sinusoidalnego praduzadanego otrzymanepomiarysa poprawne.Zadany prad jest realizowany z duza dokładnoscia zarównoco do kształtujak i wartosci. Zastoso-wanametodapomiarupradunie uwzgledniajego kierunku,stadnawykresachpradówzmierzonychotrzymanezostaływartoscibezwzglednepradówzadanych.
22
-1
-0.5
0
0.5
1
0 1 2 3 4 5
[A]
[s]
Id_0Id_1
Rysunek20: Wykrespradówzadanych(Id0 � 1)
0
0.2
0.4
0.6
0.8
1
1.2
0 1 2 3 4 5
[A]
[s]
I_0I_1
Rysunek21: Pradysilników zmierzonew trakcieekperymentu(I0 � 1).
23
-1
-0.5
0
0.5
1
0 1 2 3 4 5
[A]
[s]
Id_0Id_1
Rysunek22: Wykrespradówzadanych(Id0 � 1)
0
0.2
0.4
0.6
0.8
1
1.2
0 1 2 3 4 5
[A]
[s]
I_0I_1
Rysunek23: Pradysilników zmierzonew trakcieekperymentu(I0 � 1).
24
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
0 1 2 3 4 5
[rad/s]
[s]
fi0_dotfi1_dot
-6
-5
-4
-3
-2
-1
0
1
0 1 2 3 4 5
[rad]
[s]
fi0fi1
Rysunek24: Zmierzonepredkosci ( f idot0 � 1) i połozenia( f i0 � 1) kół.
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
0 1 2 3 4 5
[rad/s]
[s]
alfa_dot
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
0 1 2 3 4 5
[rad]
[s]
alfa
Rysunek25: Zmierzonapredkosc katowa(al f adot) i połozenie(al f a) wahadła.
5.3 Sterowanie w petli otwartej
Współdziałaniewszystkichelementówsterownikarobotazostałosprawdzoneprzezprzeprowadzeniepróbwykonaniaruchuprzyzadaniuwybranychsterowan
�u1 � u2 � T
5.3.1 Sterowanieskokowe
Eksperymentpolegał napodaniuskokowego sterowaniai obserwacji dostepnych parametrówruchu.Postac sterowan pokazanazostałanarysunku20. Pozycjei predkosci kół pokazanestałynarysunku24. Predkosc katowa i połozeniekorpusuwykreslonezostałynarysunku25.
5.3.2 Sterowaniesinusoidalne
Eksperymentpolegałnapodaniusterowaniasinusoidalnego
u1 u2 1 � 5 � sin � 2πt4 � 5 ���Nm�
(jak narysunku22) i zebraniudanych pomiarowych. Na rysunku26 wykreslonezostałypredkosci ipołozeniakół a narysunku27 predkosc katowa i połozeniekatowekorpusu.
5.4 Przykład prostegosterownika
Jako przykład prostego sterownika przedstawione zostanierecznesterowanie robotemza pomocajoystick-a. Pozycjajoystick-a dostepnaw strukturzestate ma zakreszmiennosci (0,JOY_SC), a
25
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
0 1 2 3 4 5
[rad/s]
[s]
fi0_dotfi1_dot
-8
-7
-6
-5
-4
-3
-2
-1
0
1
0 1 2 3 4 5
[rad]
[s]
fi0fi1
Rysunek26: Zmierzonepredkosci ( f idot0 � 1) i połozenia( f i0 � 1) kół.
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
0 1 2 3 4 5
[rad/s]
[s]
alfa_dot
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
0 1 2 3 4 5
[rad]
[s]
alfa
Rysunek27: Zmierzonapredkosc katowa(al f adot) i połozenie(al f a) wahadła.
pozycjasrodkowakazdejz osiodpowiadawartosci (JOY_ZERO). Prawo sterowaniabedziepostaci
u1 �jx � jy ���� "!$#%#%&('$) _* +(,"- _*
u2 �jx � jy ��� "!$#%#%&('$) _* +(,"- _*
przy czym (u1 � u2) sa sterowaniami a ( jx � jy) pozycjamijoystick-a pomniejszonymi o wartosc od-powiednio joyx0 ijoyy0. Wartosci zmiennych joyx0 i joyy0 sa ustalanena podstawie kalibracjijoystick-a. Zachowanieodpowiedniejskali wyrazeniawymagaprzemnozeniaprawej strony równanprzezskale sterowan i podzieleniaprzezskale połozen joystick-a.Ponizej przedstawionazostałaza-wartosc pliku uzytkownikauser.c. Funkcjainicjalizacji zmiennychuzytkownikauser_init() zo-staławykorzystanadowpisaniawartoscipoczatkowychdozmiennychjoyx0, joyy0, sm. Zmiennasm definiujeszerokosc martwejstrefy joystick-a. Funkcjauser_MdBusWrtSv() obsługujacazdalnywpis do rejestrówMODBUS zostaławykorzystanado kalibracji joystick-a.Zdalny wpis do rejestruo numerze1001powodujezapisanieaktualnego połozeniajoystick-ado zmiennych kalibracyjnych,a wpisanado rejestruwartosc zostajeprzypisanado zmiennejsm. Funkcjauzytkownika wywoły-wanaz petli głównej oprogramowaniajadrauser_main() nie jest uzywanai zostałapozostawionapusta. Sterowanieprademwymagazapisumodułuwartosci zadanejpradudo pola current struk-tury control i wybraniakierunkupraduprzezwpis do polaPWM wartosci PWM_PER lub -PWM_PER.Aby niewielkie błedykalibracji nie wpływały nasterowaniaw pozycji spoczynkowej joystick-a,od-czytyponizejpewnegoprogusa ignorowanei traktowanejako zerowe. W pierwszejczesciprogramudołaczonezostałypliki nagłówkowe zawierajacedefinicjezmiennych globalnych (plik globals.h),rejestrówMODBUS(modbus.h) orazskalepomiarowe(plik common.h).
26
#include "inc\globals.h"#include "inc\modbus"#include "inc\common.h"
Nastepniezadeklarowanozmienneuzywanew dalszejczesci programu.Dzieki dyrektywiestaticzakreswaznosci zmiennychzostałograniczony douser.c. Jestto korzystneponiewaz uniemozliwiawystapieniekonfliktu nazwzezmiennymi zadeklarowanymi w innych plikach tworzacych oprogra-mowanie. Nalezy unikac uzywaniazmiennych typu zmiennoprzecinkowego (tj. typu float orazdouble) zewzgledunaznaczny czaspotrzebny dowykonaniaobliczen nazmiennychtych typów.
static long u;static int ix,iy,sm,joyx0,joyy0;
Funkcjauser_MdBusWrtSvc(unsigned int ind, unsigned int cnt) umozliwia reakcje opro-gramowaniauzytkownika na zdalny wpis do rejestrówMODBUS. Parametrind okresla indeksre-jestruod którego dokonanozapisu,a argumentcnt okresla ilosc zapisanych rejestrów. Numeracjaobsługiwanych rejestrówMODBUS jestuwarunkowanastałymizdefiniowanymi w pliku modbus.hWartosc rejestruind jestzapisanaw MbRegs[ind-INT_BASE-1].
void user_MdBusWrtSvc(WORD ind, WORD cnd){ if(ind==1001)
{joyx0 = state.joy_x;joyy0 = state.joy_y;sm = MbRegs[ind-INT_BASE-1];}
}//user_MdBusWrtSvc()
Funkcjauser_init() wywoływanajednokrotnieprzezoprogramowaniajadrapo restarciesterow-nika zostałauzyta to inicjalizacji zmiennych. Poczatkowe wartosci joyx0, joyy0 przyjeto jakosrodkiskaljoystick-a.
void user_init(){sm = 100;joyx0 = joyy= = JOY_ZERO;}//user_init()
Funkcjauser_main() wywoływanaz petli głównejoprogramowaniajadranie zostałauzytaw przy-kładzie,niemniej jej definicjamusipozostac w pliku user.c.
void user_main(){}//user_main()
Funkcjauser_control() jestwywoływanakazdorazowo po wystapieniuprzerwaniacyklicznego iuaktualnieniuwartosci pomiaróww strukturzestate. W opisywanym przykładziefunkcja ta ob-licza nowe wartosci sterowan zgodniez opisanym wczesniej prawem sterowania i umieszczaje wstrukturzecontrol. Za przepisaniesterowan do układówwykonawczychodpowiedzialnejestopro-gramowaniejadrasterownika.
27
void user_control(){
ix = state.joy_x - joyx0;iy = state.joy_y - joyy0;u = (ix + iy);control.PWM[0] = (u<0)? -PWM_PER:PWM_PER;if (u<0) u *= -1;u *= CURRENT_SC;u /= JOY_SC;control.current[0] = u;u = (ix - iy);control.PWM[1] = (u<0)? -PWM_PER:PWM_PER;if (u<0) u *= -1;u *= CURRENT_SC;u /= JOY_SC;control.current[1] = u;
}//user_control()
Podsumowanie
Opisany robotstanowi ciekawy obiektsterowania,w którymuwarunkowaniadynamiczneniesamoz-liwe do pominieciaprzy realizacjizadaniasterowania. Wraz ze sterownikiem i oprogramowaniemjest kompletnym stanowiskiembadawczymumozliwiajacym implementowaniealgortmówsterowa-niaorazuzyskiwaniedanychpomiarowychpozwalajacychnaocene jakoscisterowania.Robotzostałzaprojektowany i oprogramowany w taki sposób,abyułatwic eksperymentatoromimplementowaniealgorytmówsterowania. Wykonanoszereg eksperymentów, które potwierdziły słusznosc koncepcjikonstrukcyjnych i przydatnosc stanowiska do badaniaalgorytmówsterowania obiektaminieholo-nomicznymi na rzeczywistymobiekcie. Dodatkowym potwierdzeniemprzydatnosci opracowanegostanowiskajestseriaeksperymentówprzeprowadzonychw [8].W oparciuo modelmatematyczny robotaopracowanoi zbadanosymulacyjnie [7] algorytmsterowa-nia wykorzystujacy metode linearyzacjidynamicznejdo realizacjizadaniasledzeniatrajektorii (a wwersji zmodyfikowanej,zawierajacejpseudoinwersodporny, umozliwiajacy równiez zadaniestero-waniadopunktu).Jegozaimplementowaniew obecnejwersjisterownikajestniemozliwe,zewzgledunabraksprzetowej realizacjioperacjizmiennoprzecinkowych.Kolejnymkrokiem,znaczniezwiekszajacym waloryuzytkowerobota,bedzierozbudowaniejednostkicentralnejo koprocesormatematyczny zgodniez zarysamikoncepcjiprzedstawionejw raporcie.W zakresiekonstrukcjimechanicznejuzasadnionewydajesie przeniesieniezespołównapedowychdo osi kół i usuniecieprzekładniłancuchowych.Jednym z waznych problemówwymagajacych rozwiazaniajest identyfikacjaparametrówmodelu.Warto zwrócic uwage, zewykonanestanowisko pozwalana wygodneprowadzeniaeksperymentówumozliwiajacychrealizacje tegozadaniaw najblizszejprzyszłosci.Zebranedoswiadczeniadotyczacezarównometodpomiarowych parametrówruchu,układówelek-tronicznych sterowania silnikami oraz napedu wzgledemwewnetrznego układu odniesienia,jak ioprogramowaniabeda bardzoprzydatneprzy konstruowaniuzapewne jeszczeciekawszego obiektunieholonomicznego— robotakulistegoRoBall.
28
Bibliografia
[1] GORA S., WNUK M., VFMC - swobodnieprogramowalnysterownik napedurobota, RaportSPR13/97,Inst.Cyb. Techn.PWr, 1997.
[2] TCHON K. i in. Manipulatoryi robotymobilne. AkademickaOficynaWydawniczaPLJ,War-szawa,2000.
[3] MUSZYNSKI R., TCHON K. Singularitiesand mobility of nonholonomicsystems:the ballrolling onaplane6thIFAC SymposiumonRobotControl,SYROCO’00,Vienna2000,Preprintsvol.1.
[4] KABAŁA M., TCHON K., WNUK M. Robotmobilny napedzanyw układziewewnetrznymMateriałyVII Krajowej KonferencjiRobotyki,Ladek–Zdrój,2001,t. 1, ss.149–158.
[5] KABAŁA M., WNUK M. Dwukołowy robot mobilny napedzanyw układziewewnetrznym–dokumentacjamodeluRaportSPRnr 21/2001,Inst.Cyb. Techn.PWr, 2001.
[6] KABAŁA M., TCHON K., WNUK M. Dwukołowy, nieholonomicznyrobot mobilnyMateriałKonferencjiAutomation2002,Warszawa,2002,ss.269–280.
[7] TCHON K., KABAŁA M., WNUK M. Algorytmsledzeniatrajektorii robotamobilnego MK.Matriały XIV Krajowej KonferencjiiAutomatyki,ZielonaGóra,2002.
[8] WRONKA C. Model kinematykii dynamikirobotanieholonomicznego Pracamagisterskapodkierunkiemdr. inz. RobertaMuszynskiego,Inst.Cyb. Techn.PWr, 2002.
[9] WNUK M., Komunikacjaradiowaz robotemmobilnymMK, RaportSPR19/2002,Inst. Cyb.Techn.PWr, 2002.
[10] CPU32central processorunit referencemanualCPU32RM/ADRev. 1 MotorolaInc. 1990.
[11] QSMqueuedserial modulereferencemanualQSMRM/AD MotorolaInc. 1991.
[12] MC68332user’smanualMC68332UM/ADRev. 1 MotorolaInc. 1993.
[13] TimeProcessorUnit Macro Assembler(TPUMASM)ReferenceManualMotorolaInc. 1994.
[14] L4960powerswitching regulator datasheetSGS-Thomson1995.
[15] TPU timeprocessorunit referencemanualTPURM/AD Rev. 3 MotorolaInc. 1996.
[16] MAX529datasheetMaxim.
[17] TPU literaturepackageMotorolaInc.
29
.
/
0
12345
1
0
/
.5 4 3 2
6789:;< 6789:;<6789:=; 6789:=;>?@?AB< C<D //EFG3H2 I/ /1E
JKLKM./N//O //O //EFG3H2 1I/1E .O.O
P0QN RMSIT1E 3UJ.. QWVXOQWVXO
.E Y
F Z [\]^ L5F Z [\]N44
N44
G Z_ Z] 3`a .E3`a .E
Yb__c d Z [e__c N44 ^ L5f gh g ijkmlngopq j rsqh jt unp ouv sw xy z
31
{
|
}
~����
~
}
|
{� � � �
}|{ ��� ����~
�W�||| ��� | ���}�� }�� |�� �
|�� �
� �{ ��}}�{ ���{�� �|��
���� ���
~��||� {�� �{�� �{ ��� ���� ~ ������ {� �{�� �
�� �{� � ��{{ � � ��� � � ��� � � �
���� �{| � ��~�� {|}|�� �
||����� ~ ���
�� ����� ���
� ��� � �� �
�� �¡ � �� ¢ £¤�� ¥¥����¦ �� ������ ����§¨© ª« ¬®¯ ª° ±¯¨ ²³ °´¯ ¬°µ« ±
32
¶
·
¸
¹º»¼½
¹
¸
·
¶½ ¼ » º
¶¾ ¿À Á¾ù¶·¶¸¶¹ Á¾ù»Ä·¹Å¶Æ
¶¾Æ ¸¸ÆǼ¼ ¶¾Æ
¶À¾Æ¶¾Æ ¶¾Æ·ÅÆ
·WÈÅƶ¾¾ ɶ¾¾ É ¶¾¾ É ¸¸Æ
¶¾¾ ÉÂʸÀÅ Ë·WÈŶÆǼ¼
º ½¼ ·
¶¾¾ É
Ǽ¼ Ì Í½
ÎÏÐ ÑÒ ÓÔÕÖ Ñ× ØÙ ÚÕÓÚ×ÑÙ Ø× Û
33
Ü
Ý
Þ
ßàáâã
ß
Þ
Ý
Üã â á à
ãäå æçè âéê æëì íëî ï ð ñî ï ð ñÜëì íëÜòóô õö÷ øùú û üý þ
è ðÿ ð � æ ð ñ����ãæ ð ñ���ââ��ÿÿï �ð ñ�ÿÿï
�� ����� � è ðÿ ð � æ ð ñ����ãæ ð ñ���ââ
��ÿÿï �ð ñ�ÿÿï
�� ����� �
��
� �ÿ ��ä �ð �ÿä �ì �àãâ�ìæçè ð��
��� �� ���� ���� � !�" �#$� ��%��
ãäå æçè âéê æëì íëî ï ð ñî ï ð ñÜëì íëÜòóô õö÷ øùú û üý þ
è ðÿ ð � æ ð ñ����ãæ ð ñ���ââ��ÿÿï �ð ñ�ÿÿï
�� ����� � è ðÿ ð � æ ð ñ����ãæ ð ñ���ââ
��ÿÿï �ð ñ�ÿÿï
�� ����� �
�
� �ÿ ��ä �ð �ÿä �ì �àãâ�ìæçè ð��
��� �� ���� ���� � !�" �#$� ��%��
àãâÝ��� �� ���� ���� �� ����� &
ÿ �'�äî äÜ (ð)*(
ð))
â++â,-èê +äèä+ê í)�ð
�� . � /��0�� 12ä�î ä�Üèä+ê èê +ä +â,- â+
�� . � /��0�� 21
èä+ê èê +ä +â,- æâ+î æâ+Ü3æî 3æÜ 3æÝ 3æÞ 3æß 3æ4 3æ5 3æ6 3æ7 3æ8 3æÜî 3æÜÜ 3æÜÝ 3æÜÞ 3æÜß 3æÜ4 æê å39;:î æê å39;:Ü< � 0���� �# 0� ��" 0�
à á� �� ��
à á� �� �
à * à )�� # " ��� � �
í)�ðð �ÿ= &���� ��
35
mgr inz.MarekKabaładr inz. MarekWnukInstytutCybernetykiTechnicznejPolitechnikiWrocławskiejul. Janiszewskiego11/1750-372Wrocław
Niniejszyraportotrzymuja:
1. OINT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . - 1 egz.
2. Zleceniodawca . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . - 2 egz.
3. Autorzy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . - 2 egz.
Razem: 5 egz�
Raportwpłynał do redakcjiI-6w grudniu2002roku.
38
top related