RoB2.pdf

Robot kulisty. Zapraszam do oglądania.

Witam znalazłem na elektrodzie temat o kulistym robocie i własnie dzis znalazlem info o takim robocie zapraszam do ogladania :)


Na prawach rekopisu
INSTYTUT CYBERNETYKI TECHNICZNEJ
POLITECHNIKI WROCŁAWSKIEJ
Raport serii SPR nr 20/2004

Konstrukcja
kulistego robota mobilnego
RoBall
Marek Kabała
Marek Wnuk

Słowa kluczowe: robot mobilny, naped elektryczny, mikrokontroler, sterownik

Wrocław 2004

Spis tre´ ci
s
1 Wstep

4

2 Konstrukcja mechaniczna robota RoBall
2.1 Napedy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

4
7

3 Sterownik robota RoBall
3.1 Sterownik gł´ wny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
o
3.2 Sterownik lokalny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9
9
18

4 Oprogramowanie sterownika
4.1 Oprogramowanie uruchomieniowe dla MPC555 . . . . . . . . . . . . . . . . . . . .
4.2 Oprogramowanie u˙ ytkowe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
z

21
21
23

´
5 Uwagi koncowe

27

Spis rysunk´ w
o
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

Konstrukcja mechaniczna robota RoBall . . . . . . . . . . . . . . . .
Konstrukcja ramki no´nej robota RoBall . . . . . . . . . . . . . . . .
s
Układ przeniesienia zasilania . . . . . . . . . . . . . . . . . . . . . .
Zesp´ ł napedowy firmy MAXON . . . . . . . . . . . . . . . . . . .
o
Struktura sprzetowa sterownika robota RoBall . . . . . . . . . . . . .
Gł´ wna płytka sterownika robota RoBall . . . . . . . . . . . . . . . .
o
Schemat gł´ wnej płytki sterownika . . . . . . . . . . . . . . . . . . .
o
Układy komunikacji radiowej (DECT) . . . . . . . . . . . . . . . . .
Płytka sterownik´ w mocy i przetwornicy zasilajacej . . . . . . . . . .
o
Układy mostk´ w mocy i logika sterujaca . . . . . . . . . . . . . . . .
o
Schemat mostk´ w mocy . . . . . . . . . . . . . . . . . . . . . . . .
o
Schemat logiki mostk´ w mocy . . . . . . . . . . . . . . . . . . . . .
o
Płytka z trzema zyroskopami ENC03J . . . . . . . . . . . . . . . . .
˙
Schemat przyłaczenia zyroskop´ w . . . . . . . . . . . . . . . . . . .
˙
o
Płytki włacznika zasilania i sterownika lokalnego (MC9S12C32) [14]
Schemat układu właczania zasilania . . . . . . . . . . . . . . . . . .
CW IDE z widocznym oknem zarzadzania projektem . . . . . . . . .
CW IDE z widocznym oknem debuggera . . . . . . . . . . . . . . .

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

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

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

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

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

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

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

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

5
6
7
8
10
12
13
14
15
16
17
17
18
19
20
20
22
23

Spis tablic
1
2
3
4

Podstawowe parametry mechaniczno-elektryczne RoBall
Parametry silnik´ w RE-max 21 CLL . . . . . . . . . . .
o
Parametry modułu phyCORE-MPC555 . . . . . . . . .
Parametry modułu transmisji radiowej HW86010 . . . .

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

.
.
.
.

4
8
11
14

1 Wstep
W celu mo˙ liwienia weryfikacji bada´ na temat osobliwo´ci kinematyki robot´ w nieholonomicznych
z
n
s
o
[2], w Laboratorium Robotyki Zakładu Podstaw Cybernetyki i Robotyki skonstruowano kulistego
robota nazwanego RoBall. Jego wstepna koncepcje, ze szczeg´ lnym uwzglednieniem metody napedu
o
przedstawiono na posiedzeniu Seminarium ZPCiR w dniu 04.05.2000.
Zdecydowano sie na odniesienie moment´ w napedowych, dostarczanych przez dwa silniki, do wao
hadła zawieszonego w srodku kuli z dwoma obrotowymi stopniami swobody uzyskanymi dzi eki idei
´
przegubu Cardana.
Prace nad robotem kulistym zostały poprzedzone opracowaniem dwukołowego, nieholonomicznego
robota mobilnego, nazwanego MK oraz jego ulepszonej wersji zwanej RoBik. Wyniki prac nad
tymi robotami dotyczacych zar´ wno konstrukcji, jak i modelu dynamiki oraz algorytm´ w sterowania
o
o
zostały opublikowane w materiałach konferencyjnych [3, 5, 7], pracach dyplomowych [10, 11, 13] i
raportach [4, 9, 12].
Do´wiadczenia zdobyte w wyniku wspomnianych bada´ (w zakresie jednostki centralnej sterownika,
s
n
układ´ w pomiaru przyspiesze´ i predko´ci katowych oraz komunikacji bezprzewodowej) wykorzyo
n
s
stano przy opracowaniu wstepnego projektu konstrukcji robota RoBall [6].
Niniejszy raport zawiera opis konstrukcji zrealizowanego robota kulistego RoBall. Projekt ten jest
jednym z zada´ pracy naukowej finansowanej ze srodk´ w KBN w latach 2002-2004 jako projekt
n
´
o
babawczy.

