Łukasz Jakóbiec Jakub Kucharski

Łukasz Jakóbiec Jakub Kucharski Cubot przystępna platforma robotyczna 1

Spis treści 1. Wstęp...2 1.1 Rys historyczny. Malejące ceny i rosnąca dostępność technologii a rozwój robotyki...3 2. Założenia i podstawy teoretyczne...10 2.1 Problematyka robotów prowizorycznych...10 2.2 Założenia projektowe...11 2.3 Sterowanie dyferencjalne...11 2.4 Czujnik odległości...12 3. Opis robota...14 3.1 Prototypowanie...14 3.2. Opis konstrukcji mechanicznej...16 3.2.1 Opis konstrukcji obudowy...16 3.2.2 Koła...17 3.2.3 Serwa napędowe i sposób modyfikacji serwa modelarskiego...18 3.2.5 wyprowadzenie programotora...22 3.3 Opis elektroniki...22 3.3.1 Mikrokontroler...23 3.3.2 Komunikacja bluetooth...25 3.3.3 Oprogramowanie mikrokontrolera...25 3.4 Oprogramowanie wizualizujące odczyt z czujników podczerwieni...26 3.5 Koszty części składowych...29 4. Uwagi końcowe...30 4.1 Wnioski...30 4.2 Podziękowania...31 4.3 Bibliografia...31 4.4 Załączniki...31 4.4.1 Program wizualizujący odczyty z czujnika podczerwieni...31 4.4.2 Program mikrokontrolera...37 1. Wstęp Największym kłopotem specjalizacji robotycznych na polskich wyższych uczelniach informatycznych są wysokie koszty zakupu gotowych rozwiązań robotycznych. Wychodząc od tego zagadnienia autorzy postanowili zaprojektować i zbudować robota o jak największej funkcjonalności przy zachowaniu niskiej, przystępnej ceny jego wytworzenia. 2

1.1 Rys historyczny. Malejące ceny i rosnąca dostępność technologii a rozwój robotyki Robotyka to stosunkowa młoda dziedzina nauki zajmująca się projektowaniem, wytwarzaniem i wdrażaniem technologi robotycznych. Od zajmujących się nią wymaga wiedzy z zakresu mechaniki, elektroniki oraz programowania, jak również bardzo szerokiego wachlarza tematów towarzyszących od psychologi zaczynając, na metodach matematycznych zbiorów rozmytych kończąc. "Robot jest to urządzenie techniczne przeznaczone do realizacji niektórych funkcji manipulacyjnych i lokomocyjnych człowieka, posiadające określony poziom energetyczny, informacyjny oraz poziom tzw. inteligencji maszynowej. Inteligencja maszynowa to autonomia działania w pewnym środowisku"1. Uważa się, ze historia robotyki sięga czasów starożytnych Archytasowi z Tarentu przypisuje się stworzenie mechanicznego gołębia napędzanego silnikiem parowym już 400 lat p.n.e. Arystoteles 322 lata p.n.e pisał o tworzeniu maszyn mogących działać niezależnie od ludzi. Wczesne zegary wodne i klepsydry są uważane za pierwsze przejawy nauk robotycznych w naszym świecie. Pod koniec piętnastego wieku Leonardo Da Vinci podjął próbę zaprojektowania mechanicznego rycerza(rysunek 1). 1 Teoria robotyki, dr inż. Tomasz Buratowski 3

Rysunek 1: Rekonstrukcja projektu mechanicznego rycerza wg Leonardo da Vinci Pojecie "ROBOT" w literaturze zostało użyte po raz pierwszy w sztuce czeskiego pisarza Karela Capka R.U.R w roku 1920. Słowo "robot" w języku czeskim oznacza służbę przymusową. Terminu "Robotyka" użył pierwszy raz w 1942 Isaac Asimov w swoim opowiadaniu "Zabawa w berka". W latach czterdziestych XX wieku stworzono maszyny, takie jak Z1, Z2, ENIAC uważane dziś za pierwsze komputery. Powoli to co wcześniej było tylko fikcją literacka i filmową, dzięki postępującej miniaturyzacji, stawało się możliwe do zrealizowania. 4

Rysunek 2: Projekt układu scalonego Jacka Kilby'ego W 1959 roku Jack Kilby i Robert Noyce wynaleźli układ scalony. Wynalazcy pracowali niezależnie od siebie. Produkcja podzespołów stawała się coraz tańsza W 1961 roku w fabryce General Motors rozpoczął prace pierwszy robot przemysłowy nazwany Unimate. Rysunek 3: Pierwszy manipulator w fabryce General Motors 5

Pierwszy robot mobilny zdolny do wnioskowania na temat swojego otoczenia, Shakey, powstał w 1970 w Instytucie Stanforda. Na pokładzie robota Shakey znalazło się czujniki laserowe, kamery telewizyjne oraz czujniki zderzeniowe, które ułatwiały mu nawigacje. W 1965 roku, Gordon Moore, założyciel firmy Intel ustanowił prawo, znane później jako prawo Moora: "ekonomicznie optymalna liczba tranzystorów w okładzie scalonym podwaja się co 18-24 miesiące" prawo to dziś (2007) wciąż wydaje się obowiązywać. W listopadzie 2006 Gordon Moore oświadczył, ze według niego za 2-3 lata (w 2008 lub 2009 roku) prawo to przestanie obowiązywać. 6

Rysunek 4: Ilość układów tranzystorowych w procesorach, ilustracja prawa Moore'a W 1971 Ted Hoff stworzył pierwszy mikroprocesor o wymiarach 1/8 cala na 1/16 cala. Układ miał moc obliczeniową na poziomie komputera ENIAC. W 1999 roku firma Sony wprowadziła na rynek robotycznego psa o nazwie AIBO zdolnego do interakcji z ludźmi. Jego pierwsza seria sprzedała się w Japonii w 20 minut. Rysunek 5: Robotyczny pies AIBO 7

W 2000 roku organizacja Narodów Zjednoczonych szacowała, ze na świecie istnieje 750 tysięcy sprawnych robotów przemysłowych, z tego ponad polowa znajduje się w Japonii. W tym samym roku bezzałogowy pojazd "Global Hawk" dokonał pierwszego autonomicznego przelotu bez międzylądowania nad Pacyfikiem. Lot trwał 22 godziny. Rysunek 6: Bezzałogowy pojazd latający Global Hawk W 2002 roku na rynek trafił popularny robot-odkurzacz o nazwie Roomba wyprodukowany przez firmę I-Robot. Rysunek 7: Robotyczny odkurzacz ROOMBA 8

