Implementacja i badania algorytmów sterowania robotem dwukołowym

OLITECHNIKA

ROCŁAWSKA

YDZIAŁ

LEKTRONIKI

Kierunek:

Automatyka i Robotyka (AiR)

Specialno´s´c: Robotyka (ARR)

PRACA DYPLOMOWA

MAGISTERSKA

Implementacja i badanie algorytmów sterowania robotem

dwukołowym.

Autor:

Prowadz ˛acy prac˛e:

Robert Szlawski

dr in˙z. Marek Wnuk

Ocena:

Wrocław 2004

Dedykuj˛e ˙Zonie

Składam serdeczne podzi˛ekowania

Panu

doktorowi Markowi Wnukowi

za wskazanie tematu,

po´swi˛econy czas i zaanga˙zowanie

Spis tre´sci

1 Wst˛ep

1.1 Cel i zakres pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1.2 Realizacja . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2 Wprowadzenie

2.1 Model matematyczny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2.2 Parametry modelu i ich sens fizyczny . . . . . . . . . . . . . . . . . . . . . . . . . .

2.3 Idea eksperymentu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2.3.1 Przeło˙zenie pr ˛adowe silników wraz z przekładniami – K

. . . . . . . . . . .

2.3.2 Zast˛epcza długo´s´c korpusu – l . . . . . . . . . . . . . . . . . . . . . . . . .

2.3.3 Współczynnik tarcia pomi˛edzy kołem a korpusem – k

. . . . . . . . . . . .

2.3.4 Momenty bezwładno´sci kół – I

. . . . . . . . . . . . . . . . . . . . .

2.4 Uwagi praktyczne . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3 Konstrukcja robota

3.1 Konstrukcja mechaniczna . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.1.1 Układ nap˛edowy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.1.2 Korpus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.1.3 Wahadło . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.1.4 Kaseta sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.2 Konstrukcja elektroniczna — Sterownik . . . . . . . . . . . . . . . . . . . . . . . .

3.2.1 Budowa sterownika — STER . . . . . . . . . . . . . . . . . . . . . . . . .

3.2.2 Budowa układu mocy — MOC . . . . . . . . . . . . . . . . . . . . . . . .

3.2.3 Budowa urz ˛adze´n peryferyjnych . . . . . . . . . . . . . . . . . . . . . . . .

3.2.4 Opis interfejsów zewn˛etrznych . . . . . . . . . . . . . . . . . . . . . . . . .

4 Oprogramowanie

4.1 Oprogramowanie uruchomieniowe dla MPC555 . . . . . . . . . . . . . . . . . . . .

4.2 Oprogramowanie sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

4.3 Przykład implementacji programu u˙zytkownika . . . . . . . . . . . . . . . . . . . .

5 Przebieg i wyniki eksperymentów

5.1 Skalowanie czujników . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5.1.1 Skalowanie koderów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5.1.2 Skalowanie ˙zyroskopu . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5.1.3 Statyczne zale˙zno´sci pr ˛adowe . . . . . . . . . . . . . . . . . . . . . . . . .

5.1.4 Dynamiczne zale˙zno´sci pr ˛adowe . . . . . . . . . . . . . . . . . . . . . . . .

5.2 Wyznaczanie parametrów modelu w eksperymentach . . . . . . . . . . . . . . . . .

5.2.1 Przeło˙zenie pr ˛adowe silników wraz z przekładniami – K

. . . . . . . . . . .

5.2.2 Zast˛epcza długo´s´c korpusu – l . . . . . . . . . . . . . . . . . . . . . . . . .

5.2.3 Współczynnik tarcia pomi˛edzy kołem a korpusem – k

. . . . . . . . . . . .

5.2.4 Momenty bezwładno´sci kół – I

. . . . . . . . . . . . . . . . . . . . .

5.3 Wyniki otrzymane w eksperymentach . . . . . . . . . . . . . . . . . . . . . . . . .

6 Uwagi ko´ncowe

Spis rysunków

Wygl ˛ad robota mobilnego

RoBik z przodu. . . . . . . . . . . . . . . . . . . . . . .

Schematyczny rysunek dwukołowego robota mobilnego

RoBik. . . . . . . . . . . .

Widok robota mobilnego

RoBik z boku. . . . . . . . . . . . . . . . . . . . . . . . .

Schemat pomiaru przeło˙ze´n pr ˛adowych. . . . . . . . . . . . . . . . . . . . . . . . .

Robot mobilny klasy 2.0 — przekrój . . . . . . . . . . . . . . . . . . . . . . . . . .

Układ nap˛edowy koła . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Widok korpusu robota wraz z wahadłem. . . . . . . . . . . . . . . . . . . . . . . . .

Korpus wahadła . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Sterownik robota — kaseta wraz z płytkami, widok z przodu. . . . . . . . . . . . . .

Sterownik robota — kaseta wraz z płytkami, widok z tyłu. . . . . . . . . . . . . . .

Schemat blokowy układu elektronicznego robota . . . . . . . . . . . . . . . . . . .

Schemat poł ˛acze´n modułów ze sob ˛a . . . . . . . . . . . . . . . . . . . . . . . . . .

Schemat elektryczny podł ˛aczenia sterownika MPC555 na płytce STER . . . . . . . .

Płytka sterownika robota z jednostk ˛a centraln ˛a — STER. . . . . . . . . . . . . . . .

Płytka sterownika robota z układami wykonawczymi — MOC. . . . . . . . . . . . .

Schemat ideowy przetworników AC i CA — interfejs QSPI . . . . . . . . . . . . . .

Schemat ideowy regulatora sterownika mostka mocy — poprawiony . . . . . . . . .

Schemat ideowy połowy mostka mocy . . . . . . . . . . . . . . . . . . . . . . . . .

Schemat ideowy układu zasilaj ˛acego . . . . . . . . . . . . . . . . . . . . . . . . . .

Schemat ideowy układ pomiaru temperatury . . . . . . . . . . . . . . . . . . . . . .

Schemat ideowy klawiatury . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Schemat ideowy BDM555 — Wiggler . . . . . . . . . . . . . . . . . . . . . . . . .

Schemat ideowy układu radiowej transmisji danych DECT . . . . . . . . . . . . . .

Moduły szybkiej transmisji radiowej DECT. . . . . . . . . . . . . . . . . . . . . . .

Schemat ideowy układu ADXL — pomiar przyspieszenia . . . . . . . . . . . . . . .

Poł ˛aczone moduły sterownika mi˛edzy sob ˛a. . . . . . . . . . . . . . . . . . . . . . .

CW IDE z widocznym oknem zarz ˛adzania projektem . . . . . . . . . . . . . . . . .

CW IDE z widocznym oknem debuggera . . . . . . . . . . . . . . . . . . . . . . .

Wykres zale˙zno´sci ˙

α = f ( ˙ϕ). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Wykres pr˛edko´sci k ˛atowych ˙

α i ˙ϕ w funkcji czasu. . . . . . . . . . . . . . . . . . .

Zale˙zno´s´c pr ˛adu I

set

w funkcji pr ˛adu I

cur

. . . . . . . . . . . . . . . . . . . . . . . .

Wykresy zale˙zno´sci I

act

f (I

set

)

dla swobodnych kół. . . . . . . . . . . . . . . . .

Wykresy zale˙zno´sci I

act

f (I

set

)

dla zablokowanych kół. . . . . . . . . . . . . . . .

Wykresy czasowe pr ˛adów I

set

, I

act

i pr˛edko´sci k ˛atowej ˙

ϕ dla swobodnych kół. . . . .

Wykresy czasowe pr ˛adów I

set

, I

act

i pr˛edko´sci k ˛atowej ˙

ϕ dla swobodnych kół. . . . .

Wykresy czasowe pr ˛adów I

set

, I

act

dla swobodnych kół. . . . . . . . . . . . . . . . .

Wykresy czasowe pr ˛adów I

set

, I

act

dla zablokowanych kół. . . . . . . . . . . . . . .

Wykres pr˛edko´sci k ˛atowej kół ˙

ϕ w funkcji czasu. . . . . . . . . . . . . . . . . . . .

Widok robota mobilnego

RoBik z tyłu. . . . . . . . . . . . . . . . . . . . . . . . .

Spis tablic

Parametry konstrukcyjne mierzone bezpo´srednio . . . . . . . . . . . . . . . . . . .

Wielko´sci zadawane i mierzone w sterowniku . . . . . . . . . . . . . . . . . . . . .

Parametry mechaniczne do modelu dynamiki . . . . . . . . . . . . . . . . . . . . .

Parametry techniczne przekładni planetarnej GP32C . . . . . . . . . . . . . . . . .

Parametry techniczne silnika A-max 32 . . . . . . . . . . . . . . . . . . . . . . . .

Parametry techniczne sprz˛egła SOZ25-6-12 . . . . . . . . . . . . . . . . . . . . . .

Gniazdo zasilania (ZAS) na płytce MOC . . . . . . . . . . . . . . . . . . . . . . . .

Kabel ł ˛acz ˛acy płytki STER z nadajnikiem radiowym DECT . . . . . . . . . . . . . .

Kabel ł ˛acz ˛acy płytki STER z ADXL i koderami . . . . . . . . . . . . . . . . . . . .

Gniazdo portu szeregowego (RS232) na płytce STER . . . . . . . . . . . . . . . . .

Gniazdo joystick (JOY) na płytce STER . . . . . . . . . . . . . . . . . . . . . . . .

Gniazdo BDM na płytce STER . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Podstawowe parametry

RoBika — masy, długo´sci . . . . . . . . . . . . . . . . . .

Pomiar k ˛ata

ϕ w funkcji pr ˛adu I

act

— bez dodatkowego obci ˛a˙znika . . . . . . . . . .

Pomiar k ˛ata

ϕ w funkcji pr ˛adu I

act

— z dodatkowym obci ˛a˙znikiem . . . . . . . . . .

Warto´sci ´srednie przeło˙ze´n pr ˛adowych silników . . . . . . . . . . . . . . . . . . . .

Parametry fizyczne wyznaczone w eksperymentach dla robota

RoBik. . . . . . . . .

1 Wst˛ep

Dwukołowe roboty mobilne klasy 2.0 stanowi ˛a bardzo ciekawe obiekty [10]. Ich typowym przed-

stawicielem jest tzw.

Unicycle, który, wbrew nazwie, jest konstruowany jako wózek dwukołowy.

Zazwyczaj przyjmuje si˛e, ˙ze robot (platforma mobilna) ma dodatkowe punkty styku z podło˙zem, któ-

rych wpływ na zachowanie robota jest pomijalnie mały. W przypadku dosłownego rozumienia plat-

formy dwukołowej, specyficzny grawitacyjny układ nap˛edowy wi ˛a˙ze kinematyk˛e i dynamik˛e robota

w cało´s´c. Opis matematyczny modelu staje si˛e du˙zo bardziej skomplikowany [4], dlatego badania

symulacyjne w ´srodowiskach komputerowych mog ˛a nie oddawa´c rzeczywistych wyników. W Zakła-

dzie Podstaw Cybernetyki i Robotyki został zbudowany robot mobilny

WózekMK [3] stanowi ˛acy

rzeczywisty obiekt bada´n. Nastepnie powstał pomysł zbudowania robota mobilnego o mocniejszej

jednostce centralnej i doskonalszej konstrukcji mechanicznej. Stworzenie nowego robota klasy 2.0

o nazwie

RoBik było jednym z celów tej pracy. Jej ide ˛a jest stworzenie elastycznego obiektu ba-

da´n, na którym mo˙zna b˛edzie implementowa´c i bada´c ró˙zne algorytmy sterowania. Zamierzonym

efektem pracy jest stanowisko laboratoryjne pozwalaj ˛ace efektywnie implementowa´c i wygodnie ba-

da´c ró˙zne algorytmy sterowania dla robotów nieholonomicznych. Do´swiadczenia zebrane w zwi ˛azku

z budow ˛a i eksploatacj ˛a wcze´sniejszego modelu takiego stanowiska (

WózekMK) wykazały, ˙ze za-

sadnicze znaczenie ma solidna konstrukcja mechaniczna i jednostka sterownika o du˙zej mocy ob-

liczeniowej. W szczególno´sci dotyczy to zmiennoprzecinkowej jednostki arytmetycznej, niezb˛ednej

do implementowania algorytmów sterowania w oparciu o model. W zwi ˛azku z tym w pracy poło˙zono

znaczny nacisk na wyznaczenie rzeczywistego modelu matematycznego

RoBika w oparciu o liczne

do´swiadczenia fizyczne przeprowadzone na nim.

1.1 Cel i zakres pracy
Podstawowym celem pracy jest stworzenie platformy do implementacji i bada´n algorytmów sterowa-

nia robotów dwukołowych klasy 2.0. Stworzenie platformy opiera si˛e o:

• zbudowanie nowego obiektu tego typu z siln ˛a jednostk ˛a centraln ˛a MPC555;

• stworzenie ´srodowiska programistycznego ułatwiaj ˛acego implementacj˛e własnych programów

i algorytmów sterowania oraz zdefiniowanie standardowych struktur;

• weryfikacj˛e modelu WózkaMK w odniesieniu do nowego wózka RoBik, czyli wyznaczenie

mo˙zliwie najbardziej zbli˙zonego do rzeczywisto´sci modelu nowego robota mobilnego

RoBik

na podstawie eksperymentów fizycznych;

• implementacj˛e przykładowego algorytmu sterowania robotem.

1.2 Realizacja
Realiazcja pracy w pierwszym kroku polega na dokładnym poznaniu dotychczasowego obiektu

Wó-

zekMK, jego mocnych i słabych punktów. W nast˛epnym kroku nast˛epuje zaprojektowanie nowego

obiektu, cz˛e´sci mechanicznej wraz ze sterownikiem oraz zbudowanie wózka, który b˛edzie stanowił

podmiot dalszej pracy. Maj ˛ac zbudowany wózek mo˙zna przej´s´c do cz˛e´sci badawczej, czyli wyzna-

czy´c mo˙zliwie jak najdokładniej model matematyczny stanowi ˛acy podstaw˛e algorytmu sterowania.

Opracowuj ˛ac oprogramowanie nowego sterownika nale˙zy przewidzie´c procedury autotestowe oraz

autoskaluj ˛ace oczujnikowanie wózka.

Podczas projektowania i budowy nowego robota mobilnego powstał szczegółowy raport [9] konstruk-

cyjny, w którym znajduj ˛a si˛e:

• rysunki wykonawcze i monta˙zowe układu mechanicznego,

• schematy elektryczne sterownika wraz z oczujnikowaniem,

• opis wyprowadze´n gniazd interfejsów zewn˛etrznych i wewn˛etrznych sterownika,

• rozmieszczenie elementów na płytkach wraz z wykazem elementów

• rozmieszczenie ´scie˙zek obwodów drukowanych na płytkach.

W niniejszej pracy przedstawiono zało˙zenia konstrukcyjne i model matematyczny robota

RoBik oraz

ide˛e eksperymentów prowadz ˛acych do zmierzenia parametrów fizycznych wyst˛epuj ˛acych w modelu.

zamieszczono opis konstrukcji cz˛e´sci mechanicznej i elektronicznej robota (dokładn ˛a dokumenta-

cj˛e robota mo˙zna znale´z´c w oddzielnym raporcie [9]). Opisano te˙z przygotowane oprogramowanie

słu˙z ˛ace do obsługi czujników i układów wykonawczych. Udokumentowano przebieg i wyniki eks-

perymentów pozwalaj ˛acych znale´z´c parametry fizyczne z wykorzystaniem wewn˛etrznych czujników

robota.

2 Wprowadzenie

Na posiedzeniu Seminarium ZPCiR 04.05.2000 został przedstawiony pomysł konstrukcji robota mo-

bilnego kulistego

RoBall [2]. Rozwa˙zaj ˛ac osobliwo´sci kinematyki robota poruszaj ˛acego si˛e na pła-

szczy´znie [7], zdecydowano si˛e na odniesienie momentów nap˛edowych, dostarczanych przez dwa

silniki, do wahadła zawieszonego w ´srodku kuli, maj ˛acego dwa obrotowe stopnie swobody uzyskane

dzi˛eki idei przegubu Cardana.

W celu zbadania podstawowych zało˙ze´n odno´snie nap˛edu i pomiaru parametrów ruchu rozpocz˛eto

realizacj˛e dwukołowego, nieholonomicznego robota mobilnego nazwanego

WózekMK. Wyniki prac

nad robotem

WózekMK zostały opublikowane w materiałach konferencyjnych [6, 5, 11], pracach

dyplomowych [13, 1] i raportach [4, 3].

Układ nap˛edowy robota mobilnego

WózekMK oraz RoBall opiera si˛e na tej samej zasadzie. Przesu-

ni˛ety ´srodek ci˛e˙zko´sci wraz z sił ˛a grawitacyjn ˛a stanowi nap˛ed robota. Dokładniejsze badania obu ro-

botów ukazały znacz ˛ace ró˙znice pomi˛edzy nimi.

WózekMK stanowił projekt wst˛epny do robota kuli

RoBall, mimo to okazał si˛e tak ciekawym układem, ˙ze zainspirował pomysł powstania nowego, do-

skonalszego robota mobilnego, który otrzymał nazw˛e

RoBik i został zbudowany od podstaw. Otrzy-

mał now ˛a, znacznie silniejsz ˛ajednostk˛e centraln ˛aMPC555 firmy PHYTEC [14]. Tak˙ze w konstrukcji

mechanicznej nast ˛apiły zmiany poprzez umieszczenie silników w osi kół robota. Dokładniejszy opis

budowy nowego robota