2 Konstrukcja mechaniczna robota RoBall
Podstawowe parametry mechaniczne robota RoBall zestawiono w tab. 1.
Tablica 1: Podstawowe parametry mechaniczno-elektryczne RoBall
srednica kuli
´
masa całkowita
masa moduł´ w napedowych
o
masa akumulator´ w
o
moment bezwładno´ci wahadla
s
moment napedowy (na o´)
s
moc mechaniczna (na o´)
s
maksymalna predko´c

napiecie zasilania
maksymalny prad całkowity

0.27 m
1.2 kg
0.45 kg
0.3 kg
0.003 kgm 2
0.375 Nm
5.22 W
13.9 rad/s
14.4 V
1.6 A

Korpus robota ma forme sfery, wewnatrz kt´ rej znajduja sie wszystkie elementy potrzebne do zasilao
nia, napedu i sterowania robotem (rys. 1).
Na jednej z osi sfery, symetrycznie na ka˙ dym z biegun´ w, s a umieszczone tuleje łaczace osie moduz
o
ł´ w napedowych z zewnetrzna sfera (szczeg´ ły konstrukcji pokazano na rys. 2). Obudowy moduł´ w
o
o
o
napedowych sa połaczone o´miokatna ramka no´na (1), stanowiaca podstawe całej wewnetrznej kons
s
strukcji mechanicznej.
W osi symetrii ramki, prostopadłej do osi moduł´ w napedowych połaczonych z zewnetrzna sfera,
o
sa umieszczone dwa kolejne moduły napedowe, kt´ rych silniki (3) sa zamocowane do ramki przy
o
4

Rysunek 1: Konstrukcja mechaniczna robota RoBall
5

Rysunek 2: Konstrukcja ramki no´nej robota RoBall
s
6

pomocy tulei zaciskowych (2). Osie tych moduł´ w sa skierowane do wnetrza sfery i połaczone z
o
konstrukcja wahadła (7) zawierajacego akumulatory (9) oraz układy pomiarowe (8) do wyznaczania
parametr´ w ruchu wahadła w inercyjnym układzie odniesienia.
o
´o
Zr´ dłem momentu napedowego jest przemieszczana za pomoca silnik´ w masa akumulator´ w twoo
o
rzacych wahadło. Moment napedowy jest suma momentu wytwarzanego przez zmiane poło˙ enia
z
srodka ciezko´ci oraz momentu wynikajacego z bezwładno´ci.
´
˙s
s
Układy elektroniczne sa umieszczone wewnatrz ramki no´nej. Zaleta umiejscowienia zr´ dła zasilania
s
´o
na ruchomym wahadle jest wykorzystanie masy akumulator´ w do wytwarzania momentu nap edoweo
go. Wada tego rozwiazania jest brak mo˙ liwo´ci połaczenia akumulator´ w z układami elektronicznyz
s
o
mi za pomoca przewodu, ze wzgledu na wzajemne zmiany poło˙ enia tych element´ w. Z tego wzgl edu
z
o
na osiach silnik´ w sprzezonych z ramka sa umieszczone dwa połaczenia slizgowe. Napiecie z akumuo
˙
´
lator´ w jest podłaczone do bie˙ ni (6), po kt´ rych slizgaja sie grafitowe szczotki (4, 5) zapewniajace
o
z
o
´
połaczenie elektryczne. Detale układu przeniesienia zasilania pokazano na rys. 3.

Rysunek 3: Układ przeniesienia zasilania

2.1 Napedy
Po do´wiadczeniach robotem MK, w kt´ rym wykorzystano z powodzeniem zespoły nap edowe pros
o
dukowane przez firme MAXON, do napedzania robota RoBall wybrano zespoły tej firmy (rys. 4)
zło˙ one z:
z
o silnika pradu stałego RE-max 21 CLL,
o przekładni planetarnej GP22C o przeło˙ eniu 1:53,
z

o kodera przyrostowo-impulsowego MR o rozdzielczo´ci 512 impuls´ w/obr´ t.
s
o
o

Parametry wybranych silnik´ w podano w tabeli 2
o
Moc zastosowanych silnik´ w pozwala na ruch wahadła z akumulatorami z pr edko´cia ok. 2 obr/s.
o
s
Jako zr´ dło zasilania wykorzystano 12 akumulator´ w o rozmiarze AA wykonanych w technologii
´o
o
NiMH, o pojemno´ci 2.05Ah ka˙ dy. Pozwalaja one na ponadgodzinna prace ciagła robota.
s
z
7

Rysunek 4: Zesp´ ł napedowy firmy MAXON
o

Tablica 2: Parametry silnik´ w RE-max 21 CLL
o
moc nominalna
moment maksymalny
moment przy pracy ciagłej
moment bezwładno´ci wirnika
s
predko´c bez obciazenia

˙
predko´c dopuszczalna

mechaniczna stała czasowa
masa
napiecie nominalne
rezystancja
prad startowy
dopuszczalny prad przy pracy ciagłej
prad bez obciazenia
˙
sprawno´c


8

5W
27.5 mNm
6.8 mNm
0.2?10 -6 kgm2
8600 obr/min
10600 obr/min
7 ms
0.042 kg
12 V
5.77 Ohm
2.08 A
515 mA
11 mA
86 %