Robota Roomba wyposażono w liczne czujniki, m.in: czujnik krawędzi (zapobiegający spadaniu np. ze schodów) czujnik zderzeniowy zamontowany w przednim zderzaku czujniki odometryczne W lipcu 3 i 24 tego samego roku (2002) na powierzchni Marsa wylądowały dwa łaziki robotyczne, Spirit i Opportunity. Wystrzelone w 2003 roku łaziki przejechały wiele razy większa odległość niż się spodziewano. Roboty wciąż są częściowo sprawne. Dostarczyły ogromne ilości danych, w tym zdjęć i analiz chemicznych. Rysunek 8: Łazik marsjański Niesłychanie szybki rozwój technologi, malejące koszty i spektakularne osiągnięcia są bardzo silna motywacja dla tysięcy młodych ludzi na świecie, którzy postanowili kształcić się właśnie w tej dziedzinie. 9

Robotyka jako dziedzina akademicka ma już ponad 50 lat. Na początku lat dziewięćdziesiątych w nowo założonej Polsko-Japońskiej Wyższej Szkole Technik Komputerowych powstała specjalizacja "Robotyka i systemy wieloagentowe". Jej główne osiągnięcia to wirtualne laboratorium robotyczne "VLAB" oraz robotyczna głowa "Paladyn". W 2007 roku, już w momencie oddawania tej pracy do druku, Polsko-Japońska Wyższa Szkoła Technik Komputerowych zakupiła dla potrzeb specjalizacji robotycznej 10 robotów Roomba firmy I-Robot. 2. Założenia i podstawy teoretyczne 2.1 Problematyka robotów prowizorycznych Własnoręcznie budowane roboty (tj. budowane od podstaw przez studentów) od zawsze charakteryzowały się: - niską jakością wykonania - obniżoną trwałością - okrojoną funkcjonalnością - brakiem powtarzalności wykonania podzespołów - bardzo roboczym i niereprezentacyjnym wyglądem Problemy te potęguje bardzo duże zapotrzebowanie specjalizacji robotycznej na wydziale informatyki PJWSTK na sprzęt robotyczny mogący znaleźć zastosowanie w pracach naukowych studentów zainteresowanych rozwijaniem wysokopoziomowych dziedzin robotycznych: oprogramowania, algorytmów zachowania, itp. Autorzy poniższej pracy postanowili sprawdzić tezę jakoby możliwe było skonstruowanie bardzo taniego, małego robota o dużych możliwościach funkcjonalnych. 10

2.2 Założenia projektowe a) małe rozmiary b) wygląd lepszy od prowizorycznego c) koszty produkcyjne poniżej 100 euro przy detalicznych cenach części d) praca z 4 akumulatorów AA 1.5 V e) niski pobór prądu f) szybki mikrokontroler g) łatwy dostęp serwisowy h) łączność radiowa z robotem(bluetooth) j) powtarzalność wykonania obudowy k) wysoka dostępność części mechanicznych i silników l) pomiar odległości w zakresie 90 stopni a nie tylko na wprost 2.3 Sterowanie dyferencjalne W projekcie robota zdecydowano się na sterowanie dyferencjalne. Jest to najprostszy wykonaniu i najtańszy sposób wykonania mechanizmu sterującego w robocie. 11

Gdy oba kola kręcą się w tym samym kierunku i z tą samą prędkością robot będzie jechał po linii prostej. W innym wypadku, w zależności od prędkości rotacji i jej kierunku środek rotacji może wypaść w dowolnym miejscu pomiędzy linią prostą łącząca środki obu kół. Gdy oba kola kręcą się z ta sama prędkością, ale w przeciwnym kierunku robot będzie się obracał wokół środka. 2.4 Czujnik odległości Jednym z bardzo ważnych elementów składowych robota będącego przedmiotem tej pracy jest czujnik odległości firmy Sharp GP2D12 o zasięgu działania od 10cm do 80cm. Czujniki te są bardzo popularne w laboratoriach robotycznych na całym świecie. Istnieje sześć modeli o różnych właściwościach co umożliwia dobranie optymalnego rozwiązania do potrzeb konkretnego projektu. Rysunek 9: Czujniki podczerwieni Sharp Modele można podzielić na dwie podgrupy: jedna to czujniki o analogowym wyjściu gdzie napięcie na tymże wyjściu jest odpowiednie do odległości od przeszkody, druga grupa natomiast ma odległość ustawianą na stałe potencjometrem znajdującym się wewnątrz czujnika, i działa jako czujnik krańcowy, zmieniający stan z niskiego na wysoki, po przekroczeniu zadanego progu. 12

Rysunek 10: Zakresy działania poszczególnych modeli czujników Sharp Możemy wyróżnić też drugi podział - ze względu na zasięg działania: 10-80cm lub 20-150cm. Bardzo ważnym parametrem jest minimalna odległość pracy czujnika. Podczas zbliżania się do przeszkody przy osiągnięciu zasięgu minimalnego napięcie na wyjściu osiąga swoją maksymalną wartość. Jeżeli zbliżanie będzie dalej następować napięcie zacznie z powrotem spadać dając na wyjściu efekt identyczny z sytuacją oddalania się od przeszkody. Jak widać na rysunku funkcja napięcia na wyjściu jest nieliniowa. Krzywą z rysunku 11 aproksymowano równaniem kwadratowym. Rysunek 11: Wykres odczytu napięcia z czujnika Sharp w zależności od odległości od przeszkody U = a*d^2 + b*d + c gdzie U jest mierzonym napięciem a d odległością. Parametry wyznaczono metodą najmniejszych kwadratów stosując metodę regresji liniowej. Pięciokrotnie zmierzono charakterystykę dla ośmiu pomiarów. Użyto średnich. 13

Wzór na odległość od przeszkody wygląda następująco: S = 0.00096 * odczyt * odczyt - 0.695 * odczyt + 126 Gdzie S to odległość w centymetrach od przeszkody. 3. Opis robota 3.1 Prototypowanie Podejście, które zastosowali autorzy to typowa strategia szybkiego prototypu. Pierwszym i najważniejszym zadaniem robota było sprawne poruszanie się i wykonywanie poleceń wydawanych poprzez łącze rs232. Tak powstał projekt nazwany przez autorów "Minipionier" ponieważ jego końcowy wygląd przypominał roboty firmy ActivMedia znajdujące się w laboratorium robotyki PJWSTK. Składały się na niego: Profile aluminiowe Prostokątny kawałek drewnianej sklejki Silniki i przekładnie(dwa komplety) z modelu czołgu firmy Tamiya. Elektronika oparta na układzie ATmega16. Ten prosty prototyp doświadczalny pozwolił autorom na eksperymenty z elektroniką sterującą. Przy okazji wykonania projektu "Minipionier" wynikły bardzo poważne problemy z elektroniką sterującą silnikami prądu stałego a dokładniej z konstrukcją mostków typu H (H-Bridge). Wydajny i sprawny projekt mostka typu H powstał w laboratorium dopiero pół roku później. 14