RoBik znajduje si˛e w rozdziale 3.

Rysunek 1: Wygl ˛ad robota mobilnego

RoBik z przodu.

2.1 Model matematyczny
Dwukołowy nieholonomiczny robot mobilny

RoBik nap˛edzany jest wzgl˛edem wewn˛etrznego układu

odniesienia [6, 5, 11].

RoBik składa si˛e z czterech brył:

• koła lewego,

• koła prawego,

• korpusu wraz z układem nap˛edowym,

• wahadła z akumulatorami i kaset ˛a sterownika.

Poniewa˙z korpus i wahadło s ˛aze sob ˛a sztywno poł ˛aczone w dalszych rozwa˙zaniach matematycznych

stanowi ˛a jedn ˛a cało´s´c nazwan ˛a

„korpus”. Robot składa si˛e zatem z trzech brył (dwóch identycznych

kół i korpusu). Dla uproszczenia wzorów przyjmijmy, ˙ze:

1. masa kół jest równomiernie rozło˙zona wzgl˛edem k ˛ata obrotu koła (pomijamy wentyl), rozło˙ze-

nie masy wzgl˛edem promienia jest dowolne,

2. korpus jest wywa˙zony statycznie, czyli pionowa o´s główna korpusu pozostaj ˛acego w spoczynku

jest pionow ˛a osi ˛a symetrii robota, a pozioma o´s jest równoległa do osi kół robota.

Oznaczenia wraz z ich rozmieszczeniem geometrycznym przedstawia rysunek 2.

[

Rysunek 2: Schematyczny rysunek dwukołowego robota mobilnego

RoBik.

Podobnie jak w [6] uzyskano ró˙znicowy model

RoBika [12]. Wprowadzaj ˛ac pomocnicze pr˛edko´sci

sprowadzone do osi kół mamy:

• γ

+ ˙

– pr˛edko´s´c zwi ˛azana z ruchem post˛epowym robota,

• γ

− ˙ϕ

– pr˛edko´s´c zwi ˛azana z ruchem obrotowym robota wzgl˛edem osi pionowej,

• γ

= ˙

α – pr˛edko´s´c wahania korpusu.

We współrz˛ednych (x,y,

α,γ

)

, przyjmuj ˛ac sterowania ró˙znicowe v

− u

, i oznaczaj ˛ac

θ =

(

− ϕ

)

uzyskujemy równania dynamiki o postaci:

˙x = R(

)

cos

θ, ˙y = R(γ

)

sin

θ,

− γ

α = γ

(1)

α)˙γ + Q(α,γ) = Sv,

(2)

z macierz ˛a bezwładno´sci o postaci:

α) = P =







,

(3)

gdzie:

cos

α,

cos

α + I

sin

α + m

sin

cos

α + m

(4)

Wektor momentów sił Coriolisa, od´srodkowych i tarcia lepkiego (dla k

k):

α,γ) = Q =








−m

sin

αγ

sin

α γ

1 +

cos

−

sin

α 1 +

cos

− m

sin

αγ