3 Sterownik robota RoBall
RoBall został wyposa˙ ony w sterownik pozwalajacy zachowa´ pełna autonomie robota. Wszystz
c
kie funkcje sterowania i obsługi czujnik´ w sa wykonywane lokalnie. Komunikacja z komputerem
o
nadrzednym i (ewentualnie) z operatorem słu˙ y do zadawania parametr´ w ruchu, modyfikowania
z
o
oprogramowania sterownika w pamieci FLASH, odczytywania parametr´ w ruchu i pomiar´ w z czujo
o
nik´ w.
o
Schemat blokowy sterownika przedstawiono na rys. 5.
Ze wzgledu na konieczno´c umieszczenia czesci czujnik´ w na wahadle, system sterowania jest pos´
´
o
dzielony na dwa bloki:
o sterownik gł´ wny, mocowany na ramce no´nej,
o
s
o sterownik lokalny, mocowany na wahadle.

Podstawowe bloki funkcjonalne sterownika gł´ wnego to:
o
o gł´ wna jednostka centralna (MPC555),
o
o stopnie mocy z zadawaniem napiecia i pradu silnik´ w,
o
o blok odczytu poło˙ enia i predko´ci z koder´ w przyrostowo-impulsowych,
z
s
o
o przetworniki A/D i D/A,
o moduł komunikacji radiowej,
o wewnetrzne łacze bezprzewodowe (IrDA),
o impulsowa przetwornica zasilajaca.

Podstawowe bloki funkcjonalne sterownika lokalnego to:
o lokalna jednostka centralna (MC9S12C32)
o trzy zyroskopy piezoceramiczne,
˙
o dwa dwuosiowe akcelerometry,
o wewnetrzne łacze bezprzewodowe (IrDA),
o impulsowa przetwornica zasilajaca.
3.1 Sterownik gł´ wny
o
Sterownik gł´ wny jest wykonany w postaci dw´ ch płytek drukowanych o kształcie p´ łkolistym zao
o
o
mocowanych symetrycznie do ramki no´nej robota (rys. 1).
s
Jako jednostke centralna zastosowano w sterowniku mikrokontroler firmy Motorola MPC555 o bogatych zasobach:
o Jednostka centralna - 32-bitowy RISC:
- architektura PowerPC (52.7 K Dhrystone (v.2.1) @ 40 MHz),
- wbudowana jednostka zmiennoprzecinkowa podw´ jnej precyzji,
o
9

Rysunek 5: Struktura sprzetowa sterownika robota RoBall

10

o

o

o
o

o

- układy testowo-uruchomieniowe (OnCE);
Wbudowane pamieci:
- 26Kb RAM + 6Kb RAM dla mikrokodu TPU,
- 448Kb FLASH EEPROM;
Układy transmisji szeregowej (QSMCM):
- QSPI - kolejka do 32 zada´ , 8-16 bit´ w,
n
o
- 2 × SCI - zewnetrzny zegar transmisji;
2 × CANbus (TouCAN);
Układu czasowo-licznikowe:
- 2 × TPU3 - 32 mikroprogramowane kanały,
- MIOS1 - m.in. 8 × PWMSM;
32 wej´cia analogowe (10-bitowe) - 2 × QADC64.
s

Dzieki temu, ze układy peryferyjne MPC555 stanowia nadzbi´ r zasob´ w mikrokontrolera MC68332
˙
o
o
stosowanego w sterowniku robota MK, łatwo jest przenie´c obsług e poszczeg´ lnych blok´ w sterows´
o
o
nika w nowe srodowisko. Znaczna moc obliczeniowa procesora, zwłaszcza w zakresie arytmetyki
´
zmiennoprzecinkowej, pozwala na efektywne implementowanie algorytm´ w sterowania wykorzyo
stujacych model dynamiki robota (co ma zasadnicze znaczenie w przypadku opisywanego projektu).
Ze wzgledu na małe rozmiary i bogate wyposa˙ enie dodatkowe wybrano moduł phyCORE-MPC555
z
o parametrach podanych w tab. 3. Jest on połaczony mechanicznie i elektrycznie z jedna z płyt za
Tablica 3: Parametry modułu phyCORE-MPC555
Wymiary
Jednostka centralna
Pamieci
Opcje
Interfejsy
Przetwornik
Debugger
Zasilania
Zakres temperatur
MTBF

ok.75 mm x 57 mm
32-bit MPC555/40 MHz (272-pin BGA)
do 4 MB SRAM, do 4 MB Flash-ROM
I2 C-EEPROM, I2 C RTC
2.0B CAN, QSPI, 2× UART (RS-232/TTL)
2× 16 kan. A/D 10-bit
BDM
5 V/50mA, 3.3 V/400 mA
0o - 70o C lub -40o - 85o C n
776765 godzin

pomoca wielostykowych łacz´ wek. Widok płytki pokazano na rys. 6.
o
Schemat ideowy płytki gł´ wnej sterownika przedstawiono na rys. 7.
o
Poza jednostka centralna (phyCORE-MPC555) umieszczono na niej moduł transmisji radiowej w
systemie DECT (HW86010), bezprzewodowe łacze lokalne IrDA (TOIM3232), przetwornik A/D
(MAX186) i D/A (MAX525).
Przetwornik D/A słu˙ y do azdawania wielko´ci prad´ w zasilajacych silniki.
z
s
o
Przetwornik A/D pozwala mierzy´ rzeczywiste wielko´ci prad´ w silnik´ w oraz temperature wewc
s
o
o
natrz robota.
Odczyt koder´ w impulsowych (poło˙ enia i predko´ci silnik´ w) zapewnia funkcje QDEC i QDVEL
o
z
s
o
zaimplementowane w TPU3 (Time Processor Unit) zawartym w MPC555.
11

