POLITECHNIKA WROCŁAWSKA WYDZIAŁ ELEKTRONIKI PRACA DYPLOMOWA MAGISTERSKA

Transkrypt

1 POLITECHNIKA WROCŁAWSKA WYDZIAŁ ELEKTRONIKI KIERUNEK: Automatyka i Robotyka (AiR) SPECJALNOŚĆ: Robotyka (ARR) PRACA DYPLOMOWA MAGISTERSKA Wyposażenie robota dwukołowego w cujniki ewnętrne Equipping a two wheel robot with eternal sensors AUTOR: Premysław Lisowski PROWADZĄCY PRACĘ: dr inż. Marek Wnuk, I-6 OCENA PRACY: WROCŁAW 006

2 Dedykuję Diadkom,Mamie i mojej Kochanej Ani

3 Bardo diękuję Panu Doktorowi Markowi Wnukowi a umożliwienie podjęcia tematu, poświęcony cas i wyroumiałość

4 Spis treści Wstęp Robot RoBik Cel i akres pracy Cujniki astosowane w pracy magisterskiej Cujnik do pomiaru odległości SHARP GPY0A0YK Kompas elektronicny CMPS Cujnik prechyłu firmy GEMAC Inklinometr GEMAC ISA0S Opis wyprowadeń Wynacanie charakterystyki Inklinometru Wynacanie momentu bewładności platformy utrymującej poiom Moment bewładności prowadnicy Oblicanie momentu bewładności belki platformy Model RoBika i platformy w programie Adams Robot RoBik w programie Adams Symulacje achowania RoBik-a umiesconą platformą Wstępne ałożenia dotycące sterowania platformą utrymującą poiom Implementacyjne roważania na temat oblicenia położenia kątowego wahadła Wynacanie położenia pry użyciu żyroskopu Implementacja algorytmu oblicania położenia w mikrokontrolere Implementacja algorytmu sterowania platformą Podsumowanie Literatura Dodatek A Dodatek B

5 Spis rysunków Robot Robik Poglądowy widok platformy Stanowisko do wynacania charakterystyki dalmiera Zależność napięcia od odległości dla cujnika SHARP GPY0A0YK Płytka kompasu CMPS0 wra onaconymi wyprowadeniami Prebieg sygnału do komunikacji kompasem pre I C Zależność aymutu od wychylenia kątowego Zależność aymutu od wychylenia kątowego Struktura chipu elementu mikromechanicnego Inklinometr GEMAC ISA0S Osie obrotu inklinometru Poglądowy schemat umiescenia inklinometru Charakterystyka Inklinometru GEMAC ISA0S Ilustracja do oblicenia momentu bewładności ciała stywnego a) podiał ciała stywnego na elementarne masy, b) prejście d elementu masy do elementu objętości 5 Onacenie wymiarów prowadnicy platformy Onacenie wymiarów belki platformy Ilustracja do twierdenia Steinera Model RoBik-a wra platformą wykonany w programie ADAMS Widok platformy Siła pryłożona do wahadła Robika wymusająca wychylenie kątowe Schematycny rysunek RoBika platformą anaconym kątami Odchylenie kątowe α w casie Prędkość kątowaα& w casie Pryspiesenie kątowe α& & w casie Prędkość kątowa platformy wględem wahadła Odchylenie kątowe platformy β od poycji pionowej dla P, D Wartości momentu obrotowego koniecnego do utrymania platformy w poycji pionowej dla P, D Odchylenie kątowe platformy β od poycji pionowej dla P0, D

6 9 Wartości momentu obrotowego koniecnego do utrymania platformy w poycji pionowej dla P0, D Odchylenie kątowe platformy β od poycji pionowej dla P0, D dla prowadnic wykonanych żelaa Różnica międy kątami β dla prowadnic wykonanych żelaa i aluminium Różnica międy momentami obrotowymi dla prowadnic wykonanych żelaa i aluminium Widok wahadła testowego Prędkość kątowa wahadła w casie Powięksony fragment wykresu prędkości kątowej w casie Zależności położenia w casie oblicone dla enkodera i żyroskopu Błąd położenia obliconego żyroskopu Wykres predstawiający presunięcie casowe sygnałów Błąd położenia wahadła w spocynku Błąd oblicenia położenia użyciem licb ułamkowych i całkowitych Różnica międy kątem α i β dla nastaw regulatora P D Różnica międy kątem α i β dla nastaw regulatora P4 D Różnica międy kątem α i β dla nastaw regulatora P D Różnica międy kątem α i β dla nastaw regulatora P6 D

7 Spis tablic Wybrane parametry cujnika SHARP GPY0A0YK podane pre producenta Opis wyprowadeń sygnałów kompasu elektronicnego CMPS Opis wyprowadeń inklinometru GEMAC ISA0S Wymiary i masy robota RoBik Dodatek A wartości mierone podcas wynacania charakterystyki cujnika SHARP GPY0A0YK

8 . Wstęp Cujnik to element układu pomiarowego lub automatycnego pretwarający wartość wielkości fiycnej w obiekcie badanym na wartość innej wielkości fiycnej dogodniejsej do mierenia lub dalsego pretworenia.[] Obecnie cujniki, wane również sensorami, są integralna cęścią konstrukcji każdego robota. To dięki nim sterownik może w dokładny sposób określić stan poscególnych elementów robota np. położenia pregubów, prędkości kątowej kół, kierunek ruchu, a także ebrać informacje o otoceniu np. odległość od preskody, kąt nachylenia podłoża wględem wektora grawitacji i na tej podstawie wylicyć nastawy dla pregubów lub prędkości dla kół napędających robota. Na dień disiejsy na świecie istnieje bardo dużo firm produkujących cujniki mogące mieryć właściwie wsystkie wielkości fiycne. Co ciekawe, jeden cujnik może pretworyć wartość na kilka różnych sposobów, dając konstruktorowi możliwość wyboru sposobu odcytania mieronej wielkości fiycnej. Rysunek Robot RoBik 8

9 . Robot Robik Robot RoBik jest predstawicielem grupy robotów mobilnych klasy.0 [], które stanowią bardo ciekawe obiekty badawce. Został skonstruowany jako dwukołowa platforma i alicany jest do tw. Unicycle-ów. Najcęściej w konstrukcji uwględnia się dodatkowo trecie koło, które nie ma wpływu na achowanie robota, lec służy jedynie jako dodatkowy punkt styku podłożem. Konstrukcja robota RoBik [] nie prewiduje dodatkowego punktu podparcia co powoduje, że grawitacyjny układ napędowy wiąże kinematykę i dynamikę robota w całość. Ruch robota wymusany jest popre mianę położenia środka ciężkości. Moment obrotowy prekaywany silników napędowych na koła wprawia robota w ruch wahadłowy co utrudnia astosowanie dodatkowych cujników mogących w nacnym stopniu pomóc w realiacji algorytmów śledenia trajektorii.. Cel i akres pracy W ałożeniach wstępnych pryjęto, że robot RoBik ostanie wyposażony w cujniki, których sposób diałania, a także charakterystyka pracy wymagają poiomego położenia. Należą do nich np. dalmiere lub kompas elektronicny. Aby pomiary były jak najbardiej wiarygodne te cujniki musa być umiescone na pewnego rodaju platformie, której ruchy będą kompensowały wychylenie wahadła robota RoBik. W pocątkowej cęści pracy ajmę się uruchomieniem cujników, sporądeniem ich dokładnego opisu i wynaceniem charakterystyk. Do tego celu wykorystam mikroprocesor 9SA64 firmy FreeScale. Jest on wyposażony w 0 bitowy pretwornik ADC, generator prerwań cyklicnych, 6 bitowy egar,który użyję do odcytu sygnałów PWM, a także moduł obsługi protokołu transmisji danych I C. W dalsej cęści pracy ajmę się budową wahadła, dięki któremu będę mógł aproponować sterowania silnikami porusającymi platformą i ebrać pomiary cujników dostarcających dane sterownika platformy. Rysunek Poglądowy widok platformy 9