Autorzy postanowili poszukać alternatywnego rozwiązania. Okazało się, że prostym i tanim rozwiązaniem będzie zastosowanie zwykłych serwomechanizmów modelarskich, oczywiście po odpowiednich przeróbkach. Tak powstał prototyp o nazwie "Dragster Bot". Wziął on swoją nazwę od wyglądu przypominającym amerykańskie samochody do wyścigów na 1/4 mili (długi, duże koła z tyłu). Rysunek 12: Prototyp robota Elektronika i niektóre części mechaniczne(koła, fragmenty obudowy) zostały zapożyczone z projektu "Minipionier". Tak skonstruowany prototyp radził sobie bardzo dobrze i spełniał wszystkie wymagania stawiane robotowi pierwszej generacji tj. wykonywał zaprogramowane polecenia. Przyszedł więc czas na prototypowanie oprogramowania czujnika odległości i zachowań wynikających ze zbliżania się robota do przeszkody. 15

Pierwotnie czujnik zainstalowano z przodu robota tak aby wykrywał przeszkody znajdujące się na wprost. Było to jednak za mało biorąc pod uwagę wymaganie projektowe odnośnie zakresu kątowego wykrywania przeszkód. Rozwiązaniem było umieszczenie na robocie kilku czujników. Ten pomysł wiązał się jednak z nieuchronnym podniesieniem kosztów produkcji jednego robota. Autorzy postanowili zatem umieścić czujnik odległości na trzecim serwomechaniźmie (dwa napędowe + jedno poruszające czujnikiem). W ten sposób otrzymano możliwość dokonywania pomiaru odległości w zakresie 120 stopni. 3.2. Opis konstrukcji mechanicznej 3.2.1 Opis konstrukcji obudowy. Wybór materiału padł na popularny plexiglas. Jest to bardzo wytrzymały i lekki budulec. Dodatkowo po wypolerowaniu można osiągnąć atrakcyjny, przezroczysty wygląd, umożliwiający oglądanie komponentów robota bez potrzeby jego rozkładania. 16

Modułowa koncepcja robota wymusiła segmentową konstrukcję obudowy. Składa się ona z pięciu części, trzech segmentów wyfrezowanych z dwu centymetrowej przezroczystej płyty Rysunek 13: Obudowa robota bez zamonontowanej elektroniki plexiglasowej, które po połączeniu (nałożeniu na siebie) tworzą boczne ściany robota, oraz dwóch kwadratowych płytek z dwumilimetrowego plexiglasu, które pełnią funkcje górnej i dolnej ścianki. W dolnym segmencie wyfrezowano po bokach otwory na serwa napędowe i czujniki zderzeniowe(zderzak). W środkowym segmencie wykonano otwór na wyprowadzenie programatora układu ATmega128. W górnej ściance znajduje się otwór na wyprowadzenie ruchomej części serwa omiatającego oraz przycisk zasilania. Po zainstalowaniu przetwornicy, która stabilizuje napięcie na wejściu modułu bluetooth pojawiła się potrzeba wentylacji wewnątrz obudowy robota. Między dwoma modułami zrobiono specjalną dwumilimetrową przerwę ułatwiająca przepływ powietrza. 3.2.2 Koła Koła zostały zakupione w sklepie modelarskim. Są wykonane z gumy o średniej miękkości. Plastikowe felgi zostały przyklejone do wyprowadzenia serw napędowych. 17

Rysunek 14: Koła robota 3.2.3 Serwa napędowe i sposób modyfikacji serwa modelarskiego Rysunek 15: Rozłożone serwo modelarskie Zdecydowano się na zastosowanie ogólnodostępnych serwomechanizmów modelarskich które po minimalnej modyfikacji doskonale nadają się jako gotowe elementy, na których wystarczy osadzić koła. Serwomechanizm modelarski sterowany jest przebiegiem o okresie 2ms ze zmiennym wypełnieniem, czyli żeby ustawić je w pozycji neutralnej 90 trzeba podać sygnał o wypełnieniu 50%: czyli 1ms włączony, 1ms wyłączony. Taki przebieg musi być powtarzany tak długo dopóki 18

serwo nie ustawi swojego położenia. We wszystkich platformach mobilnych stajemy przed problemem, jakiego napędu użyć, jak go wysterować i czy jest on wogóle dostępny. Często prowadzi to do stosowania rozwiązań drogich, stanowczo nadmiarowych w stosunku do projektu, w którym są one stosowane. Ponieważ jednym z założeń prezentowanej pracy jest budowa robota o jak najniższym koszcie, zdecydowano się na napęd oparty o zmodyfikowane serwomechanizmy modelarskie. Jest to niedrogi element - w cenach wahających się od 30 zł do 100 zł za standardowy rozmiar - co pozwala na modyfikacje parametrów napędu bez zmian obudowy i łatwą wymianę w przypadku awarii. Rysunek 16: Rozłożone serwo modelarskie Serwomechanizm jest układem zębatek o dużej redukcji sprzężonym z silnikiem elektrycznym z wbudowanym sterownikiem, o zestandaryzowanym sposobie komunikacji. Sterownik ten zastępuje drogie i trudno dostępne mostki typu H pozwalając na sterowanie silnikiem w obu kierunkach ze zmienna prędkością. Standardowe zastosowanie serwomechanizmów sprowadza się do wychylania orczyka w zakresie kątowym 180. Jest to uzyskiwane poprzez sprzężenie ostatniej, zdawczej zębatki z potencjometrem, który daje informację sterownikowi o aktualnym wychyleniu orczyka. 19