Rysunek 6: Gł´ wna płytka sterownika robota RoBall
o

12

Rysunek 7: Schemat gł´ wnej płytki sterownika
o
13

Zadawanie napiecia zasilajacego silniki odbywa sie przy pomocy sygnał´ w o modulowanym wypeło
nieniu (PWM - Pulse Width Modulation) z bloku MPWM mikrokontrolera MPC555.
Komunikacja z komputerem nadrzednym jest mo˙ liwa dzieki dwukierunkowemu łaczu radiowemu.
z
W opisywanym projekcie wykorzystano moduły DECT stosowane w telefonach bezprzewodowych.
Maja one te zalete, ze nie wymagaja dodatkowej modulacji sygnału danych i zapewniaja znaczna
predko´c transmisji. Parametry u˙ ytych w projekcie RoBall moduł´ w HW86010 firmy Hoft & Wessel

z
o
przedstawiono w tab. 4.
Tablica 4: Parametry modułu transmisji radiowej HW86010
Wymiary
Masa
Zakres temperatur
Zakres napiec
´
Pob´ r pradu
o
Czestotliwo´c

Moc emitowana
Interfejs danych
Sygnały modemowe
Zasieg

ok.52 mm x 37 mm x 8 mm
30 g
-10 to +55 C
3.0 - 3.6 V czesc cyfrowa, 3.3 - 4.7 V czesc radiowa
´´
´´
100 - 150 mA
1.88 - 1.9 GHz
250 mW max.
RS-232, 3.3V, do 115.2 KBd
RTS/CTS, DTR/DSR, DCD, RI
do 300 m (w terenie), do 60 m (w budynku)

Jeden z moduł´ w jest przyłaczony do płytki gł´ wnej sterownika (rys. 8). Wsp´ łpracuje on z układem
o
o
o

Rysunek 8: Układy komunikacji radiowej (DECT)

asynchronicznej transmisji szeregowej SCI0 mikrokontrolera MPC555. Dodatkowe sygnały steruj ace
sa wyprowadzone z bloku QADC. Dopasowanie poziom´ w napi ec zapewniaja układy MAX3379 i
o
´
14

MAX3393 (rys. 7). Drugi moduł wsp´ łpracuje z komputerem nadrz ednym (PC) przez port szeregowy
o
RS232C i jest umieszczony na module uruchomieniowym z zestawu HW86910.
Układy mocy (mostki oraz przetwornice zasilajaca) zmontowano na odpowiednio ukształtowanej
płytce uniwersalnej przystosowanej zar´ wno do monta˙ u przewlekanego, jak i powierzchniowego
o
z
(rys. 9).

Rysunek 9: Płytka sterownik´ w mocy i przetwornicy zasilaj acej
o

Logika sterujaca dla mostk´ w została zmontowana na oddzielnej płytce i poł aczona z mostkami mocy
o
przy pomocy łacz´ wek (rys. 10).
o
Schemat mostk´ w mocy przedstawiono na rys. 11.
o
Schemat logiki sterujacej przedstawiono na rys. 12.
Układy regulacji pradu zostały wykonane w oparciu o do´wiadczenia z robota MK [9], z zadawaniem
s
napieciowym przez 12-bitowy przetwornik D/A.
15

Rysunek 10: Układy mostk´ w mocy i logika steruj aca
o
16

Rysunek 11: Schemat mostk´ w mocy
o

Rysunek 12: Schemat logiki mostk´ w mocy
o
17

3.2 Sterownik lokalny
W celu zapewnienia poprawnego pomiaru przyspiesze´ liniowych i pr edko´ci katowych w dw´ ch
n
s
o
płaszczyznach niezbedne jest umieszczenie co najmniej czesci czujnik´ w pomiarowych bezpo´rednio
´
o
s
na wahadle. Przekazywanie danych do jednostki centralnej sterownika przy zachowaniu pełnej swobody ruchu wahadła zapewnia lokalny mikrokontroler (MC9S12C32) umieszczony wraz z czujnikami
i układem zasilania na wahadle. Wstepnie przetworzone dane pomiarowe sa przekazywane wewnatrz
sfery w spos´ b ciagły za pomoca dwukierunkowej transmisji bezprzewodowej z wykorzystaniem
o
podczerwieni (IrDA).
Widok płytki z trzema zyroskopami piezoceramicznymi ENC03J, mocowanej na wahadle, przedsta˙
wiono na rys. 13.

Rysunek 13: Płytka z trzema zyroskopami ENC03J
˙