10 Najprawdopodobniej platforma ostanie umiescona na belce robota (), do której też prymocowana jest kaseta akumulatorami i sterownikiem. Belka wykona jest rury kwasoodpornej o średnicy 5mm. Takie rowiąanie wymusa aby prowadnice ( i 4), dięki którym platforma będie mogła porusać się po belce miały kstałt fragmentów pierścieni. To rowiąanie wydaje się być optymalne. Na prowadnicach ostanie umiescona druga belką, która będie miejscem montażu cujników. 0

11 . Cujniki astosowane w pracy magisterskiej Określenie położenia robota jest bardo trudnym adaniem. Pomocne do tego bywają różne cujniki, w które może być on wyposażony. Dięki nim będie możliwe uyskanie informacji na temat otocenia, w którym się najduje, a także wybranie optymalnej trajektorii ruchu.. Cujnik do pomiaru odległości SHARP GPY0A0YK SHARP GPY0A0YK jest to cujnik fotoelektrycny, który może by wykorystywany do wykrywania preskody najdującej się pred robotem lub pomiaru odległości od preskody. Cujnik reaguje na mianę intensywności docierającego do niego strumienia światła. Sensor posiada integrowany w jednej obudowie nadajnik światła podcerwonego, w tym prypadku jest to dioda, która wysyła światło o odpowiedniej długości fali. Zastosowanie takiego rowiąania daje dużą nieależność pomiaru od oświetlenia ewnętrnego ponieważ światło widialne można w bardo prosty sposób odfiltrować. Jeśli jakiś obiekt najdie się na drode pred cujnikiem odbite od niego wiąka podcerwona dotre do odbiornika. Cujnik budowano w oparciu o fototranystor, dięki cemu informację o odległości od preskody odcytujemy popre napięcie. W cujnikach można także wykorystać światło laserowe jednak takie rowiąania bywają cęsto bardo kostowne. Wybrane parametry cujnika podane pre producenta [4] Parametr Symbol Zakres Jednostki Napięcie asilania V CC 4,5 do 5,5 V Temperatura pracy T OPR -0 do 60 O C Zakres pomiaru odległości L od 0 do 50 cm Napięcie na wyjściu cujnika * V O min. 0,5 ma. 0,55 V typ. 0,4 Różnica napięć wyjściowych** V O min.,8 ma., V typ.,05 Prąd pracy typ. I CC ma. 55 ma * dla L50cm ** dla L mieniającego się 50c do 0 cm Ponieważ producent nie dostarca na swojej stronie dokładnej charakterystyki cujnika preprowadiłem cykl pomiarów, prowadących do wynacenia wspomnianej charakterystyki. Do tego celu użyłem kilku elementów: - cujnika SHARP Y0A0, - woltomiera, - miary odległości, - preskody o wymiarach 5, cm Wymiary preskody nie mają ocywiście wpływu na wygląd charakterystyki, ale preskoda o takich gabarytach gwarantowała mi stałe odcyty odległości. Pomiarów dokonałem w warunkach oświetlenia diennego, możliwe jest że pry takim świetle będie najcęściej wykorystywany cujnik. Aby dobre móc

12 odcytać odległość od cujnika ułożyłem dwie miarki centymetrowe równolegle. Takie rowiąanie niwelowało możliwość ustawienia preskody pod kątem wględem cujnika. Ponieważ jest to cujnik fotoelektrycny i diała w oparciu o odbicie strumienia światła od preskody chciałem sprawdić jaki wpływ na pomiar odległości będie miał kolor preskody. Dlatego preprowadiłem pięć cykli pomiarowych a każdym raem mieniając kolor preskody. Kolejno były to kolory: biały, żółty, cerwony, niebieski i carny. Jeden cykl pomiarowy polegał na presuwaniu preskody co cm w akresie 0-60cm i a każdym raem odcytaniu napięcia na wyjściu cujnika. Rysunek Stanowisko do wynacania charakterystyki dalmiera Powyżse djęcie predstawia stanowisko pomiarowe. Wyniki pomiarów umieściłem w tabeli najdującej się w dodatku A. Preprowadenie powyżsych pomiarów powoliło mi prekonać się, iż kolor preskody w więksej miere nie ma nacenia ma pomiar odległości. W asadie odcyty odbiegają od siebie o kilka mv więc różnica jest pomijalnie mała. Jedynie wyniki pomiarów dla koloru carnego różnią się od wyników poostałych kolorów. Sądę, że najlepsym rowiąaniem jest w tym wypadku uśrednienie wartości mieronych dla każdej odległości ponieważ nie ma możliwości astosowania detektora barwy preskody. Analiując wykres, mogę potwierdić, że recywisty akres pomiarowy mieści się pomiędy 0-50[cm] i w asadie tylko w nim możemy w sposób jednonacny określić odległość od preskody. W okolicach odległości 50cm i dalej, napięcie wyjściowe nie ustalało się na stałym poiomie tylko wahało się w akresie kilkunastu mv co faktycnie może utrudnić, wręc uniemożliwić, preprowadenie pomiarów. W celu lepsego obraowania wyników pomiarów predstawiłem je na wykresie na następnej stronie. Niestety widać wyraźnie, że charakterystyka cujnika jest

13 nieliniowa co w pewnym stopniu komplikuje pomiar odległości. Proponuję dwa, według mnie, najlepse rowiąania dla tego problemu: wprowadić do pamięci mikrokontrolera tablicę, w której będą apisane wartości napięć wyjściowych cujnika i odpowiadające im odległości, prybliżyć poniżsą charakterystykę worem matematycnym.

14 ,000,500,000 napięcie [V],500 biały żółty cerwony niebieski carny,000 0,500 0, odleglość [cm] Wykres Zależność napięcia od odległości dla cujnika SHARP GPY0A0YK 4

15 Aby wynacyć równanie krywej użyłem programu CurveEpert. [5]. Program wynacył, że najlepiej charakterystykę cujnika opisuje równanie 5-tego stopnia następującej postaci: y a + b + c + d + e 4 + f 5 () Wartości współcynników wynosą kolejno: a b c d e f -.66 Łatwo auważyć, że użycie takiego wielomianu do wylicenia odległości od preskody racej nie wchodi w grę ponieważ należałoby użyć jednostki miennoprecinkowej, a i tak cas wykonania obliceń mógłby być byt długi. Dlatego uważam, że najlepsym rowiąaniem jest wpisanie do pamięci mikrokontrolera tablicy wartościami napięć i odpowiadającymi im odległościami. 5

