LegOS jest niezależnym od firmy LEGO systemem operacyjnym stworzonym do programowania sterowników RCX. Jego autorem jest Markus L. Noga, który jednocześnie prowadzi oficjalną stronę webową LegOS. Poniższy dokument powstał na podstawie HOWTO dla systemu LegOS napisanego przez Luisa Villa.Szereg pożytecznych informacji o legOS można uzyskać śledząc archiwum listy dyskusyjnej lugnet.com. Jądro systemu legOS zostało opisane przez Stiga Nielssona.
System LegOS oferuje wielowątkowość z wywłaszczaniem, dynamiczne zarządzenie pamięcią, semafory POSIX, bezpośredni dostęp do wyświetlacza, przycisków, czujników i silników, komunikację za pomocą łącza podczerwieni. Zwięźle to ujmując, LegOS pozwala na pełne wykorzystanie sprzętowych możliwości RCX.
LegOS można programować za pomocą C/C++ albo assemblera procesorów Hitachi 8/3292, przy czym do kompilacji wykorzystuje się kompilatory skrośne GNU takie jak gcc, g++ lub gas. W naszym laboratorium narzędzia te dostępne są pod Linuxem w katalogach /usr/local/crossgcc. Programy dla legOS wykonywane są bezpośrednio na procesorze H8/3292. W efekcie, programy te działają szybciej niż interpretowane oprogramowanie systemu firmowego lub NQC. Dodatkowo, legOS umożliwia pełne wykorzystanie i bezpośredni dostęp do 32KB RAM RCX, zatem programy w legOS mogę mieć zdecydowanie więcej niż 32 zmienne NQC. Po dołączeniu biblioteki float programy dla legOS mogą wykorzystywać liczby zmiennoprzecinkowe pojedynczej precyzji float, choć kosztuje to dodatkowe 5kB RAM, natomiast same operacje zmiennoprzecinkowe wykonywane są stosunkowo powoli. Programowanie LegOS nie różni się zatem wiele od programowania poważnych systemów operacyjnych. Może tylko interfejs graficzny programów jest nieco ascetyczny :-)
Sterowanie efektorami robota umożliwiają trzy pary funkcji systemowych, po jednej dla każdego z silników. Poszczególne silniki i funkcje im odpowiadające nazywane są zgodnie z oznaczaniami wyjść na RCX. Omawiając wykorzystanie tych funkcji będziemy opisywać wyłącznie sterowanie silnikiem a. Wszystkie komendy związane z silnikami zadeklarowane są w pliku nagłówkowych dmotor.h .
Funkcja motor_a_dir(MotorDirection dir) służy do zmiany kierunku obrotów silnika a. Typ wyliczeniowy MotorDirection może przyjąć jedną z czterech wartości
| off | wyłączenie zasilania silnika |
| fwd | silnik obraca się "do przodu" |
| rev | silnik obraca się "do tyłu" |
| brake | zablokowanie silnika |
Nazwy fwd i rev posiadają znaczenie wyłącznie umowne, bowiem kierunek obrotu silnika zależy od polaryzacji połączenia silnika z RCX. Uwaga: Zablokowanie silnika za pomocą brake nie działa równie skutecznie jak w NQC.
Funkcja motor_a_speed(unsigned char speed) pozwala na ustawienie prędkości obrotów silnika a. Parametr speed może przyjmować wartości pomiędzy MIN_SPEED a MAX_SPEED. Nazwa funkcji jest nieco myląca, bowiem funkcja zadaje nie tyle prędkość silnika, co reguluje moc zadawaną silnikowi poprzez zmianę współczynnika wypełnienia w modulacji PWM. W rezultacie, prędkość obrotowa silnika staje się zależna od oporów ruchu, a przy ustalonych oporach jest stała w dużym zakresie zmienności parametru speed. Jedynym zauważalnym efektem zmiany prędkości silnika jest wówczas zmiana momentu obrotowego. Sposób sterowania silnikiem należy uznać za jedną z wad MindStorms, bowiem jest prawie niemożliwe oprogramowania robota tak, aby poruszał się z ustaloną prędkością po linii prostej dla zmiennych oporów ruchu. Zalecanym sposobem obejścia tej trudności jest zastosowanie czujników obrotu pozwalających na założenie pętli regulującej prędkość i położenie silnika. Niestety czujniki takie nie są obecnie dostępne w naszym laboratorium. Co więcej, ewentualne ich dołączenie wiąże się z utratą dwóch wejść analogowych w RCX oraz mocy obliczeniowej procesora Hitachi.
W przykładzie sterowania silnikami dla pojazdu na gąsienicach zakładamy, że silnik lewej gąsienicy dołączony jest do wyjścia A w RCX, natomiast prawej do C.
#include < dmotor.h > #include < unistd.h >
int main(int argn,char *args[])
{
/* pojazd rusza do przodu z maksymalną prędkością */
motor_a_dir(fwd);
motor_c_dir(fwd);
motor_a_speed(MAX_SPEED);
motor_c_speed(MAX_SPEED);
msleep(2000);
/* po 2000 milisekund pojazd zaczyna się obracać */
motor_a_dir(rev);
msleep(2000);
/* pojazd zaczyna poruszać sie do tyłu */
motor_c_dir(rev);
msleep(2000);
/* pojazd staje */
motor_a_dir(off);
motor_c_dir(off);
}Wyniki pomiarów czujnikami dostępne są poprzez wbudowane w procesor 10-bitowe przetworniki analogowe-cyfrowe. Rejestry danych poszczególnych kanałów znajdują się w zakresie adresów od 0xffe0 do 0xffe7 i dostępne są jako ulotne zmienne AD_A, AD_B,AD_C i AD_D w pliku sys/h8.h. Wartości tych rejestrów modyfikowane są przez procedury systemowe z częstotliwością ok. 1kHz. W pliku nagłówkowym dsensor.h poszczególnym kanałom A/D przyporządkowane są makrodefinicje odpowiadające czujnikom ponumerowanym zgodne z opisem na RCX
| SENSOR_1 | AD_C |
| SENSOR_2 | AD_B |
| SENSOR_3 | AD_A |
| BATTERY | AD_D |
Rejestry danych poszczególnych kanałów reprezentowane są jako liczby 16-bitowe, przy czym 6 najmłodszych bitów nie posiada żadnego znaczenia. Do usunięcia nieistotnych bitów służy makrodefinicja ds_scale(pomiar), której rezultatem jest 10-bitowa liczba odpowiadająca podanemu pomiarowi.
Na poziomie programu sterującego do odczytu wartości wskazań czujników wystarczy odwołać się do właściwej zmiennej, zatem ciąg instrukcji
unsigned int odczyt;
odczyt=SENSOR_1;
powoduje przypisanie zmiennej odczyt pomiaru czujnikiem pierwszym.
Na poziomie sprzętowym rozróżnia się dwa rodzaje czujników: bierne i aktywne. Czujniki bierne nie potrzebują do działania żadnego zasilania, natomiast czujniki aktywne wymagają dostarczenia dodatkowego prądu z RCX. Systemu legOS można poinformować o charakterze danego czujnika za pomocą funkcji ds_passive(volatile unsigned *sensor) dla czujników biernych i ds_active(volatile unsigned *sensor) dla aktywnych. Domyślnie, w chwili rozpoczęcia wykonywania programu wszystkie czujniki traktowane są jako bierne.
Czujnik dotykowy jest czujnikiem biernym. Pomiary tym czujnikiem (niewyskalowane) przyjmują wartości od 0 dla przycisku w pełni wciśniętego do 0xffc0 dla przycisku w pełni rozwartego. Czujnik może zwracać wartości pośrednie w podanym zakresie odpowiadające stopniowi wciśnięcia czujnika, jednak informacja ta nie jest zbyt wiarygodna. Często przyjmuje się, że wartość pomiaru poniżej 0x2000 oznacza, że czujnik został wciśnięty.
Czujnik światła jest czujnikiem aktywnym, zatem przed wykorzystaniem go należy wywołać funkcję ds_active(&SENSOR_X), gdzie X=1,2,3. Aktywacja czujnika powoduje włączenia diody oświetaljącej jego najbliższe otoczenie. Możliwe jest potraktowanie czujnika światła jako czujnika pasywnego. W takim trybie czujnik ten mierzy ilość docierającego światła, jednak wartości pomiarów nie są zbyt wiarygodne. Tryb ten może być przydatny do namierzania źródła promieniowania podczerwonego.
Czasami wygodnie jest przeliczyć wartości pomiarów czujnikiem światła do przedziału pomiędzy LIGHT_MIN a LIGHT_MAX. Do tego celu można wykorzystać makrodefinicję LIGHT(x), gdzie x jest wartością pomiaru światła, lub też do czujników światła odwoływać się poprzez makrodefinicje LIGHT_X, gdzie X=1,2,3. Należy zaznaczyć, że makrodefinicja LIGHT_X nie może być wykorzystana do inicjalizacji trybu pracy czujnika. Uwaga: Makrodefinicje LIGHT(X) lub LIGHT_X przydatne są w zadaniach typu śledzenie linii na podłożu. Jeżeli czujniki światła wykorzystywany jest w innym celu, wówczas pomiary mogą przekroczyć LIGHT_MAX.
W poniższym przykładzie wyświetlane są przeskalowane wskazania czujnika dotykowego albo czujnika światła po przetworzeniu za pomocą makra LIGHT . Przyjmujemy, że czujnik dotykowy podłączony jest do pierwszego wejścia RCX, natomiast czujnik światła do wejścia drugiego. Wciśnięcie klawisza VIEW przełącza pomiędzy czujnikami.
#include < dsensor.h >
#include < unistd.h >
#include < conio.h >
#include < dkey.h >
#include < dlcd.h >
int main(int argn,char *args[])
{
unsigned char czujnik=0; /*0- czujnik dotykowy, 1 - czujnik światła */
while(1)
{
if (czujnik)
lcd_int(LIGHT_2); /* wyświetl pomiar światła */
else /* heksadecymalnie wyświetl przeskalowany
pomiar czujnikiem dotykowym */
cputw(ds_scale(SENSOR_1));
if (dkey_pressed(KEY_VIEW)) /* jeśli wciśnięty jest klawisz VIEW */
{
if ((czujnik=!czujnik)) /* jeśli będziemy obserwować światło */
{
ds_active(&SENSOR_2);/* włącz czujnik światła */
/* zaznacz, że obserwujemy światło */
dlcd_hide(LCD_S1_SELECT);
dlcd_show(LCD_S2_SELECT);
}
else /* obserwujemy czujnik dotykowy */
{
ds_passive(&SENSOR_2); /* wyłącz czujnik światła */
/* zaznacz, że obserwujemy dotyk */
dlcd_hide(LCD_S2_SELECT);
dlcd_show(LCD_S1_SELECT);
}
msleep(250); /* trochę czasu na puszczenie klawisza VIEW */
}
lcd_refresh();
msleep(50); /* odczekaj 50 milisekund */
}
}
Do czwartego kanału AD podłączone jest napięcie z baterii zasilających RCX. Pomiar napięcia możemy przeliczyć na napięcie w miliwoltach za pomocą następującej funkcji
int mget_battery_mv()
{
long b;
int mv;
b = BATTERY;
b = ds_scale(b) * 0xABD4L;
mv = b / 0x618L;
return mv;
}
Nowe baterie alkaliczne dostarczają napięcia ok.9.5V, które w miarę wyczerpywania baterii maleje. Świeżo naładowane akumulatory niklowo-wodorkowe udostępniają ok.8.0V. Napięcie to po krótkim czasie użytkowania RCX spada do stałego poziomu 7.3V. Wprawdzie RCX do prawidłowego działania potrzebuje napięcia powyżej 6V, jednak dowolny spadek napięcia zmniejsza moment silników. Stosując akumulatory należy zatem liczyć się z mniejszą mocą silników robota w porównaniu z zasilaniem RCX z nowych baterii alkalicznych.
W RCX pracującym pod kontrolą legOS można zapamiętać 8 programów. Obsługa programów jest identyczna jak w ogólnym przypadku. LegOS jest w stanie wykonywać jednocześnie tylko jeden program, ale w obrębie danego programu aktywnych może być do dwudziestu wątków. Każdy z nich dysponuje własnym stosem o rozmiarze określonym w czasie inicjalizacji (domyślnie 512 bajtów). Wątek może odwołać się do dowolnej zmiennej globalnej oraz do segmentu danych programu. Zmienne lokalne wątku są przechowywane na stosie, tak więc nie są widoczne dla innych wątków programu. Procesor Hitachi nie zapewnia ochrony pamięci, zatem możliwe jest odwołanie do dowolnej komórki pamięci stosując adresy bezwzględne np.
*(unsigned char *)0xffb7|=0x01;
ustawia 0 bit bajtu o adresie 0xffb7. W ten sposób możliwy jest odczyt/zapis do dowolnego portu I/O, bowiem porty te są wbudowane w przestrzeń adresową pamięci. Podany przykład ustawia 0 bit portu 4 na 1 co odpowiada zmianie zasięgu nadajnika podczerwieni na daleki. Szczegółowa mapa pamięci dostępna jest na stronie Kekoa Proudfoot.
Rozpoczynając działanie programu system legOS uruchamia procedurę int main(int argn, char *argv[]) i zaznacza proces jako jednowątkowy. Nowe wątki powołuje się do życia za pomocą funkcji pid_t execi(int (*code_start) (int, char **), int argc, char **argv, priority_t priority, size_t stack_size) z modułu unistd.h . Parametr code_start wskazuje na procedurę początkową wątku. Parametry argc i argv pozwalają na przekazanie nowemu wątkowi szeregu parametrów. Znaczenie tych parametrów jest identyczne jak w dowolnym systemie UNIX z tą różnicą, że args[0] nie jest nazwą programu tylko dowolnym parametrem programu. Jeżeli nie ma potrzeby przekazania wątkowi jakichkolwiek parametrów, to możemy ustawić argc=0 i argv=NULL. Parametr priority nadaje priorytet danemu wątkowi. Priorytet określa kolejność wykonywania wątków w sytuacji, gdy wiele wątków konkuruje o przydział procesora. Wymaga się, aby każdy wątek posiadał odmienny priorytet. Parametr stack_size ustawia wielkość stosu nowego wątku. W tm.h zdefiniowana jest stała DEFAULT_STACK_SIZE=512, którą można podstawić w miejsce parametru stack_size. Funkcja execi zwraca identyfikator wątku, bądź -1 jeżeli utworzenie wątku nie powiodło się.
Wątki w momencie uruchomienia pozostają uśpione, aż do inicjalizacji zarządcy zadań za pomocą funkcji tm_start() z sys/tm.h. Funkcja tm_start() w pierwszej kolejności wywołuje wątek o najwyższym priorytecie. Po uruchomieniu wątek może zakończyć swoje działania na kilka sposobów. Pierwszym z nich jest zakończenie wykonywania procedury głównej code_start. Kolejnym, wyjście za pomocą funkcji exit(int ) z unistd.h. Wątek może też zostać zabity w wyniku wywołania funkcji kill(pid_t pid), której parametrem jest identyfikator wątku, bądź w rezultacie wykonania funkcji killall(priority_t p), która zabija wszystkie wątki o priorytecie niższym bądź równym p z pominięciem wątku wywołującego tę funkcję.
LegOS oferuje wywłaszczania wątków. Wątek, który jest aktualnie wykonywany może zostać wstrzymany przez system w wyniku wywołania jednej z podstawowych funkcji sleep, msleep lub wait_event lub wywłaszczony po upływie TM_DEFAULT_SLICE=20 milisekund. Każda z wymienionych funkcji zawiesza wątek do czasu zajścia odpowiedniego zdarzenia i wywołuje procedurę zmiany kontekstu. Procedura ta sprawdza w kolejności malejących priorytetów czy nie zaszły zdarzenia pozwalające pobudzić jeden z wątków. Jeśli tak, to następuje przełączenie kontekstu procesora, a wątek związany z danym zdarzeniem jest wykonywany od miejsca, w którym utracił dostęp do procesora. Jeżeli nie zaszło żadne zdarzenie umożliwiające pobudzenie jednego z wątków, wówczas wykonywane jest domyślne zadanie puste tm_idle_task.
Funkcje msleep(unsigned int milisec) i sleep(unsigned int sec) zawieszają wykonanie wątku odpowiednio na milisec milisekund bądź sec sekund. Warto podkreślić, że msleep zwykle zawiesza wątek na czas określony z dokładnością do TM_DEFAULT_SLICE.
Funkcja wakeup_t wait_event(wakeup_t(*wakeup) (wakeup_t), wakeup_t data) jest podstawowym narzędziem umożliwiającym sterowanie wykonywaniem wątków za pomocą zdarzeń. Parameter wakeup powinien wskazywać na funkcję, która zwraca niezerową wartość jeśli zaszło odpowiednie zdarzenie. Parameter data określa dodatkową informację, która jest przekazywana do funkcji wakeup.
Funkcje msleep,sleep i wait_event wywoływane są w szereg u funkcji bibliotecznych, tak więc przełączenie kontekstu może nastąpić w wyniku wywołania innych niż podane funkcji systemowych. Do najważniejszych z nich należy zaliczyć funkcje związane z semaforami, funkcje do obsługi klawiatury, transmisji łączem podczerwieni oraz procedury do odtwarzania dźwięków.
Do synchronizacji działania wątków można wykorzystać semafory POSIX zadeklarowane w pliku nagłówkowym semaphore.h. Do ich obsługi dostępnych jest 6 funkcji systemowych:
Poniższy przykład utworzony został dla robota gąsienicowego z dwoma czujnikami dotykowymi dołączonymi do wejść 1 i 2 RCX, oraz czujnikiem światła dołączonym do wejścia 3. Założono, że czujniki dotykowe informują o obecności przeszkody odpowiednio z lewej i z prawej strony robota. Czujnik światła powinien zostać zwrócony w kierunku ruchu robota, bowiem jego zadaniem jest wyszukiwanie źródeł światła. Silniki napędzające lewą i prawą gąsienicę robota podłączone są odpowiednio do wejść A i C.
Program składa się z czterech wątków track_light,avoid_obstacles, detect_light i filter_light odpowiedzialnych odpowiednio za śledzenie źródła światła, unikanie przeszkód, detekcję obecności silnego źródła światła, oraz filtrację pomiarów natężenia światła.
Wątek track_light wywoływany jest wtedy, gdy w wyniku ruchu robota nastąpiła zmiana intensywności odbieranego światła o więcej niż LIGHT_LOST_THR. Po zajściu tego zdarzenia robot obraca się wyszukując kierunek o lokalnie maksymalnej świetlności. Następnie robot zaczyna poruszać się do przodu wzdłuż znalezionego kierunku.
Zadaniem wątku avoid_obstacles jest odsuwanie robota od wykrytych przeszkód. Po zajściu zdarzenia polegającego na wciśnięciu jednego z czujników dotykowych, wątek natychmiast cofa robota i obraca go w kierunku przeciwnym do wykrytej przeszkody. Na zakończenie robot rozpoczyna poruszać się do przodu.
Wątek detect_light pobudzany jest wtedy, gdy intensywność światła odbieranego przez czujnik przekroczy próg LIGHT_THR. Zdarzenie to jest interpretowane jako znalezienie się w bezpośredniej bliskości źródła światła. Zadaniem wątku jest odegranie pojedynczej nuty oznajmiającej sukces poszukiwań.
Zadaniem wątku filter_light jest filtracja zakłóceń pomiaru intensywności światła. Wynik filtracji umieszczany jest w zmiennej filtered_light.
Wątki track_light i avoid_obstacles konkurują ze sobą o dostęp do silników robota. Konflikty dostępu rozwiązywane są za pomocą semafora motor_sem.
#include < sys/tm.h>
#include < dmotor.h>
#include < dsensor.h>
#include < dlcd.h>
#include < dkey.h>
#include < dsound.h>
#include < conio.h>
#include < unistd.h>
#include < semaphore.h>
#include < sys/lcd.h>
/* gdzie jest podłączony czujnik światła ? */
#define LIGHT_INPUT SENSOR_3
#define LIGHT_SENSOR LIGHT_3
/* gdzie podłączono czujniki dotykowe */
#define LEFT_TOUCH_SENSOR SENSOR_1
#define RIGHT_TOUCH_SENSOR SENSOR_2
/* maksymalna wartość pobudzonego czujnika dotykowego */
#define TOUCH_THR 0xf000
/* minimalne natężenie światłą wskazujące na
bliskość jego źródła */
#define LIGHT_THR 120
/* minimalna zmniana natężenia światła, które wskazuje na
potrzebę zmiany kierunku ruchu pojazdu */
#define LIGHT_CHANGED_THR 3
/* stale opisujace filtro dolnoprzepustowy dla natezenia swiatla.
rownanie filtru:
f[k+1]=(FILTER_IC*f[k]+(FILTER_DENOM-FILTER_IC)*s[k])/FILTER_DENOM,
gdzie
s[k] - natezenie swiatla w chwili k,
f[k] - wyjscie filtru w chwili k
*/
#define FILTER_IC 92
#define FILTER_DENOM 100
/* 1/3 czasu na poszukiwanie kierunku z maksimum natezenia
swiatla (ms)*/
#define SEARCH_TIME 250
/* makrodefinicja do debagownia, wypisuje na wyswietlaczu
numer linii programu */
#define DBG {lcd_int(__LINE__);lcd_refresh();}
/* kilka zmiennych globalnych (a fuj)*/
static sem_t _motor_sem; /* semafor dla silników */
static sem_t *motor_sem=&_motor_sem;
static int filtered_light=0;
void play_note(unsigned char pitch,unsigned char length)
{
/* w note zawarta jest jednonutowa melodia */
note_t note[]={{PITCH_A0,1},{PITCH_END,0}};
note[0].pitch=pitch;
note[0].length=length;
/* rozpocznij odgrywanie melodii */
dsound_play(note);
/* poczekaj na zakończenie melodii */
wait_event(dsound_finished,0);
}
inline void error_beep()
{
play_note(PITCH_A0,8);
}
unsigned char error_msg(unsigned char error)
{
lcd_int(error);
lcd_refresh();
error_beep();
return error;
}
/* ***********************************
Funkcje wątku avoid_obstacles
************************************** */
wakeup_t obstacle_detected(wakeup_t _sensor_no)
{
unsigned char *sensor_no=
(unsigned char *)(unsigned int)_sensor_no;
if (LEFT_TOUCH_SENSOR < TOUCH_THR)
{
*sensor_no=0;
return 1;
}
if (RIGHT_TOUCH_SENSOR < TOUCH_THR)
{
*sensor_no=1;
return 1;
}
return 0;
}
int avoid_obstacles(int argc,char *argv[])
{
while(1)
{
/* obstacle _dir po detekcji zdarzenia zawiera numer
wciśnietego czujnika dotykowego (0-lewy, 1-prawy).
Patrz obstacle_detected */
unsigned char obstacle_dir;
wait_event(obstacle_detected,(unsigned int)&obstacle_dir);
/* poczekaj na dostęp do silników */
sem_wait(motor_sem);
/* wycofaj pojazd */
motor_a_dir(rev);
motor_c_dir(rev);
/* pokaż na wyświetlaczu gdzie jest przeszkoda */
if (obstacle_dir)
dlcd_show(LCD_A_LEFT);
else
dlcd_show(LCD_A_RIGHT);
lcd_refresh();
/* zagraj nutkę */
play_note(PITCH_A6,1);
/* zaczekaj na zakonczenie manewru */
msleep(300);
/* odwróć pojazd w kierunku przeciwnym do przeszkody
i ruszaj do przodu */
if (obstacle_dir)
{
motor_c_dir(fwd);
msleep(300);
motor_a_dir(fwd);
dlcd_hide(LCD_A_LEFT);
}
else
{
motor_a_dir(fwd);
msleep(300);
motor_c_dir(fwd);
dlcd_hide(LCD_A_RIGHT);
}
/* zwolnij semafor */
sem_post(motor_sem);
}
return 0;
}
/* ***********************************
Funkcje wątku detect_light
************************************** */
wakeup_t light_detected(wakeup_t light_thr)
/* zwróc 1 jeśli poziom światła przekracza zadany próg */
{
return filtered_light>light_thr;
}
int detect_light(int argc,char *argv[])
{
while(1)
{
/* poczekaj dopóki intensywność światła nie przekroczy
wartości progowej */
wait_event(light_detected,LIGHT_THR);
/* zagraj nutę */
play_note(PITCH_A4,2);
/* odczekaj drugie tyle */
msleep(120);
}
return 0;
}
/* ***********************************
Funkcje wątku track_light
************************************** */
wakeup_t light_changed(wakeup_t last_light)
{ /* zwroc 1 jesli intensywnosc swiatla wyszla poza
przedzial < -LIGHT_CHANGED_THR,+LIGHT_CHANGED_THR) wokol
last_light.
*/
return (filtered_light+LIGHT_CHANGED_THR) < last_light ||
(filtered_light>last_light+LIGHT_CHANGED_THR);
}
int track_light(int argc,char *argv[])
{
/* ostatnio zarejestrowana intensywność światła */
int last_light=filtered_light;
while(1)
{
/* śpij dopóki nic sie nie dzieje */
wait_event(light_changed,last_light);
if (last_light>filtered_light)
{
/* pojazd juz nie jedzie w kierunku swiatla
znajdz nowy kierunek zgodnie z algorytmem:
obroc sie troche w lewo, potem w prawo, znajdzi maksimum swiatla
natezenia swiatla, a potem skieruj sie na znalezione
maksimum. Jezeli w czasie obrotu pojazd sie z czyms zderzy,
to trudno, nie znajdzie maksimum*/
time_t end_time;
long rot_time=0;
unsigned int max_light;
/* obroc sie w lewo */
DBG
sem_wait(motor_sem);
motor_a_dir(fwd);
motor_c_dir(rev);
sem_post(motor_sem);
msleep(SEARCH_TIME);
DBG
sem_wait(motor_sem);
/* obracaj sie w prawo i rejestruj swiatlo */
motor_a_dir(rev);
motor_c_dir(fwd);
sem_post(motor_sem);
DBG
max_light=LIGHT_SENSOR;
end_time=sys_time+2*SEARCH_TIME;
while(sys_time < end_time)
{
msleep(20);
if (LIGHT_SENSOR>=max_light)
{
max_light=LIGHT_SENSOR;
rot_time=end_time-sys_time;
}
}
/* obroc pojazd w kierunku maksimum natezenia swiatla */
DBG
sem_wait(motor_sem);
motor_a_dir(fwd);
motor_c_dir(rev);
sem_post(motor_sem);
DBG
msleep(rot_time > 0 ? rot_time : 0 );
/* i do przodu w kierunku swiatla */
DBG
sem_wait(motor_sem);
motor_a_dir(fwd);
motor_c_dir(fwd);
sem_post(motor_sem);
DBG
}
msleep(100);
last_light=filtered_light;
}
return 0;
}
/* ***********************************
Watek filtrujacy natezenia swiatla
************************************** */
int filter_light(int argc,char *argv[])
{
while(1)
{
filtered_light=FILTER_IC*filtered_light+(FILTER_DENOM-FILTER_IC)*LIGHT_SENSOR;
filtered_light/=FILTER_DENOM;
msleep(20);
}
}
/* ***********************************
Częśc główna programu
************************************** */
int main(int argc,char *argv[])
{
/* pojazd porusza się z maksymalną prędkością */
motor_a_speed(MAX_SPEED);
motor_c_speed(MAX_SPEED);
motor_a_dir(fwd);
motor_c_dir(fwd);
/* utworzenie semafora dla silników */
sem_init(motor_sem,0,1);
/* uruchom watek avoid_obstacles */
if (execi(avoid_obstacles,0,NULL,1,DEFAULT_STACK_SIZE) < 0)
return error_msg(1);
/* uruchom watek filtracji natezenia swiatla*/
if (execi(filter_light,0,NULL,2,DEFAULT_STACK_SIZE) < 0)
{
killall(PRIO_HIGHEST);
return error_msg(2);
}
/* uruchom watek detect_light*/
if (execi(detect_light,0,NULL,3,DEFAULT_STACK_SIZE) < 0)
{
killall(PRIO_HIGHEST);
return error_msg(3);
}
/* uruchom wątek track_light*/
if (execi(track_light,0,NULL,4,DEFAULT_STACK_SIZE) < 0)
{
killall(PRIO_HIGHEST);
return error_msg(4);
}
/* aktywuj czujnik światła */
ds_active(&LIGHT_INPUT);
/* start task manager */
tm_start();
return 0;
}
LegOS dostarcza protokół LNP (LegOS Network Protocol) do stratnej komunikacji za pomocą transmisji IR. Warstwa łącza(kanałowa) nazywana jest w LNP warstwą logiczną. Umożliwia ona przesyłanie pojedynczych bajtów za pomocą transmisji szeregowej IR. Funkcje tej warstwy zadeklarowane są plikach nagłówkowych lnp/lnp-logical.h i lnp/sys/lnp-logical.h, a opis najważniejszych z nich zawiera poniższa tabelka
| lnp_init() | inicjalizacja warstwy |
| lnp_logical_write(const void *buf, size_t len) | wyślij len bajtów z pamięci wskazywanej przez buf |
| lnp_logical_range(int range) | transmisji daleka (range==1), lub bliska (range==0) |
Nie istnieją funkcje umożliwiające odbiór paczek, bowiem zadanie to realizowane jest przez procedurę obsługi przerwania. Warstwa logiczna nie jest więc zwykle bezpośrednio wykorzystywana do komunikacji z RCX.
Komunikację z innymi sterownikami RCX oraz z PC umożliwia warstwa ze sprawdzaniem poprawności danych (integrity layer IL). Paczka danych warstwy IL zbudowana jest następująco
| 0xF0 | długość+2 | dane | CRC |
Pierwszy bajt zawiera kod paczki, dwa kolejne długość segmentu danych (w konwencji Big Endian), a po nich następują bajty danych. Ostatni bajt paczki zawiera jednobajtowy kod CRC całości obliczany zgodnie z poniższą procedurą.
unsigned short lnp_checksum(
const unsigned char *data,unsigned length)
{
unsigned char a = 0xff, b = 0xff;
while (length > 0) {
a = a + *data;
b = b + a;
data++;
length--;
}
return a + (b << 8);
}Do wysłania paczki z warstwy IL służy procedura int lnp_integrity_write(const unsigned char *data,unsigned char length), która po pomyślnym nadaniu length bajtów wskazywanych przez data zwraca 0. Oczywiście rozszerzenie data o nagłówek, długość i kod CRC paczki jest jednym z zadań podanej procedury.
Odbiór paczek warstwy IL możliwy jest po zainstalowaniu procedury obsługi paczki za pomocą lnp_integrity_set_handler(lnp_integrity_handler_t handler), gdzie handler jest funkcją postaci handler(const unsigned char *data, unsigned char length). Procedura obsługi paczki jest wywoływana przez jądro systemu za każdym razem, gdy do systemu dotrze paczki IL o poprawnym nagłówku i kodzie CRC. Parametr data wskazuje wówczas na dane paczki, tzn. na pierwszy bajt za nagłówkiem paczki, natomiast length zawiera długość otrzymanej paczki. Warstwa IL w przybliżeniu odpowiada warstwie sieciowej modelu ISO/OSI. Przykład zastosowania komunikacji za pomocą warstwy IL znajduje się w programie przesyłającym wyniki pomiarów do PC.
Kolejną, choć nie nadrzędna względem IL, jest warstwa z adresowaniem paczek (addressing layer AL). Zadaniem procedur tej warstwy jest przesyłanie paczek do konkretnych sterowników RCX. Adresat paczki określony jest przez dwie liczby: numer sterownika oraz numer portu. Numer sterownika ustalany w czasie kompilacji jądra i domyślnie przyjmuje wartość 0. Numer ten zwykle nie jest wykorzystywany do czekogolwiek. Numer portu może przyjmować wartości pomiędzy od 1 do 0x0f, natomiast port 0 zarezerwowany jest przez procedury ładowania programów RCX. Paczki AL zbudowana jest następująco:
| 0xF1 | długość+2 | numer_nadawcy | port_nadawcy | numer-odbiorcy | port_odbiorcy | dane | CRC |
Paczka AL jest więc paczką IL dla danych rozszerzonych o wiodący bajt adresu nadawcy i następujący po nim bajt adresu odbiorcy. Operator | jest alternatywą bitową. Oczywiście typ paczki AL jest inny niż paczki IL. Warto podkreślić, że paczki AL nie są interpretowane jako paczki IL, zatem nie są obsługiwane przez procedury warstwy IL.
Do wysłania paczki AL służy funkcja int lnp_addressing_write(const unsigned char *data,unsigned char length, unsigned char dest,unsigned char srcport). Jej zadaniem jest wysłanie danych o długości length bajtów wskazywanych przez data do odbiorcy określonego przez dest z portu nadawcy o numerze srcport. Pomyślne nadanie funkcja sygnalizuje zwracając 0.
Odbiór paczek AL jest podobny do odbioru paczek IL. W tym celu dla portu port należy za pomocą lnp_addressing_set_handler(unsigned char port, lnp_addressing_handler_t handler) ustawić procedurę odbioru paczek. Procedura handler jest określona identycznie jak w przypadku IL, tyle że do procedury tej docierają wyłącznie paczki przeznaczone dla danego odbiorcy i danego portu. Warstwę AL podobnie jak warstwę IL należy zaliczyć do warstwy sieciowej w modelu ISO/OSI.
W przykładowym programie zilustrowane jest wysyłanie paczek warstwy IL. Zadaniem programu jest zgromadzenie co najwyżej MAX_MX_NO odczytów czujnika światła podłączonego do pierwszego wejścia w czasie co najwyżej MAX_MX_TIME milisekund. Po wciśnięciu klawisza VIEW zgromadzone pomiary są wysyłane (do PC?) za pomocą procedur warstwy IL. Poniższy program współpracuje z programem demonstrującym wykorzystanie LNPD.
#include < dsensor.h>
#include < dmotor.h>
#include < dkey.h>
#include < sys/lcd.h>
#include < unistd.h>
#include < lnp/lnp-logical.h>
#include < lnp/lnp.h>
#include < string.h>
#include < stdlib.h>
#include < conio.h>
#define MAX_MX_NO 400
#define MAX_MX_TIME 6000
/*
paczka danych dla PC
*/
typedef struct
{
unsigned long packet_id;
time_t mx_time;
unsigned short int mx_value;
} __attribute__ ((packed)) mx_pack;
static mx_pack mx[MAX_MX_NO];
int main(int argn,char *argv[])
{
int mx_no=0,i;
long start_time;
ds_active(&SENSOR_1);
motor_a_dir(fwd);
motor_c_dir(rev);
motor_a_speed(MAX_SPEED);
motor_c_speed(MAX_SPEED);
start_time=sys_time;
/* zbieraj pomiary */
while((mx_no < MAX_MX_NO) && (sys_time-start_time) < MAX_MX_TIME)
{
mx[mx_no].packet_id=mx_no;
mx[mx_no].mx_time=sys_time-start_time;
mx[mx_no].mx_value=LIGHT_1;
++mx_no;
msleep(20);
}
motor_a_dir(off);
motor_c_dir(off);
lcd_int(mx_no);
lcd_refresh();
/* poczekaj na klawisz VIEW */
while(getchar()!=KEY_VIEW)
msleep(20);
/* wyslij pomiary */
ds_passive(&SENSOR_1);
for(i=0;i < mx_no;++i)
{
lcd_int(mx_no-i-1);
lcd_refresh();
lnp_integrity_write((unsigned char *)&mx[i],sizeof(mx[0]));
msleep(20);
}
cputs("DONE");
lcd_refresh();
return 0;
}Odbieranie przez PC paczek wysyłanych przez RCX mogłoby być trochę złożone pod Linuxem, gdyby Martin Cornelius nie napisał demona LNPD. Celem tego programu jest po pierwsze ułatwienie komunikacji z RCX, a po drugie umożliwienie sterowania RCX poprzez TCP/IP. Nas interesować będzie jednak tylko lokalna komunikacja z RCX.
API LNPD jest kopią API LNP dla RCX, zatem nie będziemy go tu objaśniać. Najważniejszą różnicą jest konieczność inicjalizacji procedur komunikacji z LNPD z pomocą lnp_init. Oczywiście, demon LNPD powinien być uprzednio uruchomiony.
Zamieszczony poniżej program współpracuje z programem wysyłającym wyniki pomiarów czujnikiem światła. Wyniki te zapisywane są do pliku o nazwie określonej przez pierwszy argument wywołania programu, bądź do standardowego wyjścia. Warto zwrócić uwagę na dwie sprawy. Po pierwsze ix86 są procesorami stosującymi konwencję Little Endian do zapisu liczb całkowitych, natomiast H8/3292 stosuje Big Endian. Wprowadza to konieczność konwersji pomiędzy liczbami całkowitymi. Po drugie, kompilator gcc domyślnie wyrównuje długość struktur do wielokrotności szerokości szyny danych (zwykle do 4 bajtów). Powoduje to, że struktura mx_pack zamiast spodziewanych dziesięciu bajtów długości będzie składała się bajtów dwunastu. Atrybut __attribute__ ((packed)) nakazuje kompilatorowi minimalizację pamięci wykorzystanej do reprezentacji danej struktury.
#include < stdio.h>
#include < stdlib.h>
#include < unistd.h>
#include < sys/time.h>
#include < sys/types.h>
#include < liblnp.h>
/* paczka danych otrzymywana z RCX */
typedef struct
{
unsigned long packet_id;
time_t mx_time;
unsigned short mx_value;
} __attribute__ ((packed)) mx_pack;
FILE *dataFile=NULL; /* to zapisuj kolejne paczki */
/* konwersja long z Big Endian (H8/3292) na Little Endian (ix86)*/
inline unsigned long be_long2le_long(unsigned long p)
{
return (p >> 24) | (p << 24) |
((p >> 8) & 0x0000FF00) | ((p << 8) & 0x00FF0000);
}
/* konwersja short z Big Endian(format H8/3292) na Little Endian (ix86)*/
inline unsigned short be_short2le_short(unsigned short p)
{
return (p >> 8) | (p << 8);
}
/* kolejne paczki zapisuj do dataFile */
void logdata_handler(const unsigned char* data,unsigned char length)
{
if (length==sizeof(mx_pack))
{
fprintf(dataFile,"%d\t%d\t%d\n",
be_long2le_long(((mx_pack *)data)->packet_id),
be_long2le_long(((mx_pack *)data)->mx_time),
be_short2le_short(((mx_pack *)data)->mx_value));
fflush(dataFile);
}
else
fprintf(stderr,"Bad packet size %d[should be %d]\n",length,sizeof(mx_pack ));
}
int main(int argc, char *argv[])
{
if (argc!=2)
dataFile=stdout;
else if ( ( dataFile = fopen(argv[1],"w") ) == NULL )
{
perror(argv[1]);
exit(2);
}
if ( lnp_init(0,0,0,0,0) )
{
perror("lnp_init");
exit(1);
}
else fprintf(stderr,"init OK\n");
lnp_integrity_set_handler (logdata_handler);
while (1)
sleep(1);
return 0;
}
W powyższym opisie pominęliśmy wiele cech i funkcji systemowych. Niektóre z nich zostały wykorzystane w przedstawionych przykładach, inne nie pojawiły się wcale. Podstawowym źródłem o funkcjach systemowych są dobrze skomentowane pliki nagłówkowe w /usr/local/crossgcc/legOS/include oraz procedury jądra systemu w /usr/local/crossgcc/legOS/kernel. W zamieszczonej poniższej tabelce opisane jest ogólne zastosowanie funkcji znajdujących się w niektórych plikach nagłówkowych. Innym źródłem informacji o funkcjach systemu jest pełny opis API legOS na oficjalnej stronie legOS.
| conio.h | wysoko-poziomowa obsługa wyświetlacza |
| dbutton.h | obsługa klawiszy RCX na poziomie portów |
| dkey.h | wysoko-poziomowa obsługa klawiszy RCX |
| sys/lcd.h | obsługa wyświetlacza na poziomie portów |
| dmotor.h | sterowanie silnikami |
| dsensor.h | obsługa czujników |
| dsound.h | dźwięki RCX |
| lnp/lnp-logical.h | warstwa logiczna sieciowego protokołu transmisji IR |
| lnp/lnp.h | niższe warstwy sieciowego protokółu transmisji IR |
| mem.h | definicja NULL i size_t |
| semaphore.h | semafory POSIX |
| stdlib.h | dynamiczne zarządzanie pamięcią, generator liczb losowych |
| string.h | obsługa bloków pamięci i ciągów znaków |
| time.h | czas systemowy |
| tm.h | zarządca zadań |