Celem przeróbki jest pominiecie potencjometrów w tym układzie i wycięcie mechanicznych ograniczników wychylenia, które w normalnym trybie działania serwomechanizmu zabezpieczają go przed uszkodzeniem przez nadmierne wychylenie. Rysunek 17: Rozłożone serwo modelarskie Tak wiec aby dostać się do tych elementów należy na początku zdjąć zębatki zasłaniające dolną warstwę. Na zdjęciu zaznaczono kolorem niebieskim ograniczniki które trzeba spiłować, tak aby ograniczniki metalowe nie miały się na czym zatrzymać. Rysunek 18: Rozłożone serwo modelarskie Następnie otwieramy serwo aby dostać się do przewodów łączących potencjometr ze sterownikiem. Należy je obciąć jak najbliżej potencjometru, a zamiast niego przylutować dwa rezystory - tak jak zaznaczono na schemacie (rysunek 19). Wybór tych wartości jest dość prosty. Ponieważ potencjometr ma wartość 5KΩ a potrzebne jest zablokowanie go w pozycji neutralnej, 20

zatem zastępujemy go rezystorami o wartości 2,5KΩ.. Rysunek 19: Schemat dzielnika rezystancyjnego zastępującego potencjometr Po skończeniu lutowania można złożyć serwomechanizm i sprawdzić czy ustawiając jakąś wartość serwo się kręci. Jeżeli słychać zgrzytanie najprawdopodobniej ograniczniki nie zostały dokładnie usunięte i zahaczają o ich metalowy odpowiednik. Dzięki użyciu rezystorów o połowie wartości rezystancji potencjometru uzyskuję się ułatwiony sposób znalezienia momentu, w którym serwo się nie porusza. Jako że sterowanie nim jest generacją przebiegu prostokątnego o czasie trwania 2ms i zmiennym wypełnieniu to należy ustalić przebieg o wypełnieniu 50%. Aby to uzyskać w zastosowanym języku programowania mikrokontrolera należy na początku skonfigurować go do pracy z zadana ilością serw (poniżej linijka z programu mikrokontrolera). Config Servos = 3, Servo1 = Porta.3, Servo2 = Porta.4, servo3 = porta.6, Reload = 20 Sterowanie serwomechanizmem odbywa się przez zapis do elementu tablicy o indeksie 21

odpowiadającym numerowi z konfiguracji. Wartości które możemy podać są wartościami typu Byte (zatem od 0 do 255) jednak skuteczne wartości sterujące mieszczą się w przedziale od ok. 30 do 150. Należy już to ustalać eksperymentalnie. Servo(1) = 98 '(wartość może się różnić) Ta instrukcja spowoduje zatrzymanie się serwomechanizmu. Wartość 98 została ustalona właśnie eksperymentalnie i różni się dla każdego serwomechanizmu. Wpisując wartości powyżej lub poniżej tego parametru uzyskujemy obrót w zadanym kierunku. 3.2.5 wyprowadzenie programotora Programatory typu ISP (in system programming) umożliwiają programowanie mikrokontrolera bez konieczności demontażu go z urządzenia, w którym ma działać. Rozwiązanie takie ułatwia zmianę oprogramowania mikrokontrolera. Dodatkowo zbędne są specjalistyczne układy programujące. Złacze programatora zostało wyprowadzone na przedniej ściance robota. 3.3 Opis elektroniki Po długich rozważaniach odnośnie wyboru mikrokontrolera zdecydowano użyć w projekcie urządzenia firmy Atmel, układu AVR, ATmega128. Jego zalety to przede wszystkim niski koszt, duża wydajnosc i dobra obsługa elektroniki peryferyjnej. 22

3.3.1 Mikrokontroler Rysunek 20: Układ elektroniczny z podłączanym serwem modelarskim Sercem układu jest jeden z najbardziej zaawansowanych układów rodziny AVR firmy Atmel. Model ATmega128 posiada 128KB pamięci programu, 4KB pamięci operacyjnej i 4KB EEPROM'u do przechowywania wszelkiego rodzaju danych konfiguracyjnych o wielokrotnym odczycie ale ograniczonej żywotności cykli zapisu. Dodając do tego niski pobór prądu ok. 10mA przy 11MHz taktowania i wydajności obliczeniowej ok. 1MIPS na 1MHz otrzymujemy doskonały mikrokontroler do wszelkich zastosowań robotycznych. 23

Rysunek 21: Schemat wyprowadzeń układu ATmega128 W zaprojektowanym układzie wykorzystaliśmy następujące elementy mikrokontrolera: Timer0 do obsługi serwomechanizmów modelarskich Timer2 do wykonywania czasowych części programu Przetwornik analogowo cyfrowy do odczytów z czujnika odległości firmy Sharp Port Serial1 do komunikacji z hostem Układ brown out zapobiegający niepoprawnemu działaniu procesora ATmega128 jak wszystkie AVR'y jest programowalny w systemie ISP (wspomniany wcześniej) co znacznie upraszcza całą budowę i testowanie układu, ponieważ eliminuje konieczność wyjmowania mikrokontrolera z układu jak było to konieczne w przypadku starszych generacji mikrokontrolerów. 24

3.3.2 Komunikacja bluetooth Kolejnym założeniem była łatwa możliwość odciążenia procesora ze złożonych czynności takich jak planowanie trasy czy odnajdywanie się w terenie. Osiągnięto to przez integracje łączności bezprzewodowej. W robocie zdecydowano sie na komunikacje z wykorzystaniem technologii bluetooth, która umożliwia połączenie robota z takimi urzadzeniami jak np. telefony, palmtopy czy po prostu komputery. Zastosowany moduł umożliwia również komunikację z innymi modułami tego samego typu, zatem osiagalna jest implementacja zachowań grupowych (z komunikacją jawną) jezeli zaszła by taka potrzeba. 3.3.3 Oprogramowanie mikrokontrolera Oprogramowanie mikrokontrolera zostało napisane w języku BASCOM. Jego dużą zaletą jest bardzo duża ilość dostępnych bibliotek funkcji obsługi urządzeń peryferyjnych takich jak wyświetlacze LCD, LED, serwomechanizmy i przetworniki analogowo cyfrowe. Generowany kod jest zauważalnie większy niż odpowiadający kod w Asemblerze, jednak użyty mikrokontroler ATmega128 jest wyposażony we względnie dużą ilość pamięci, która w zupełności wystarcza dla potrzeb projektu. Aby komunikować się z robotem należy skonfigurować w systemie połączenie typu "serial over bluetooth". Komendy wydawane robotowi w programie takim jak "HyperTerminal" czy "Putty" należy zatwierdzać klawiszem enter (znak końca linii - CR, nie CR/LF!). Dostępne, zaimplementowane w programie mikrokontrolera rozkazy to: init inicjalizuje robota po podłączeniu zasilania reboot resetuje program robota (stan identyczny jak po podłączeniu zasilania) calib wywołuje podprogram do kalibracji serwomechanizmów scan czujnik Sharp wykonuje odczyt w pełnym zakresie i zwraca na konsole łańcuch zaczynający się od słowa dist, zawierający uśrednione czterdzieści odczytów oddzielonych 25