16 . Kompas elektronicny CMPS0 Kompas elektronicny CMPS0 ostał specjalnie aprojektowany do użycia w konstrukcji robota lub systemach nawigacji w celu wynacenia kierunku w którym porusa się robot. Układ wykorystuje sensory pola magnetycnego Philips KMZ5, które są wystarcająco cułe aby wykrywać pole magnetycne Ziemi. Wyprowadenia pinów modułu CMPS0: Rysunek 4 Płytka kompasu CMPS0 wra onaconymi wyprowadeniami Kompas elektronicny jest asilany napięcie +5V pry nominalnym użyciu prądu 5mA. Istnieją dwie możliwości odcytania informacji (aymutu magnetycnego) popre: sygnał PWM obecny na wyprowadeniu nr 4, protokół transmisji danych i c, wyprowadenia nr,. Sygnał PWM jest to sygnał prostokątny, którego stosunek casu trwania stanu wysokiego do okresu niesie informację o aymucie. Stan wysoki mienia się od ms (dla 0 O ) do 6,99ms (dla 59,9 O ) cyli o µs/ O. Stan niski utrymuje się pre 65ms więc okres wynosi odpowiednio 65ms+długośc stanu wysokiego. PWM jest generowany pre 6-bitowy timer co daje rodielcość µs, jednak nie jest wskaane pre producenta używanie sygnału PWM do uyskania rodielcości więksych niż 0, O (0 µs). Należy także pamiętać aby podcas używania wyjścia PWM piny i (SDA i SCL) podłącyć popre reystor podciągający o wartości około 47kΩ do +5V. Wyprowadenia nr i umożliwiają skorystanie protokołu i c do odcytania aymutu ale także powalają na preprowadenie testu kalibracji kompasu 6

17 Rysunek 5 Prebieg sygnału do komunikacji kompasem pre I C Protokół komunikacji kompasem jest analogicny jak komunikacji popularnymi pamięciami EEPROM 4C04. Najpierw wysyłany jest bit startu po nim 8-bitowy adres kompasu (0C0) niskim stanem bitu R/W, a następnie numery rejestrów które będiemy chcieli cytać. Później wysyłamy powtórony bit startu i po nim ponownie adres kompasu, tym raem bitem R/W ustawionym w stan wysoki (0C). Tera możemy już cytać jeden lub dwa bajty w ależności od tego cy cytam rejestr 8- bitowy cy 6-bitowy. Pry cytaniu 6-bitowego rejestru należy cytać bajt wysoki jako pierwsy. Moduł posiada 6 rejestrów, których dokładny opis amiescono poniżej: Nr rejestru Funkcja rejestru 0 Numer i seria oprogramowania Aymut apisany w postaci 8-bitowej licby 0-55 dla całego obrotu, Aymut apisany w postaci 6-bitowej licby dla 0,0 O -59,9 O 4, 5 Test wew. sygnał różnicowy sensora 6, 7 Test wew. sygnał różnicowy sensora 8, 9 Test wew. kalibracja sensora 0, Test wew. kalibracja sensora Nieużywane, cytane jako 0 Nieużywane, cytane jako 0 4 Nieużywane, cytane jako 0 5 Rejestr kalibracji wpisanie 55 do rejestru ropocyna procedurę Interfejs i c nie posiada reystorów podciągających, te powinny być podłącone do głównej syny interfejsu. Należy podłącyć je do obu linii SDA i SCL ale tylko po jednym na każdą linię. Proponuję użyć reystora o wartości k8 jeśli syna ma pracować cęstotliwością do 400kH ik jeśli cęstotliwością około MH. Kompas ostał aprojektowany do pracy cęstotliwością linii (SCL) około 00kH. Istnieje możliwość podniesienia cęstotliwości pracy nawet do MH jednak nie jest to efektywne ponieważ powyżej cęstotliwości 60kH mikrokontroler może nie być w stanie odpowiedieć na tyle sybko aby móc cytać cyny i c. Następnie należało sprawdić, cy odchylenie kompasu od poiomu ma wpływ na pomiar aymutu. W tym celu umiescono kompas na ramieniu wahadła, a kąt 7

18 wychylenia mieryłem enkoderem firmy AustriaMicroSystems AS5040. Wyniki pomiarów predstawiam na poniżsym wykresie: 50,0 00,0 aymut [stopnie] 50,0 00,0 50,0 00,0 50,0 0, wychylenie wahadła [stopnie] Wykres Zależność aymutu od wychylenia kątowego Jak widać, wychylenie ma nacny wpływ na pomiar aymutu. Dlatego kompas musi być umiescony na platformie, która będie w miarę możliwości utrymywała poiom. Poa tym należałoby sprawdić o ile można odchylić kompas od poiomu utrymując dokładność pomiaru aymutu na poiomie ± O. Dokonano tego w podobny sposób jak popredni pomiar tym raem jednak wychylając wahadło o nacnie mniejsy kąt. Wyniki tego pomiaru obrauje poniżsy wykres: 8

19 05 00 aymut [stopnie] wychylenie wahadła [stopnie] Wykres Zależność aymutu od wychylenia kątowego Jak widać uyskanie adowalających dokładności nawet ± O może stanowić spory problem ponieważ nawet pry małych wychyleniach wahadła wartość aymutu ulega nacnej mianie. Możliwe, że wpływ na to ma ciągła miana wychylenia kompasu ponieważ kiedy leży on nieruchomo to odcyty spełniają ałożone dokładności. Dokładniejse badanie tego problemu będie możliwe po uruchomieniu platformy utrymującej poiom.. Cujnik prechyłu firmy GEMAC[6] Firma GEMAC opracowuje i produkuje seroką gamę jedno- i dwuwymiarowych cujników prechyłu w technologii pojemnościowej orientowanych na najróżniejse aplikacje. Cujniki prechyłu oferowane są analogowym wyjściem napięciowym, prądowym lub cyfrowym wyjściem równoległym, wględnie w standardie seregowym RS lub CAN-bus. Wykonanie łącem CAN-bus posiada też 4 programowalne wyjścia, które mogą być używane również be łąca jako wyjścia sterujące. Inklinometry firmy GEMAC wykorystują jako sensor mikromechanicny element kremowy. Strukturę elementu mikromechanicnego pokauje djęcie mikroskopowe anaconą strefą struktury siatki, palca o serokości 9um i elektrody. 9

20 Rysunek 6 Struktura chipu elementu mikromechanicnego [6] Tego rodaju chip stanowi rodaj różnicowego kondensatora. W ależności od położenia kondensatora wględem kierunku siły pryciągania iemskiego awiesona elastycnie masa (elektroda wewnętrna) wpływa na mianę pojemności kondensatora prawie proporcjonalnie do wychylenia cujnika. Te nikłe miany pojemności pry asilaniu precyyjnym napięciem referencyjnym dają miany napięcia wychwytywane pre układ ASIC... Inklinometr GEMAC ISA0S0-0 Inklinometr służy do pomiaru odchylenia w akresie ±0 O rodielcością 0,0 O w jednej osi.. Seroki akres temperatur pracy -40 o C 85 o C, niski dryft temperaturowy <0,0 stopnia/k, nieliniowość niżsa niż 0, stopnia, pry odporności na udar o wartości 00g powalają na wsechstronne astosowania. Cujnik pobiera tylko - ma prądu asilania o wartości napięcia 5-0V. Inklinometr ma romiary mm i masę tylko 00g. Rysunek 7 Inklinometr GEMAC ISA0S0-0 0

21 Rysunek 8 Osie obrotu inklinometru.. Opis wyprowadeń Ponieważ inklinometr ostał aprojektowany jako rowiąanie dla premysłu więc jego łące jest solidnie wykonana. Jest ona wciskana i jednoceśnie prykręcana aluminiową nakrętką, co daje naprawdę solidnie połącenie i uniemożliwia jej prypadkowe odłącenie. Poniżej najduje się schematycny obra gniada umiesconego w obudowie inklinometru. PIN Prenacenie Zasilanie Vcc Wyjście dla osi Y Nie dostępne w tym modelu GND 4 Wyjście dla osi X 5 Napięcie odniesienia

