Zdalne sterowanie robotem NXT
Transkrypt
Zdalne sterowanie robotem NXT
Zdalne sterowanie robotem NXT Komunikacja z robotem, lista obsługiwanych poleceo, przykłady nawiązywania połączenia 1. Wstęp: Robot LEGO NXT komunikuje się z komputerem wykorzystując technologię Bluetooth. Do poprawnego działania połączenia niezbędne są sterowniki Fantom Driver do pobrania z http://mindstorms.lego.com/en-us/support/files/default.aspx#Driver. Dodatkowo, ponieważ projekt wykonywany jest przy użyciu firmware’u LeJOS, przy kompilacji programów niezbędne będą również biblioteki Javy NXJ do komunikacji z robotem, do pobrania z : http://lejos.sourceforge.net/nxj-downloads.php. 2. Instrukcje sterowania robotem Wszystkie instrukcje sterowania robotem są obecnie kodowane w postaci pojedynczej, 32-bitowej liczby całkowitej. Ideowo podobne są one do instrukcji procesora gdzie bity na odpowiedniej pozycji kodują różne polecenia wraz ze swoimi prefiksami i sufiksami. W odróżnieniu od instrukcji procesora polecenia sterujące są kodowane dziesiątkowo (cyfra na odpowiedniej pozycji odpowiada danemu poleceniu) co ułatwia debugging. Docelowo komendy będą kodowane binarnie przy użyciu minimalnej długości instrukcji, jeżeli okaże się że przepustowośd łącza Bluetooth wymaga optymalizacji ilości przesyłanych informacji (mało prawdopodobne). Obecnie wszystkie instrukcje są 5-cyfrowymi liczbami całkowitymi. Pozycją pierwszą nazywad będziemy najbardziej znaczącą cyfrę. W każdej instrukcji odpowiednie pozycje odpowiadają za: 1 pozycja: kategoria polecenia 2 pozycja: podkategoria polecenia pozycje 3-5: dana liczbowa np. instrukcja 31025 jest rozumiana jako: 3 – polecenie kategorii 3 (jazda na wprost na zadaną odległośd) 1 - podkategoria 1 (jazda do tyłu 025 – wartośd liczbowa 25, (odległośd 25 cm) Po otrzymaniu takiej instrukcji robot powinien przemieścid się w linii prostej do tyłu o 25 cm. 3. Kompletna lista obecnie obsługiwanych poleceń: 1xxxx – Kategoria poleceo ogólnych 10xxx - podkategoria poleceo systemowych 10000 – Przerwij wszystkie działania. Zatrzymaj się używając hamulców 10999 – Rozłącz, czekaj na kolejne połączenie 2xxxx – Kategoria poleceo jazdy w zadanym kierunku. Ruch będzie utrzymywany aż do odebrania kolejnego polecenia. 20xxx – domyślna podkategoria 20000 – zatrzymaj się używając hamulców 20001 – jedź do przodu, prosto 20002 – jedź po łuku do przodu w prawo 20003 – obrót w miejscu w prawo 20004 – jedź po łuku do tyłu w prawo 20005 – jedź do tyłu, prosto 20006 – jedź po łuku do tyłu w lewo 20007 – obrót w miejscu w lewo 20008 – jedź po łuku do przodu w lewo 20009 – zatrzymaj się bez używania hamulców 3xxxx – kategoria poleceo jazdy na wprost na zadaną odległośd (w cm) 30xxx – jazda do przodu 30001 : 30999 – jazda na wprost na odległośd 1-999 cm 31xxx – jazda do tyłu 31001 : 31999 – jazda do tyłu na odległośd 1-999 cm 4xxxx – kategoria poleceo obrotu o zadany kąt skierowany 40xxx – obrót o dodatni kąt skierowany (obrót przeciwnie do ruchu wskazówek zegara) 40001 : 40999 – obrót o kąt 1-999 stopni 41xxx – obrót o ujemny kąt skierowany (obrót zgodnie z ruchem wskazówek zegara) 41001: 41999 – obrót o kąt ujemny 1-999 stopni 5xxxx - kategoria bezpośrednich poleceo uniesienia / opuszczenia kamery 50xxx-domyślna podkategoria 50000 – zatrzymaj ruch, utrzymuj położenie kamery aktywnie 50001 – pochyl kamerę w dół 50002 – pochyl kamerę w górę 50004 – zatrzymaj ruch, utrzymuj kamerę pasywnie 50005 – zatrzymaj ruch poprzez odcięcie zasilania do silnika, brak utrzymywania położenia kamery 6xxxx – kategoria pochylenia kamery o kąt (w przypadku podania kąta większego niż ograniczenia robot automatycznie ustawia się w największym dozwolonym położeniu) 60xxx – pochylenie kamery w dół 60001:60999 – pochylenie o zadany kąt (w praktyce maks ok. 75) 61xxx – pochylenie kamery w górę 61001:61999 – pochylenie o zadany kąt (w praktyce maks ok. 75) 7xxxx – kategorie ustawienia kamery na zadany kąt pochylenia (dozwolony zakres kątów pochylenia kamery to [-25,55], po przekroczeniu, robot automatycznie ustawia się na odpowiedni koniec przedziału) 70xxx – kąt dodatni (pochylenie w dół) 70000 – wypoziomowanie (mniejwięcej) kamery 70001:70999 ustawienie kamery na zadany kąt (w praktyce maks 25) 71xxx – kąt ujemy (pochylenie w górę) 71001:71999 ustawienie kamery na zadany kąt (w praktyce maks 55) 8xxxx – kategoria poleceo-zapytao odnośnie odczytów z czujników robota. Wymaga odebrania informacji zwrotnej w postaci pojedynczej wartości całkowitej po każdym wysłanym poleceniu. 80000 – zapytanie o stan baterii, odczyt z czujnika odległości oraz kąt pochylenia kamery. Zwraca pojedynczą liczbę int. Dane zapisane w sposób: 100000*Kąt pochylenia+1000*odległośd+napięcie/10 (3 pierwsze liczby kąt pochylenia, jeżeli kąt ujemny cała liczba będzie ujemna, 3 kolejne liczby to odległośd, 3 ostatnie to napięcie/10 w miliwoltach) Obecnie odpytywanie częściej niż co 500 milisekund jest bezcelowe, gdyż dane z czujników odświeżane są 2 razy na sekundę. Możliwośd zwiększenia jeżeli wartośd ta okaże się niewystarczająca 9xxxx – kategoria poleceo sterowania mocą (prędkością) silników 90xxx – ustawienie prędkości silników na portach A i B (silniki napędowe) 90001 : 90100 – ustawienie prędkości obrotowej silników w zakresie (1 – 100) *9 *deg/s+ (teoretycznie, zależy od poziomu baterii i obciążenia silników. W uproszczeniu w zakresie 1 – 100%) 91xxx – ustawienie prędkości silników na porcie C (silnik pochylania) Zalecane prędkości w zakresie( 0-30) 91001 : 91100 – ustawienie prędkości obrotowej silnika w zakresie (1 – 100) *9 *deg/s+ (teoretycznie, zależy od poziomu baterii i obciążenia silników. W uproszczeniu w zakresie 1 – 100%) Dowolna liczba ujemna – powoduje zatrzymanie, zamknięcie połączenia i wyłączenie robota. 4. Przykładowy program umożliwiający połączenie z NXT /* prosty program do komunikacji dwukierunkowej z NXT. Program łączy się * z pierwszym znalezionym NXT i wysyła liczby całkowite podane w terminalu. * Zakładamy że NXT odbiera liczbe, mnoży ją prze -1 i odsyła z powrotem. * Program odbiera przesłane liczby i wyświetla je w terminalu. * * @author Mayday * na podstawie jednego z przykladów ze strony http://lejos.sourceforge.net/ */ import lejos.pc.comm.*; //biblioteka LeJOS dla komputerów do komunikacji z NXT import java.io.*; // biblioteka we-wy javy import java.util.Scanner; // skaner potrzebny do wprowadzania wartości z lini komend public class BTSend { public static void main(String[] args) { // utworzenie obiektu klasy connector identyfikującej połaczeni z NXT NXTConnector conn = new NXTConnector(); // łączenie z dowolnym NXT boolean connected = conn.connectTo("btspp://"); if (!connected) { //reakcja na błąd połączenia z NXT System.err.println("Błąd połączenia z NXT!"); System.exit(1); } //utworzenie obiektów strumieni wejścia i wyjścia DataOutputStream dos = conn.getDataOut(); DataInputStream dis = conn.getDataIn(); int i=1; // obiekt skaner pozwalający na wprowadzanie liczb z konsoli Scanner sc = new Scanner(System.in); //wysyłaj i odbieraj dopóki nie podane 0 while(i!=0) { //odczytanie kolejnego inta z konsoli i = sc.nextInt(); // struktura try - catch nizbęda do obsługi wyjątków try { System.out.println("Wysylam " + i); //wpisanie inta do strumienia wyjscia dos.writeInt(i); //"spłukanie" strumienia (wysłanie wartosci) dos.flush(); //obsługa wyjątka we-wy } catch (IOException ioe) { System.out.println("Wyjatek IO podczas wysylania:"); System.out.println(ioe.getMessage()); break; } try { //odczytanie inta ze struminia wejscia i wypisanie na ekran System.out.println("Odebrano: " + dis.readInt()); } catch (IOException ioe) { System.out.println("Wyjatek IO podczas odczytu:"); System.out.println(ioe.getMessage()); break; } } try { dis.close();//zamknięcie strumienia we dos.close();//zamknięcie strumiania wy conn.close(); //zamknięcie połączenia z NXT } catch (IOException ioe) { System.out.println("Wyjątek IO podczas zamykania polaczenia:"); System.out.println(ioe.getMessage()); } } }