Konstrukcja i oprogramowanie dwukołowego robota mobilnego...

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

Rysunek1: Pierwszawersjamodelu

Rysunek2: Pierwszawersjamodelu– konstrukcja

5

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

Rysunek4: Drugawersjamodelu,widok z przodu

Rysunek5: Drugawersjamodelu,widok z tyłu

7

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

Rysunek8: Zmontowany układsterownika

Rysunek9: Zdemontowanakasetkasterownika

10

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

Rysunek17: Strukturaoprogramowaniasterownika

19

/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

Dodatek

Schematyukładów elektronicznychsterownika.

30

.

/

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

Konstrukcja i oprogramowanie dwukołowego robota mobilnego...

Documents

Transcript of Konstrukcja i oprogramowanie dwukołowego robota mobilnego...