pionowymi kreskami (pipe). params wyswietla aktualne paramtery serwomechanizmu ms1,ms2 "manual serwo", ustawianie wartości serw napędowych z pominięciem zabezpieczeń programowych robota. s1,s2 wartości 0,1,2,3,4,5,6,7,8,9,10 ustawia prędkości obrotu serwomechanizmy(zakres obu kierunków) go sprawia, że robot rusza z zainicjalizowanymi wartościami s1, s2 s zatrzymuje robota 3.4 Oprogramowanie wizualizujące odczyt z czujników podczerwieni. Rysunek 22: Zrzut ekranu z oprogramowania SonarView 26

Jednym z głównych założeń projektu Cubot było zastąpienie grupy czujników pojedyńczym, ruchomym czujnikiem odległości firmy Sharp. Zdecydowano o napisaniu oprogramowania do wizualizacji odczytów w środowisko graficznym TrueVision3D. Jest to zestaw bibliotek pośredniczących pomiędzy kodem projektanta a bibliotekami DirectX. Dane dostarczane są do do komputera poprzez kanał komunikacji bezprzewodowej, a następnie przetwarzane. 8 linijek kodu wystarczy zeby zinicjalizowac "engine", ustawic paramatry jak np. tlo sceny na czarne (0,0,0) czy tez miarę kątowa na stopnie lub radiany. TV8 = New TrueVision3D.TVEngine Scene = New TrueVision3D.TVScene TextureFactory = New TrueVision3D.TVTextureFactory TV8.SetSearchDirectory(path) texture = New TrueVision3D.TVGlobals TV8.Init3DWindowedMode(Me.Handle.ToInt32) TV8.SetAngleSystem(TrueVision3D.CONST_TV_ANGLE.TV_ANGLE_DEGREE) Scene.SetSceneBackGround(0, 0, 0) W taki sposób tworzymy instancje TVMesh, która obsługuje renderowanie plików 3DS, tworzonych głównie za pomocą 3D Studio, ulatwiajac stworzenie samego modelu robota: bot = New TrueVision3D.TVMesh bot = Scene.CreateMeshBuilder bot.load3dsmesh("3ds/bot.3ds",,, False, False, False) Natomiast ta część kodu jest odpowiedzialna za stworzenie watku wizualizacji odczytu z robota: Dim scanerthread As Thread scanerthread = New Thread(AddressOf drawreadouts) scanerthread.start() 27

If rs_data.contains("dist") = True Then System.Console.WriteLine(" :)") received = False rs_data = rs_data.remove(0, 5) string_data = rs_data.split(" ") Exit Do End If Loop Ponieważ każdą paczkę danych z informacjami o odleglości otrzymujemy w postaci jednego łańcucha znaków zaczynajacego sie słowem kluczowym dist i nastepnie wartościami oddzielanymi znakiem, musimy je rozdzielić oraz wpisać do tablicy i odrzucić wartości nie będące liczbiami jak np. rozpoczynajace dist. For i = 0 To string_data.length - 2 data(i) = Integer.Parse(string_data(i)) data(i) = 0.00096 * (data(i) * data(i)) - 0.695 * data(i) + 1.26 * 100 Next Dane są odbierane z robota w postacji ciagu znaków ASCII, nalezy je zatem przekonwertowac do zmiennej liczbowej a następnie przeliczyć z danych nieprzetworzonych (odczyty zależne od napięcia na czujniku) na odległość w centymetrach. readouts.resetmesh() readouts.setposition(tankposition.x, TankPosition.y, TankPosition.z - 100) For i = 1 To string_data.length - 3 x1 = (math.cost(45 + 2.5 * i) * data(i)) * 20 z1 = (math.sint(45 + 2.5 * i) * data(i)) * 20 x2 = (math.cost(45 + 2.5 * (i + 1)) * data(i + 1)) * 20 z2 = (math.sint(45 + 2.5 * (i + 1)) * data(i + 1)) * 20 readouts.addwall(0, x1, z1, x2, z2, 50) Next 28

x1 = rcos(phi) x2 = rsin(phi) Zastosowana metoda łączenia punktów skrajnych umozliwia wyrysowanie tylko zakresu 180 pola widzenia, co w przypadku projektu Cubot w zupelnosci wystarcza gdyż czujniki sprawdzają otoczenie w zakresie 90 stopni. 3.5 Koszty części składowych Moduł bluetooth: 120 PLN Procesor ATmega128: 20 PLN Pozostałe części elektroniczne: 20 PLN Serwomechanizmy modelarskie: 3x 30 PLN Kółka: 2 x 16 PLN Koszt plexi i frezowanie CNC: 100 PLN Czujnik sharp: 40 PLN Razem: ~ 420 PLN 29

4. Uwagi końcowe 4.1 Wnioski Większość założeń projektu została spełniona. Zakładana cena została przekroczona o mniej niż 10%. Niefortunnym rozwiązaniem okazało się użycie dwóch kół bez kółka podpierającego. W projekcie zamierzano użyć naklejek teflonowych, ale po zmianie środka ciężkości, spowodowanej rozbudową robot wpada w drgania. Dużą wadą robota są użyte serwa - najmniejsze dostępne na rynku. Mają one w środku mało trwałe plastikowe zębatki. Jedno z serw jest delikatnie uszkodzone po tym jak robot wjechał na kabel. Połączenie wyjścia serwomechanizmu z plastikową felgą też jest dosyć słabe i narażone na uszkodzenia. Samo użycie serwomechanizmów zamiast silników prądu stałego i mostków typu H też nie jest idealnym rozwiązaniem ponieważ nie daje się nimi pracować płynnie a tylko w pewnym zakresie. Poruszanie się robota po łuku jest więc wykonalne tylko dla konkretnych promieni skrętu. Jest to również rozwiązanie energetycznie gorsze. Jednym z największych sukcesów projektu jest oprogramowanie wizualizujące odczyt z ruchomego czujnika w trzech wymiarach. Ten element można łatwo stosować w każdym robocie wyposażonym w ruchomy czujnik odległości. Podsumowując, na dzień dzisiejszy autorzy użyli by silników prądu stałego z mostkami typu H, dodali kółko podpierające i zmodyfikowali kształt obudowy. Zamiast czterech baterii 1.5 V użyto by akumulatora modelarskiego by przedłużyć czas działania. 30