Schemat przyłaczenia zyroskp´ w do przetwornika A/D mikrokontrolera MC9S12C32 pokazano na
˙
o
rys. 14.
W celu skompensowania zale˙ no´ci napiecia wyj´ciowego zyroskop´ w od temperatury wprowadzono
zs
s
˙
o
pomiar temperatury wszystkich zyroskop´ w przy pomocy układu LM35 zamocowanego na miedzia˙
o
nym radiatorze zapewniajacym jednakowa temperature wszystkich układ´ w (rys. 13). Kalibracja
o
układu polega na zdjeciu charakterystyki temperaturowej warto´ci napiecia wyj´ciowego dla zerowej
s
s
predko´ci katowej (układ w spoczynku) dla wszystkich zyroskop´ w. W trakcie pomiaru kompensacja
s
˙
o
odczytu jest dokonywana w sterowniku lokalnym przed wysłaniem pomiaru do sterownika gł´ wnego.
o
Właczanie zasilania robota kulistego zostało rozwiazane dzieki wykorzystaniu silnik´ w jako pradnic.
o
Przy energicznym obr´ ceniu korpusu robota silniki generuj a wystarczajaco du˙ o energii by zasili´
o
z
c
przeka´ nik z samopodtrzymaniem, kt´ ry mo˙ na wyłaczy´ sygnałem z portu sterownika lokalnego.
z
o
z
c
Płytke włacznika zasilania pokazano na rys. 15, a jej schemat - na rys. 16.
Na płytce zamontowano r´ wnie˙ układy transmisji lokalnej IrDA (TOIM3232 i TFDS4500) oraz
o
z
gło´niczek pozwalajacy na akustyczna sygnalizacje właczania i wyłaczania zasilania kuli.
s

18

Rysunek 14: Schemat przyłaczenia zyroskop´ w
˙
o

19

Rysunek 15: Płytki włacznika zasilania i sterownika lokalnego (MC9S12C32) [14]

Rysunek 16: Schemat układu właczania zasilania

20

4 Oprogramowanie sterownika
Sterownik robota RoBall jest przystosowany do zaimplementowania opisywanego wcze´niej swos
bodnie programowalnego sterownika [1, 6], kt´ ry zapewnia łatwe implementowanie własnych algoo
rytm´ w. Jadro sterownika zawiera procedury i definicje struktur danych umo˙ liwiaj ace eksperymeno
z
tatorowi implementowanie własnego algorytmu sterowania bez szczeg´ łowej znajomo´ci zastosowao
s
nego w sterowniku sprzetu.
U˙ ytkownik (eksperymentator), chcac zrealizowa´ własny algorytm sterowania musi dostarczy´ (naz
c
c
pisa´ w jezyku C) inicjalizacje własnych zmiennych roboczych, petle gł´ wna (mo˙ e by´ pusta) oraz
c
o
z
c
obsługe komunikacji z komputerem nadrzednym (MODBUS).
Struktury danych zapewniajace komunikacje miedzy czescia u˙ ytkownika a jadrem zawieraja po´
z
miary - chwilowe warto´ci mierzonych i obliczanych parametr´ w (pr edko´ci, przyspieszenia, katy,
s
o
s
prady itp.) i sterowania - wytworzone przez jednokrokowa procedure sterownika warto´ci sygnał´ w
s
o
sterujacych (prady i napiecia zadane itp.).
4.1 Oprogramowanie uruchomieniowe dla MPC555
Jako srodowisko uruchomieniowe dla oprogramowania sterownika robota RoBall wybrano pakiet
´
CodeWarrior for PowerPC Embedded Systems, Release 6.5 firmy Metrowerks. W Laboratorium
Robotyki ICT PWr jest u˙ ywana licencjonowana instalacja tego pakietu zakupiona ze srodk´ w KBN
z
´
o
w celu realizacji opisywanego projektu. 1
Centralnym narzedziem pakietu jest srodowisko zintegrowane (IDE - Integrated Development Envi´
ronment). Widok okna IDE przedstawiono na rys. 17. Dostarcza ono nast epujacych usług:
o rozbudowane zarzadzanie projektem,

o łatwy w obsłudze edytor kodu zr´ dłowego,
˙o

o zintegrowany debugger,

o przegladarka r´ znych format´ w plik´ w zr´ dłowych i wynikowych,

o
o ˙o

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

CW IDE umo˙ liwia łatwe i elastyczne wykonywanie podstawowych prac programistycznych:
z
o Tworzenie i poprawianie kod´ w zr´ dłowych program´ w. Edytor zapewnia kolorowe pod´wieo ˙o
o
s
tlanie składni, szybki dostep do plik´ w zr´ dłowych i dołaczanych, prace w trybie wielu okien
o ˙o
itp. Przegladarka oferuje dialogowe okna pomagajace w operowaniu procedurami, strukturami
danych, zmiennymi, klasami i plikami.
o Zarzadzanie wieloma konfiguracjami docelowymi (ang. targets). Pozwala to na przechowywanie w projekcie wielu ustawie´ konfiguracji plik´ w zr´ dłowych, kompilatora i linkera.
n
o ˙o
Przykładem moga by´ konfiguracje przeznaczone do pracy w pamieci RAM, ROM i do prograc
mowania pamieci FLASH u˙ ywajace wsp´ lnych zr´ deł.
z
o
˙o
o Tworzenie oprogramowania dla r´ znych procesor´ w w tym samym srodowisku. Dost epne

o
´
sa kompilatory, asemblery i linkery dla wielu r´ znych platform (np. dla mikrokontroler´ w

o
MC9S12, czy procesor´ w sygnałowych DSP563xx).
o
1 CodeWarrior jest pakietem komercyjnym o znacznej cenie (w wersji pełnej ok. $5000). W najbli˙ szej przyszło´ ci przewidziana
z
s
jest migracja do srodowiska publicznie dostepnego (GNU) w systemie Linux (make, gcc, gdb, ddd z obsługa BDM).
´

21

Rysunek 17: CW IDE z widocznym oknem zarz adzania projektem

