To jest stara wersja strony!
Take the PlNXT API. Build tutorial and demo cases.
Łukasz Bandała - feanaro@student.agh.edu.pl
Celem projektu było przygotowanie demonstracji zastosowania PlNXT API. Zastanawiając się nad licznymi możliwościami prezentacji potencjału połączenia języka prolog oraz klocków NXT, ostatecznie zdecydowałem się na budowę i oprogramowanie robota umożliwiającego układanie kostki rubika. Język prolog doskonale nadaje się do rozwiązywania tego typu problemów.
Podstawowym problemem była ograniczona ilość silników i elementów konstrukcyjnych. W pierwszej kolejności należało sprecyzować zadania robota:
Posiadając tylko 3 silniki można się spodziewać sporych problemów z wykonaniem wszystkich tych czynności. Okazuje się jednak, że problemem tym zajął się ktoś już kiedyś.
Konstrukcja Tilted Twister jest dokładnie tym czego poszukiwałem. Wykorzystująca wszelkie elementy w podstawowym zestawie NXT w sposób wysoce optymalny, umożliwia wykonywanie wszystkich wymienionych powyżej czynności. Można to zobaczyć tutaj.
Kompletną instrukcję budowy można pobrać ze strony projektu. Ponadto znajdują się tam również kody źródłowe dla C++.
Posiadając już zbudowaną konkretną konstrukcję, nie pozostało mi nic innego jak przystąpić do próby napisania oprogramowania w języku Prolog z wykorzystaniem biblioteki PlNXT.
Biblioteka składa się z kilku modułów. Najbardziej zewnętrznymi (z punktu widzenia użytkownika) z nich są 3 następujące: nxt_movment, threads oraz nxt_sensomoto. Pierwszy jest skierowany do podstawowych robotów jeżdżących. Stąd jego mała przydatność w tym projekcie. Druga dotyczy wywoływaczy (triggers) oraz liczników. Jak okazało się podczas prac, również ten moduł jest mało użyteczny przy oprogramowywaniu twistera. Trzecia natomiast posiada podstawowe czynności związane z komponentami NXT, co okazało się zdecydowanie nabardziej przydatne.
W celu ujednoznacznienia wykorzystywanych zwrotów, wprowadziłem drobny podział w nazewnictwie:
Uwaga! Początkowa próba wykorzystania wbudowanego w biblotece predykatu nxt_motor(Motor,Speed,angle(Angle)) nie przyniosła oczekiwanego rezultatu. Obroty silników o ustalony kąt były zdecydowanie nieprecyzyjne i pozostawiały bardzo dużo do życzenia jeżeli chodzi o jakikolwiek determinizm. Ratunkiem w ów sytuacji okazał się predykat nxt_motor_get_rotations(Motor,Rot_count). Umożliwił on zaimplementowanie prostego regulatora P. Jak wiadomo, zdecydowanie najlepiej budować serwomechanizmy w oparciu o regulator typu PD. Jednakże w tej sytuacji lekko zmodyfikowane P okazało się wystarczające. Pobierając dane o pozycji silnika, wypracowywane jest odpowiednie sterowanie.
Pojawił się niestety jeszcze jeden kłopot. Otóż ze względu na zbliżanie się silnika do zadanej wartości, stopniowo jest mu zadawana coraz mniejsza prędkość (w założeniu jest ona z przedziału 900 do 0). Niestety przy bardzo niskich wartościach i jakimkolwiek obciążeniu silnika, nie jest w stanie się on poruszyć. Konieczne zatem było dodanie pewnej tolerancji błędu pozycji oraz przełączenie na regulację dwupołożeniową w bliskim otoczeniu wartości zadanej. Wprowadzenie tej tolerancji wymogło również stosowanie pomiaru pozycji absolutnej (od włączenia układu) zamiast o wiele łatwiejszego w implementacji, pomiaru pozycji względnej ( w stosunku do poprzedniej pozycji - powoduje to stopniową kumulację błędu). Skomplikowało to zdecydowanie predykaty odpowiedzialne za wszelki ruch.
Jest to podstawowy obrót. Może odbywać się zgodnie z ruchem wskazówek zegara lub odwrotnie. Zakładając prawidłową pozycję obrotowej platformy, obrót ten odbywa się głównie o 90 stopni. Wyjątkiem jest sytuacja analizy rozmieszczenia kolorowych kwadratów na kostce. W tym przypadku wykorzystywany jest również obrót o 45 stopni. Z tego powodu zaimplementowane są w zasadzie 4 predykaty obrotu platformy. 2 z nich odpowiadają za obroty o 90 stopni (jeden z pozycji początkowej plus wielokrotność kąta 90 stopni, drugi z pozycji początkowej przesuniętej o 45 stopni plus wielokrotność kąta 90 stopni). Kolejne 2 natomiast odpowiadają za ruch o 45 stopni (przejście i powrót z pozycji początkowej do pozycji przesuniętej).
W sposób empiryczny sprawdziłem ilość obrotów silnika odpowiadających za 90 stopniowy obrót (można to również próbować przeliczyć na podstawie stosunku ilości zębów zębatek - patrz: przekładnia ).
Na obrót o kąt 90 stopni platformy przypada 315 kroków.
Poniżej prezentuję kody źródłowe poszczególnych obrotów:
% rotacje calej kostki rr :- wait_till(wolny), retract(wolny), ruch_A(-1,1.7), sleep(0.3), assert(wolny). rl :- wait_till(wolny), retract(wolny), ruch_A(+1,1.7), sleep(0.3), assert(wolny). % wzmocnione rotacje calej kostki rrs :- wait_till(wolny), retract(wolny), ruch_A(-1,2.3), sleep(0.3), assert(wolny). rls :- wait_till(wolny), retract(wolny), ruch_A(+1,2.3), sleep(0.3), assert(wolny). ruch_A(Z,M) :- nxt_motor_get_rotations('A',V1), % badanie polozenia w odniesienniu do absolutnej poczatkowej pozycji V2 is V1+157 ,V3 is V2-(V2 mod 315), repeat, nxt_motor_get_rotations('A',R), X is (R-V3), P is (Z*315-X)*M, dwupolozeniowy(P,P2,25), % zmiana regulatora przy odpowiednim progu nxt_motor('A',P2), A is abs(X), % tolerancja roznicy polozenia A =< 317, A >= 315, nxt_motor('A',0).
% rotacje przy pozycji przesuniętej o 45 stopni ar :- wait_till(wolny), retract(wolny), ruch_A_45(-1), sleep(0.3), assert(wolny). al :- wait_till(wolny), retract(wolny), ruch_A_45(+1), sleep(0.3), assert(wolny). ruch_A_45(Z) :- nxt_motor_get_rotations('A',V1), % badanie polozenia w odniesienniu do absolutnej poczatkowej pozycji V2 is V1+315 ,V3 is V2-(V2 mod 315), repeat, nxt_motor_get_rotations('A',R0), R is R0+157, X is (R-V3), P is (Z*315-X)*1.7, dwupolozeniowy(P,P2,25), % zmiana regulatora przy odpowiednim progu nxt_motor('A',P2), A is abs(X), % tolerancja roznicy polozenia A =< 316, A >= 314, nxt_motor('A',0).
dopelnienie_do_90 :- nxt_motor_get_rotations('A',V), G is V - (V mod 315), repeat, nxt_motor_get_rotations('A',R), P is (G-R)*1.5, dwupolozeniowy(P,P2,25), % zmiana regulatora przy odpowiednim progu nxt_motor('A',P2), A is abs(R), % tolerancja roznicy polozenia B is abs(G), A =< B+1, A >= B-1, nxt_motor('A',0),sleep(0.1). dopelnienie_do_45 :- nxt_motor_get_rotations('A',V), G is V - ((V+157) mod 315), repeat, nxt_motor_get_rotations('A',R), P is (G-R)*1.5, dwupolozeniowy(P,P2,25), % zmiana regulatora przy odpowiednim progu nxt_motor('A',P2), A is abs(R), % tolerancja roznicy polozenia B is abs(G), A =< B+1, A >= B-1, nxt_motor('A',0),sleep(0.1).
Obrót ten zmienia orientację w poziomie czyli przerzuca kostkę na inną stronę. Ramię jest wyrzucane i cofane. Uderzenie w kostkę powoduje przeniesienie punktu równowagi. Grawitacja dokonuje reszty przywracając kostce prawidłową pozycję na platformie. Konstrukcja twistera do tej złożonej czynności wykorzystuje jedynie silnik 'C'. Jest to zarazem jej największa zaleta (pozostałe silniki mogą być wykorzystane przy innych czynnościach) i wada (obrót ten nie zawsze kończy się sukcesem). Kod odpowiedzialny za ten obrót również zawiera regulator:
% przerzucenie kostki f :- wait_till(wolny), retract(wolny), nxt_stop, nxt_motor('C',500), % wyrzut ramienia sleep(0.5), cofnij, % cofnięcie ramienia sleep(0.4), assert(wolny). cofnij :- repeat, nxt_motor_get_rotations('C',R), P is (-R)*9, dwupolozeniowy(P,P2,100), % zmiana regulatora przy odpowiednim progu nxt_motor('C',P2), A is abs(R), % tolerancja roznicy polozenia A =< 5, nxt_motor('C',0).
Obrót ten jest prawdopodobnie najbardziej podatny na zmianę kostki, niedokładny obrót platformy i niedopasowanie klocków (za mocno lub za słabo dociśnięte klocki w różnych miejscach).
Właściwa i główna czynność dotycząca kostki Rubika. Łączy w sobie częściowo wszystkie powyższe czynności. Ramię jest lekko wysuwane i tym samym utrzymuje 2 górne linie kostki, nie dopuszczając do ich obrotu. Następnie wykonywany jest obrót platformy, który powoduje ruch dolnej warstwy kostki.
% ruchy kostki mr :- ruch_C_gora(38),wait_till(nxt_is_stopped),rrs,wait_till(nxt_is_stopped),ruch_C_dol,sleep(0.4). ml :- ruch_C_gora(38),wait_till(nxt_is_stopped),rls,wait_till(nxt_is_stopped),ruch_C_dol,sleep(0.4). ruch_C_gora(G) :- retract(wolny), repeat, nxt_motor_get_rotations('C',R), P is (G-R)*5, dwupolozeniowy(P,P2,25), % zmiana regulatora przy odpowiednim progu nxt_motor('C',P2), A is abs(R), % tolerancja roznicy polozenia A =< G+1, A >= G, nxt_motor('C',0), assert(wolny). ruch_C_dol :- retract(wolny), repeat, nxt_motor_get_rotations('C',R), P is (-R)*5, dwupolozeniowy(P,P2,25), % zmiana regulatora przy odpowiednim progu nxt_motor('C',P2), A is abs(R), % tolerancja roznicy polozenia A =< 5, nxt_motor('C',0), assert(wolny).
% regulator dwupołożeniowy (w zasadzie trójpołożeniowy) dwupolozeniowy(P,P2,_) :- P == 0, P2 is 0. dwupolozeniowy(P,P2,T) :- P <0, P > -T , P2 is -T. dwupolozeniowy(P,P2,T) :- P >0, P < T , P2 is T. dwupolozeniowy(P,P2,T) :- (P > T ; P < -T), P2 is P.
Konstrukcja robota wykorzystuje 3 czujniki: świetlny, ultrasoniczny i dotykowy. W oprogramowaniu wykorzystywane są jedynie 2 z nich. Przeznaczeniem obu jest odczytywanie rozmieszczenia kolorowych kwadratów na kostce. Wykorzystywane są do tego predykaty odpowiadające za obroty. Oto kody odpowiadające za poszczególne czynności:
pozycjonuj :- repeat, nxt_motor('B',-150), nxt_touch(V,force), V==1, nxt_motor('B',0), nxt_motor_reset_position('B',absolute), sleep(0.1).
obrot_ramienia(Z,T) :- obrot_ramienia(Z,T,T). obrot_ramienia(Z,T1,T2) :- retract(wolny), repeat, nxt_motor_get_rotations('B',R), P is (Z-R)*2, dwupolozeniowy(P,P2,5), nxt_motor('B',P2), R =< Z+T1, % tolerancja roznicy polozenia R >= Z-T2, nxt_motor('B',0), assert(wolny), sleep(0.2).
czytaj_sciane(sciana(V1b,V2b,V3b,V4b,V5b,V6b,V7b,V8b,V9b)) :- wait_till(nxt_is_stopped), obrot_ramienia(155,0), nxt_light(V1a), kolor(V1a,V1b), obrot_ramienia(187,0), nxt_light(V2a), kolor(V2a,V2b), rl, wait_till(nxt_is_stopped), nxt_light(V3a), kolor(V3a,V3b), rl, wait_till(nxt_is_stopped), nxt_light(V4a), kolor(V4a,V4b), rl, wait_till(nxt_is_stopped), nxt_light(V5a), kolor(V5a,V5b), rl, obrot_ramienia(177,0), wait_till(nxt_is_stopped), dopelnienie_do_45,wait_till(nxt_is_stopped), nxt_light(V6a), kolor(V6a,V6b), al,wait_till(nxt_is_stopped), nxt_light(V7a), kolor(V7a,V7b), al,wait_till(nxt_is_stopped), nxt_light(V8a), kolor(V8a,V8b), al,wait_till(nxt_is_stopped), nxt_light(V9a), kolor(V9a,V9b), dopelnienie_do_90,rl,rl, obrot_ramienia(54,4). kolor(X,Y) :- X =< 20, Y=w. % biały kolor(X,Y) :- X >= 21, X =< 30, Y=y. % żółty kolor(X,Y) :- X >= 31, X =< 37, Y=g. % zielony kolor(X,Y) :- X >= 38, X =< 44, Y=r. % czerwony kolor(X,Y) :- X >= 45, X =< 49, Y=o. % pomarańczowy kolor(X,Y) :- X >= 50, Y=b. % niebieski
badaj(cube(V1, V2, V3, V4, V5, V6, V7, V8, V9, V10, V11, V12, V13, V14, V15, V16, V17, V18, V19, V20, V21, V22, V23, V24, V25, V26, V27, V28, V29, V30, V31, V32, V33, V34, V35, V36, V37, V38, V39, V40, V41, V42, V43, V44, V45, V46, V47, V48, V49, V50, V51, V52, V53, V54)) :- pozycjonuj, obrot_ramienia(54,4), nxt_light_LED(activate), % czytanie ścian normalne odczyt(V1, V2, V3, V4, V6, V7, V8, V9, V10, V11, V12, V13, V15, V16, V18, V19, V20, V21, V22, V23, V24, V25, V27, V28, V30, V31, V32, V33, V34, V36, V38, V39, V40, V41, V42, V43, V44, V45, V46, V47, V48, V49, V51, V52, V53), % ściana czytana do góry nogami czytaj_sciane(sciana(V5,V29,V17,V14,V26,V37,V54,V35,V50)), rl,f,rl. odczyt(V1, V2, V3, V4, V6, V7, V8, V9, V10, V11, V12, V13, V15, V16, V18, V19, V20, V21, V22, V23, V24, V25, V27, V28, V30, V31, V32, V33, V34, V36, V38, V39, V40, V41, V42, V43, V44, V45, V46, V47, V48, V49, V51, V52, V53) :- czytaj_sciane(sciana(V3,V9,V21,V27,V15,V40,V32,V44,V36)), f, czytaj_sciane(sciana(V1,V10,V7,V13,V16,V41,V47,V39,V49)), f, czytaj_sciane(sciana(V6,V24,V12,V18,V30,V46,V34,V42,V38)), f, czytaj_sciane(sciana(V4,V19,V22,V28,V25,V43,V51,V45,V53)), rr,f, czytaj_sciane(sciana(V2,V11,V23,V20,V8,V48,V33,V52,V31)), f,f.