W planach jest dorobienie plexiglasowej płytki pełniącej funkcję zderzaka, którego mechanizm jest już wyprowadzony z przodu robota. 4.2 Podziękowania Autorzy dziękują profesorowi Lechowi Polkowskiemu, doktorowi Adamowi Szmigielskiemu oraz Pawłu Ośmiałowskiemu za poświęcony czas i wsparcie podczas tworzenia projektu Cubot. 4.3 Bibliografia 1. BASCOM-AVR: http://avrhelp.mcselec.com/index.html?tableofcontents.htm 2. TRUEVISION3D: http://www.truevision3d.com/tvdna/ 3. http://en.wikipedia.org/wiki/robotics 4. http://www.robotyka.com/teoria_spis.php, Teoria robotyki, dr inż. Tomasz Buratowski 4.4 Załączniki 4.4.1 Program wizualizujący odczyty z czujnika podczerwieni Option Strict Off Option Explicit On Imports System.Math Imports System Imports System.Text Imports System.IO.Ports Imports System.Threading Public Class mainwindow Private TV8 As TrueVision3D.TVEngine Private Tank As TrueVision3D.TVMesh Private Scene As TrueVision3D.TVScene 31

Private InputEngine As TrueVision3D.TVInputEngine Private DoLoop As Boolean Private TankPosition As DxVBLibA.D3DVECTOR Private KolaPosition As DxVBLibA.D3DVECTOR Private texture As TrueVision3D.TVGlobals Private camera As DxVBLibA.D3DVECTOR Private TextureFactory As TrueVision3D.TVTextureFactory Private readouts As TrueVision3D.TVMesh Private bot As TrueVision3D.TVMesh Private arrow As TrueVision3D.TVMesh Private bgtexture As Integer Dim path, tekst As String Dim math As New TrueVision3D.TVMathLibrary Public rs_data As String Dim sending, received As Boolean Private snganglex As Single Private sngangley As Single Private snganglez As Single Dim data(42) As Integer Dim welcomescreen As Form Dim donescanning = True, requestscan, running As Boolean Dim load_status As Boolean Dim lastcommand As String Dim commandbox As rscommands Private Sub rsdata(byval sender As Object, ByVal e As SerialDataReceivedEventArgs) rs_data = "" Try rs_data = bluetooth.readline() rs_data = bluetooth.readline() System.Console.WriteLine(" K <- R ") commandbox.receivedata(rs_data) sending = False received = True Catch received = False End Try End Sub Public Sub loader(byref screen As Object) welcomescreen = screen System.Console.WriteLine("we are being loaded...") load_status = True End Sub Public Sub send(byval command As Object) 'If sending = False Then 32

bluetooth.write(command + Chr(13)) System.Console.WriteLine(" K -> R ") sending = False received = False lastcommand = command If command = "scan" Then requestscan = True Threading.Thread.Sleep(10) 'End If End Sub Public Function getstatus() As Boolean Return Load_status End Function Private Sub mainwindow_load(byval sender As System.Object, ByVal e As System.EventArgs) Handles MyBase.Load path = Application.StartupPath Try bluetooth.open() Catch ex As UnauthorizedAccessException MsgBox("Blad otwierania portu.", MsgBoxStyle.Critical, "Communication Error") End End Try commandbox = New rscommands bluetooth.discardoutbuffer() bluetooth.discardinbuffer() AddHandler bluetooth.datareceived, AddressOf rsdata rs_data = "#" TV8 = New TrueVision3D.TVEngine InputEngine = New TrueVision3D.TVInputEngine Scene = New TrueVision3D.TVScene TextureFactory = New TrueVision3D.TVTextureFactory TV8.SetSearchDirectory(path) texture = New TrueVision3D.TVGlobals TV8.Init3DWindowedMode(Me.Handle.ToInt32) TV8.SetAngleSystem(TrueVision3D.CONST_TV_ANGLE.TV_ANGLE_DEGREE) Scene.SetSceneBackGround(0, 0, 0) readouts = New TrueVision3D.TVMesh readouts = Scene.CreateMeshBuilder bot = New TrueVision3D.TVMesh bot = Scene.CreateMeshBuilder bot.load3dsmesh("3ds/bot.3ds",,, False, False, False) bot.setposition(tankposition.x, TankPosition.y, TankPosition.z - 100) arrow = New TrueVision3D.TVMesh arrow = Scene.CreateMeshBuilder 33

arrow.load3dsmesh("3ds/arrow.3ds") TextureFactory.LoadTexture("tex\arrow_tex.png", "arrow",,, TrueVision3D.CONST_TV_COLORKEY.TV_COLORKEY_USE_ALPHA_CHANNEL) arrow.settexture(texture.gettex("arrow")) arrow.scalemesh(4, 4, 4) arrow.setrotation(snganglex, sngangley - 180, snganglez) arrow.setposition(0, 70, -100) 'readouts.createbox(150, 150, 150) TankPosition = readouts.getposition camera = TankPosition camera.x = camera.x + 40 bgtexture = Scene.LoadTexture("tex\wall.png",,, "bgtexture") System.Console.Write("Waiting for init... ") send("reboot") Do System.Console.Write(".") send("init") Threading.Thread.Sleep(500) Loop Until sending = False System.Console.WriteLine("done.") Thread.Sleep(50) drawreadouts() load_status = True commandbox = New rscommands commandbox.show() commandbox.setloader(me) Me.Show() Dim scanerthread As Thread scanerthread = New Thread(AddressOf drawreadouts) Dim updatethread As Thread updatethread = New Thread(AddressOf updatescreen) scanerthread.start() updatethread.start() DoLoop = True running = True looping() End Sub Private Sub updatescreen() If received = True Then commandbox.receivedata(rs_data) End If Threading.Thread.Sleep(100) 34