o Por´ wnywanie plik´ w obejmujace podkatalogi i otwarte okna edytora.
o
o
Narzedzia zawarte w pakiecie obejmuja r´ wnie˙ obsługe wbudowanych wersji systemu Linux (Emo
z
bedded Linux Application Development) z narzedziami GNU (kompilator, asembler, linker, archiwizator) przeniesionymi do srodowiska Windows z wykorzystaniem biblioteki Cygwin.DLL.
´
Debugger symboliczny (CodeWarrior Debugger) pozwala uruchamia´ programy nie tylko na wbuc
dowanym symulatorze PowerPC, ale r´ wnie˙ na r´ znych modułach sprz etowych oferowanych przez
o
z

inne firmy. W naszym przypadku takim modulem jest phyCORE-MPC555 firmy Phytec. Do komunikacji z modułem docelowym wykorzystywane sa r´ zne interfejsy obsługujace złacze BDM lub JTAG

mikrokontrolera MPC555 w systemie docelowym oraz port r´ wnoległy, szeregowy, lub Ethernet w
o
komputerze roboczym. W naszym przypadku wykorzystano interfejs r´ wnoległy (zgodny z Wiggler
o
firmy Macraigor Systems). Widok okna debuggera w srodowisku CW IDE przedstawiono na rys. 18.
Debugger pozwala:
o przeglada´ program w postaci kodu zr´ dłowego, zdezsemblowanej zawarto´ci pami eci, lub jedc
´o
s
nocze´nie w obu formach,
s
o uruchamia´ program krok po kroku na poziomie kodu zr´ dłowego lub j ezyka maszynowego,
c
´o
o ustawia´ pułapki zar´ wno dla kodu w pamieci RAM, jak i ROM (FLASH),
c
o
22

Rysunek 18: CW IDE z widocznym oknem debuggera

o przeglada´ zawarto´c zmiennych, kom´ rek pamieci i rejestr´ w przez wskazanie ich w oknie
c

o
o
kodu wska´ nikiem myszy,
z
o modyfikowa´ przez wyb´ r mysza i wprowadzenie nowej warto´ci.
c
o
s
Wiecej informacji o opisanym srodowisku mo˙ na znale˙ c na stronie:
´
z

& lt; http://www.metrowerks.com/MW/Develop/Embedded/PowerPC/Default.htm & gt; .
4.2 Oprogramowanie u˙ ytkowe
z
Definicje stałych, makrodefinicje i struktury danych zwiazane ze sprzetem robota RoBall umieszczono w pliku roball hw.h.
Niekt´ re parametry sa wydzielone na poczatku pliku, aby ułatwi´ u˙ ytkownikowi konfigurowanie
o
cz
oprogramowania sterownika:
#define TPSEC

1000

// requested number of ticks per second

#define PWM_FRQ

1000

// requested PWM frequency [Hz]

23

Parametr TPSEC oznacza czestotliwo´c przerwa´ cyklicznych. Decyduje on o szybko´ci pracy steros´
n
s
wnika (procedura u˙ ytkownika bedzie wykonywana TPSEC razy na sekunde).
z
Wyb´ r czestotliwo´ci generowanego sygnału PWM (u˙ ywanego przy napi eciowym sterowaniu silo
s
z
o

nik´ w) jest mo˙ liwy przez zmienianie parametru PWM FRQ, kt´ ry oznacza cz estotliwo´c tego przeo
z
biegu. Sygnał PWM jest generowany za pomoca modułu MPWM mikrokontrolera MPC555.
Obsługe TPU ułatwiaja nastepujace definicje stałych i makrodefinicje:
#define TRAM_BASE 0x302000
#define XTAL 20000000
#define TCR1CK ((XTAL)/4)
#define PWM_PER ((TCR1CK+(PWM_FRQ & gt; & gt; 1))/PWM_FRQ)

// TPU time base TCR1

/* TPU_A channels usage */
#define QDECCHAN0
#define QDVELCHAN0

10
12

// TPU channel for QDECA ch0
// TPU channel for QDVEL ch0

#define QDECCHAN1
#define QDVELCHAN1

2
0

// TPU channel for QDECA ch1
// TPU channel for QDVEL ch1

Parametr XTAL oznacza czestotliwo´c rezonatora kwarcowego na module phyCORE-MPC555 i nie

powinien by´ zmieniany przez u˙ ytkownika. Na jego podstawie okre´lono TCR1CK - cz estotliwo´c
c
z
s

podstawy czasu dla TPU3 oraz wyliczono (korzystajac z definiowanej przez u˙ ytkownika czestotliz
wo´ci PWM - PWM FRQ) parametr PWM PER potrzebny do inicjalizacji i obsługi kanał´ w PWM.
s
o
Pozostałe definicje odpowiadaja sprzetowej konfiguracji sterownika: QDECCHANx, QDVELCHANx - kanały do obsługi naped´ w.
o
Przetworniki analogowo-cyfrowe i cyfrowo-analogowe sa przyłaczone do magistrali QSPI. Poni˙ sze
z
makra definiuja maski sygnał´ w PCS do wybierania poszczeg´ lnych urzadze´ na tej magistrali:
o
o
n
#define DESELECT

0xF

// no SLAVEs selected

#define DACSEL

0xD

// DAC MAX525 is selected by LOW on PCS1

/*
DAC MAX525 outputs:
A (0)
current value to be set in ch0
B (1)
current value to be set in ch1
C (2)
unused
D (3)
unused

(bipolar)
(bipolar)

*/
#define ADCSEL