g`Rsin








(5)

i macierz sterowa´n:

S =





1 0

0 1

0 0



.

(6)

2.2 Parametry modelu i ich sens fizyczny
Parametry robota mobilnego przedstawione w modelu mo˙zna podzieli´c na dwie grupy:

• proste, które mo˙zna w łatwy sposób wyznaczy´c (długo´sci, masy),

• zło˙zone, których wyznaczenie opiera´c si˛e b˛edzie na eksperymentach (momenty bezwładno´sci,

współczynniki tarcia, przekładnia pr ˛adowa silników).

Eksperymenty pomiarowe mo˙zna przeprowadzi´c na

RoBiku wykorzystuj ˛ac jego sterownik wraz

z oczujnikowaniem, zadaj ˛ac odpowiednie wymuszenia pr ˛adowe na silniki i mierz ˛ac wychylenia k ˛a-

towe kół, pr˛edko´s´c k ˛atow ˛akół wzgl˛edem korpusu oraz przyspieszenia, wychylenie i pr˛edko´s´c k ˛atow ˛a

korpusu. Zestawienie parametrów modelu i wielko´sci dost˛epnych w sterowniku podano w tablicach

1, 2, 3.

Tablica 1: Parametry konstrukcyjne mierzone bezpo´srednio

Parametr

Symbol Wymiar

masa korpusu wraz z osi ˛a

[kg]

masa koła

[kg]

promie´n koła

[m]

rozstaw kół

[m]

Tablica 2: Wielko´sci zadawane i mierzone w sterowniku

Parametr

Symbol

Wymiar

Wielko´sci zadawane w sterowniku (wymuszenia)

pr ˛ady silników

[A]

Wielko´sci odczytywane ze sterownika (pomiary)

poło˙zenia k ˛atowe kół wzgl˛edem korpusu

[rad]

pr˛edko´sci k ˛atowe kół wzgl˛edem korpusu

, ˙

[rad·s

−1

]

Tablica 3: Parametry mechaniczne do modelu dynamiki

Parametr

Symbol

Wymiar

momenty bezwładno´sci kół

[kg·m

]

momenty bezwładno´sci korpusu

[kg·m

]

zast˛epcza długo´s´c wahadła (korpusu)

[m]

przekładnia pr ˛adowa silnika

[kg·m

·s

−2

·A

−1

]

współczynnik tarcia pomi˛edzy kołem a korpusem

[kg·m

·rad·s

−3

]

współczynnik tarcia toczenia koła

[kg·m

·rad·s

−3

]

Wypadkowy współczynnik tarcia sprowadzony do osi koła jest zło˙zony z dwóch parametrów fizycz-

nych:

k = k

(7)

Sygnały steruj ˛ace v

i v

s ˛a zast˛epczymi momentami nap˛edowymi odpowiadaj ˛acymi, odpowiednio,

składowej post˛epowej i składowej obrotowej ruchu

RoBika:

(

(8)

− u

(

− i

(9)

2.3 Idea eksperymentu
W eksperymencie nale˙zy najpierw zmierzy´c wszystkie parametry proste – m

, m

, R

, d. Poniewa˙z

wielko´sci ˛a steruj ˛ac ˛a w modelu dynamiki jest moment nap˛edowy silnika, a wielko´sci ˛a zadan ˛a – pr ˛ad

silnika, w pierwszej kolejno´sci nale˙zy wyznaczy´c przeło˙zenie pr ˛adowe silników wraz z przekład-

niami – K

. Charakter eksperymentu jest kaskadowy. Składa si˛e on z ci ˛agu do´swiadcze´n, w których

wyznacza si˛e pojedyncze parametry potrzebne do nast˛epnego do´swiadczenia. Eksperymenty s ˛a opi-

sane poni˙zej w proponowanej kolejno´sci ich wykonywania.

2.3.1 Przeło˙zenie pr ˛adowe silników wraz z przekładniami – K

Koła nale˙zy umocowa´c sztywno do podło˙za, korpus pozostaje swobodny. Przy zadanym pr ˛adzie

w stanie ustalonym mierzony jest k ˛at wychylenia korpusu

. Pomiar ten nale˙zy powtórzy´c z do-

dan ˛a mas ˛a m

podwieszon ˛a do korpusu na znanym ramieniu l

(patrz rysunek 4). Pr ˛ad drugiej serii

Rysunek 3: Widok robota mobilnego

RoBik z boku.

0 .

Rysunek 4: Schemat pomiaru przeło˙ze´n pr ˛adowych.

pomiarowej oznaczono jako i

, a k ˛at wychylenia korpusu

, wyniki bada´n posłu˙z ˛a do rozwi ˛azania

układu równa´n:

gl sin

= (

gl + m

)

sin

(10)

z którego wyznaczymy przeło˙zenie pr ˛adowe:

sin

− i

sin

(11)

2.3.2 Zast˛epcza długo´s´c korpusu – l

W układzie pomiarowym z porzedniego do´swiadczenia (przytwierdzone koła) zadajemy wymuszenie

i i mierzymy

ϕ w stanie ustalonym. Znaj ˛ac przyspieszenie ziemskie g, oraz m

i K

otrzymujemy:

i = m

gl sin

ϕ,

(12)

sk ˛ad wyznaczamy zast˛epcz ˛a długo´s´c korpusu robota:

l =

gsin

(13)

2.3.3 Współczynnik tarcia pomi˛edzy kołem a korpusem – k

Wyznaczaj ˛ac współczynnik tarcia pomi˛edzy jednym kołem a korpusem robota mocujemy sztywno

korpus do podło˙za tak, aby koła obracały si˛e swobodnie. Tarcie przybli˙zono tarciem lepkim. Dla

zadanego pr ˛adu silnika i równanie ruchu w stanie ustalonym ma posta´c:

i = k

∞

(14)

Wyznaczamy współczynnik tarcia:

∞

(15)

2.3.4 Momenty bezwładno´sci kół – I

Układ pomiarowy pozostaje ten sam (korpus sztywno zamocowany do podło˙za, koła swobodnie wi-

sz ˛ace). Z wcze´sniejszych do´swiadcze´n znamy ju˙z: m

, m

, R

, d, K

, l, k

. Nadajemy kołom pewn ˛a

pocz ˛atkow ˛a pr˛edko´s´c ˙

i obserwujemy ruch tego koła. Poło˙zenie k ˛atowe opisane jest równaniem:

ϕ(t) = ϕ

1 − e

−

2Iw1

(16)

pr˛edko´s´c:

ϕ(t) = ˙ϕ

−

2Iw1

(17)

Zapami˛etujemy pary chwil czasu t

, t

takie, ˙ze:

ϕ(t

) =

2 ˙

ϕ(t

(18)

Wyznaczony moment bezwładno´sci koła wzgl˛edem jego osi obrotu:

2ln2

(

− t

(19)

Przy zało˙zeniu o symetrycznym rozmieszczeniu masy koła wzgl˛edem osi obrotu moment bezwład-

no´sci koła wzgl˛edem ´srednicy wynosi:

1
2

(20)

2.4 Uwagi praktyczne
Zaproponowana w [12] metoda kaskadowego pomiaru parametrów fizycznych mo˙ze poci ˛aga´c za sob ˛a

bł˛edy, ze wzgl˛edu na to, ˙ze wyniki wcze´sniejszych pomiarów rzutuj ˛a na nast˛epne. Mimo to, posta-

nowiono sprawdzi´c w praktyce jej przydatno´s´c dla opisanego w dalszej cz˛e´sci pracy robota. Opis

i wyniki wst˛epnych eksperymentów zamieszczono w rozdziale 5.

3 Konstrukcja robota

Opis konstrukcji robota zastał podzielony na dwie cz˛e´sci:

• cz˛e´s´c mechaniczn ˛a, w której opisany jest dokładnie korpus robota oraz wahadło i mocowanie

kół,

• cz˛e´s´c elektroniczn ˛a, w której znajduj ˛a si˛e schematy sterownika z krótkim opisem zasady dzia-

łania.

3.1 Konstrukcja mechaniczna

Rysunek 5: Robot mobilny klasy 2.0 — przekrój

RoBik składa si˛e z dwóch kół oraz korpusu zawieszonego mi˛edzy nimi (rysunek 5). Aby zminimali-

zowa´c koszt robota, zastosowane zostały dwa przednie 20” koła rowerowe.

Korpus robota mo˙zna podzieli´c na dwie cz˛e´sci:

• układ nap˛edowy zawarty w osi kół (rysunek 6):

1. silnik wraz z przekładni ˛a planetarn ˛a,
2. tuleja mocuj ˛aca silnik,
3. korpus układu nap˛edowego,

Rysunek 6: Układ nap˛edowy koła

4. sprz˛egło,
5. o´s koła,
6. ło˙zyska,
7. tuleja mocuj ˛aca ło˙zyska,
8. dystans mi˛edzy ło˙zyskami,
9. podkładka dystansowa koła,

10. gumowa podkładka,
11. stalowe pier´scienie,
12. konus,
13. koło,

• wahadło z kaset ˛a sterownika oraz akumulatorami umieszczonymi pod układem nap˛edowym.

Szczegółowe rysunki wykonawcze elementów mechanicznych umieszczone s ˛a w raporcie [9]

3.1.1 Układ nap˛edowy

Sercem układu nap˛edowego jest silnik pr ˛adu stałego typu A-max 32 firmy Maxon [16], zaopatrzony

w ceramiczne, planetarne przekładnie GP32C. Parametry techniczne umieszczone s ˛a w tablicach

4 i 5.

Tablica 4: Parametry techniczne przekładni planetarnej GP32C

maksymalny wyj´sciowy

6 Nm

moment obrotowy

przeło˙zenie

1:66

sprawno´s´c

70 %

´sredni luz stopnia wyj´sciowego

Tablica 5: Parametry techniczne silnika A-max 32

moc mechaniczna

20W

znamionowe napi˛ecie zasilania

30V

pr˛edko´s´c obrotowa bez obci ˛a˙zenia

6130 obr/min

maksymalny pr ˛ad ci ˛agły

1.02 A

maksymalny ci ˛agły moment obrotowy

47.1 mNm

przeło˙zenie pr ˛adowe

46 mNm/A

sprawno´s´c

77 %

Moment nap˛edowy z przekładni podawany jest na o´s koła za po´srednictwem sprz˛egła

SOZ25-6-12

typu

OLDHAM [17] zbudowanego z trzech cz˛e´sci:

• dwóch pier´scieni zaciskowych wykonanych ze stopu aluminium,

• wkładki sprz˛egaj ˛acej przenosz ˛acej moment nap˛edowy, wykonanej z czarnego acetatu.

Parametry umieszczone s ˛a w tablicy 6.

Tablica 6: Parametry techniczne sprz˛egła SOZ25-6-12

moment bezwładno´sci

252gcm

maksymalny moment

4Nm

moment zrywaj ˛acy

13Nm

maksymalna

±0.2mm

kompensacja

±0.5

ugi˛ecie skr˛etne

0.28

/Nm

sztywno´s´c

205Nm/Rad

masa

31g

Na osi (rysunek 6, element nr 5) zamocowane jest sztywno koło rowerowe (element nr 13). W miejsce

ło˙zyska koronkowego wstawiony jest pier´scie´n (element nr 11) wykonany z pancerza stalowego linki

hamulcowej. Cało´s´c jest skr˛econa za pomoc ˛a konusa rowerowego (element nr 12) o gwincie M8x1

skontrowanego nakr˛etk ˛a o tym samym gwincie.

3.1.2 Korpus

Cały układ nap˛edowy umieszczony jest w korpusie (rysunek 6, element nr 3) wykonanym z rury

kwasoodpornej. Na ´srodku rury wyfrezowany jest otwór, przez który s ˛a doprowadzone przewody

do silników i akcelerometru. Silnik (element nr 1) jest zamocowany w tulei (element nr 2) za po-

moc ˛a czterech ´srub M3x10, natomiast tuleja przytwierdzona jest do rury korpusu o´smioma ´srubami:

M4x10 oraz M4x16.

O´s koła umocowana jest w dwóch ło˙zyskach (element nr 6) typu 6005 H2, pomi˛edzy którymi znaj-

duje si˛e tuleja dystansowa (element nr 8) oraz tuleja mocuj ˛aca ło˙zyska (element nr 7) w rurze korpusu,

za pomoc ˛a czterech ´srub M4x10. Pomi˛edzy kołem, a ło˙zyskami znajduje si˛e podkładka dystansowa

(element nr 9) wraz z podkładk ˛a gumow ˛a. Zadaniem obu podkładek jest zlikwidowanie luzu wzdłu˙z-

nego na ło˙zyskach.

3.1.3 Wahadło

Rysunek 7: Widok korpusu robota wraz z wahadłem.

Do korpusu jest przymocowane wahadło (rysunek 8) przy pomocy bocznych ´srub mocuj ˛acych tuleje

wewn ˛atrz rury korpusu. Kosz wahadła składa si˛e z trzech cz˛e´sci:

• dwóch ´scianek bocznych,

• dna wahadła, w którym umieszczone s ˛a akumulatory.

Wahadło zostało wykonane z 1mm blachy kwasoodpornej, ze wzgl˛edu na agresywne działanie opo-

rów akumulatora. Akumulatory w skrzynce wahadła umieszczone s ˛a w pozycji le˙z ˛acej. Nierów-

nomierny rozkład masy w akumulatorze jest kompensowany przez ´srodkowy akumulator uło˙zony

w przeciwn ˛a stron˛e. Takie uło˙zenie akumulatorów ma zmniejszy´c przesuni˛ecie ´srodka ci˛e˙zko´sci

wahadła, w stosunku do jego geometrycznego ´srodka. Ponad akumulatorami, do ´scianek bocznych

wahadła przymocowana jest kaseta sterownika.

3.1.4 Kaseta sterownika

Kaseta sterownika wykonana jest ze stopu aluminium, a jej konstrukcja składa si˛e z siedmiu elemen-

tów poł ˛aczonych ze sob ˛a za pomoc ˛a ´srub M3x10. Elementy konstrukcji to:

• dwie ´scianki boczne, które mocowane s ˛a do wahadła,

• ´scianka ´srodkowa dziel ˛aca przestrze´n wewn ˛atrz kasety na dwie równe komory,

Rysunek 8: Korpus wahadła

• pokrywa górna,

• pokrywa dolna,

• pokrywa przednia, w której umiejscowiony jest interfejs u˙zytkownika oraz zł ˛acza zewn˛etrzne

(BDM, RS232, joystick),

• pokrywa tylna, w której umiejscowione s ˛a zł ˛acza lokalne (zasilaj ˛ace, DECT, ADXL+KODER)

oraz wył ˛acznik zasilania.

Rysunek 9: Sterownik robota — kaseta wraz z płytkami, widok z przodu.

Aby umo˙zliwi´c wywa˙zenie wahadła, w kasecie umieszczone s ˛a dwa obci ˛a˙zniki przesuwane po gwin-

towanych pr˛etach M6 i M4. Pr˛ety te s ˛a wzajemnie prostopadłe, równoległe odpowiednio do osi OX

i OY. Ka˙zdy obci ˛a˙znik składa si˛e z dwóch wałków mosi˛e˙znych, co pozwala ustali´c poło˙zenie przez

wzajemne kontrowanie. Kaseta sterownika podzielona jest na dwie cz˛e´sci. W prawej komorze znaj-

duje si˛e sterownik (mikrokontroler MPC555), natomiast w lewej - układy mocy (zasilacz i mostki).

3.2 Konstrukcja elektroniczna — Sterownik
Robot mobilny

RoBik jest robotem autonomicznym. Posiada sterownik, którego schemat blokowy

przedstawiony jest na rysunku 11. Układ elektroniczny dokonuje pomiarów wszystkich niezb˛ednych

Rysunek 10: Sterownik robota — kaseta wraz z płytkami, widok z tyłu.

parametrów ruchu, wylicza sterowanie oraz wymusza moment nap˛edowy, zadaj ˛ac silnikom wcze´sniej

wyliczony pr ˛ad.

Rysunki monta˙zowe płytek (rozmieszczenie elementów i ´scie˙zek) znajduj ˛a si˛e w raporcie [9].

Rysunek 11: Schemat blokowy układu elektronicznego robota

Moduły sterownika zostały podzielone na trzy grupy funkcjonalne:

• układ sterowania oparty na module sterownika MPC555 firmy PHYTEC (rysunek 13), nazwa

własna

STER,

• układ stopnia mocy (rysunki 16, 17, 18, 20, 19), nazwa własna MOC,

• urz ˛adzenia peryferyjne.

Rysunek 12: Schemat poł ˛acze´n modułów ze sob ˛a

3.2.1 Budowa sterownika — STER

Układ elektroniczny jest podzielony fizycznie na dwie bazowe płytki.

STER jest to jedna z płytek

bazowych umieszczonych w prawej komorze skrzynki sterownika, na której znajduj ˛a si˛e:

• moduł sterownika MPC555 zamocowany na dwóch zł ˛aczach 160-stykowych (2x80),
• moduł ˙zyrokompasu ENC-03J wraz ze wzmacniaczem operacyjnym U1,
• przetwornik AC typu MAX186 [18] pracuj ˛acy na magistrali QSPI,
• stabilizator napi˛ecia LowDrop 3V3,
• dwa ´zródła pr ˛adowe do współpracy z joystickiem,
• klucze steruj ˛ace diodami LED,
• wzmacniacz steruj ˛acy kontrastem wy´swietlacza LCD,
• zł ˛acza wej´sciowo/wyj´sciowe (komunikacja z pozostałymi układami oraz interfejsy zewn˛etrzne).

Dokładny schemat poł ˛acze´n na płytce STER znajduje si˛e na rysunku 13. Uło˙zenie elementów na

płytce przedstawiaj ˛a rysunki umieszczone w raporcie [9].

Jednostka centralna (JC), jak ˛astanowi moduł firmy PHYTEC

phyCORE-MPC555, jest to wielowar-

stwowa płytka drukowana o wymiarach 72 x 57 mm, zawieraj ˛aca nast˛epuj ˛ace układy:

Rysunek 13: Schemat elektryczny podł ˛aczenia sterownika MPC555 na płytce STER

• mikrokontroler rodziny MPC555 posiadaj ˛acy:

– 32-bitowy mikrokontroler PowerPC, zegar 40MHz,
– 64-bitow ˛a jednostk˛e zmiennoprzecinkow ˛a,
– 26 kB SRAM,
– 448 kB FLASH,
– podwójny interfejs UART/SPI,
– podwójny interfejs CAN 2.0B,

– podwójny układ TPU, ka˙zdy po 16 kanałów,
– dwa 16-bitowe systemy liczników,
– 8 kanałów, 16-bitowych układów PWM,
– dwa 10-bitowe ADC (7µs),
– uniwersalne, wielofunkcyjne sygnały I/O,
– port testowo-uruchomieniowy JTAG/BDM,

• Pami˛e´c:

– SRAM 1 MB,
– Flash-ROM 1 MB,
– I

C 4 kB,

• I

C zegar czasu rzeczywistego (RTC),

• Power-Down/Wake-Up wspierany przez RTC,

• podwójny port UART/SPI (RS232/TTL),

• podwójny port CAN (CAN/TTL),

Rysunek 14: Płytka sterownika robota z jednostk ˛a centraln ˛a — STER.

Za pomoc ˛a interfejsu QSPI do jednostki centralnej podł ˛aczone s ˛a 12-bitowe przetworniki:

• układ MAX186 [18] (o´smiokanałowy, 12-bitowy przetwornik analogowo-cyfrowy uni/bipolarny

o 4V zakresie pomiaru, z wbudowanym ´zródłem napiecia odniesienia) słu˙z ˛acy do mierzenia war-

to´sci wychylenia k ˛atowego zadawanego przez ˙zyroskop ENC-03J (jeden kanał), pomiaru przy-

spiesze´n X i Y zadanych przez akcelerometr ADXL202 (dwa kanały) oraz pomiaru wychylenia

joysticka (dwa kanały),

• układ MAX188 [18] (o´smiokanałowy, 12-bitowy przetwornik analogowo-cyfrowy uni/bipolarny

o 4V zakresie pomiaru, wymaga zewn˛etrznego ´zródła napi˛ecia odniesienia) zamontowany na

drugiej płytce bazowej,

• układ MAX5742 [18] (czterokanałowy, 12-bitowy przetwornik cyfrowo-analogowy unipolarny,

wymaga zewn˛etrznego ´zródła napiecia odniesienia) zamontowany na drugiej płytce bazowej,

• gniazdo QSPI umo˙zliwiaj ˛ace doł ˛aczenie dodatkowego modułu.

Dwa bloki TPU jednostki centralnej obsługuj ˛a nast˛epuj ˛ace funkcje:

• pomiar przyspiesze´n zadanych z akcelerometru ADXL202 za pomoc ˛a fali prostok ˛atnej o modu-

lowanym współczynniku wypełnienia (6 kanałów),

• pomiar wychylenia k ˛atowego silników nap˛edowych przy pomocy koderów impulsowych HEDS

550 (9 kanałów),

• obsługa klawiszy, siedmiu na panelu oraz czterech przycisków joysticka, ostatni klawisz i ostatni

przycisk joysticka s ˛a wspólne (10 kanałów),

• cztery kanały wyprowadzone na zł ˛acze TPU (rozwojowo), pierwszy z kanałów alternatywnie

obsługuje gło´snik zamontowany wraz z klawiszami panelu kontrolnego,

• sterowanie mostkiem mocy w trybie pracy PWM — napi˛eciowym (3 kanały, funkcja przeł ˛a-

czana, za pomoc ˛a zworek na płytce, pomi˛edzy wyj´sciami bloku TPU, a PWM).

Port MPIO jednostki centralnej obsługuje zintegrowany wy´swietlacz ciekłokrystaliczny LCD o ma-

trycy: 2 linie po 20 znaków. Wy´swietlacz LCD podł ˛aczony jest do płytki STER za pomoc ˛a zł ˛acza

LCD.

Kontrast wy´swietlacza jest zadawany przez przetwornik CA, z którego napi˛ecie jest wzmocnione

za po´srednictwem wzmacniacza operacyjnego (dopasowanie poziomów napi˛ecia). Na linii MPIO11

znajduje si˛e klucz wł ˛aczaj ˛acy pod´swietlenie wy´swietlacza LCD. Linie MPIO12-14 steruj ˛a diodami

LED. Ostatnia linia tego portu jest przył ˛aczona do pierwszego klawisza panelu i wykorzystywana

podczas uruchamiania systemu. Sprawdzane s ˛a wtedy równie˙z poło˙zenia zworek JP4, JP15 i JP16.

Układ PWM jest wykorzystany do sterowania mostkiem mocy w trybie PWM (dwa kanały). Za

pomoc ˛a zworek PWM1 i PWM2 wybieramy, który blok ma sterowa´c mostkiem - PWM czy TPU.

Sygnał PWM doprowadzony jest do mostka mocy za pomoc ˛a gniazda KONC.

Moduł SCI jest modułem dwukanałowym. Ka˙zdy z kanałów mo˙ze pracowa´c w standardzie TTL

albo RS232, wykorzystuj ˛ac konwerter poziomów MAX232. Do pierwszego kanału został podł ˛a-

czony układ transmisji radiowej DECT (poprzez zł ˛acze DECT, tabela 8). Kanał ten został ustawiony

w standardzie TTL przez rozł ˛aczenie zworki J13 na module PhyCORE555 [14]. Drugi kanał jest

przeznaczony do bezpo´sredniej komunikacji z komputerem nadrz˛ednym za pomoc ˛astandardu RS232.

Sygnały s ˛a doprowadzone do gniazda RS232 (tabela 10).

Wyprowadzenia wbudowanego w jednostce centralnej przetwornika analogowo-cyfrowego zostały

przeznaczone do współpracy z układem transmisji radiowej DECT. Tryb pracy wyprowadze ´n przet-

wornika AC został ustawiony w wyj´sciowo-wej´sciowy port cyfrowy.

Interfejs BDM jednostki centralnej został wyprowadzony na zewn ˛atrz sterownika za pomoc ˛a gniazda

BDM (tabela 12).

Na płytce STER znajduj ˛a si˛e jeszcze dwa ´zródła pr ˛adowe wymuszaj ˛ace pr ˛ad 26µA w potencjome-

trach X i Y joysticka. Potencjometry joysticka pracuj ˛a w układzie zmiennej rezystancji od 0 do

150k

Ω, Zakres pomiarowy przetwornika od 0 do 4V implikuje pr ˛ad ´zródła. Przy ´zródłach pr ˛adowych

s ˛a zworki SJ1 i SJ2 których zwarcie powoduje zwi˛ekszenie pr ˛adu z 26µA na 40µA, dla joysticków

z potencjometrami 100k

Ω.

Jednostka centralna wymaga napi˛ecia zasilania 3,3V, dlatego na płytce STER umieszczony został

stabilizator napi˛ecia LM1086-3.3. Jest to monolityczny układ scalony o niskim spadku napi˛ecia (Low

Drop), wi˛ec mo˙ze by´c zasilany z +5V.

Mikrostyk resetuj ˛acy jednostk˛e centraln ˛ajest podł ˛aczony przewodami do układu, równolegle do przy-

cisku podł ˛aczony jest kondensator 1nF. Mikrostyk na czas resetu zwiera sygnał

/HRESIN do masy.

3.2.2 Budowa układu mocy — MOC

MOC jest to druga bazowa płytka umieszczona w lewej komorze skrzynki sterownika, zawieraj ˛aca

nast˛epuj ˛ace układy:

• przetworniki AC typu MAX188 [18] oraz CA typu MAX5742 [18] (rysunek 16), których pod-

stawow ˛a funkcj ˛a jest zadawanie warto´sci pr ˛adu silnika oraz mierzenie zadanej i rzeczywistej

warto´sci tego pr ˛adu,

• dwa układy mierz ˛ace temperatur˛e wewn ˛atrz skrzynki sterownika (rysunek 20),

• dwa układy logiczne steruj ˛ace i zabezpieczaj ˛ace mostki przed jednoczesnym wł ˛aczeniem obu

połówek mostka (rysunek 17),

• dwa mostki zbudowane na tranzystorach MOSFET (rysunek 18 przedstawia tylko połow˛e jed-

nego mostka, druga jest identyczna),

• zasilacz impulsowy ±5V (rysunek 19).

Przetwornik analogowo cyfrowy MAX188 jest ubo˙zsz ˛a wersj ˛a układu MAX186 (wymaga zewn˛e-

trznego ´zródła napi˛ecia odniesienia). Podstawow ˛a funkcj ˛a, jak ˛a spełnia MAX188 jest pomiar war-

to´sci pr ˛adów silników nap˛edowych. Przetwornik posiada osiem kanałów analogowych, mierz ˛acych

odpowiednio:

0 rzeczywisty pr ˛ad silnika lewego,

1 rzeczywisty pr ˛ad silnika prawego,

2 zadan ˛a warto´s´c pr ˛adu na silnik lewy,

Rysunek 15: Płytka sterownika robota z układami wykonawczymi — MOC.

3 zadan ˛a warto´s´c pr ˛adu na silnik prawy,

4 temperatur˛e wewn˛etrzn ˛a (radiatora),

5 temperatur˛e zewn˛etrzn ˛a,

6 napi˛ecie akumulatora +36V,

7 napi˛ecie zasilania -5V.

Kanały mog ˛apracowa´c w trybie unipolarnym lub bipolarnym. Zakres pomiarowy kanału wynosi (dla

2,5V ´zródła napi˛ecia odniesienia podł ˛aczonego do wej´scia REFADJ): w trybie unipolarnym od 0V

do 4,095V, w trybie bipolarnym od -2,048V do 2,047V. Kanały 4, 5 i 6 pracuj ˛a unipolarnie, pozostałe

s ˛a w trybie pracy bipolarnej. MAX188 przesyła dane pomiarowe przy pomocy ł ˛acza szeregowego

QSPI do jednostki centralnej umieszczonej na drugiej płytce. Linie ł ˛acza QSPI doprowadzone s ˛a do

gniazda STER.

Układ MAX5742 jest przetwornikiem cyfrowo-analogowym, posiadaj ˛acym cztery kanały pracuj ˛ace

tylko w trybie unipolarnym, od 0V do 2,5V. Celem przetwornika CA jest zada´c pr ˛ad silnika do układu

regulatora i sterownika mostka silników. Kanały układu maja nast˛epuj ˛ace funkcje:

0 pr ˛ad silnika lewego,

1 pr ˛ad silnika prawego,

2 kontrast wy´swietlacza LCD,

3 nieu˙zywany.

Układ regulatora wymaga napi˛ecia bipolarnego. Dlatego do kanałów 0 i 1 doł ˛aczone zostały wzmac-

niacze operacyjne, które odejmuj ˛a od napi˛ecia wyj´sciowego połow˛e napi˛ecia ´zródła odniesienia

i wzmacniaj ˛a sygnał dwa razy. W rezultacie z sygnału przetwornika CA o zakresie od 0V do 2,5V

otrzymano sygnał w zakresie od −2,5V do 2,5V. Dla weryfikacji poprawno´sci zadanego pr ˛adu napi˛e-

cie wyj´sciowe z wzmacniaczy operacyjnych podawane jest na przetwornik AC (MAX188). Jednostka

centralna komunikuje si˛e z przetwornikiem CA za po´srednictwem interfejsu QSPI.

Rysunek 16: Schemat ideowy przetworników AC i CA — interfejs QSPI

Zadane napi˛ecie z przetwornika CA przesuni˛ete i wzmocnione, podane jest do regulatora pr ˛adu (rysu-

nek 17). Regulacja polega na porównaniu warto´sci zadanej pr ˛adu silnika nap˛edowego z rzeczywist ˛a.

Warto´s´c rzeczywista pr ˛adu jest mierzona jako spadek napi˛ecia na rezystorach pomiarowych 0,1

Ω

w układzie mostka. Napi˛ecie z dwóch razystorów jednego mostka podane jest na wej´scie ró˙znicowe

wzmacniacza operacyjnego U1A układu MAX414 [18]. W mostku pr ˛ad mo˙ze płyn ˛a´c tylko przez

jeden rezystor, w zale˙zno´sci od trybu pracy mostka (obroty w lewo, prawo, mostek wył ˛aczony). Po

znaku ró˙znicy potencjału na rezystorach układ rozpoznaje kierunek pr ˛adu płyn ˛acego w silniku. War-

to´s´c napi˛ecia ze wzmacniacza U1A podana jest na filtr RC oraz ,równolegle, na układ porównuj ˛acy

z warto´sci ˛azadan ˛a. Filtr RC ma za zadanie u´sredni´c warto´s´c pr ˛adu silnika i poda´c j ˛a po wzmocnieniu

w wzmacniaczu U1B do przetwornika AC. Sterownik co 1ms odczytuje warto´s´c pr ˛adu silnika, dla-

tego zgodnie z twierdzenie Kotielnikowa-Shannona maksymalna cz˛estotliwo´s´c mierzona nie powinna

przekroczy´c 500Hz. Filtr RC ma spadek 3dB dla cz˛estotliwo´sci 50Hz, dla 1kHz spadek ten wynosi

34dB.

Układem porównuj ˛acym jest komparator MAX9202 [18], który odpowied´z logiczn ˛a porównania po-

daje do cz˛e´sci cyfrowej sterownika mostka mocy. W układzie komparatora zastosowano dodatnie

sprz˛e˙zenie zwrotne, które realizuje p˛etl˛e histerezy, która zapobiega wzbudzeniu.

Podstawowym zadaniem cz˛e´sci cyfrowej jest zabezpieczenie mostka. Polega ono na wprowadze-

niu czasu opó´znienia pomi˛edzy wył ˛aczeniem jednej gał˛ezi mostka, a wł ˛aczeniem drugiej gał˛ezi tego

mostka. Wprowadzony czas opó´znienia (25µs) jest realizowany przy pomocy układu 74HCT123.

Czas opó´znienia ogranicza maksymaln ˛a cz˛estotliwo´s´c przeł ˛aczania mostka. 25µs na jedno przeł ˛acze-

nie daje 50µs na okres, a co za tym idzie, maksymalna cz˛estotliwo´s´c pracy mostka wynosi 20kHz.

Do cz˛e´sci logicznej mostka doprowadzony jest jeszcze sygnał PWM z jednostki centralnej. Słu˙zy on

do sterowania napi˛eciowego silnika. Stan niski sygnału PWM wył ˛acza mostek, stan wysoki wł ˛acza

mostek w konfiguracji zadanej przez regulator. Sygnałem PWM mo˙zna regulowa´c pr˛edko´s´c obrotow ˛a

silnika. Kierunek obrotów ustawia si˛e zadaj ˛ac maksymalny pr ˛ad (znak pr ˛adu jest ´sci´sle powi ˛azany

z kierunkiem obrotów silnika, maksymalna warto´s´c zadaego pr ˛adu wył ˛acza układ regulacji pr ˛adu).

Rysunek 17: Schemat ideowy regulatora sterownika mostka mocy — poprawiony

Cz˛e´s´c logiczna mostka podaje sygnał na wzmacniacze tranzystorowe pracuj ˛ace w układzie OB (rysu-

nek 18). Dzi˛eki bazom przył ˛aczonym do ´zródła napi˛ecia 2,7V, wej´scie wzmacniaczy jest zgodne ze

standardem TTL. Podstawowym zadaniem wzmacniaczy OB jest konwersja poziomów logicznych

z TTL na 0–36V. Sygnał ze wzmacniaczy OB podawany jest na parowane tranzystory (npn i pnp)

pracuj ˛ace w układzie OC. Zadaniem tej pary jest szybkie dostarczenie ładunku do bramki tranzytora

mocy oraz jak najszybsze rozładowanie tej bramki. Diody DZ1 i DZ2 zapewniaj ˛a, ˙ze napi˛ecie ste-

ruj ˛ace tranzystory mocy nie przekroczy warto´sci maksymalnej. W mostkach zostały wykorzystanie

tranzystory MOSFET z serii IRF540N [19] i IRF9540N [19]. Maja one bardzo nisk ˛a rezystancj˛e

kanału w stanie nasycenia (około 0,1

Ω), oraz du˙zy pr ˛ad kanału (wi˛ekszy ni˙z 20A).

Mostek mocy wymaga napi˛ecia zasilania +36V, dlatego jest bezpo´srednio zasilany z trzech aku-

mulatorów ˙zelowych 12V, 5Ah. Pozostała cz˛e´s´c układu elektronicznego wymaga napi˛ecia +5V

(układy cyfrowe TTL), ±5V (wzmacniacze operacyjne, komparatory i przetworniki AC), +3,3V

(jednostka centralna). Napi˛ecia te s ˛a wytwarzane przy pomocy przetwornicy impulsowej L4960

[15] (rysunek 19), która obni˙za napi˛ecie akumulatorów do warto´sci +5V. Ujemne napi˛ecie powstaje

w wyniku indukowania si˛e siły elektromotorycznej w drugim uzwojeniu cewki L9. Napi˛ecie to

po wyprostowaniu i filtracji pojemno´sci ˛a podane jest na scalony stabilizator LowDrop LM2940-5.

Rysunek 18: Schemat ideowy połowy mostka mocy

Cewka L9 jest wykonana na bazie dławika z rdzeniem toroidalnym, na którym jest nawini˛ete 50 zwo-

jów drutu miedzianego lakierowanego o ´srednicy 1mm. Indukcyjno´s´c dławika wynosi 125µH. Na

rdze´n zostało nawini˛ete 60 zwojów drugiego uzwojenia z plecionki czterech przewodów o ´srednicy

0,3mm. Wtórne uzwojenie cewki wymusza sił˛e elektromotoryczn ˛a o obci ˛a˙zalno´sci do 1A. Napi˛ecie

3,3V jest wytwarzane bezpo´srednio przy jednostce centralnej przy pomocy scalonego stabilizatora

LowDrop LM3940-3.3, który czerpie energi˛e z linii zasilania +5V.

Rysunek 19: Schemat ideowy układu zasilaj ˛acego

System został wyposa˙zony w dwa układy pomiaru temperatury. Pierwszy z nich mierzy temperatur˛e

przy elementach mocy (mostkach i zasilaczu), drugi - ma czujnik zamontowany w przeciwległej cz˛e-

´sci sterownika, a sygnał doprowadzony jest przewodami do zł ˛acza Z1 (styk1-Baza, styk2-Kolektor).

Pomiar temperatury opiera si˛e o temperaturowy współczynnik zł ˛acza półprzewodnikowego. Przy sta-

łym, wymuszonym pr ˛adzie zł ˛acza 1mA, napi˛ecie na nim zmienia si˛e o około −2mV na

C. Zmiana ta

jest wzmocniona przez wzmacniacze operacyjne i podana na przetwornik AC. Zakres pomiaru mie´sci

si˛e w przedziale od −50

C do 150

C, bior ˛ac pod uwag˛e zakres przetwornika (0-4V) wzmocnienie

wzmacniacza wynosi 10 razy. Dla czujnika (tranzystor BD911) zostało dobrane napi˛ecie odniesienia

0,633V, które jest podane na nieodwracaj ˛ace wej´scie wzmacniacza operacyjnego.

Rysunek 20: Schemat ideowy układ pomiaru temperatury

Dokładne rozmieszczenie elementów na płytce MOC mo˙zna zobaczy´c na rysunkach umieszczonych

w raporcie [9].

Układ elektroniczny umieszczony na płytce MOC okazał si˛e na tyle skomplikowany, ˙ze mimo zasto-

sowania technologii dwuwarstwowej nie udało si˛e zrealizowa´c wszystkich poł ˛acze´n. S ˛a one wyko-

nane z izolowanego przewodu o nast˛epuj ˛acym przekroju:

• 2.5mm

– Z1

• 1.5mm

– Z2, Z3, Z4

• 1mm

– Z5

• 0.5mm

– Z6, Z7.

Rozmieszczenie zwór przedstawione jest w raporcie [9].

3.2.3 Budowa urz ˛adze´n peryferyjnych

Urz ˛adzenia peryferyjne s ˛ato pozostałe układy, które nie zostały zamontowane na płytkach bazowych,

ale s ˛a do nich podł ˛aczone kablowo. W tej grupie znajduj ˛a si˛e:

• interfejs kablowy BDM555 (Background Debug Mode) (rysunek 22) — Wiggler,

• interfejs radiowy DECT (rysunek 23),

• układ pomiaru przyspiesze´n ADXL (rysunek 25),

• dwa kodery poło˙zenia wirnika silników,

• wy´swietlacz ciekłokrystaliczny LCD,

• klawiatura (rysunek 21).

Rysunek 21: Schemat ideowy klawiatury

Klawiatura (rysunek 21) jest umieszczona na przedniej ´sciance skrzynki sterownika. Stanowi istotn ˛a

cz˛e´s´c interfejsu u˙zytkownika. Konstrukcja opiera si˛e na siedmiu mikrostykach zwieraj ˛acych linie

portu TPU do masy. Moduł TPU jednostki centralnej przy pomocy funkcji DIO uruchomionej w ka-

nałach obsługuj ˛acych przyciski próbkuje z zadan ˛acz˛estotliwo´sci ˛a(np. 1kHz) stan wej´scia. Procedura

uruchamiana cyklicznie (np. co 20 ms) testuje 16-bitow ˛a histori˛e próbkowania przycisku i ustawia

odpowiednie flagi (patrz 4). Na płytce z przyciskami zamocowany jest gło´snik dynamiczny, który

d´zwi˛ekowo sygnalizuje u˙zytkownikowi przyci´sni˛ecie klawisza i inne zdarzenia, oraz trzy diody LED

sterowane z portu MPIO jednostki centralnej.

Rysunek 22: Schemat ideowy BDM555 — Wiggler

BDM (Background Debug Mode) jest to standard ułatwiaj ˛acy współprac˛e z mikrokontrolerem. Przy

pomocy interfejsu BDM oraz niewielkiego urz ˛adzenia (rysunek 22) praca mikrokontrolera mo˙ze by´c

monitorowana przez komputer osobisty za po´srednictwem ł ˛acza LPT. Firmowy program umo˙zliwia

pisanie oprogramowania dla mikrokontrolera, uruchamianie go w trybie krokowym, pułapkowym

albo ci ˛agłym. w łatwy sposób mo˙zna zajrze´c do przestrzeni pami˛eci, zmienia´c zawarto´s´c rejestrów,

itd. Układ został zbudowany w oparciu o moduł interfejsu BDM zastosowany w płytce PCM995

firmy PHYTEC dla modułu PhyCORE-MPC555, zgodny z Wiggler firmy Macraigor Systems.

Rysunek 23: Schemat ideowy układu radiowej transmisji danych DECT

Układ transmisji radiowej (rysunek 23) został zamontowany w

RoBiku za wzgl˛edu na konieczno´s´c

posiadania szybkiego, prostego i niekłopotliwego sposobu przesyłania danych pomi˛edzy robotem

a komputerem nadrz˛ednym. Interfejs BDM, czy RS232 wymagaj ˛a poł ˛aczenia kablowego robota mo-

bilnego ze stacjonarnym komputerem. Kabel ci ˛agni˛ety za robotem mógłby niekorzystnie wpłyn ˛a´c na

wyniki do´swiadcze´n. Dlatego dodany został bezprzewodowy interfejs radiowy.

Moduł DECT (HW86010 firmy Hoeft & Wessel) jest poł ˛aczony za po´srednictwem gniazda DECT (ta-

bela 8) z jednostk ˛acentraln ˛a, wykorzystuj ˛ac jeden kanał SCI w standardzie TTL. Do konwersji pozio-

mów logicnych TTL na 3,3V (u˙zywane przez DECT) zastosowano układy MAX3393E i Max3379E

firmy Maxim [18].

Akcelerometr ADXL202 firmy Analog Device [20] (rysunek 25) jest to układ mierz ˛acy przyspiesze-

nia w dwóch prostopadłych osiach X i Y. Jest on umiejscowiony w osi kół robota, w samym ´srodku

korpusu układu nap˛edowego. Miejsce to jest tak wybrane, aby pr˛edko´s´c obrotowa zwi ˛azana z wy-

chyleniami wahadła miała jak najmniejszy wpływ na wyniki pomiarów przyspiesze´n całego robota.

Akcelerometr jest podł ˛aczony do układu TPU jednostki centralnej oraz alternatywnie do przetwornika

AC. Poniewa˙z akcelerometr znajduje si˛e pomi˛edzy silnikami, tu˙z przy koderach, przewód ł ˛acz ˛acy ak-

celerometr z jednostk ˛a centraln ˛a jest równie˙z podł ˛aczony do koderów (tabela 9).

Rysunek 24: Moduły szybkiej transmisji radiowej DECT.

Rysunek 25: Schemat ideowy układu ADXL — pomiar przyspieszenia

3.2.4 Opis interfejsów zewn˛etrznych

Wcze´sniej opisane układy s ˛a ze sob ˛a poł ˛aczone (rysunek 12) i stanowi ˛a sterownik robota mobilnego

RoBik. Dokładny opis poł ˛acze´n znajduje si˛e w raporcie konstrukcyjnym, poni˙zsze tabele opisuj ˛a

gniazda interfejsów zewn˛etrznych:

Rysunek 26: Poł ˛aczone moduły sterownika mi˛edzy sob ˛a.

Tablica 7: Gniazdo zasilania (ZAS) na płytce MOC

MOC (nr styku) kierunek

nazwa sygnału

gn. ZAS

funkcja

→

silnik_prawy

→

−silnik_prawy

←

akumulator

−

−akumulator

→

silnik_lewy

→

−silnik_lewy

3,5

Tablica 8: Kabel ł ˛acz ˛acy płytki STER z nadajnikiem radiowym DECT

STER (nr styku) kierunek

nazwa sygnału

DECT (nr styku)

gn. DECT

funkcja

gn. NADAJ

→

B_AD8 − RSTBI

←

B_AD14 − DSRO

←

RXD1_TT L − RXDO

→

B_AD9 − RTSI

→

T XD1_TT L − TXDI

←

B_AD13 −CTSO

→

B_AD10 − DTRI

←

B_AD12 − RIIO

−

GND

→

Tablica 9: Kabel ł ˛acz ˛acy płytki STER z ADXL i koderami

STER (nr styku)

kierunek

nazwa sygnału

ADXL

Koder L Koder P

gn. ADXL_ENK

gn. ZL

−

GND

←

A_T PU0,2 −CH.A

→

←

A_T PU1 −CH.B

6,8,13,15

−

GND

1,3,8,10

←

A_T PU10,12 − X.Out

←

A_T PU11,13,B_T PU13,15 −Y.Out

10,11

→

5,6

←

AC.CH0 − X.Filtr

←

AC.CH1 −Y.Filtr

←

A_TPU4,B_TPU1 −CH.B

→

←

A_T PU3,5,B_TPU0,2 −CH.A

−

GND

2,19

Tablica 10: Gniazdo portu szeregowego (RS232) na płytce STER

STER (nr styku) kierunek nazwa sygnału

gn. RS232

funkcja

→

T xD2

←

RxD2

−

GND

1,4,6,7,8,9

Tablica 11: Gniazdo joystick (JOY) na płytce STER

STER (nr styku) kierunek

nazwa sygnału

gn. JOY

funkcja

1,4,5,8,9

−

GND

←

B_TPU10 − F1

←

AC.CH6 − X

←

AC.CH7 −Y

←

B_TPU11 − F2

←

B_TPU12 − F3

←

B_T PU9,KLAW7 − F4

11,12,13,15

Tablica 12: Gniazdo BDM na płytce STER

STER (nr styku) kierunek nazwa sygnału

gn. BDM

funkcja

→

V FLS0

→

HRESET

3,5

−

GND

←

DSCK

→

V FLS1

←

HRESIN

←

DSDI

→

5V/ + 3V3

→

DSDO

4 Oprogramowanie

Sterownik robota

RoBik jest wyposa˙zony w mikrokontroler MPC555. Oprogramowanie dla mikro-

kontrolera mo˙zna przygotowywa´c zarówno w j˛ezyku asembler, jak i w C oraz C++. Do przygotowy-

wania i uruchamiania oprogramowania niezb˛edne jest odpowiednie ´srodowisko dost˛epne na kompu-

terze roboczym (w naszym przypadku klasy PC).

Aby ułatwi´c u˙zytkownikowi implementowanie własnych algorytmów i przeprowadzanie eksperymen-

tów, przygotowano struktury danych, definicje i funkcje obsługuj ˛ace sprz˛et sterownika. Zastosowa-

nie mnemotechnicznych nazw parametrów, struktur danych, wielko´sci mierzonych i procedur obsługi

sprz˛etu zapewnia programi´scie mało zorientowanemu w sprz˛ecie sterownika wygodne posługiwanie

si˛e przygotowanym oprogramowaniem.

4.1 Oprogramowanie uruchomieniowe dla MPC555
Jako ´srodowisko uruchomieniowe dla oprogramowania sterownika robota

RoBik wybrano pakiet Co-

deWarrior for PowerPC Embedded Systems, Release 6.5 firmy Metrowerks. Wybór ten, poza wzgl˛e-

dami merytorycznymi, był podyktowany faktem posiadania w Laboratorium Robotyki ICT PWr li-

cencjonowanej instalacji tego pakietu zakupionej ze ´srodków KBN w celu realizacji projektu

Ro-

Ball

Centralnym narz˛edziem pakietu jest ´srodowisko zintegrowane (IDE - Integrated Development Envi-

ronment). Widok okna IDE przedstawiono na rys. 27. Dostarcza ono nast˛epuj ˛ace usługi:

• rozbudowane zarz ˛adzanie projektem,

• łatwy w obsłudze edytor kodu ´zródłowego,

• zintegrowany debugger,

• przegl ˛adarka ró˙znych formatów plików ´zródłowych i wynikowych,

• zaawansowane kompilatory C, C++, EC++, asembler i linker dla PowerPC.

CW IDE umo˙zliwia łatwe i elastyczne wykonywanie podstawowych prac programistycznych:

• Tworzenie i poprawianie kodów ´zródłowych programów. Edytor zapewnia kolorowe pod´swie-

tlanie składni, szybki dost˛ep do plików ´zródłowych i doł ˛aczanych, prac˛e w trybie wielu okien

itp. Przegl ˛adarka oferuje dialogowe okna pomagaj ˛ace w operowaniu procedurami, strukturami

danych, zmiennymi, klasami i plikami.

• Zarz ˛adzanie wieloma konfiguracjami docelowymi (ang. targets). Pozwala to na przechowywa-

nie w projekcie wielu ustawie´n konfiguracji plików ´zródłowych, kompilatora i linkera. Przykła-

dem mog ˛a by´c konfiguracje przeznaczone do pracy w pami˛eci RAM, ROM i do programowania

pami˛eci FLASH ze wspólnych ´zródeł.

• Tworzenie oprogramowania dla ró˙znych procesorów w tym samym ´srodowisku. Dost˛epne s ˛a

kompilatory, asemblery i linkery dla wielu ró˙znych platform (np. dla mikrokontrolerów MC9S12,

czy procesorów sygnałowych DSP563xx).

• Porównywanie plików obejmuj ˛ace podkatalogi i otwarte okna edytora.

CodeWarrior jest pakietem komercyjnym o znacznej cenie (w wersji pełnej ok. $5000). W najbli˙zszej przyszło´sci przewidziana jest migracja do

´srodowiska publicznie dost˛epnego (GNU) w systemie Linux (make, gcc, gdb, ddd z obsług ˛a BDM).

Rysunek 27: CW IDE z widocznym oknem zarz ˛adzania projektem

Narz˛edzia zawarte w pakiecie obejmuj ˛a równie˙z obsług˛e wbudowanych wersji systemu Linux (Em-

bedded Linux Application Development) z narz˛edziami GNU (kompilator, asembler, linker, archiwi-

zator) przeniesionymi do ´srodowiska Windows z wykorzystaniem biblioteki Cygwin.DLL.

Debugger symboliczny (CodeWarrior Debugger) pozwala uruchamia´c programy nie tylko na wbu-

dowanym symulatorze PowerPC, ale równie˙z na ró˙znych modułach sprz˛etowych oferowanych przez

inne firmy. W naszym przypadku takim modułem jest phyCORE-MPC555 firmy Phytec. Do komuni-

kacji z modułem docelowym wykorzystywane s ˛a ró˙zne interfejsy obsługuj ˛ace zł ˛acze BDM lub JTAG

mikrokontrolera MPC555 w systemie docelowym oraz port równoległy, szeregowy lub Ethernet w

komputerze roboczym. W naszym przypadku wykorzystano interfejs równoległy (zgodny z Wiggler

firmy Macraigor Systems). Widok okna debuggera w ´srodowisku CW IDE przedstawiono na rys. 28.

Debugger pozwala:

• przegl ˛ada´c program w postaci kodu ´zródłowego, zdezasemblowanej zawarto´sci pami˛eci lub jed-

nocze´snie w obu formach,

• uruchamia´c program krok po kroku na poziomie kodu ´zródłowego lub j˛ezyka maszynowego,
• ustawia´c pułapki zarówno dla kodu w pami˛eci RAM, jak i ROM (FLASH),
• przegl ˛ada´c zawarto´s´c zmiennych, komórek pami˛eci i rejestrów przez wskazanie ich w oknie

kodu wska´znikiem myszy,

Rysunek 28: CW IDE z widocznym oknem debuggera

• modyfikowa´c przez wybór mysz ˛a i wprowadzenie nowej warto´sci.

Wi˛ecej informacji o opisanym ´srodowisku mo˙zna znale´z´c na stronie:
<

http://www.metrowerks.com/MW/Develop/Embedded/PowerPC/Default.htm>.

4.2 Oprogramowanie sterownika
Program u˙zytkownika maj ˛acy realizowa´c algorytm sterowania lub eksperymenty pomiarowe powi-

nien by´c napisany w j˛ezyku C i zawiera´c funkcj˛e main(). Przygotowane procedury inicjalizacji

poszczególnych bloków sterownika powinny zosta´c wywołane na pocz ˛atku tej funkcji. W dalszym

ci ˛agu nale˙zy zainicjalizowa´c własne (wynikaj ˛ace z implementacji algorytmu) struktury danych i uru-

chomi´c główn ˛a p˛etl˛e programu (while(1){...}).

W celu obsłu˙zenia przerwania cyklicznego generowanego przez PIT (ang. Periodic Interrupt Timer)

nale˙zy doł ˛aczy´c procedur˛e void InterruptHandler(int cause). Jako argument cause otrzyma

ona adres wektora przerwania (w przypadku przerwania zewn˛etrznego w MPC555 – 0x500). W tej

procedurze nale˙zy umie´sci´c kod realizuj ˛acy pojedynczy krok implementowanego algorytmu. B˛edzie

on wywoływany TPSEC razy na sekund˛e.

Definicje stałych, makrodefinicje i struktury danych zwi ˛azane ze sprz˛etem robota

RoBik umiesz-

czono w pliku nagłówkowym robik_hw.h (´zródła procedur znajduj ˛a si˛e w pliku robik_hw.c).

Niektóre parametry s ˛a wydzielone na pocz ˛atku pliku, aby ułatwi´c u˙zytkownikowi konfigurowanie

oprogramowania sterownika:

#define TPSEC

1000

// requested number of ticks per second

#define PWM_FRQ

1000

// requested PWM frequency [Hz]

#define USE_MPWM

// set to 1 if MPWM in use, 0 otherwise

#define USE_TPU_B

// set to 1 if ch2 uses TPU_B, 0 otherwise

#define JOY_ZONE

// dead zone for joystick neutral position

#define KBDRATE

1000

// KBD samples per second

#define NKBD

// KBD scans per second

#define ADXL_PERIODS

// requested nbr of accumulated PPWA periods

Parametr TPSEC oznacza cz˛estotliwo´s´c przerwa´n cyklicznych. Decyduje on o szybko´sci pracy stero-

wnika (procedura u˙zytkownika b˛edzie wykonywana TPSEC razy na sekund˛e).

Wybór cz˛estotliwo´sci generowanego sygnału PWM (u˙zywanego przy napi˛eciowym sterowaniu silni-

ków) jest mo˙zliwy przez zmienianie parametru PWM_FRQ, który oznacza cz˛estotliwo´s´c tego przebiegu.

Sygnał PWM mo˙ze by´c generowany za pomoc ˛amodułu MPWM mikrokontrolera MPC555 (USE_PWM

ustawionego na 1) lub przez funkcj˛e PWM w TPU3 (USE_PWM ustawion ˛a na 0). Ustawienie tego pa-

rametru musi by´c zgodne z wybranymi poł ˛aczeniami zworek na płytce sterownika.

Parametr JOY_ZONE okre´sla szeroko´s´c martwej strefy wokół poło˙zenia neutralnego joysticka.

Skanowanie klawiatury w przerwaniu cyklicznym odbywa si˛e z cz˛estotliwo´sci ˛a zadan ˛a przez NKBD.

Parametr KBDRATE oznacza cz˛estotliwo´s´c próbkowania przycisków przez funkcj˛e DIO w TPU3 u˙zyt ˛a

do filtrowania drga´n styków (ang. debounce).

Odczyt akcelerometrów przy pomocy funkcji PPWA TPU3 mo˙ze si˛e odbywa´c z prost ˛afiltracj ˛a(przez

u´srednianie pewnej ilo´sci kolejnych okresów).

Ilo´s´c u´srednianych okresów podaje parametr

ADXL_PERIODS

W celu ułatwienia programowej obsługi portów równoległych u˙zytych do sterowania wy´swietlaczem

LCD i diodami LED zdefiniowano odpowiednie struktury danych i makrodefinicje pozwalaj ˛ace czy-

telnie odwoływa´c si˛e do bitów portu z poziomu programu napisanego w j˛ezyku C:

struct LcdPort{

VUINT16 SW:1;
VUINT16 L_R:1;
VUINT16 L_Y:1;
VUINT16 L_G:1;
VUINT16 L_LCD:1;
VUINT16 E:1;
VUINT16 RW:1;
VUINT16 RS:1;
VUINT16 BUS:8;

};

#define LCD_PORT

(*(struct LcdPort *) 0x00306100)

struct LcdPDir{

VUINT16 SW:1;
VUINT16 LEDS:4;
VUINT16 CTL:3;
VUINT16 BUS:8;

};

#define LCD_PDIR

(*(struct LcdPDir *) 0x00306102)

#define LCD_BUS_PORT

LCD_PORT.BUS

#define LCD_RW_PIN

LCD_PORT.RW

#define LCD_RS_PIN

LCD_PORT.RS

#define LCD_E_PIN

LCD_PORT.E

#define LCD_LITE

LCD_PORT.L_LCD

#define LCD_BUS_DIR

LCD_PDIR.BUS

#define LED_GREEN

LCD_PORT.L_G

#define LED_YELLOW

LCD_PORT.L_Y

#define LED_RED

LCD_PORT.L_R

#define SWITCH

LCD_PORT.SW

#define AUX_SW

LCD_PORT.SW

#define InitLeds()

{LCD_PDIR.LEDS=0xf;\
LCD_LITE=1;\
LED_GREEN=1;\
LED_YELLOW=1;\
LED_RED=1;}

Poszczególne bity portu MPIOSMDR w MPC555 s ˛a zdefiniowane jako pola bitowe w strukturze
struct LcdPort

. Odpowiednie makrodefinicje odwołuj ˛a si˛e do tych pól w obszarze przestrzeni

adresowej zdefiniowanej przez LCD_PORT, jako wyłuskanie stałego wska´znika o warto´sci równej ad-

resowi MPIOSMDR w MPC555. Analogicznie okre´slone s ˛a pola bitowe rejestru kierunku tego portu

- MPIOSMDDR (struct LcdPDir, LCD_PDIR).

Obsług˛e TPU ułatwiaj ˛a nast˛epuj ˛ace definicje stałych i makrodefinicje:

#define TRAM_BASE 0x302000

#define XTAL 20000000
#define TCR1CK ((XTAL)/4)

// TPU time base TCR1

#define PWM_PER ((TCR1CK+(PWM_FRQ>>1))/PWM_FRQ)

/* TPU_A channels usage */

#define PWMCHAN0

// TPU channel for PWM ch0 (MPWM0 instead)

#define QDECCHAN0

// TPU channel for QDECA ch0

#define QDVELCHAN0 2

// TPU channel for QDVEL ch0

#define PWMCHAN1

// TPU channel for PWM ch1 (MPWM1 instead)

#define QDECCHAN1

// TPU channel for QDECA ch1

#define QDVELCHAN1 5

// TPU channel for QDVEL ch1

#define BEEPCHAN

// TPU channel for beeper (PWM)

#define PPWACHX

// TPU channel for PPWA ACCX

#define PPWACHY

// TPU channel for PPWA ACCY

#define PERCHANX

// TPU channel for PPWA X period

#define PERCHANY

// TPU channel for PPWA Y period

/* TPU_B channels usage */

#define DIOKLAW1

// TPU channel for DIO (KLAW1)

#define DIOKLAW2

// TPU channel for DIO (KLAW2)

#define DIOKLAW3

// TPU channel for DIO (KLAW3)

#define DIOKLAW4

// TPU channel for DIO (KLAW4)

#define DIOKLAW5

// TPU channel for DIO (KLAW5)

#define DIOKLAW6

// TPU channel for DIO (KLAW6)

#define DIOKLAW7

// TPU channel for DIO (KLAW7)

#define DIOJOY2

// TPU channel for DIO (JOY2)

#define DIOJOY7

// TPU channel for DIO (JOY7)

#define DIOJOY10

// TPU channel for DIO (JOY10)

#define DIOJOY14

// TPU channel for DIO (JOY14)

#define PPWACHYB

// TPU channel for PPWA ACCY

#define PERCHANYB

// TPU channel for PPWA Y period

Parametr XTAL oznacza cz˛estotliwo´s´c rezonatora kwarcowego na module phyCORE-MPC555 i nie

powinien by´c zmieniany przez u˙zytkownika. Na jego podstawie okre´slono TCR1CK - cz˛estotliwo´s´c

podstawy czasu dla TPU3 oraz wyliczono (korzystaj ˛ac z definiowanej przez u˙zytkownika cz˛estotli-

wo´sci PWM - PWM_FRQ) parametr PWM_PER potrzebny do inicjalizacji i obsługi kanałów PWM.

Pozostałe definicje odpowiadaj ˛asprz˛etowej konfiguracji sterownika (wykorzystaniu kanałów TPU_A

i TPU_B do obsługi ró˙znych sygnałów): PWMCHANx, QDECCHANx, QDVELCHANx - kanały do obsługi na-

p˛edów, BEEPCHAN - obsługa sygnału d´zwi˛ekowego, PPWACHx, PERCHANx - odczyt wypełnienia i okresu

przebiegu wyj´sciowego akcelerometru dwuosiowego, DIOKLAWx, DIOJOYx - odczyt przycisków kla-

wiatury i joysticka.

Przetworniki analogowo-cyfrowe i cyfrowo-analogowe s ˛a przył ˛aczone do magistrali QSPI. Poni˙zsze

makra definiuj ˛a maski sygnałów PCS do wybierania poszczególnych urz ˛adze´n na tej magistrali:

#define DESELECT

0xF

// no SLAVEs selected

#define DACSEL

0xE

// DAC MAX5742 (MOC) is selected by LOW on PCS0

DAC MAX5742 outputs:

A (0)

current value to be set in ch0 (bipolar)

B (1)

current value to be set in ch1 (bipolar)

C (2)

LCD contrast V_LCD

(unipolar)

D (3)

unused

#define ADC1SEL

0xD

// ADC1 MAX188 (MOC) is selected by LOW on PCS1

ADC1 MAX188 inputs:

current measured in ch0 V1ADC

(bipolar)

current measured in ch1 V2ADC

(bipolar)

current set in ch0 V1DAC

(bipolar)

current set in ch1 V2DAC

(bipolar)

temperature 1 sense TMP1

(unipolar)

temperature 2 sense TMP2

(unipolar)

ACCU test (36V)

(unipolar)

-5V sense

(bipolar)

#define ADC2SEL

0xB

// ADC2 MAX186 (STER) is selected by LOW on PCS2

ADC2 MAX186 inputs:

ADXL X.FILTR

(unipolar)

ADXL Y.FILTR

(unipolar)

ENC03J OUT

(unipolar)

unused

JOYX

(unipolar)

JOYY

(unipolar)

DACSEL

słu˙zy do wybrania przetwornika cyfrowo-analogowego, ADC1SEL - przetwornika analogowo-

cyfrowego na płytce MOC, a ADC2SEL - przetwornika analogowo-cyfrowego na płytce STER. W ko-

mentarzach podano przypisanie kanałów przetworników do sygnałów sterownika.

Odczytywanie wielko´sci mierzonych z poziomu programu napisanego w j˛ezyku C ułatwiaj ˛a nast˛epu-

j ˛ace makrodefinicje:

#define Pos0

(tpua->PARM.R[QDECCHAN0][TPU_QDEC_POSITION_COUNT])

#if USE_TPU_B
#define Pos1

(tpub->PARM.R[QDECCHAN0][TPU_QDEC_POSITION_COUNT])

#else
#define Pos1

(tpua->PARM.R[QDECCHAN1][TPU_QDEC_POSITION_COUNT])

#endif // if USE_TPU_B

#define Vel0

(tpua->PARM.R[QDVELCHAN0][TPU_QDVEL_DIFTIME])

#if USE_TPU_B
#define Vel1

(tpub->PARM.R[QDVELCHAN0][TPU_QDVEL_DIFTIME])

#else
#define Vel1

(tpua->PARM.R[QDVELCHAN1][TPU_QDVEL_DIFTIME])

#endif // if USE_TPU_B

#define Iact0

(((INT16)((QSMCM.RECRAM[0].R)<<2))>>4)

#define Iact1

(((INT16)((QSMCM.RECRAM[1].R)<<2))>>4)

#define Iset0

(((INT16)((QSMCM.RECRAM[2].R)<<2))>>4)

#define Iset1

(((INT16)((QSMCM.RECRAM[3].R)<<2))>>4)

#define Temp1

(((UINT16)(QSMCM.RECRAM[4].R))>>2)

#define Temp2

(((UINT16)(QSMCM.RECRAM[5].R))>>2)

#define AccuV

(((UINT16)(QSMCM.RECRAM[6].R))>>2)

#define M_5V

(((INT16)((QSMCM.RECRAM[7].R)<<2))>>4)

#define AdxlX

(((UINT16)(QSMCM.RECRAM[8].R))>>2)

#define AdxlY

(((UINT16)(QSMCM.RECRAM[9].R))>>2)

#define AdxlXH (tpua->PARM.R[PPWACHX][TPU_PPWA_LW])
#define AdxlXP (tpua->PARM.R[PERCHANX][TPU_PPWA_LW])
#if USE_TPU_B
#define AdxlYH (tpub->PARM.R[PPWACHYB][TPU_PPWA_LW])
#define AdxlYP (tpub->PARM.R[PERCHANYB][TPU_PPWA_LW])
#else
#define AdxlYH (tpua->PARM.R[PPWACHY][TPU_PPWA_LW])
#define AdxlYP (tpua->PARM.R[PERCHANY][TPU_PPWA_LW])
#endif // if USE_TPU_B

#define Gyro

(((UINT16)(QSMCM.RECRAM[10].R))>>2)

#define JoyX

(((UINT16)(QSMCM.RECRAM[11].R))>>2)

#define JoyY

(((UINT16)(QSMCM.RECRAM[12].R))>>2)

Niektóre z nich s ˛a okre´slone warunkowo z automatycznym uwzgl˛ednieniem konfiguracji zworek na

płytce sterownika dzi˛eki uwzgl˛ednieniu parametru USE_TPU_B opisanego wcze´sniej.

Odczyt obejmuje pomiary: parametrów ruchu kół (Posx - poło˙zenie, Velx - pr˛edko´s´c), wielko´sci

pr ˛adów (Isetx - zadane, Iactx - zmierzone), temperatur (Tempx), napi˛e´c (AccuV - akumulator, M_5V

- ujemne napi˛ecie zasilaj ˛ace -5V), przyspiesze´n liniowych (Adxlx - odczyt analogowy, AdxlxH - sze-

roko´s´c impulsu, AdxlxP - okres przebiegu), pr˛edko´sci k ˛atowej wahadła (Gyro) oraz poło˙ze´n joysticka

(Joyx).

Podobnie ułatwione jest zadawanie sygnałów steruj ˛acych:

#define SetI0(x) QSMCM.TRANRAM[13].R = (0<<12) | (((x) + 0x826) & 0x0fff)
#define SetI1(x) QSMCM.TRANRAM[14].R = (1<<12) | (((x) + 0x814) & 0x0fff)

#if USE_MPWM
#define SetU0(x) MIOS1.MPWMSM0PULR.R = (x)
#define SetU1(x) MIOS1.MPWMSM1PULR.R = (x)
#else // if USE_MPWM
#define SetU0(x) tpu_pwm_update(tpua, PWMCHAN0, PWM_PER, (UINT16)(x), 0)
#if USE_TPU_B
#define SetU1(x) tpu_pwm_update(tpub, PWMCHAN0, PWM_PER, (UINT16)(x), 0)
#else
#define SetU1(x) tpu_pwm_update(tpua, PWMCHAN1, PWM_PER, (UINT16)(x), 0)
#endif // if USE_TPU_B
#endif // if USE_MPWM
Odpowiednie makrodefinicje obejmuj ˛a podstawienia i wywołania funkcji pomocniczych ustawiaj ˛a-

cych odpowiednie wielko´sci w TPU, MPWM i przetworniku cyfrowo-analogowym. Ustawianie pr ˛a-

dów zadanych (SetIx) odbywa si˛e przez wpisanie warto´sci zadanej do kolejki nadawczej QSPI. Usta-

wianie napi˛e´c (SetUx) odbywa si˛e (w zale˙zno´sci od parametru USE_MPWM) przez podstawienie warto-

´sci do rejestru MPWMSMxPULR modułu MPWM lub przez wywołanie funkcji

tpu_pwm_update

ustawiaj ˛acej czas stanu wysokiego PWM w odpowiednim kanale TPU. Wybór TPU

jest mo˙zliwy dzi˛eki ustawieniu parametru USE_TPU_B.

Poni˙zsze funkcje słu˙z ˛a do obsługi wy´swietlacza LCD:

void LcdInit(void);
void WriteLcdIr(unsigned char);
void WriteLcdDr(unsigned char);
unsigned char ReadLcdIr(void);
unsigned char ReadLcdDr(void);
void PutLcdCtrl(unsigned char);
void PutLcdData(unsigned char);
Funkcje ni˙zszego poziomu obsługuj ˛a port LCD_PORT tak, by zapewni´c odpowiednie sekwencje

sygnałów steruj ˛acych dla sterownika LCD. Zapis bajtu x do rejestru steruj ˛acego zapewnia funkcja
WriteLcdIr(x)

, a do rejestru danych - WriteLcdDr(x). Do odczytywania bajtu z rejestru steruj ˛a-

cego słu˙zy funkcja ReadLcdIr(), a z rejestru danych - ReadLcdDr().

Funkcje wy˙zszego poziomu korzystaj ˛a z wcze´sniej opisanych. Inicjalizacja (LcdInit) pozwala usta-

wi´c stan pocz ˛atkowy wbudowanego sterownika LCD. Wysyłanie kodu steruj ˛acego (komendy) do

sterownika LCD obsługuje PutLcdCtrl(c). Funkcja ta oczekuje na gotowo´s´c sterownika przed wpi-

saniem komendy. W podobny sposób wysyłanie znaku c na LCD zapewnia PutLcdData(c).

Inicjalizacja sprz˛etu sterownika polega na wywołaniu kilku procedur odpowiedzialnych za ustawienie

parametrów pracy poszczególnych bloków. Parametry i prototypy tych procedur podano poni˙zej.

#define PIT_TB (XTAL/256)

// PIT time base frequency

/* calculated PIT time constant */
#define PIT_CONST

((PIT_TB+(TPSEC>>1))/TPSEC)

EXTERN INT32 pulse; // free running tick counter

void PitInit(void);

PIT_TB

wyznacza cz˛estotliwo´s´c podstawy czasu dla bloku PIT (ang. Periodic Interrupt Timer). Pa-

rametr dla dzielnika cz˛estotliwo´sci PIT (PIT_CONST) jest wyliczany na podstawie wybranej przez

u˙zytkownika cz˛estotliwo´sci przerwa´n cyklicznych (parametru TPSEC).

Zmienna globalna pulse jest licznikiem przerwa´n cyklicznych wyznaczaj ˛acych podstaw˛e czasu dla

sterownika.

Procedura PitInit() uruchamia przerwania cykliczne z cz˛estotliwo´sci ˛a zadan ˛a przez TPSEC.

extern const unsigned char tpumska[];

// TPU mask with QDVEL (MW’04)

EXTERN struct TPU3_tag *tpua;

// pointer for TPU routines

EXTERN struct TPU3_tag *tpub;

// pointer for TPU routines

void TpuInit(void);

W celu pomiaru pr˛edko´sci k ˛atowej kół wykorzystano funkcj˛e QDVEL, której nie ma w standardowej

masce TPU3. Obraz binarny maski mikrokodu z dodan ˛afunkcj ˛aQDVEL (tpumska[]) jest doł ˛aczany

przez linker. Wygodny dost˛ep do rejestrów TPU_A i TPU_B jest mo˙zliwy dzi˛eki funkcjom i struk-

turom zdefiniowanym w pliku tpu.h. Zmienne globalne tpua i tpub s ˛a wska´znikami na struktury

opisuj ˛ace rejestry TPU3 inicjowanymi w procedurze TpuInit(). W tej˙ze procedurze s ˛a inicjali-

zowane wszystkie kanały TPU_A i TPU_B u˙zywane w ustalonej konfiguracji (według parametru
USE_TPU_B

Obsług˛e sygnału d´zwi˛ekowego umo˙zliwiaj ˛a funkcje:

void VoiceOn(void);
void VoiceOff(void);
void SetBeeperPitch(double);

VoiceOn()

wł ˛acza sygnał przez ustawienie PWM_HI równego połowie PWM_PER dla kanału

BEEPCHAN

, w którym jest uruchomiona funkcja PWM. VoiceOff() wył ˛acza sygnał d´zwi˛ekowy przez

wyzerowanie PWM_HI. setBeeperPitch(x) ustawia cz˛estotliwo´s´c sygnału d´zwi˛ekowego na x.

Klawiatura i przyciski joysticka s ˛a obsługiwane przez TPU_B przy pomocy funkcji DIO (ang. Di-

screte Input-Output). Proponowane wykorzstanie przycisków klawiatury podano poni˙zej:

KBD
tpub:

key:

K_1

K_3

K_2

K_4

K_5

K_6

K_7

fun:

aux

esc

ent

front panel mapping:

esc

ent

aux

*/
#define AUXCHAN

// TPU_B ch for aux

#define LARCHAN

// TPU_B ch for left

#define ESCCHAN

// TPU_B ch for esc

#define DARCHAN

// TPU_B ch for down

#define UARCHAN

// TPU_B ch for up

#define RARCHAN

// TPU_B ch for right

#define ENTCHAN

// TPU_B ch for enter

#define JSW2CH

// TPU_B ch for DIO (JOY2)

#define JSW7CH

// TPU_B ch for DIO (JOY7)

#define JSW10CH

// TPU_B ch for DIO (JOY10)

#define JSW14CH

// TPU_B ch for DIO (JOY14)

#define KPERIOD ((TCR1CK + (KBDRATE >> 1))/KBDRATE) // DIO sampling rate const
#define TKBD

((TPSEC + (NKBD >> 1))´NKBD)

// KBD scan constant

#define PIN_HISTORY(x) (tpub->PARM.R[x][TPU_DIO_PIN_LEVEL])

void KbdInit (void);
void ScanKbd(void);
Zdefiniowano numery kanałów TPU odpowiadaj ˛acych poszczególnym przyciskom, okres skanowania

klawiatury w przerwaniu cyklicznym - TKBD (w oparciu o zadan ˛a cz˛estotliwo´s´c skanowania - NKBD)

oraz okres próbkowania przycisków w funkcji DIO - KPERIOD (na podstawie zadanej cz˛estotliwo´sci
KBDRATE

Procedura ScanKbd() jest wywoływana w przerwaniu cyklicznym i słu˙zy do obsługi znaczników

(flag) klawiatury zdefiniowanych w postaci pól bitowych w strukturach typu T_KBD_FLAGS:
typedef union {

UINT16 R;
struct {

UINT16 :2;
UINT16 JSW10:1;
UINT16 JSW7:1;
UINT16 JSW2:1;
UINT16 ENT:1;

UINT16 RIGHT:1;
UINT16 UP:1;
UINT16 DOWN:1;
UINT16 ESC:1;
UINT16 LEFT:1;
UINT16 AUX:1;
UINT16 :2;
} B;

}T_KBD_FLAGS;

Extern T_KBD_FLAGS KbdFlag;
EXTERN T_KBD_FLAGS KbdEdge;
EXTERN T_KBD_FLAGS KbdAux;

// static flags
#define KallFlag

KbdFlag.R

#define KauxFlag

KbdFlag.B.AUX

#define KescFlag

KbdFlag.B.ESC

#define KleftFlag

KbdFlag.B.LEFT

#define KdownFlag

KbdFlag.B.DOWN

#define KupFlag

KbdFlag.B.UP

#define KrightFlag KbdFlag.B.RIGHT
#define KentFlag

KbdFlag.B.ENT

#define Jsw2Flag

KbdFlag.B.JSW2

#define Jsw7Flag

KbdFlag.B.JSW7

#define Jsw10Flag

KbdFlag.B.JSW10

// dynamic (edge) flags
#define KallEdge

KbdEdge.R

#define KauxEdge

KbdEdge.B.AUX

#define KescEdge

KbdEdge.B.ESC

#define KleftEdge

KbdEdge.B.LEFT

#define KdownEdge

KbdEdge.B.DOWN

#define KupEdge

KbdEdge.B.UP

#define KrightEdge KbdEdge.B.RIGHT
#define KentEdge

KbdEdge.B.ENT

#define Jsw2Edge

KbdEdge.B.JSW2

#define Jsw7Edge

KbdEdge.B.JSW7

#define Jsw10Edge

KbdEdge.B.JSW10

// auxiliary flags
#define KallAux

KbdAux.R

#define KauxAux

KbdAux.B.AUX

#define KescAux

KbdAux.B.ESC

#define KleftAux

KbdAux.B.LEFT

#define KdownAux

KbdAux.B.DOWN

#define KupAux

KbdAux.B.UP

#define KrightAux

KbdAux.B.RIGHT

#define KentAux

KbdAux.B.ENT

#define Jsw2Aux

KbdAux.B.JSW2

#define Jsw7Aux

KbdAux.B.JSW7

#define Jsw10Aux

KbdAux.B.JSW10

#define Jsw14Aux

KbdAux.B.ENT

Zadeklarowano struktury i makrodefinicje dla flag statycznych (oznaczaj ˛acych, ˙ze przycisk jest aktu-

alnie wł ˛aczony - KbdFlag), dynamicznych (oznaczaj ˛acych, ˙ze wyst ˛apiło zbocze - KbdEdge) i pomoc-

niczych (zwi ˛azanych z d´zwi˛ekow ˛a sygnalizacj ˛a wł ˛aczenia przycisku - KbdAux).

Obsługa transmisji radiowej przez moduł DECT wykorzystuje sygnały steruj ˛ace poł ˛aczone z portem

modułu QADC_B MPC555:

// handshake signals
#define RSTBI

QADC_B.PORTQA.B.PQA0

#define RTSI

QADC_B.PORTQA.B.PQA1

#define DTRI

QADC_B.PORTQA.B.PQA2

#define LED_MOC QADC_B.PORTQA.B.PQA3
#define RIIO

QADC_B.PORTQA.B.PQA4

#define CTSO

QADC_B.PORTQA.B.PQA5

#define DSRO

QADC_B.PORTQA.B.PQA6

void DectPortInit(void);
void DectDataMode(void);
void DectConfigMode(void);
int DectOutgoingCall(void);

Procedura DectPortInit() ustawia odpowiedni tryb pracy i kierunki linii portu, DectDataMode()

wprowadza moduł DECT (HW-86010) w tryb transmisji danych, DectConfigMode() wł ˛acza tryb

konfiguracji. Przy pomocy funkcji DectOutgoingCall() mo˙zna nawi ˛aza´c poł ˛aczenie z odpowied-

nim stacjonarnym modułem DECT. Funkcja ta zwraca warto´s´c 0 w przypadku nawi ˛azania poł ˛aczenia

lub -1 w przypadku przekroczenia czasu oczekiwania (ang. time-out).

4.3 Przykład implementacji programu u˙zytkownika
Poni˙zej został zamieszczony wydruk pliku main.c zawieraj ˛acego przykładow ˛a implementacj˛e algo-

rytmu r˛ecznego sterowania robotem

RoBik przy pomocy joysticka.

/***********************************************************************
*
* File:

main.c

*
* Description: RoBik controller test
*

with Phytec phyCORE-MPC555 module

*
***********************************************************************/

#include <stdio.h>
#include <time.h>
#include <math.h>

#include "robik_hw.h"

/***********************************************************************
*
* Name:

Wait

*
* Prototype:
*

void Wait(unsigned long)

*
* Arguments:

unsigned long count - number of loops

*
* Result:

none

*
* Function:

spend some time doing nothing

*
***********************************************************************/

void Wait(unsigned long count)
{

volatile unsigned long n;
for(n = 0; n < count; n++) {}

}

struct tm syst;
int tick, tkbd;

INT32 position0, lastposition0;
INT32 fpos0;
INT16 period0;
INT32 speed0;

INT32 position1, lastposition1;
INT32 fpos1;
INT16 period1;
INT32 speed1;

int joyx, joyy, joyx0, joyy0;

INT32 alpha;

/***********************************************************************
*
* Name:

InterruptHandler

*
* Prototype:
*

void InterruptHandler(long cause)

*
* Arguments:

long cause - the cause of interrupt

*
* Result:

none

*
* Function:

periodic interrupt service for RoBik functionality

*
***********************************************************************/

void InterruptHandler(long cause)
{

static int i=0;

if(cause == 0x500){

// external interrupt

USIU.PISCR.B.PS = 1;

// negate PIT flag by writing "1" to it

pulse++;

position0 = (INT32)Pos0; // read encoder position
fpos0 &= 0xffff0000;
fpos0 += position0;
if((lastposition0-position0) > 32767)
{

fpos0 += 0x10000;

// carry

}
if((position0-lastposition0) > 32767)
{

fpos0 -= 0x10000;

// borrow

}
lastposition0 = position0;
period0

= (INT16)Vel0;

// read encoder period

speed0 = (period0==(INT16)QDVEL_ZERO_VAL)?0:TCR1CK/period0;

position1 = (INT32)Pos1; // read encoder position
fpos1 &= 0xffff0000;
fpos1 += position1;
if((lastposition1-position1) > 32767)
{

fpos1 += 0x10000;

// carry

}

if((position1-lastposition1) > 32767)
{

fpos1 -= 0x10000;

// borrow

}
lastposition1 = position1;
period1

= (INT16)Vel1;

// read encoder period

speed1 = (period1==(INT16)QDVEL_ZERO_VAL)?0:TCR1CK/period1;

if(++tkbd >= TKBD)
{

ScanKbd();

}

/***********************************************************************
*
* Name:

main

*
* Prototype:
*

void main(void)

*
* Arguments:

none

*
* Result:

none

*
* Function:

RoBik controller test main function

*
***********************************************************************/

void main()
{

int inram = 1;
int print = 0;
int speedl, speedr, signl, signr;

char buf[128];
char *ptr;
char c=’q’;

position0 = lastposition0 = fpos0 = 0;

// initialize axis0 counters

position1 = lastposition1 = fpos1 = 0;

// initialize axis1 counters

tkbd = tick = 0;

// initialize tick counters

KbdInit();

// initialize KBD flags

/* serial init */

InitPort1();
InitializeUART(kBaud9600);

if((unsigned long) &main < 0x00C00000)

// ram code is linked to address 0x00C02000

{

inram = 0;

}

alpha = 0;

initialize alpha integrator

if(!inram) PitInit();

periodic interrupt in ROM version only

SpiInit();

set up QSPI (ADC/DAC) periodic transfers

LcdInit();

set up LCD interface

TpuInit();

set up TPU/MPWM functions

pulse = 0;

sprintf(buf, "RoBik Test MW’04

");

PutLcdCtrl(0x80);

// upper line

for(ptr=buf;*ptr;ptr++) PutLcdData((unsigned char)*ptr);
if(inram)
{

sprintf(buf, " no interrupts

");

PutLcdCtrl(0xc0);

// lower line

for(ptr=buf;*ptr;ptr++) PutLcdData((unsigned char)*ptr);

}
Wait(200000);
QSMCM.TRANRAM[15].R = 0x2000;

// 0V on LCD conrast

/* DECT port init */

DectPortInit();

/* DECT reset in data mode */

DectDataMode();

Wait(200000);

/* DECT PT outgoing call */

sprintf(buf, "Call in progress

");

PutLcdCtrl(0x80);

// upper line

for(ptr=buf;*ptr;ptr++) PutLcdData((unsigned char)*ptr);

if(DectOutgoingCall() == 0)
{

sprintf(buf, "Connection ready

");

}
else
{

sprintf(buf, "Time-out

");

}
PutLcdCtrl(0x01);

// clear screen, home

for(ptr=buf;*ptr;ptr++) PutLcdData((unsigned char)*ptr);

/* program the LEDs at RoBik front panel */

InitLeds();

// all LEDS ON

Wait(100000);
LCD_LITE=0;

// all LEDS OFF

LED_YELLOW=0;
LED_RED=0;
LED_GREEN=0;
Wait(500000);
LED_RED=1;

// only RED ON

Wait(500000);
LED_YELLOW=1;

// RED and YELLOW ON

Wait(500000);
LED_YELLOW=0;
LED_RED=0;
LED_GREEN=1;

// only GREEN ON

joyx0 = JoyX;

// x joystick neutral position

joyy0 = JoyY;

// y joystick neutral position

SetI0(0);
SetI1(0);

SetU0(0);
SetU1(0);

while(1)

// main loop

{

printf("%8d %8d %8d %6d %6d %6d\r\n",

pulse,
fpos0,
fpos1,
speed0,
speed1,
Gyro

);

/* manual (joystick) control */

joyx = JoyX - joyx0;

// x joystick (L <-> R)

if(joyx < JOY_ZONE && joyx > -JOY_ZONE) joyx = 0;

// dead zone

if(joyx >0) joyx -= JOY_ZONE;
if(joyx <0) joyx += JOY_ZONE;

joyy = JoyY - joyy0;

// y joystick (F <-> B)

if(joyy < JOY_ZONE && joyy > -JOY_ZONE) joyy = 0;

// dead zone

if(joyy >0) joyy -= JOY_ZONE;
if(joyy <0) joyy += JOY_ZONE;

speedl = (joyy + joyx);
if(speedl <0) {signl = 1; speedl = -speedl;}
else {signl = 0;}
if (!signl) SetI0(-1000); else SetI0(1000);

// set direction

if (speedl>PWM_PER) speedl=PWM_PER;

// limit

SetU0(speedl);

speedr = (joyy - joyx);
if(speedr <0) {signr = 1; speedr = -speedr;}
else {signr = 0;}
if (!signr) SetI1(-1000); else SetI1(1000);

// set direction

if (speedr>PWM_PER) speedr=PWM_PER;

// limit

SetU1(speedr);

}

/* end of file main.c */

5 Przebieg i wyniki eksperymentów

W celu przeprowadzenia eksperymentów maj ˛acych za zadanie zbadanie konstrukcji sterownika i okre-

´slenie jego parametrów, jak równie˙z dokonanie pomiarów parametrów fizycznych robota (patrz 2.2),

RoBiku zostały zaimplementowane odpowiednie programy testowe. W ka˙zdym z nich procedura

wywoływana cyklicznie co 1ms pozwala zadawa´c momenty nap˛edowe kół oraz zebra´c próbki pomia-

rowe. Pomiary mog ˛a by´c zbierane w dwojaki sposób:

• gromadzone w tablicy pomiarowej umieszczonej w przestrzeni RAM sterownika (taka technika

stosowana była, gdy próbki pomiarowe były ´sci´sle powi ˛azane z czasem eksperymentu, np. przy

wyznaczaniu parametrów dynamicznych), a po zako´nczonym eksperymencie przesyłane drog ˛a

radiow ˛a do jednostki nadrz˛ednej,

• przesyłane na bie˙z ˛aco podczas eksperymentu do jednostki nadrz˛ednej za pomoc ˛a ł ˛acza radio-

wego (ta technika nie zapewnia równomiernego próbkowania).

W jednostce nadrz˛ednej, któr ˛a stanowi komputer PC próbki pomiarowe dostarczone ze sterownika

RoBika w dogodnym formacie

do obróbki w programach, takich jak gnuplot, gnumeric, matlab.

Eksperymenty dziel ˛a si˛e na dwie grupy:

• skaluj ˛ace czujniki robota (kodery, ˙zyroskop, akcelerometr, zale˙zno´sci pr ˛adowe),

• okre´slaj ˛ace parametry modelu robota.

Przed przyst ˛apieniem do eksperymentów nale˙zy zna´c podstawowe parametry

RoBika, takie jak masy

i długo´sci. Wyniki wykonanych pomiarów zamieszczono w tablicy 5.

Tablica 13: Podstawowe parametry

RoBika — masy, długo´sci

Wielko´s´c fizyczna

Symbol

Warto´s´c

Jednostka

Masa jednego koła

1,9

[kg]

Masa korpusu

3,27+8,06

[kg]

Masa obci ˛a˙znika

3,94

[kg]

Promie´n koła

0,25

[m]

Rozstaw kół

0,58

[m]

Długo´s´c wahadła

0,176

[m]

Warto zwróci´c uwag˛e na zakłócenia towarzysz ˛ace pomiarom. Aby zminimalizowa´c szum zakłócaj ˛acy

dodany do pomiaru, nale˙zy przeprowadzi´c kilka serii pomiarowych. Ka˙zda seria musi by´c bardzo

bogata w próbki. W wyniku usuni˛ecia próbek obarczonych grubymi bł˛edami oraz u´srednienia serii

pomiarowych otrzymujemy najbardziej wiarygodne wyniki. Dobre wyniki daje zastosowanie filtracji

hybrydowej, polegaj ˛acej na odrzuceniu pewnej liczby (ok. 20%) próbek o warto´sciach najwi˛ekszych

i takiej samej liczby próbek o warto´sciach najmniejszych, oraz u´srednienia pozostałych.

5.1 Skalowanie czujników
Eksperymenty maj ˛ana celu wyznaczenie stałych współczynników okre´slaj ˛acych zale˙zno´s´c pomi˛edzy

wielko´sci ˛a fizyczn ˛a, a warto´sci ˛a sygnału podanego z czujnika.

algorytm w sterowniku dostosowuje format próbek dogodnie do dalszych obróbek numerycznych, jakie b˛ed ˛aprzeprowadzane w jednostce nadrz˛ed-

nej

5.1.1 Skalowanie koderów

Kodery HEDS55 firmy Hewlett Packard umieszczone na wirnikach silników maj ˛a rozdzielczo´s´c po-

miarow ˛a 500 impulsów na obrót oraz dwa kanały pomiarowe. Sterownik wykrywa ka˙zde zbocze

z obu kanałów, co daje 2000 impulsów na pełny obrót silnika. Mi˛edzy kołem, a silnikiem jest prze-

kładnia planetarna o przeło˙zeniu 1:66 co daje 132000 impulsów kodera na jeden pełny obrót koła.

Ostatecznie, obrotowi kół o 1

odpowiada 366,(6) impulsów (1rad – 21008,47 impulsów).

5.1.2 Skalowanie ˙zyroskopu

Pomiar k ˛ata wychylenia koła wzgl˛edem korpusu odbywa si˛e przy pomocy koderów umieszczonych

w silnikach nap˛edowych. W algorytmach sterowania istotnym jest k ˛at wychylenia korpusu robota

wzgl˛edem pionowej osi globalnego układu odniesienia. Pomiar k ˛ata obrotu koła w normalnym ukła-

dzie pracy nie niesie informacji o wychyleniu korpusu, dlatego w układzie sterownika znajduje si˛e

˙zyroskop, który mierzy pr˛edko´s´c k ˛atow ˛a wahania korpusu. Po scałkowaniu pr˛edko´sci otrzymamy

k ˛at wychylenia korpusu. By móc poprawnie korzysta´c z ˙zyroskopu, nale˙zy go najpierw wyskalowa´c,

czyli znale´z´c zale˙zno´s´c pomi˛edzy k ˛atem wychylenia, a sygnałem z ˙zyroskopu. Eksperyment polega

na unieruchomieniu obu kół robota w stosunku do podło˙za i wprawieniu korpusu w ruch wahadłowy

przy równoczesnym rejestrowaniu pr˛edko´sci k ˛atowej ( ˙

ϕ) i odczytu ˙zyroskopu ( ˙α).

-2000

-1500

-1000

-500

500

1000

1500

2000

-40000

-30000

-20000

-10000

10000

20000

30000

40000

Predkosc katowa alpha

Predkosc katowa phi

dot_alpha(dot_phi)

Y=0.0468*X

Rysunek 29: Wykres zale˙zno´sci ˙

α = f ( ˙ϕ).

Na wykresie (rysunek 29) wyra´znie wida´c liniowy charakter zale˙zno´sci pr˛edko´sci k ˛atowej ˙

α od pr˛ed-

ko´sci k ˛atowej ˙

ϕ. Aproksymuj ˛ac prost ˛a otrzymano równanie postaci: ˙α = 0.0468 · ˙ϕ.

Pr˛edko´s´c ˙

ϕ odczytywana z funkcji QDVEL TPU jest mierzona w jednostkach [impuls/s]. Korzy-

staj ˛ac ze znanego skalowania koderów mamy: pr˛edko´sci obrotowej koła 1[

/s] odpowiada odczyt

366,(6)[impulsów/s]. Wobec tego pr˛edko´s´c k ˛atowa wahadła w zewn˛etrznym układzie odniesienia

/s] odpowiada odczytowi ˙zyroskopu 17,16 jednostek.

Na wykresie (rysunek 30) pokazano przebiegi czasowe ˙

α i ˙ϕ podczas wykonywania eksperymentu.

Wida´c, ˙ze przebiegi s ˛a niemal idealnie zgodne. Widoczne lokalne bł˛edy mog ˛a by´c spowodowane

-40000

-30000

-20000

-10000

10000

20000

30000

40000

50000

55000

60000

65000

70000

75000

80000

Predkosc katowa

Czas [ms]

Predkosc katowa phi

Predkosc katowa alpha

Rysunek 30: Wykres pr˛edko´sci k ˛atowych ˙

α i ˙ϕ w funkcji czasu.

zbyt słabym przytwierdzeniem kół do podło˙za podczas eksperymentu. Silne wahania korpusu powo-

dowały ugi˛ecia opon, a co za tym idzie, niewielkie obroty kół.

5.1.3 Statyczne zale˙zno´sci pr ˛adowe

Układ sterownika zadaje warto´sci pr ˛adu I

cur

w zakresie od -2048 do 2047 jednostek. Na wyj´sciu

przetwornika CA pojawia si˛e napi˛ecie o zakresie od -2,5V do 2,5V, które jest podawane do regulatora

pr ˛adu i równocze´snie do przetwornika AC, który pracuje w zakresie od -2,048V do 2,047V. Dlatego

sygnał bezwzgl˛edny podany ze sterownika wi˛ekszy ni˙z 1677 b˛edzie niepoprawnie rejestrowany. Jest

to tylko lokalne sprz˛e˙zenie zwrotne sprawdzaj ˛ace poprawno´s´c układu elektronicznego. Wielko´s´c

czytana z niego ma nazw˛e I

set

. W układzie jest jeszcze mierzony rzeczywisty pr ˛ad silników I

act

Eksperyment ma ukaza´c zale˙zno´sci pomi˛edzy I

cur

, I

set

, a I

act

-2000

-1000

1000

2000

-2000

-1500

-1000

-500

500

1000

1500

2000

2500

Zmierzony prad zadany

Prad zadany

Iset(Icur)

Y=1,22*X

-2000

-1000

1000

2000

-2000

-1500

-1000

-500

500

1000

1500

2000

2500

Zmierzony prad zadany

Prad zadany

Iset(Icur)

Y=1,22*X

Rysunek 31: Zale˙zno´s´c pr ˛adu I

set

w funkcji pr ˛adu I

cur

Na wykresie (rysunek 31) wida´c liniow ˛a zale˙zno´s´c pomi˛edzy sygnałami I

set

, a I

cur

. Liniowy współ-

czynnik wynosi 2500/2048 = 1,22. Mo˙zna przyj ˛a´c, ˙ze przetworniki CA i AC działaj ˛a poprawnie

i wiarygodnie.

W układzie najwa˙zniejszy jest rzeczywisty pr ˛ad silnika, który poci ˛aga za sob ˛a odpowiedni moment

nap˛edowy. Wyznaczone charakterystyki pr ˛adu silnika I

act

w funkcji sygnału wymuszaj ˛acego I

set

stanowi ˛a wa˙zny element bada´n. Wykres 33 przedstawia zale˙zno´sci I

act

f (I

set

)

, a dokładniej –

charakter układu regulatora pr ˛adu. W przypadku, gdy koła swobodnie si˛e kr˛eciły, wida´c, ˙ze pr ˛ad

rzeczywisty nie zawsze odpowiada warto´sci zadanej. Spowodowane jest to tym, ˙ze regulator pr ˛adu

jest zasilany z akumulatorów o napi˛eciu 36V. Regulator, wymuszaj ˛ac pr ˛ad, zmienia ´sredni ˛a warto´s´c

napi˛ecia zasilaj ˛acego silnik. Pami˛etaj ˛ac, ˙ze silnik ma liniow ˛a charakterystyk˛e zale˙zno´sci pr˛edko´sci

obrotowej od napi˛ecia zasilania, mo˙zna zauwa˙zy´c, ˙ze skoro napi˛ecie zasilania jest ograniczone, to

maksymalna pr˛edko´s´c obrotowa równie˙z jest ograniczona. Skoro silnik osi ˛agn ˛ał maksymaln ˛a pr˛ed-

ko´s´c, to znaczy ˙ze regulator pr ˛adu si˛e ju˙z nasycił i nie da wi˛ekszego napi˛ecia. W takim przypadku

pr ˛ad silnika zaczyna male´c bo znika wpływ bezwładno´sci kół przy stałej pr˛edko´sci k ˛atowej. Ener-

gia pobierana przez silnik tracona jest tylko na siły tarcia. Gdy koła zostały zablokowane wzgl˛edem

korpusu, silnik nie mógł si˛e obraca´c i zmniejszy´c poboru pr ˛adu. Dlatego zale˙zno´s´c I

act

f (I

set

)

dla

układu z zablokowanymi kołami robota jest bliska liniowej. Mo˙zna j ˛aaproksymowa´c funkcj ˛aliniow ˛a

o równaniu: y = 0.8 · x.

Warto´s´c pr ˛adu silnika nie jest wyskalowana w amperach poniewa˙z pr ˛ad silnika jest stopniem po´sred-

nim pomi˛edzy zadan ˛a w sterowniku liczb ˛a, a momentem nap˛edowym. Mo˙zna jednak pokusi´c si˛e

o oszacowanie pr ˛adu silnika, znaj ˛ac konstrukcj˛e toru pomiarowego pr ˛adu silnika. Załó˙zmy, ˙ze przez

silnik płynie pr ˛adu 1A, na rezystorze pomiarowym 0,1

Ω odkłada si˛e napi˛ecie 0,1V. To napi˛ecie jest

wzmocnione we wzmacniaczu ró˙znicowym do warto´sci 0,47V i podane na filtr dolnoprzepustowy, po

którym jest mierzone w przetworniku AC. Zakres przetwornika wynosi od -2,048V do 2,047V, co jest

w nim przekładane na warto´s´c liczbow ˛a w zakresie od -2048 do 2047. Łatwo zauwa˙zy´c, ˙ze napi˛ecie

0,47V podane na wej´scie AC da na wyj´sciu warto´s´c 470, czyli: 1A pr ˛adu silnika daje 470 jednostek

przetwornika AC

5.1.4 Dynamiczne zale˙zno´sci pr ˛adowe

Dynamiczne wła´sciwo´sci regulatorów pr ˛adu mo˙zna okre´sli´c na podstawie przebiegów czasowych

odpowiedzi układu na pobudzenie jednostkowe I

cur

i · 1(t). Wida´c wyra´znie na wykresach 35, ˙ze

silnik maksymalnie pobiera tyle pr ˛adu, na ile pozwoli regulator. Natomiast, gdy pr˛edko´s´c obrotowa

silnika jest bliska maksymalnej, pr ˛ad silnika spada do warto´sci koniecznej zrównowa˙zeniu siły oporu

(tarcia).

Na wykresie (rysunek 37) s ˛a przedstawione zale˙zno´sci I

act

f (I

set

)

, gdzie I

set

jest sygnałem wymu-

szaj ˛acym pr ˛ad silnika zadawanym z joysticka. Mo˙zna zauwa˙zy´c, ˙ze w przypadku z zablokowanymi

kołami pr ˛ad silnika nad ˛a˙za za warto´sci ˛a zadan ˛a, natomiast po odblokowaniu kół szybko wraca do

warto´sci bliskiej zeru.

W tym momencie mo˙zna pokusi´c si˛e o wyznaczenie współczynnika wzmocnienia pełnej p˛etli stero-

wania pr ˛adem. P˛etla ta wygl ˛ada nast˛epuj ˛aco: zadajemy pr ˛ad I

cur

, zostaje on przetworzony i zmie-

rzony jako I

set

oraz podany na regulator. Regulator wymusza w silniku pr ˛ad, który jest u´sredniany

i mierzony w przetworniku AC (do sterownika wraca jako I

act

). Z charakterystyk statycznych wi-

da´c, ˙ze zale˙zno´s´c pomi˛edzy pr ˛adem I

cur

, a pr ˛adem I

set

wygl ˛ada nast˛epuj ˛aco: I

set

2500

2048

· I

cur

. Na

charakterrystykach dynamicznych, z zablokowanymi kołami robota mo˙zna doszuka´c si˛e zale˙zno´sci

pomi˛edzy pr ˛adem I

act

, a pr ˛adem I

set

. Zale˙zno´s´c t˛e mo˙zna opisa´c wzorem: I

act

· I

set

. Podstawiaj ˛ac

Jest to orientacyjne oszacowanie nie uwzgl˛edniaj ˛ace tolerancji u˙zytych rezystorów

-2000

-1500

-1000

-500

500

1000

1500

2000

-2000

-1500

-1000

-500

500

1000

1500

2000

Prad silnika Iact

Prad zadany Iset

Iact(Iset)

Y=0.8*X

Rysunek 32: Wykresy zale˙zno´sci I

act

f (I

set

)

dla swobodnych kół.

-1500

-1000

-500

500

1000

1500

-1500

-1000

-500

500

1000

1500

Prad silnika Iact

Prad zadany Iset

Iact(Iset)

Y=0.8*X

Rysunek 33: Wykresy zale˙zno´sci I

act

f (I

set

)

dla zablokowanych kół.

set

otrzymamy wzór: I

act

250

256

· I

cur

. Taki wynik mo˙zna przybli˙zy´c zale˙zno´sci ˛a I

act

≈ I

cur

5.2 Wyznaczanie parametrów modelu w eksperymentach
Po sprawdzeniu i wyskalowaniu czujników robota mobilnego

RoBik mo˙zna przej´s´c do cz˛e´sci ekspe-

rymentów zwi ˛azanych z wyznaczaniem parametrów modelu robota. Teoretyczny opis eksperymen-

tów umieszczony jest w rozdziale 2.2.

200

400

600

800

1000

1200

1400

500

600

700

800

900

1000

1100

Prady

Czas [ms]

Pobudzenie - Iset(t)

Reakcja - Icur(t)

Predkosc katowa - alpha(t)

Rysunek 34: Wykresy czasowe pr ˛adów I

set

, I

act

i pr˛edko´sci k ˛atowej ˙

ϕ dla swobodnych kół.

-1200

-1000

-800

-600

-400

-200

200

500

600

700

800

900

1000

1100

Prady

Czas [ms]

Pobudzenie - Iset(t)

Reakcja - Icur(t)

Predkosc katowa - alpha(t)

Rysunek 35: Wykresy czasowe pr ˛adów I

set

, I

act

i pr˛edko´sci k ˛atowej ˙

ϕ dla swobodnych kół.

5.2.1 Przeło˙zenie pr ˛adowe silników wraz z przekładniami – K

W eksperymencie dla ka˙zdego silnika powstały dwie serie pomiarowe. Korpus został najpierw wy-

chylony przez silnik lewy, a nast˛epnie przez prawy i w ko´ncu przez oba. U´srednione wyniki pomiarów

zostały przedstawione w tablicach 14, 15.
Pami˛etaj ˛ac wzór: K

sin

−i

sin

, z serii pomiarowej mo˙zna wyznaczy´c ´sredni ˛a warto´s´c

przeło˙zenia pr ˛adowego silników: K

0,00491. W tablicy 16 s ˛a ´srednie warto´sci przeło˙ze´n pr ˛ado-

wych w zale˙zno´sci od konfiguracji do´swiadczenia.

Znaj ˛ac wyniki do´swiadcze´n, mo˙zna rozwa˙zy´c czym mogły by´c spowodowane takie rozbie˙zno´sci.

Tablica 14: Pomiar k ˛ata

ϕ w funkcji pr ˛adu I

act

— bez dodatkowego obci ˛a˙znika

set

act

ϕ [

]

Silnik lewy wymusza wychylenie

1167,2

947,15

33,68

1488,5

1298,05

63,49

Silnik prawy wymusza wychylenie

959,1

771,9

21,97

1143,3

948,45

26,8

Oba silniki wymuszaj ˛a wychylenie
1945,75

1596,7

47,88

2103,7

1745,15

68,28

Tablica 15: Pomiar k ˛ata

ϕ w funkcji pr ˛adu I

act

— z dodatkowym obci ˛a˙znikiem

set

act

ϕ [

]

Silnik lewy wymusza wychylenie

927,5

727

11,99

1475,85 1262,45

26,74

Silnik prawy wymusza wychylenie

935,95

740,5

10,78

1292,3

1095,3

17,09

Oba silniki wymuszaj ˛a wychylenie
1022,05

691,75

13,06

1536,1

1158,45

20,51

2694,25

2301,7

48,99

Tablica 16: Warto´sci ´srednie przeło˙ze´n pr ˛adowych silników

Konfiguracja do´swiadczenia

Lewy silnik

0,00388

Prawy silnik

0,00438

Oba silniki

0,00623

Warto´s´c ´srednia

0,0049

-2000

-1000

1000

2000

1000

2000

3000

4000

5000

6000

7000

8000

9000

10000

Prady

Czas [ms]

Prad zadany - Iset(t)

Prad silnika - Iact(t)

Rysunek 36: Wykresy czasowe pr ˛adów I

set

, I

act

dla swobodnych kół.

-2000

-1000

1000

2000

1000

2000

3000

4000

5000

6000

7000

8000

9000

10000

Prady

Czas [ms]

Prad zadany - Iset(t)

Prad silnika - Iact(t)

Rysunek 37: Wykresy czasowe pr ˛adów I

set

, I

act

dla zablokowanych kół.

Pojedynczy silnik musi pokona´c dodatkowo siły tarcia zwi ˛azane z drugim zespołem nap˛edowym.

Aby sprawdzi´c, ˙ze wyniki otrzymane w eksperymencie s ˛a bliskie oczekiwanych nale˙zy przeprowa-

dzi´c nast˛epuj ˛ace szacowania: znaj ˛ac stał ˛a momentu nap˛edowego (warto´s´c katalogowa dla silnika A-

max32) równ ˛a 46

mNm

, przeło˙zenie przekładni planetarnej równe 1:66 oraz sprawno´s´c tej przekładni

równ ˛a 70% mo˙zemy porówna´c: K

· N

Icur

z 0,046 · 66 · 0,7, gdzie N

Icur

– zadany współczynnik 1A.

Podstawiaj ˛a´c warto´sci liczbowe otrzymano: K

· N

Icur

2,306, a z prawej strony 2,1252. Jak wida´c s ˛a

to liczby bardzo zbli˙zone do siebie. Podczas eksperymentu nie został popełniony gruby bł ˛ad.

5.2.2 Zast˛epcza długo´s´c korpusu – l

Korzystaj ˛ac z wyników umieszczonych w tablicy 14 oraz wzoru: l =

gsin

, gdzie g jest przyspiesze-

niem ziemskim, mo˙zna okre´sli´c zast˛epcz ˛a długo´s´c korpusu. Po podstawieniu i wyliczeniu otrzymano

zast˛epcz ˛a długo´s´c korpusu, która ma warto´s´c l = 0,086m.

5.2.3 Współczynnik tarcia pomi˛edzy kołem a korpusem – k

Na wykresie (rysunek 38) przedstawiona jest zale˙zno´s´c ˙

ϕ = f (t). Wbrew oczekiwaniom, od razu mo-

˙zemy dostrzec, ˙ze tarcie ma bardzo mał ˛a składow ˛a lepk ˛a. Wykres jest prawie prostoliniowy, posiada

tylko niewielk ˛a składow ˛a eksponencjaln ˛a oraz sinusoidaln ˛a, której przyczyn ˛a mo˙ze by´c niewywa˙ze-

nie kół

RoBika.

Wykres został aproksymowany funkcj ˛a: Y (t) = Y

·e

−

−M ·t. Najlepsz ˛a aproksymacj˛e (dla obu kół)

uzyskano dla T =4800 i M=1,5.

-5000

5000

10000

15000

20000

500

1000

1500

2000

2500

3000

3500

4000

4500

5000

Predkosc kol

Czas [ms]

Predkosc kola 0

20000*exp(-x/4800)-1.5*x

Rysunek 38: Wykres pr˛edko´sci k ˛atowej kół ˙

ϕ w funkcji czasu.

Poprawione równanie ruchu (uwzgl˛edniaj ˛ace składow ˛a liniow ˛a tarcia) wygl ˛ada:

· i − k

· ˙ϕ

∞

(21)

gdzie:

M · I

(22)

Podstawiaj ˛ac otrzymano:

K − I · i

∞

(23)

Korzystaj ˛ac z eksperymentu, w którym koła robota były rozp˛edzane małym momentem nap˛edowym

(moment nap˛edowy musiał by´c w stanie ustalonym zrównowa˙zony przez siły tarcia) został wyzna-

czony pr ˛ad i=18,425 i pr˛edko´s´c ˙

∞

=1,1325rad/s. Wielko´sci te podstawione zostały do wzoru i otrzy-

mano: I

=0,06. Znaj ˛ac I

mo˙zna obliczy´c warto´sci k

=1,254e-05 i k

=0,09.

5.2.4 Momenty bezwładno´sci kół – I

Podczas wyznaczania współczynnika tarcia pomi˛edzy kołem a korpusem, został wyznaczony moment

bezwładno´sci I

=0,06. Przy zało˙zeniu o symetrycznym rozmieszczeniu masy koła wzgl˛edem osi

obrotu moment bezwładno´sci koła wzgl˛edem ´srednicy wynosi:

1
2

(24)

5.3 Wyniki otrzymane w eksperymentach
Wyniki otrzymane w eksperymentach przeprowadzonych na robocie mobilnym

RoBik zostały zgro-

madzone i umieszczone w tablici 17.

Tablica 17: Parametry fizyczne wyznaczone w eksperymentach dla robota

RoBik.

Parametr

Symbol

Warto´s´c

Wymiar

przekładnia pr ˛adowa silnika

0,0049

[kg·m

·s

−2

·A

−1

]

zast˛epcza długo´s´c korpusu

0,086

[m]

współczynnik tarcia koło – korpus

1,254e-05

[kg·m

·rad·s

−3

]

0,09

[kg·m

·rad·s

−2

]

momenty bezwładno´sci koła

0.06

[kg·m

]

0.03

[kg·m

]

k ˛at obrotu koł ˛a

366,(6)

impulsów/

21008,47

impulsów/rad

pr˛etko´s´c wychylenia wahadła

17,16

jednostek

1966,4

jednostek

rad

Rysunek 39: Widok robota mobilnego

RoBik z tyłu.

6 Uwagi ko´ncowe

W wyniku niniejszej pracy powstało stanowisko badawcze pozwalaj ˛ace wygodnie implementowa´c

i bada´c algorytmy sterowania z wykorzystaniem rzeczywistego obiektu, jakim jest dwukołowy robot

mobilny

RoBik. Jest to mo˙zliwe dzi˛eki sterownikowi wyposa˙zonemu w zmiennoprzecinkowy mikro-

kontroler MPC555, który mo˙zna w prosty sposób programowa´c w j˛ezyku C. Wzoruj ˛ac si˛e na wcze´s-

niej wykonanym robocie

MK opracowano now ˛a, ulepszon ˛a konstrukcj˛e mechaniczn ˛a. Opracowano

i uruchomiono moduły elektroniczne składaj ˛ace si˛e na sterownik robota. Dokumentacja konstruk-

cyjna robota została przedstawiona w postaci raportu [9].

Robot

MK został zmodernizowany przez wymian˛e jednostki centralnej sterownika na MPC555 z za-

chowaniem cz˛e´sci obiektowej. W ramach modernizacji opracowano i wykonano płytk˛e przej´sciow ˛a

pozwalaj ˛ac ˛a umie´sci´c moduł phyCORE-MPC555 w miejscu dotychczas u˙zywanego (EM332). Ra-

port [8] zawiera dokumentacj˛e wykonanej przeróbki.

Dla

RoBika przygotowano i przetestowano oprogramowanie obsługuj ˛ace wszystkie czujniki, układy

wykonawcze i moduły komunikacji z otoczeniem. Robot jest wyposa˙zony w panel u˙zytkownika

(klawiatura, wy´swietlacz LCD, sygnalizator d´zwi˛ekowy i diody LED). Wej´scie dla standardowego

joysticka umo˙zliwia sterowanie r˛eczne. Na szczególn ˛a uwag˛e zasługuje ł ˛acze radiowe DECT, które

zapewnia szybk ˛a bezprzewodow ˛a komunikacj˛e pomi˛edzy sterownikiem robota, a komputerem robo-

czym (PC). Dzi˛eki temu mo˙zna zdalnie uruchamia´c eksperymenty, zadawa´c parametry ruchu, odczy-

tywa´c stan robota i odbiera´c serie pomiarowe rejestrowane w sterowniku.

Jako prosty przykład implementacji programu u˙zytkownika przedstawiono kod ´zródłowy programu

umo˙zliwiaj ˛acego r˛eczne sterowanie robota przy pomocy joysticka, z równoczesn ˛a transmisj ˛a wybra-

nych parametrów ruchów.

Przeprowadzono eksperymenty w celu kalibracji czujników wewn˛etrznych robota. Uzyskane wyniki

potwierdzaj ˛a poprawno´s´c konstrukcji sterownika.

Na podstawie pracy [12] przeprowadzono prób˛e eksperymentalnego wyznaczenia fizycznych para-

metrów wyst˛epuj ˛acych w modelu dynamiki robota

RoBik. Z powodzeniem wyznaczono przekładni˛e

pr ˛adow ˛a nap˛edów K

, zast˛epcz ˛a długo´s´c wahadła l oraz momenty bezwładno´sci kół I

Wyznaczenie współczynnika tarcia kół k

napotkało trudno´sci. Powszechnie przyjmowany model tar-

cia lepkiego okazał si˛e nieadekwatny. Po analizie wyników eksperymentu zaproponowano wst˛epnie

model tarcia o dwóch składowych (tarcie lepkie – zale˙zne od pr˛edko´sci i tarcie niezale˙zne od pr˛ed-

ko´sci). Zagadnienie modelowania zjawiska tarcia jest ciekawe, jednak wykracza poza zamierzony

zakres niniejszej pracy.

Opracowane stanowisko nadaje si˛e do badania własno´sci dynamicznych robotów mobilnych oraz

implementowania i badania algorytmów sterowania.Pozwoli, mi˛edzy innymi, na przeprowadzenie

eksperymentów, których wyniki mog ˛a posłu˙zy´c do modyfikacji modelu dynamiki robota, zwłaszcza

w zakresie zjawiska tarcia.

Literatura

[1] KABAŁA M. Sterowanie robotem mobilnym z uwzglednieniem jego własno´sci dynamicznych

Praca magisterska pod kierunkiem dr in˙z. M. Wnuk, Inst. Cyb. Techn. PWr, Wrocław, 2002.

[2] KABAŁA M., WNUK M. Kulisty robot mobilny RoBall (projekt wst˛epy) Raport serii SPR nr

37/2002.

[3] KABAŁA M., WNUK M. Konstrukcja i oprogramowanie dwukołowego robota mobilnego

Raport serii SPR nr 20/2002, Inst. Cyb. Techn. PWr, Wrocław 2002.

[4] KABAŁA M., WNUK M. Dwukołowy robot mobilny nap˛edzany w układzie wewn˛etrznym —

dokumentacja modelu Raport SPR nr 21/2001, Inst. Cyb. Techn. PWr, Wrocław, 2001.

[5] KABAŁA M., TCHO ´N K., WNUK M. Dwukołowy, nieholonomiczny robot mobilny Materiał

Konferencji Automation 2002, Warszawa, 2002, ss.269–280.

[6] KABAŁA M., TCHO ´N K., WNUK M. Robot mobilny nap˛edzany w układzie wewn˛etrznym

Materiały VII Krajowej Konferencji Robotyki, L ˛adek–Zdrój, 2001, t. 1, ss.149–158.

[7] MUSZY ´NSKI R., TCHO ´N K. Singularities and mobility of nonholonomic systems: the ball

rolling on a plane 6th IFAC Symposium on Robot Control, SYROCO’00, Vienna 2000, Preprints

vol.1.

[8] SZLAWSKI R., WNUK M. Modernizacja dwukołowego robota mobilnego MK

Raport serii SPR nr 11/2004, Inst. Cyb. Techn. PWr, Wrocław 2004.

[9] SZLAWSKI R., WNUK M. Konstrukcja dwukołowego robota mobilnego RoBik

Raport serii SPR nr 12/2004, Inst. Cyb. Techn. PWr, Wrocław 2004.

[10] TCHO ´N K. i inni Manipulatory i roboty mobilne: Modele, planowanie ruchu i sterowanie,

Akademicka Oficyna Wydawnicza, Warszawa 2000.

[11] TCHO ´N K., KABAŁA M., WNUK M. Algorytm ´sledzenia trajektorii robota mobilnego MK.

Materiały XIV Krajowej Konferencji Automatyki, Zielona Góra, 2002.

[12] WNUK M. Pomiary parametrów fizycznych dwukołowego robota mobilnego

w: Post˛epy robotyki: Przemysłowe i medyczne systemy robotycznerobotyczne (w druku).

[13] WRONKA C. Model kinematyki i dynamiki robota nieholonomicznego Praca magisterska pod

kierunkiem dr in˙z. Roberta Muszy´nskiego, Inst. Cyb. Techn. PWr, Wrocław, 2002.

[14] phyCORE–MPC555 Hardware Manual A product of a PHYTEC 2003.
[15] L4960 power switching regulator datasheet SGS-Thomson 1995.
[16] http://www.maxonmotor.com/
[17] http://www.wobit.com.pl/produkty/sprzegla/oldchama.htm
[18] http://www.maxim-ic.com/
[19] http://www.irf.com/
[20] http://www.analog.com/

Wyszukiwarka

Podobne podstrony:
Implementacja algorytmów sterowania osi robota
Implementacja algorytmów sterowania osi robota
Układy Napędowe oraz algorytmy sterowania w bioprotezach
9 podstawowe algorytmy sterowania nowy
nieodporny sprawozdanie, Pwr, Metody i algorytmy sterowania cyfrowego, sprawka
Badanie układu sterowania z regulatorem PID
lichtenstein, struktury?nych i złożoność obliczeniowa,Badanie?ektywności algorytmów pseudowielomiano
BADANIA SYMULACYJNE STEROWANIA Nieznany
39 Badanie układów sterowania z regulatorami nieciągłymi
5M Badanie prostownik w sterow Nieznany
10 Podstawowe algorytmy sterowania
Metody i algorytmy sterowania cyfrowego spr2, Pwr, Metody i algorytmy sterowania cyfrowego, Metody p
sprawozdanie filtr Noi, Pwr, Metody i algorytmy sterowania cyfrowego, sprawka
Badanie prostownika sterowanego 1-fazowego, Politechnika Lubelska
Metody i algorytmy sterowania cyfrowego spr3, Pwr, Metody i algorytmy sterowania cyfrowego, Metody p

więcej podobnych podstron