End Sub Private Sub drawreadouts() Do If rs_data.contains("dist") Then Dim string_data As String() Dim i As Integer Dim inv As Integer Dim x1 As Integer Dim z1 As Integer Dim x2 As Integer Dim z2 As Integer Do If rs_data.contains("dist") = True Then System.Console.WriteLine(" :)") received = False rs_data = rs_data.remove(0, 5) string_data = rs_data.split(" ") Exit Do End If Loop inv = string_data.length - 1 For i = 0 To string_data.length - 2 inv = inv - 1 System.Console.WriteLine("inv: " & inv & ", i: " & i) data(inv) = Integer.Parse(string_data(i)) 'data(i) = (600 - data(i)) / 2 data(inv) = 0.00096 * (data(inv) * data(inv)) - 0.695 * data(inv) + 1.26 * 100 'System.Console.WriteLine("data[" & i & "] = " & data(i)) Next readouts.resetmesh() readouts.setposition(tankposition.x, TankPosition.y, TankPosition.z - 100) For i = 1 To string_data.length - 3 x1 = (math.cost(45 + 2.5 * i) * data(i)) * 20 z1 = (math.sint(45 + 2.5 * i) * data(i)) * 20 x2 = (math.cost(45 + 2.5 * (i + 1)) * data(i + 1)) * 20 z2 = (math.sint(45 + 2.5 * (i + 1)) * data(i + 1)) * 20 'System.Console.WriteLine("Data[" & i & "]: (" & x1 & ", " & z1 & "), (" & x2 & ", " & z2 & ")") readouts.addwall(0, x1, z1, x2, z2, 50) Next 'readouts.settexture(texture.gettex("bgtexture")) 35

requestscan = False donescanning = True End If Thread.Sleep(100) Loop Until running = False End Sub Private Sub looping() Do requestscan = True System.Windows.Forms.Application.DoEvents() TV8.Clear() Scene.RenderAllMeshes() Scene.SetCamera(camera.x, camera.y, camera.z, TankPosition.x, TankPosition.y, TankPosition.z) camera = math.movearoundpoint(tankposition, 450, snganglex + 180, sngangley - 45) 'sngangley = sngangley - (TV8.TimeElapsed / 350) 'System.Console.WriteLine("y: " & sngangley) 'snganglex = snganglex + (TV8.TimeElapsed / 50) TV8.RenderToScreen() Loop While DoLoop = True End Sub Private Sub mainwindow_closed(byval eventsender As System.Object, ByVal eventargs As System.EventArgs) Handles MyBase.Closed ' The user asked to quit but clicked on the 'X' button at up right. requestscan = False Me.Hide() If lastcommand = "scan" Then Thread.Sleep(1000) Threading.Thread.Sleep(100) bluetooth.discardoutbuffer() bluetooth.discardinbuffer() send("reboot") Thread.Sleep(200) bluetooth.discardoutbuffer() bluetooth.discardinbuffer() bluetooth.close() DoLoop = False running = False Scene = Nothing TV8 = Nothing End End Sub End Class 36

4.4.2 Program mikrokontrolera $crystal = 11059000 $baud = 115200 echo off Config Timer1 = Pwm, Prescale = 64, Pwm = 10, Compare A Pwm = Clear Up, Compare B Pwm = Clear Up Pwm1a = 1023 config portc = output Config Pind.6 = Output Config Pind.7 = Output Dir1 Alias Portd.6 'jezeli set to jedzie do tylu Dir2 Alias Portd.7 Config Servos = 5, Servo1 = Portc.2, Servo2 = Portc.3, Servo3 = Portc.6, Servo4 = Portc. 5,servo5 = portc.7, Reload = 8, timer = timer0 config adc = single,prescaler = auto, reference = avcc config serialin = buffered, size = 20 'config serialout = buffered, size = 42 dim el_turn as eram byte dim l_turn as byte dim er_turn as eram byte dim r_turn as byte dim es5_min as ERAM Byte dim es5_mid as ERAM Byte dim es5_max as ERAM Byte dim s5_min as byte dim s5_mid as byte dim s5_max as byte dim s1_val as byte dim es1_val as eram byte dim s2_val as Byte dim es2_val as eram byte 37

dim s3_val as byte dim es3_val as eram byte dim s4_val as byte dim es4_val as eram byte dim i as byte s1_val = es1_val s2_val = es2_val s3_val = es3_val s4_val = es4_val s5_min = es5_min s5_mid = es5_mid s5_max = es5_max l_turn = el_turn r_turn = er_turn if s1_val > 140 then s1_val = 75 s2_val = 75 s3_val = 75 s4_val = 75 endif Servo(1) = s1_val Servo(2) = s2_val Servo(3) = s3_val Servo(4) = s4_val servo(5) = s5_min start adc 'adc1 front 'adc2 back 'config timer2 = timer, async = off, prescale = 32 38

Enable Interrupts 'enable timer2 'on timer2 scancount dim param as string * 8 dim speed as Integer dim dist as integer dim tmp as byte dim tracing as bit dim angle as byte dim scancountvalue as byte dim scannerdirection as bit dim donescan as bit dim frontdata(40) as word dim backdata(40) as word dim frontindex as Byte dim backindex as byte dim scansoutcome as word dim timeangle as word declare sub reset_servos() declare sub printdata() run: param = "" disable timer0 Do if ischarwaiting() = 1 then input "", param end if if param = "init" then exit do Loop enable timer0 'start timer2 'nasz maly scanerek speed = 800 39

CLEAR SERIALIN print chr(13) ; "Hello at basicbra!nz." Pwm1a = 1023 if s1_val > 140 then s1_val = 75 s2_val = 75 s3_val = 75 s4_val = 75 endif Servo(1) = s1_val Servo(2) = s2_val Servo(3) = s3_val Servo(4) = s4_val servo(5) = s5_min frontindex = 0 backindex = 0 looper: do if ischarwaiting() = 1 then input "", param select case param case "speed": input "", param speed = val(param) case "dir": toggle dir1 toggle dir2 case "reset": reset dir1 reset dir2 pwm1a = 1023 40

speed = 800 call reset_servos() case "go": pwm1a = speed case "stop": pwm1a = 1023 reset dir1 reset dir2 case "back": set dir1 set dir2 case "ff": reset dir1 reset dir2 case "angle": input "angle(1-200): ", param angle = val(param) servo(1) = angle servo(2) = angle servo(3) = angle servo(4) = angle reset dir1 reset dir2 case "left": input "", param 'angle(1-90): angle = val(param) angle = angle / 2 servo(1) = s1_val+angle servo(2) = s2_val+angle servo(3) = s3_val+angle 41

servo(4) = s4_val+angle reset dir1 reset dir2 case "right": input "", param 'angle(1-90): angle = val(param) angle = angle /2 servo(1) = s1_val-angle servo(2) = s2_val-angle servo(3) = s3_val-angle servo(4) = s4_val-angle reset dir1 reset dir2 case "lturn": servo(1) = s1_val - l_turn servo(2) = s2_val + l_turn servo(3) = s3_val - r_turn servo(4) = s4_val + r_turn set dir1 reset dir2 case "rturn": servo(1) = s1_val - l_turn servo(2) = s2_val + l_turn servo(3) = s3_val - r_turn servo(4) = s4_val + r_turn reset dir1 set dir2 case "rvturn": reset dir1 set dir2 servo(1) = s1_val - l_turn 42

