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());
}
}
}