22 .. Wynacanie charakterystyki Inklinometru Każdy producent najcęściej podaje charakterystykę swojego produktu. Jednak awse mając taką możliwość należy ją weryfikować w warunkach, w których cujnik będie używany. Wceśniej napisałem że, cujnik diała w akresie ±0 O dokładnością 0,0 O. Ponieważ inklinometr posiada wyjście napięciowe nacy to, że informacja o prechyleniu pretwarana jest na napięcie. W ależności od wychylenia jego wartość mienia się od 0 do 0V. Z tych informacji łatwo wywnioskować, że idealnie położenie poiome powinno odpowiadać napięciu równemu 5V. Nie mam możliwości wyskalowania inklinometru w ten sposób aby sprawdić, cy na pewno 5V to będie idealnie położenie poiome jednak mogę w prosty sposób sprawdić o ile mieni się wartość napięcia na wyjściu cujnika pry odchyleniu o 0,4 O. Do tego celu wykorystam enkoder amontowany na osi obrotowej silnika. Rysunek 9 Poglądowy schemat umiescenia inklinometru

23 Napięcie na wyjściu inklinometru [V] kąt BETA odchylenia inklinometru [stopnie] Wykres 4 Charakterystyka Inklinometru GEMAC ISA0S0-0

24 . Wynacanie momentu bewładności platformy utrymującej poiom [7] Moment bewładności układu punktów naywamy sumę momentów bewładności punktów wględem tej samej osi obrotu. n n J J i miri () i i Podielmy ciało stywne o masie M na n małych seścianów, o masach m i, i,,, n. Otrymam w ten sposób stywny układ punktów materialnych, gdy masy poscególnych seścianów są skupione w ich środkach mas. Będie to ocywiście prybliżeniem, gdyż seścianiki są rociągłe, ale prybliżeniem tym lepsym, im są one mniejse. Rysunek 0 Ilustracja do oblicenia momentu bewładności ciała stywnego a) podiał ciała stywnego na elementarne masy, b) prejście d elementu masy do elementu objętości Moment bewładności tego układu nie jest równy momentowi bewładności ciała stywnego, lec będie do niego dążył, gdy licba seścianików dążyć będie do nieskońconości Zatem momentem bewładności ciała stywnego jest granica momentu bewładności J u, gdy n J l im n n i m r Aby w łatwiejsy sposób oblicyć całkę należy prejść od całkowania po masie do całkowania po jego wymiarach. Należy wyraić element masy a pomocą elementu objętości. Mamy dm δdv, gdie δ jest gęstością ciała w danym punkcie. Wtedy wór na moment bewładności pryjmuje postać J r δdv (4) V Zastępując kolei element objętości elementami wymiarów liniowych: dv ddyd, otrymujemy J r δ ddyd (5) V i i M r dm () 4

25 5. Moment bewładności prowadnicy Aby uprościć sobie oblicenie momentu bewładności prowadnicy platformy mienię układ współrędnych kartejańskiego na cylindrycny. Ponieważ prowadnice mają kstałt wyciętego fragmentu walca, a oś obrotu pokrywa się osią walca wór na moment bewładności pryjmuje postać [ ] [ ] [ ] [ ] [ ] ( )( ) R R r dr r d r dr d r d dr d d dr r d d dr r r J R R R R R R R R V R R p ϕ ϕ δ ϕ δ ϕ δ ϕ δ ϕ δ ϕ δ ϕ δ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ (6) Ocywiście w tym momencie wymiary prowadnic są sacowane w recywistości mogą one ulec mianie niemniej chodi tutaj o prybliżone osacowanie momentu bewładności całej platformy. Rysunek Onacenie wymiarów prowadnicy platformy Na powyżsym rysunku poglądowym widać wsystkie dane potrebne do oblicenia momentu bewładności. Pryjąłem, że R 0,075m, R 0,05m, Φ π, 0, 0,0m. Zakładając, że prowadnice ostaną wykonane aluminium, którego

26 gęstość δ wynosi około 740 kg/m i podstawiając te dane do woru na moment bewładności walca otrymałem J p R δ 4 4 R 4 4 ( ϕ ϕ )( ) ( 0,075m) ( 0,05m) kg 740 m 4 4 kg 740 0, m m π 0,0m π 0,0m, π 0-4 kg m. Oblicanie momentu bewładności belki platformy Moment bewładności belki oblicę korystając współrędnych kartejańskich. Wór na moment ma postać J s V r d dy d δ (7) Zakładając na pocątku, że oś obrotu pokrywa się osią symetrii belki 0Z ora wymiary belki wynosą, y,, co predstawione jest na poniżsym rysunku oblicenie momentu wygląda następująco Rysunek Onacenie wymiarów belki platformy 6

27 7 y r + ( ) ( )[ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] ( ) ( ) [ ] y y y y y y y y y y d y d y dy y d dy d dy y d d y dy d J y y y y y y y y y y y y y y y y s δ δ δ δ δ δ δ δ δ δ δ Znając moment bewładności belki wględem osi prechodącej pre jej środek masy i nając położenie środka masy mogę w prosty sposób oblicyć moment bewładności J belki platformy wględem osi równoległej do popredniej osi, leżącej w odległości a od niej. Do tego celu posłużę się twierdeniem Steinera Rysunek Ilustracja do twierdenia Steinera Weźmy dowolny element dm masy danego ciała. Moment bewładności J ciała wględem osi równoległej do osi prechodącej pre środek masy wyraża się worem (.). Według rysunku mogę napisać ( ) y a a y a r (8) Pry cym i y są współrędnymi elementu masy w układie współrędnych o pocątku w środku masy

28 r + y Wobec tego sukany moment bewładności wynosi (9) J M ( a + a + r ) dm a dm + a dm + r dm (0) M M M a ponieważ aconst, więc J M a + a dm + r dm () M M Współrędna środka masy wyrażona jest worem s M M dm () Jeżeli pocątek układu współrędnych pokrywa się e środkiem masy, to s 0, cyli Ponadto M dm 0 () r dm J s (4) M jest momentem bewładności belki wględem osi prechodącej pre środek masy, ostatecnie otrymuję J b Ma + J s (5) Powyżsy wór nosi nawę twierdenia Steinera. Należy jesce wyraić element masy M a pomocą elementu objętości. Do tego celu wykorystam wór M M δ V V δ (6) Podstawiając go do woru na moment bewładności otrymałem b δ V a J s (7) J + Ostatecnie wór na moment bewładności J b belki ma postać 8

