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:
Obrót o 90 stopni z pozycji początkowej - niewiadoma 'Z' jest mnożnikiem opowiedzialnym za kierunek (wartości to +1 lub -1). Niewiadoma 'M' odpowiada jest współczynnikiem P regulatora. Obrót ten wykorzystywany jest również przy ruchach kostką, stąd większe obciążenie i konieczność zwiększenia współczynnika.
% 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).
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).
Obrót ramienia - jest to predykat odpowiadający za ruch ramienia z czujnikiem światła. Podobnie jak w przypadku pozostałych silników również tutaj potrzebna jest regulacja. Predykat ten przyjmuje niewiadomą 'Z', która jest wartością zadaną oraz 2 (lub tylko 1) niewiadome 'T', które są dolną i górną granicą tolerancji położenia.
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.