0xB

// ADC MAX186 is selected by LOW on PCS2

/*
ADC MAX186 inputs:
24

0
1
2
3
4
5

current measured in
current measured in
temperature 1 sense
temperature 2 sense
temperature 3 sense
temperature 4 sense

ch0 V1ADC
ch1 V2ADC
TMP1
TMP2
TMP3
TMP4

(bipolar)
(bipolar)
(unipolar)
(unipolar)
(unipolar)
(unipolar)

*/
DACSEL słu˙ y do wybrania przetwornika cyfrowo-analogowego, ADCSEL - przetwornika analogowoz
cyfrowego. W komentarzach podano przypisanie kanał´ w przetwornik´ w do sygnał´ w sterownika.
o
o
o
Odczytywanie wielko´ci mierzonych z poziomu programu napisanego w j ezyku C ułatwiaja nastepus
jace makrodefinicje:
#define Pos0
#define Pos1

(tpua- & gt; PARM.R[QDECCHAN0][TPU_QDEC_POSITION_COUNT])
(tpua- & gt; PARM.R[QDECCHAN1][TPU_QDEC_POSITION_COUNT])

#define Vel0
#define Vel1

(tpua- & gt; PARM.R[QDVELCHAN0][TPU_QDVEL_DIFTIME])
(tpua- & gt; PARM.R[QDVELCHAN1][TPU_QDVEL_DIFTIME])

#define Iact0
#define Iact1

(((INT16)((QSMCM.RECRAM[0].R))) & gt; & gt; 4)
(((INT16)((QSMCM.RECRAM[1].R))) & gt; & gt; 4)

#define
#define
#define
#define

(((UINT16)(QSMCM.RECRAM[2].R)) & gt; & gt; 4)
(((UINT16)(QSMCM.RECRAM[3].R)) & gt; & gt; 4)
(((UINT16)(QSMCM.RECRAM[4].R)) & gt; & gt; 4)
(((UINT16)(QSMCM.RECRAM[5].R)) & gt; & gt; 4)

Temp1
Temp2
Temp3
Temp4

Odczyt obejmuje pomiary: parametr´ w ruchu osi napedowych (Posx - poło˙ enie, Velx - predko´c),
o
z

wielko´ci prad´ w (Iactx) oraz temperatur (Tempx).
s
o
Podobnie ułatwione jest zadawanie sygnał´ w sterujacych:
o
#define SetI0(x) QSMCM.TRANRAM[6].R = 0x3000 | (((x) + 0x800) & 0x0fff)
#define SetI1(x) QSMCM.TRANRAM[7].R = 0x7000 | (((x) + 0x800) & 0x0fff)
#define SetU0(x) MIOS1.MPWMSM0PULR.R = (x)
#define SetU1(x) MIOS1.MPWMSM1PULR.R = (x)
Odpowiednie makrodefinicje obejmuja podstawienia i wywołania funkcji pomocniczych ustawiajacych odpowiednie wielko´ci w MPWM i przetworniku cyfrowo-analogowym. Ustawianie pr ad´ w
s
o
zadanych (SetIx) odbywa sie przez wpisanie warto´ci zadanej do kolejki nadawczej QSPI. Ustawias
nie napiec (SetUx) odbywa sie przez podstawienie warto´ci do rejestru MPWMSMxPULR modułu
´
s
MPWM.
Inicjalizacja sprzetu sterownika polega na wywołaniu kilku procedur odpowiedzialnych za ustawienie
parametr´ w pracy poszczeg´ lnych blok´ w. Parametry i prototypy tych procedur podano poni˙ ej.
o
o
o
z
25

#define PIT_TB (XTAL/256)

// PIT time base frequency

/* calculated PIT time constant */
#define PIT_CONST
((PIT_TB+(TPSEC & gt; & gt; 1))/TPSEC)
EXTERN INT32 pulse; // free running tick counter
void PitInit(void);

PIT TB wyznacza czestotliwo´c podstawy czasu dla bloku PIT (ang. Periodic Interrupt Timer). Parametr dla dzielnika czestotliwo´ci PIT (PIT CONST) jest wyliczany na podstawie wybranej przez
s
u˙ ytkownika czestotliwo´ci przerwa´ cyklicznych (parametru TPSEC).
z
s
n
Zmienna globalna pulse jest licznikiem przerwa´ cyklicznych wyznaczaj acych podstawe czasu dla
n
sterownika.
Procedura PitInit() uruchamia przerwania cykliczne z czestotliwo´cia zadana przez TPSEC.
s
Dostep do zasob´ w TPU jest mo˙ liwy dzieki wska´ nikom tpua i tpub:
o
z
z
extern const unsigned char tpumska[];
EXTERN struct TPU3_tag *tpua;
EXTERN struct TPU3_tag *tpub;

// TPU mask with QDVEL (MW'04)
// pointer for TPU routines
// pointer for TPU routines