29 δ J b δ y a + δ y a + [ y + y ] [ y + y ] (8) Podstawiając do woru δ 740 kg/m, 0,0m, y0,0m, 0,m, a także a0,05m otrymuję moment bewładności równy kg J b 740 0,m ( ) [( ) ( ) ] 0,0m 0,0m 0,05m + 0,0m 0,0m + 0,0m 0,0m m kg ,m 0, m [ 0, m 0, m ] + + m kg ,m 0, m [ 0, m 0, m ] + + m kg ,m( 0, m ) 6,705 0 kg m m Umiescenie cujników na platformie wymaga policenia momentów bewładności dla każdego cujnika. Jednak dla cujników, których moment bewładności jest nacnie mniejsy od momentu bewładności całej platformy można pominąć taki sposób postępowania ponieważ ich umiescenie nie będie miało nacącego wpływu na achowanie platformy. Sposób oblicania momentu bewładności jest analogicny jak dla belki platformy. Korystając twierdenia Steinera mogę be problemów wylicyć moment bewładności dowolnego cujnika. Najważniejse jest aby nać w prybliżeniu jego wymiary, masę ora odległość jaką będie on umiescony od osi obrotu. Umiescenie na platformie cujnika prechyłu policenia jego momentu bewładności ponieważ waży on około 0,kg. Jego wymiary to 0,05m. y0,055m, 0,06m. Stosując wór (8) J c δ y a M My Ma + + 0,kg 0,0005m 0,kg 0,0005m + M [ y + y ] y a + [ y + y ] y 0,kg 0,00065m 0,kg 0,009065m + + 0,kg 0,00065m 0,kg 0,009065m + + 0, kg m 8, kg m Mając oblicone już najważniejse momenty bewładności należy godnie twierdeniem Steinera sumować je do siebie aby uyskać moment bewładności całej platformy wra cujnikami. 9

30 J J + J + J, π 0 p 8, , b kg m kg m c 7, , kg m -4 kg m -4 kg m + 6, , kg m + -4 kg m + Mając oblicony moment bewładności całego modułu mogę osacować moment obrotowy silnika, jaki będie potrebny do wprawienia platformy w ruch. Do tego celu będę jesce potrebował maksymalnego pryspiesenia kątowego jakie uyskuje robot podcas ropędania się lub hamowania. Najwiękse wartości pryspieseń kątowych występują w casie stopu awaryjnego [] Sacunkowo potrafią one sięgać chwilami nawet 500 O /sec. Aby oblicyć moment obrotowy potrebny do nadania platformie takich pryspieseń posłużę się drugą asadą dynamiki dla ruchu obrotowego wokół osi stałej. Brmi ona następująco: Dla ruchu obrotowego ciała stywnego wokół osi stałej moment układu sił diałających na ciało licony wględem osi obrotu jest równy ilocynowi momentu bewładności ciała wględem osi obrotu i pryspiesenia kątowego, gdie M moment obrotowy J moment bewładności ε pryspiesenie kątowe M J ε (9) Podstawiając wceśniej oblicone i osacowane wartości otrymałem, że moment obrotowy odniesiony do osi obrotu powinien wynosić około, Nm. Jednak ten moment obrotowy byłby koniecny do uyskania pryspieseń w układnie inercyjnym. Natomiast wóek RoBik i umiescona na nim platforma są układem nieinercyjnym. W tym wypadku moment bewładności diała na nasą koryść. Jeżeli wahadło acnie się porusać, to siły tarcia występujące w prowadnicach będą wprawiały w ruch platformę prenosąc cęść momentu obrotowego, którego wartość rośnie proporcjonalnie do sił tarcia. Im więksy będie moment bewładności platformy tym mniejsy wpływ na achowanie platformy będie miał moment obrotowy prenosony pre prowadnice. Aby osacować wartość momentu obrotowego koniecnego do utrymania platformy w poycji pionowej, wykorystam program MSC Adams. 0

31 4. Model RoBika i platformy w programie Adams MSC Adams jest programem do interaktywnej symulacji ruchu. Podprogramy Adams View, Adams Solver, Adams Postprocesor dają użytkownikowi możliwość preniesienia więksości elementów najważniejsych systemów CAD, lub wprowadenia samemu własnego modelu mechanicnego. Pełna paleta połąceń pregubowych, translacyjnych, stałych, momentów sił, astosowania napędów umożliwia budowanie modelu mechanicnego naprawdę bliżonego do recywistego. Kiedy wprowadony wirtualny model jest kompletny program Adams sprawda go pod kątem kinematyki, i dynamiki ora poprawności poscególnych połąceń, a następnie preprowada równocesne symulacje kinematycne, dynamicne, statycne. Reultaty symulacji są dostępne w postaci wykresów, plotów, raportów, a także prebieg symulacji można obserwować na żywo, ponieważ na ekranie monitora widać jak achowuje się model. 4. Robot RoBik w programie Adams Wirtualny model powinien być jak najbardiej bliżony do recywistego robota. Program daje możliwość wprowadania wymiarów bryły, masy, wyboru materiału jakiego jest on wykonany i jesce wielu innych właściwości. Budowanie modelu polega właśnie na tworeniu takich brył i łąceniu ich innymi popre różne jointy rotacyjne, translacyjne, stałe w wirtualny model.

32 Rysunek 4 Model RoBik-a wra platformą wykonany w programie ADAMS W poniżsej tabeli amiescone są wymiary i masy, według których tworyłem model wirtualnego RoBika[8]: wielkość fiycna wartość jednostka Masa jednego koła,9 [kg] Wymiar jednego koła 5,0 [cm] Masa korpusu RoBika Belka Wahadło Wymiary RoBika Belka Wahadło Wymiar prowadnicy korpusu,7 8,06,6 5 0,4 7,6,0 we współ. cylindrycnych [kg] [kg] [cm] [cm],5 π,0 [cm] [ o ] [cm] Wymiar belki platformy 0,0,0,0 [cm] [cm] [cm] Wymiar cujnika 5,5,5 6,0 [cm] [cm] [cm] Masa cujnika 0, [kg]

33 Rysunek 5 Widok platformy Masy jednorodnych brył, które składają się na platformę nie są nane. Nie stanowi to problemu ponieważ w programie Adams można wybrać materiał jakiego będą wykonane poscególne bryły. Według mnie aluminium będie najlepsym rowiąanie e wględu na prostotę obróbki, a także gęstość. Stosunkowo duże elementy wykonane aluminium nie są ciężkie, a są wystarcająco trwałe. Ocywiste jest, że moment bewładności rośnie wra masą platformy, jednak należy pamiętać, że wtedy też więksa się moment obrotowy jaki treba będie dostarcyć aby utrymać platformę w pionie. 4. Symulacje achowania RoBik-a umiesconą platformą Podcas symulacji koła są na stałe prymocowane do podłoża i nie obracają się. W ruch wprawiane jest jedynie wahadło Robika popre pryłożenie siły. Wartość siły nie jest stała i pojawia się w różnych odstępach casu co powoduje różne miany wychylenia wględem wektora grawitacji, a także mianę prędkości kątowej.

34 Wykres 5 Siła pryłożona do wahadła Robika wymusająca wychylenie kątowe Rysunek 6 Schematycny rysunek RoBika platformą anaconym kątami Prebieg wychylenia kątowego wahadła α wględem wektora grawitacji, anaconego cerwoną strałką na powyżsym rysunku mogłem w bardo prosty sposób uyska jako wynik symulacji. Program Adams oblica także prędkość kątową 4

35 wahadła, dięki cemu mogłem porównać ją prędkościami jakie uyskiwał recywisty model. Wykres 6 Odchylenie kątowe α w casie Wykres 7 Prędkość kątowaα& w casie 5