servo(2) = s2_val + l_turn servo(3) = s3_val - r_turn servo(4) = s4_val + r_turn input "", param timeangle = val(param) waitms 100 pwm1a = 800 waitms timeangle pwm1a = 1023 case "lvturn": set dir1 reset dir2 servo(1) = s1_val - l_turn servo(2) = s2_val + l_turn servo(3) = s3_val - r_turn servo(4) = s4_val + r_turn input "", param timeangle = val(param) waitms 100 pwm1a = 800 waitms timeangle pwm1a = 1023 case "llturn": dim t_arg as byte input "", param t_arg = val(param) servo(1) = s1_val - t_arg servo(2) = s2_val + t_arg servo(3) = s3_val + t_arg servo(4) = s4_val - t_arg case "rlturn": input "", param t_arg = val(param) 43

servo(1) = s1_val - t_arg servo(2) = s2_val + t_arg servo(3) = s3_val + t_arg servo(4) = s4_val - t_arg case "zero": Servo(1) = s1_val Servo(2) = s2_val Servo(3) = s3_val Servo(4) = s4_val reset dir1 reset dir2 case "dump": print "s5_min: " ; s5_min print "s5_max: " ; s5_max case "scan": goto scansub case "sconfig": goto sharpconfig case "tconfig": goto turnconfig case "trace": dim oldservostate as byte oldservostate = servo(5) servo(5) = s5_mid waitms 200 dist = getadc(2) tracing = 1 print "tracking distance " ; dist goto tracesub 44

case "reboot" goto run case "zconfig": goto zeropointconfig case "x": pwm1a = 1023 case else pwm1a = 1023 'call reset_servos() print "ERR" tracing = 0 'reset dir1 'reset dir2 end select 'print ">": endif loop tracesub: dim tracedist as word call reset_servos() pwm1a = speed do tracedist = getadc(2) if dist < tracedist then set dir1 set dir2 elseif dist > tracedist then reset dir1 reset dir2 45

elseif tracedist < 100 OR tracedist > 480 then servo(1) = s1_val - l_turn servo(2) = s2_val + l_turn servo(3) = s3_val - r_turn servo(4) = s4_val + r_turn set dir1 reset dir2 wait 1 reset dir1 set dir2 wait 1 pwm1a = 1023 reset dir1 reset dir2 pwm1a = 1023 endif loop until ischarwaiting() = 1 servo(5) = oldservostate goto looper scansub: dim fin as byte do scancountvalue = 0 if servo(5) >= s5_max then scannerdirection = 0 servo(5) = s5_max if frontdata(2) <> 0 then call printdata() fin = 1 end if end if if servo(5) <= s5_min then scannerdirection = 1 servo(5) = s5_min call printdata() 46

fin= 1 end if if scannerdirection = 0 then frontindex = frontindex + 1 else if frontindex > 0 then frontindex = frontindex - 1 end if scansoutcome = 0 for i = 0 to 4 scansoutcome = scansoutcome + getadc(1) waitms 4 next frontdata(frontindex) = scansoutcome / 5 scansoutcome = 0 for i = 0 to 4 scansoutcome = scansoutcome + getadc(2) waitms 4 next backdata(frontindex) = scansoutcome / 5 scancountvalue = 0 if scannerdirection = 0 then servo(5) = servo(5) - 3 else servo(5) = servo(5) + 3 end if waitms 10 loop until fin = 1 fin = 0 goto looper zeropointconfig: echo on print "Enter zero value for servos (now "; s1_val ; "," ; s2_val ; ","; 47 s3_val ; "," ;

s4_val ;"):" do input "servo 1 (n to skip) > ", param if param <> "n" then s1_val = val(param) input "servo 2 (n to skip) > ", param if param <> "n" then s2_val = val(param) input "servo 3 (n to skip) > ", param if param <> "n" then s3_val = val(param) input "servo 4 (n to skip) > ", param if param <> "n" then s4_val = val(param) servo(1) = s1_val servo(2) = s2_val servo(3) = s3_val servo(4) = s4_val wait 1 input "save? (y/n) ", param if param = "y" then es1_val = s1_val es2_val = s2_val es3_val = s3_val es4_val = s4_val end if input "repeat? (y/n)", param if param = "n" then goto run end if loop echo off goto run turnconfig: echo on print "Enter turn values for servos (now "; l_turn ; "," ; r_turn ;"):" do input "l_turn (n to skip) > ", param 48

if param <> "n" then l_turn = val(param) input "r_turn (n to skip) > ", param if param <> "n" then r_turn = val(param) servo(1) = s1_val - l_turn servo(2) = s2_val + l_turn servo(3) = s3_val - r_turn servo(4) = s4_val + r_turn wait 1 input "save? (y/n) ", param if param = "y" then el_turn = l_turn er_turn = r_turn end if input "repeat? (y/n)", param if param = "n" then goto run loop echo off goto run sharpconfig: echo on do print "Enter min,mid,max values for servo:" input "min (n to next): ", param if param <> "n" then s5_min = val(param) input "mid (n to next): ", param if param <> "n" then s5_mid = val(param) input "max (n to next): ", param if param <> "n" then s5_max = val(param) print "testing sharp..." print "min " ; s5_min servo(5) = s5_min wait 2 print "mid " ; s5_mid 49

servo(5) = s5_mid wait 2 print "max " ; s5_max servo(5) = s5_max wait 2 input "save? (y/n) ", param if param = "y" then es5_min = s5_min es5_mid = s5_mid es5_max = s5_max exit do end if input "repeat? (y/n)", param if param = "n" then goto run loop echo off goto run scancount: scancountvalue = scancountvalue + 1 return sub reset_servos() Servo(1) = s1_val Servo(2) = s2_val Servo(3) = s3_val Servo(4) = s4_val return end sub sub printdata() dim printingindex as byte print " " print "fdist "; if scannerdirection = 0 then for printingindex = 0 to 31 50

print frontdata(printingindex) ; " " ; next for printingindex = 0 to 31 print backdata(printingindex) ; " " ; next else for printingindex = 31 to 0 step -1 print frontdata(printingindex) ; " " ; next for printingindex = 31 to 0 step -1 print backdata(printingindex) ; " " ; next endif print chr(13) return end sub 51