void TpuInit(void);
W celu pomiaru predko´ci katowej k´ ł wykorzystano funkcje QDVEL, kt´ rej nie ma w standardowej
s
o
o
masce TPU3. Obraz binarny maski mikrokodu z dodana funkcja QDVEL (tpumska[]) jest dołaczany
przez linker. Wygodny dostep do rejestr´ w TPU A i TPU B jest mo˙ liwy dzieki funkcjom i struko
z
z
turom zdefiniowanym w pliku tpu.h. Zmienne globalne tpua i tpub s a wska˙ nikami na struktury
z
opisujace rejestry TPU3 inicjowanymi w procedurze TpuInit(). W tej˙ e procedurze s a inicjalizowane wszystkie kanały TPU A i TPU B u˙ ywane w ustalonej konfiguracji.
z
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 RIIO
#define CTSO
#define DSRO
void
void
void
int

QADC_B.PORTQA.B.PQA4
QADC_B.PORTQA.B.PQA5
QADC_B.PORTQA.B.PQA6

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

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˙ na nawiaza´ połaczenie z odpowiedz
c
nim stacjonarnym modułem DECT. Funkcja ta zwraca warto´c 0 w przypadku nawi azania połaczenia

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

´
5 Uwagi koncowe
Opisane stanowisko jest przewidziane do badania algorytm´ w sterowania ruchem robota mobilnego
o
napedzanego wzgledem wewnetrznego układu wsp´ łrzednych w oparciu o jego model dynamiki.
o
Zastosowanie w sterowniku robota jednostki centralnej o du˙ ej mocy obliczeniowej, wyposa˙ onej w
z
z
szybki układ zmiennoprzecinkowy (mikrokontroler MPC555 z procesorem RISC (PowerPC), dwoma
blokami TPU, interfejsami szeregowymi, przetwornikiem analogowo-cyfrowym) ma szczeg´ lne znao
czenie przy realizacji zło˙ onych algorytm´ w sterowania.
z
o

27

Literatura
[1] Gora S., Wnuk M., VFMC - swobodnie programowalny sterownik nap edu robota, Raport SPR
13/97, Inst. Cyb. Techn. PWr, 1997,
[2] Muszy´ ski R., Tcho´ K., Singularities and mobility of nonholonomic systems: the ball rolling
n
n
on a plane, 6th IFAC Symposium on Robot Control, SYROCO'00, Vienna 2000, Preprints vol.1,
ss. 259-264.
[3] Kabała M., Tcho´ K., Wnuk M., Robot mobilny napedzany w układzie wewnetrznym, VII KKR,
n
Ladek Zdr´ j, 2001, Prace Naukowe ICT PWr., Konferencje 46, t.1, ss. 149-158.
o
[4] Kabała M., Wnuk M., Dwukołowy robot mobilny napedzany w układzie wewnetrznym (dokumentacja modelu), Raport SPR 21/2001, Inst. Cyb. Techn. PWr, 2001.
[5] Kabała M., Tcho´ K., Wnuk M., Dwukołowy, nieholonomiczny robot mobilny, Konferwncja
n
Automation 2002, Warszawa, 2002, ss. 269-280.
[6] Kabała M., Wnuk M., Kulisty robot mobilny RoBall (projekt wstepny), Raport SPR 37/2002,
Inst. Cyb. Techn. PWr, 2002.
[7] Tcho´ K., Kabała M., Wnuk M., Algorytm sledzenia trajektorii robota mobilnego MK, XIV Kran
´
jowa Konferencja Automatyki, Zielona G´ ra, 2002.
o
[8] Wnuk M., Komunikacja radiowa z robotem mobilnym MK, Raport SPR 19/2002, Inst. Cyb.
Techn. PWr, 2002.
[9] Kabała M., Wnuk M., Konstrukcja i oprogramowanie dwukołowego robota mobilnego, Raport
SPR 20/2002, Inst. Cyb. Techn. PWr, 2002.
[10] Wronka C., Model kinematyki i dynamiki robota nieholonomicznego, Praca magisterska pod
kierunkiem dr. in˙ . R. Muszy´ skiego, ICT PWr., Wrocław, 2002.
z
n
[11] Kabała M., Sterowanie robotem mobilnym z uwzglednieniem jego własno´ci dynamicznych,
s
Praca magisterska pod kierunkiem dr. in˙ . M. Wnuka, ICT PWr., Wrocław, 2002.
z
[12] Szlawski R., Wnuk M., Konstrukcja dwukołowego robota mobilnego RoBik, Raport SPR
12/2004, Inst. Cyb. Techn. PWr, 2004.
[13] Szlawski R., Implementacja i badanie algorytmow sterowania robotem dwukołowym, Praca ma´
gisterska pod kierunkiem dr. in˙ . M. Wnuka, ICT PWr., Wrocław, 2004.
z
[14] Wnuk M., Moduł z mikrokontrolerem MC9S12C32, Raport ICT PWr. serii SPR 19/04, Wrocław,
2004.
[15] MC68332 User's Manual, MC68332UM/AD, Motorola Inc., 1991.
[16] Queued Serial Module Reference Manual, QSMRM/AD, Motorola Inc., 1991.
[17] Time Processor Unit Reference Manual, TPURM/AD, Rev.3, Mototrola Inc., 1996.
28

mgr in˙ . Marek Kabała
z
dr in˙ . Marek Wnuk
z
Instytut Cybernetyki Technicznej
Politechniki Wrocławskiej
ul. Janiszewskiego 11/17
50-372 Wrocław

Niniejszy raport otrzymuja:
1. OINT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . - 1 egz.
2. Zleceniodawca . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . - 2 egz.
3. Autorzy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . - 2 egz.

Razem :

Raport wpłynał do redakcji I-6
w grudniu 2004 roku.

29

5 egz.


Download file - link to post