36 Wykres 8 Pryspiesenie kątowe α& & w casie Porównując wyniki symulacji otrymanych w programie Adams wynikami pomiarów położenia i prędkości awartymi w pracy magisterskiej[] stwierdam, że rąd wartości jest taki sam i nie odbiegają one nacnie od siebie. Takie wyniki powalają sadić, że wirtualny model jest bliżony do recywistego i dalse wyniki symulacji będą odwierciedlały wartości mogące występować w recywistości. Do sterowania platformą aproponowałem sterownik PD ponieważ jego implementacja w programie Adams jak i w recywistości nie będie trudna. Wsystkie dane potrebne do wysterowania będę w casie recywistym otrymywał cujników umiesconych na platformie jaki w robocie. Prędkość kątowa będie odcytywana żyroskopu umiesconego w osi obrotu wahadła, natomiast wychylenie kątowe wahadła otrymam całkując prędkość po casie. Natomiast położenie platformy będie odcytywane na bieżąco enkoderów umiesconych w napędach platformy. 6

37 α& S α PD β s + a β & Powyżsy schemat predstawia sterownik wykonany w programie Adams. Wyjście PD jest podłącone bepośrednio do elementu wymusającego ruch platformy, cyli silnika. W wirtualnym modelu amiast silnika wykorystywany jest moment obrotowy jaki powinien ostać pre niego wygenerowany w danym momencie casu aby wychylnie platformy od pinu było jak najmniejse. W recywistości będie to np. wypełnienie sygnału PWM sterującego prędkością obrotową silnika. Wykres 9 Prędkość kątowa platformy wględem wahadła 7

38 Wykres 0 Odchylenie kątowe platformy β od poycji pionowej dla P, D Wykres Wartości momentu obrotowego koniecnego do utrymania platformy w poycji pionowej dla P, D Moment obrotowy silnika, którego oś obrotu będie pokrywała się osią obrotu platformy musi wynosić minimum 0, Nm. Jednak odchylenie platformy od pionu jest byt wielkie i wynosi w najgorsym wypadku aż 5 O dlatego treba więksyć nastawy regulatora. Preprowadę tera symulację dla nastaw P0, D. 8

39 Wykres Odchylenie kątowe platformy β od poycji pionowej dla P0, D Wykres Wartości momentu obrotowego koniecnego do utrymania platformy w poycji pionowej dla P0, D Zmiana nastaw regulatora nacnie poprawiła achowanie się platformy. Symulacje pokaały, że kąt odchylenia platformy od pionu w skrajnym prypadku, kiedy prędkość kątowa wahadła była najwięksa, wyniósł 0, O co stanowi bardo dobry wynika. Należy tutaj wróci jesce uwagę, że moment obrotowy koniecny do utrymania platformy w pionie mniejsył się o połowę. Według mnie dieje się tak 9

40 dlatego, że regulator sybciej reaguje na mianę położenia platformy. Zwięksone nastawy spowodowały mniejsenie odchylenia od pionu ponad 0-cio krotnie, a co a tym idie nie potreba aż tak dużego momentu aby prywrócić platformę ponownie do poycji pionowej. Nastawy regulatora P0, D dają bardo dobre wynik jednak mając takie narędie jak Adams można be najmniejsego problemu preprowadić symulację i porównać wyniki dla prypadku kiedy mianie ulegnie materiał jakiego ostaną wykonane np. prowadnice. Dla wceśniejsych pomiarów pryjąłem, że będą one wykonane aluminium o gęstości 740 kg/m. W tym prypadku mienię materiał na stal o gęstości 7080 kg/m. Chcę sprawdić jak miana ta wpłynie na wygląd momentu obrotowego. Z jednej strony powinien się on mniejsyć ponieważ wrośnie bewładność całej platformy co spowoduje, że mniejsy się wpływ ruchów wahadła. Z drugiej jednak strony do kompensowania ewentualnych wychylenie treba będie pryłożyć już więksy moment obrotowy. Wykres 4 Odchylenie kątowe platformy β od poycji pionowej dla P0, D dla prowadnic wykonanych żelaa Po preprowadeniu symulacji widać, że nienacnie mniejsył się kąt β dlatego aby lepiej móc aobserwować tę mianę predstawię różnicę międy wartościami kątów β dla prowadnic wykonanych aluminium i prowadnic wykonanych żelaa. 40

41 Wykres 5 Różnica międy kątami β dla prowadnic wykonanych żelaa i aluminium Jak widać na powyżsym wykresie kiedy wahadło nie porusa się to odchylenie platformy od pionu jest mniejse, jednak kiedy ropocyna się ruch wahadłowy to widać, że różnica premiennie więksa się i mniejsa. Podobnie wygląda wykres różnicy momentów obrotowych Wykres 6 Różnica międy momentami obrotowymi dla prowadnic wykonanych żelaa i aluminium 4

42 Powyżse doświadcenie pokaało, że miana momentu bewładności platformy podwójny wpływ na jej achowanie. Z jednej strony gdy prędkość kątowa wahadła oscyluje wokół era to odchylenie wahadła od pionu jest mniejse, jednak jeżeli prędkość kątowa nacenie wrośnie reakcja sterownika na mianę położenia ulega opóźnieniu i stąd biorą się apewne te różnice pomiędy kątami β. Jest ot ciekawy prykład na to jak miana momentu bewładności platformy ma pływ na sterowanie nią. Jednak symulacje preprowadone w Adamsie miały na celu osacowanie jaki moment obrotowy powinien mieć silnik, który będie użyty do napędenia platformy. Silnik powinien spełniać dwa podstawowe wymagania: jego prędkość obrotowa nawet po astosowaniu prekładni musi być więksa od prędkości kątowej jaką uyskuje wahadło moment obrotowy musi powinien być więksy niż maksymalny moment pojawiający się w symulacjach. Dane sprętowe jakie ma robot mobilny RoBik nakładają na pierwse kryterium ogranicenie prędkości kątowej do 00 O /s ponieważ są to maksymalne prędkości jakie można mieryć dięki amontowanemu żyroskopowi. Z preprowadonych symulacji wynika, że moment jaki musi mieć silnik nie może być mniejsy niż 0,Nm. W recywistym wahadle badawcym astosowane ostało Serwo modelarskie firmy Hitec model HS-. Jego prędkość kątowa pry asilaniu 6V to około 400 O /s. Jednak chwilowo można nawet asilać je napięciem 7,V co daje prędkości dochodące nawet do 480 O /s. Pierwse kryterium ostało spełnione nawet dużą reerwą. Moment obrotowy dla napięcia 6V wynosi,7 kg cm, co w preliceniu na Nm daje wynik 0,6 Nm. Ocywiście chwilowe asilenie serwa napięciem 7,V spowoduje dodatkowy wrost momentu obrotowego lec uważam, e nie będie to koniecne ponieważ oba powyżej kryteria są spełnione dużym apasem. 4

43 5. Wstępne ałożenia dotycące sterowania platformą utrymującą poiom Aby apewnić odpowiednie sterownie platformą należy prede wsystkim mieć informację o ile od pionu wychyliło się wahadło robota. Informację taką można uyskać wykorystując żyroskop firmy Murata Gyrostar ENC-0J, który jest na wyposażeniu cujników robota RoBik. Miery on w casie recywistym prędkość kątową wahadła i pretwara ją na napięcie mieniające się w akresie 0,5-4,75V. Mając prędkość kątową w prosty sposób można wylicyć położenie kątowe. Jednak rowiąanie takie niesie e sobą pewne niebepieceństwo. Otóż każda operacja całkowania prędkości i wylicenia dięki temu położenia obarcona jest błędem. Jeśli całkujemy prędkość w identycnych odstępach casu należy sumować e sobą wychylenia kątowe. Takie diałanie powoduje, że ewentualne błędy pojawiające się w wyliceniach będą sumowały się e sobą i po pewnym casie pomiar będie tracił na dokładności. Abym mógł aproponować jak najlepse sterowanie platformą, jak najdokładniejsy pomiar położenia budowałem wahadło badawce. Na jego końcu umiescony jest silnik, którego daniem będie preciwdiałanie wychyleniu od pionu drugiego ramienia wahadła, które będie symulowało platformę. Wykres 7 Widok wahadła testowego Wahadło wyposażyłem na pocątek w następujące cujniki: dwa enkodery AustriaMicroSystems AS5040 [9], których jeden jest umiescony w punkcie acepienia dłużsego ramienia wahadła, a drugi w silniku na osi obrotowej drugiego ramienia wahadła, 4

44 żyroskop Analog Devices ADXRS00 umiescony w osi obrotu dłużsego ramienia. [0] Zdecydowałem się na takie romiescenie cujników następujących powodów: ) dięki enkoderom umiesconym na osiach obrotów ramion wahadła będę mógł wylicyć błąd odchylenia od pionu krótsego ramienia wahadła, ) umiescenie na tej samej osi obrotu żyroskopu i enkodery powoli mi oblicyć błąd jaki wkradł się w wylicenie położenia wahadła. 5. Implementacyjne roważania na temat oblicenia położenia kątowego wahadła. Jak wiadomo, jeśli na wahadło diałają jakieś bliżej nieokreślone siły ewnętrne, np. opór powietra lub siła tarcia to kąt wychylenia wahadła będie się mniejsał aż to całkowitego atrymania się wahadła. Teoretycnie wykres prędkości kątowej w casie można porównać sinusoidą, której ma amplituda w każdym okresie się mniejsa. Wahadło ma najwięksą prędkość kątową kiedy jego ramie jest skierowane równolegle do wektora grawitacji. Wykres prędkości kątowej badanego wahadła predstawiony jest poniżej. 00 prędkośc kątowa (stopnie/s) ,00,00 4,00 6,00 8,00 0,00,00 4, cas (s) Wykres 8 Prędkość kątowa wahadła w casie Położenie wahadła jest równe polu powierchni pod wykresem. Problemem jest to, e oblicenia położenia musę dokonać w casie recywistym co pryspara sporych problemów. Aby tego dokonać prede wsystkim treba prędkość kątową odcytywać w określonych i stałych odstępach casu. Do tego celu musę wykorystać prerwanie cyklicne. Jednak warunkiem koniecnym pry obsłude prerwania jest jak najsybse dokonanie tego. Chodi mi o to, żeby akońcyć 44

45 pomiar prędkości i aktualiację położenia anim ostanie wygenerowanie kolejne prerwanie cyklicne. Uniemożliwia to dokonanie bardo skomplikowanych obliceń. Kolejnym problemem jest ogranicenie sprętowe. Mikrokontroler, którego używam nie posiada jednostki miennoprecinkowej co wymusa góry operowanie na licbach całkowitych. Rysunek 7 Powięksony fragment wykresu prędkości kątowej w casie Powyżej predstawiony jest fragment wykresu prędkości kątowej w casie, który posłuży mi do predstawienia sposobu oblicenia położenia wahadła. Jak już wceśniej napisałem położenie wahadła jest równe polu powierchni pod krywą. W ależności od kierunku ruchu położenie będie więksane lub mniejsane. Należy ropatryć 6 prypadków które mogą aistnieć podcas wylicania położenia. >; >0,>0 II) <; >0,>0 45

46 46 pole poloenie poloenie t t pole + + ) ( pole poloenie poloenie t t pole + + ) ( <; <0,<0 pole poloenie poloenie t t pole + ) ( >; <0,<0 pole poloenie poloenie t t pole + ) ( <; >0;<0 ) ( ) ( pole poloenie poloenie t pole pole poloenie poloenie t pole + >; <0;>0 ) ( ) ( pole poloenie poloenie t pole pole poloenie poloenie t pole +

47 Mając już wyprowadone wsystkie potrebne do wylicenia położenia równania chciałem sprawdić jakim błędem będą obarcone oblicenia. Do tego celu koniecne było presłanie wartości prędkości kątowych wahadła odcytanych cęstotliwością około 40H do komputera. Zdecydowałem się na takie rowiąanie ponieważ na pocątku uniknę problemu licbami ułamkowymi. Fragment kodu napisanego w jęyku C i oblicającego położenie predstawiłem poniżej: vkat >> predkosc[]; //pobranie wartości prędkości kątowej mieronej pre //pretwornik ADC predkosc[]((predkosc[]*0,00484)/0,005); //prelicenie prędkości mieronej //pre ADC na prędkość podawaną //w O /s do{ if (predkosc[]>predkosc[0]){ //Prypadek I if ((predkosc[0]>0) && (predkosc[]>0)) { pole(predkosc[]-predkosc[0])*0.5* predkosc[0]* ; poloenie(poloenie+pole); }; //Prypadek IV If ((predkosc[0]<0) && (predkosc[]<0)){ pole(predkosc[0]-predkosc[])*0.5* predkosc[]* ; poloenie(poloenie+pole); }; //Prypadek VI If ((predkosc[0]<0) && (predkosc[]>0)){ pole(predkosc[0]*predkosc[0]* ) /(*(predkosc[]-predkosc[0])); poloenie(poloenie-pole); pole(predkosc[]*predkosc[]* )/(*(predkosc[]- predkosc[0])); poloenie(poloenie+pole); }; }; if (predkosc[]<predkosc[0]){ //Prypadek II if((predkosc[0]>0) && (predkosc[]>0)){ pole(predkosc[0]-predkosc[])*0.5* predkosc[]* ; poloenie(poloenie+pole); }; //Prypadek III if((predkosc[0]<0) && (predkosc[]<0)){ pole(predkosc[]-predkosc[0])*0.5* predkosc[0]* ; poloenie(poloenie+pole); }; //Prypadek V if((predkosc[0]>0) && (predkosc[]<0)){ pole(predkosc[0]*predkosc[0]* )/(*(predkosc[0]- predkosc[])); poloenie(poloenie+pole); pole(predkosc[]*predkosc[]* )/(*(predkosc[0]- predkosc[])); poloenie(poloenie-pole); }; }; predkosc[0]predkosc[]; 47

48 vkat >> predkosc[]; predkosc[]((predkosc[]* )/0.005); }while(!vkat.eof()); 5. Wynacanie położenia pry użyciu żyroskopu Mając już pewne teoretycne podłoże należy ocywiście sprawdić jak takie rowiąanie achowuję się w recywistości. W tym celu wahadło wprawiłem w ruch i mieryłem prędkość kątową, a także aby móc weryfikować błąd jakim oblicam położenie odcytywałem je korystając enkodera. 60 wychylenie kątowe [stopnie] żyroskop encoder -60 0,0,0,9,9,9 4,9 5,8 6,8 7,8 8,8 9,7 0,7,7 cas[s] Wykres 9 Zależności położenia w casie oblicone dla encodera i żyroskopu 48

49 6,0 5,0 4,0,0 bład położenia [stopnie],0,0 0,0 -,0 -,0 -,0-4,0-5,0 0,0 0,7,,0,6, 4,0 4,6 5, 6,0 6,6 7, 7,9 8,6 9, 9,9 0,6,,9,6,,9 cas [s] Wykres 0 Błąd położenia obliconego żyroskopu Jak widać na powyżsym wykresie błąd oblicenia położenia waha się od -4 O do prawie 5 O. Jest on duży i może on być spowodowany pewną bewładnością żyroskopu i opóźnioną casem reakcji na amianę prędkości. To presunięcie doskonale widać na wykresie predstawiającym mały wycinek casu, w którym nastąpiło wychylenie. wychylenie kątowe [stopnie] ,7,8,8,9,9,9,0,0,0,,,,,,,,,4 żyroskop encoder -50 cas [s] Wykres Wykres predstawiający presunięcie casowe sygnałów 49

50 Kolejnym problemem jaki występuje jest błąd pretwornika ADC. Kiedy wahadło jest w stanie spocynku co jakiś cas, upełnie prypadkowy, pojawiają się odcyty wskaujące na to, że wahadło jest w ruchu. Automatycnie ma to wpływ na mianę położenia. Jeśli wahadło będie najdowało się w stanie spocynku pre dłużsy okres casu to błąd wględem recywistego położenia będie nacny. Aby apobiegać takiej sytuacji najcęściej stosuje się rowiąanie polegające na dokonaniu n pomiarów w jednym cyklu, sumowaniu wyników, odjęciu od sumy ma. i min. wartości mieronych, ora podieleniu sumy pre n-. 0,5 wychylenie kątowe [stopnie] 0-0,5 - -,5 - -,5 - -,5-4 -4,5 0,0,0,, 4, 5, 6, 7, 8, 9, 0,4,4,4,5 cas [s] Wykres Błąd położenia wahadła w spocynku n4 - I próba n4 - II próba n4 - II próba n6 - I próba n6 - II próba n6 - III próba n8 - I próba n8 - II próba n8 - III próba n - I próba n - II próba n - III próba Jak widać na powyżsym wykresie więksenie licby pomiarów w jednym cyklu nacnie mniejsa wpływ błędu pretwornika ADC na położenie. Najlepsym rowiąaniem byłoby ocywiście dokonanie jak najwięksej licby pomiarów w jednym cyklu jednak należy pamiętać o kompromisie pomiędy dokładnością pomiaru i pojawianiem się błędu pretwornika ADC, a casem trwania jednego cyklu pomiarowego. Nie może on trwać byt długo ponieważ mikroprocesor może nie być w stanie wykonać innych operacji i obsłużyć innych prerwań. Ja pryjąłem, że licba pomiarów w jednym cyklu będie wynosiła n8. Jednak występowanie tego błędu będie niwelowane dięki cujnikowi prechyłu. 5. Implementacja algorytmu oblicania położenia w mikrokontrolere. Jak pokaały wceśniejse symulacje algorytm oblicania położenia żyroskopu diałał dobre i dawał adowalające wyniki. Jednak był on testowany na komputere. Recywiste dane pomiarowe ebrane pre mikrokontroler były presyłane i program napisane w jęyku C wylicał położenie. 50

51 Implementacja tegoż algorytmu w mikrokontrolere wymagała wprowadenia w nim pewnych mian. Należało maksymalnie uprościć prebieg obliceń, a także reygnować używania licb miennoprecinkowych. Zapewne ten drugi wymóg będie miał wpływ na mniejsenie dokładności obliceń, a co a tym idie różnica międy położeniem wynaconym, a recywistym będie się więksała. predkatowa[](vkat()-polkalgyro)*96; //predkatowa katowa w stopniach/sekunde predkatowa[]predkatowa[]/00; if (predkatowa[]>predkatowa[0]){ if(predkatowa[0]>0 && predkatowa[]>0){ pole(predkatowa[]-predkatowa[0])*5+predkatowa[0]*0; polepole/0000; wychwahadlau(wychwahadlau+pole); }; if((predkatowa[0]<0) && (predkatowa[]<0)){ pole(predkatowa[0]-predkatowa[])*5+predkatowa[]*0; polepole/0000; wychwahadlau(wychwahadlau+pole); }; if((predkatowa[0]<0) && (predkatowa[]>0)){ pole(predkatowa[0]*predkatowa[0]*0)/(*(predkatowa[]-predkatowa[0])); polepole/0000; wychwahadlau(wychwahadlau-pole); }; }; pole(predkatowa[]*predkatowa[]*0)/(*(predkatowa[]-predkatowa[0])); polepole/0000; wychwahadlau(wychwahadlau+pole); if (predkatowa[]<predkatowa[0]){ if((predkatowa[0]>0) && (predkatowa[]>0)){ pole(predkatowa[0]-predkatowa[])*5+predkatowa[]*0; polepole/0000; wychwahadlau(wychwahadlau+pole); }; if((predkatowa[0]<0) && (predkatowa[]<0)){ pole(predkatowa[]-predkatowa[0])*5+predkatowa[0]*0; polepole/0000; wychwahadlau(wychwahadlau+pole); }; if((predkatowa[0]>0) && (predkatowa[]<0)){ pole(predkatowa[0]*predkatowa[0]*0)/(*(predkatowa[0]-predkatowa[])); polepole/0000; wychwahadlau(wychwahadlau-pole); } }; pole(predkatowa[]*predkatowa[]*0)/(*(predkatowa[0]-predkatowa[])); polepole/0000; wychwahadlau(wychwahadlau+pole); if (wychwahadlau>0000) wychwahadlau(wychwahadlau-(((wychwahadlau/0000)*0000); if ((wychwahadlau<-0000) (wychwahadlau(wychwahadlau-(((wychwahadlau/0000)*0000); 5

52 W pierwsym etapie oblicana jest prędkość kątowa wahadła. Od aktualnego pomiaru prędkości odejmowana jest tw. wartość kalibrowana, cyli wartość napięcia jaka była na wyjściu żyroskopu kiedy wahadło najdowało się w stanie spocynku. Następnie jest ona mnożona pre wartość 96 dlatego, że prędkość mierona jest w cyklicnych prerwaniach o cęstotliwości około96h. Następnie sereg warunków koniecnych do spełnienia wybiera sposób oblicenia położenia. W końcowej faie najduje się rowiąanie, którego wprowadenia wymagała niemożność użycia licb mienno precinkowych. Wychylenie wahadła jest podielone na ceść ułamkową i całkowitą. Prekrocenie pre ceść ułamkową wartości lub 0000 skutkuje odpowiednio mianą cęści całkowitej wychylenia wahadła o licbę stopni obliconą e woru wychwahadlau/0000.,5 błąd oblicenia położenia stopnie],5 0,5 0-0,5 - -,5 - -, cas [ s] Wykres Błąd oblicenia położenia użyciem licb ułamkowych i całkowitych Powyżsy wykres predstawia błąd jaki jest wprowadony do pomiaru popre niemożność korystania licb ułamkowych. Jak widać nie jest on stały tylko w casie ruchu wahadła ulega mianie. Powodowany jest tym, e każdym obliceniem położenia, co ma miejsce około 96 ray na sekundę, tracona jest jakaś wartość wychylenia w procesie dielenia licb całkowitych. Błąd ten będie miał nacący wpływ na sterowanie położeniem wahadła jednak nie można go wyeliminować. Jedynym rowiąaniem mogącym prynieść poprawę może być ponowne opracowanie sposobu oblicania położenia prędkości korystając tylko licb całkowitych. Kolejnym utrudnieniem jest sam odcyt wychylenia wahadła enkodera. Ponieważ odbywa się on popre sygnał PWM, dlatego dla maksymalnego możliwego wypełnienia, cyli położenia 59,9 O różnica wartości timera achowanego na bocu rosnącym i malejącym wynosi 480. Z prostych obliceń wynika, że teoretycnie 5