Przed przystąpieniem do zadania należy przygotować labirynt (makietę). W celu lepszego sterowania robotem dobrze jest również pozbyć się kleszczy z przodu (patrz budowa robota TriBot). Kleszcze mogą utrudniać poruszanie się robota po planszy. W ramach prezentowanego niżej algorytmu przyjęto:
pole(nr_pola,[nr_pomiaru,następne_pole,ultrasonic_value])
W ten sposób zdefiniowane pola można łączyć i budować prostą bazę wiedzy o otoczeniu robota. Czarne cyfry na rysunku przedstawiają numery poszukiwań. Pomiary przeprowadzane są co 450 więc jest ich 8. Czerwone cyfry oznaczają numery pól. Niebieskie cyfry są przypisanym numerem pola, do którego można się dostać podążając w kierunku pomiaru np. 3. Żółta cyfra 0 oznacza z kolei ścianę (przeszkodę), którą napotkamy w zadanym kierunku. Wartość ściany definiuje pomiar echosondy. Jeżeli odległość jest mniejsza niż ustalona wartość to wówczas przyjmujemy, że jest w tym miejscu ściana lub inna przeszkoda. Poniżej zaprezontowano algorytm, który przy wykorzystaniu takiej postaci wiedzy steruje robotem TriBot.
Rule: 1 if listOfAddedNodes.has?(current_node) == false then listOfAddedNodes.add(current_node) and node.add(current_node,[1,0,ultras_v]) then turn = 45 and node.add(current_node,[2,0,ultras_v]) then turn = 45 and node.add(current_node,[3,0,ultras_v]) then turn = 45 and node.add(current_node,[4,0,ultras_v]) then turn = 45 and node.add(current_node,[5,previous_node,ultras_v]) then turn = 45 and node.add(current_node,[6,0,ultras_v]) then turn = 45 and node.add(current_node,[7,0,ultras_v]) then turn = 45 and node.add(current_node,[8,0,ultras_v]) Rule: 2 if listOfAddedNodes.has?(current_node) == true then goTo(Rule 5) Rule: 3 if messure_no in [0;8] then messure_no++ and goTo(Rule 5) Rule: 4 if messure_no not in [0;8] then messure_no := 0 and goTo(Rule 7) Rule: 5 if ultras_v(current_node,messure_no) > X and next_node(current_node,messure_no) == 0 then turn = messure_no*45 and goStepFoward and node_counter++ and node.update(current_node,[messure_no,node_counter,ultras_v]) and current_node = node_counter and goTo(Rule 1) else goTo(Rule 3) Rule: 6 if messure_no in [0;8] then messure_no++ and goTo(Rule 7) else STOP Rule: 7 if ultras_v(current_node,messure_no) > X and next_node(current_node,messure_no) > 0 then turn = messure_no*45 and goStepFoward and current_node = next_node(current_node,messure_no) and node.update(current_node,[messure_no,-1,0]) and goTo(Rule 2) else goTo(Rule 6)
messured_nodes(node_no). node(node_no,messure_no,next_node,ultrasonic_val,messured). current_node(node_no,messure_no). current_messure(number). %---------------------- % dokonywanie pomiarów %---------------------- start :- current_node(X,_), node(X,Y,_,_), messured_nodes(Q), X \= Q, nxt_ultrasonic_sensor(port,Value1), assert(node(X,1,0,Value1), nxt_turn(speed,45), nxt_ultrasonic_sensor(port,Value2), assert(node(X,2,0,Value2), nxt_turn(speed,45), nxt_ultrasonic_sensor(port,Value3), assert(node(X,3,0,Value3), nxt_turn(speed,45), nxt_ultrasonic_sensor(port,Value4), assert(node(X,4,0,Value4), nxt_turn(speed,45), nxt_ultrasonic_sensor(port,Value5), assert(node(X,5,0,Value5), nxt_turn(speed,45), nxt_ultrasonic_sensor(port,Value6), assert(node(X,6,0,Value6), nxt_turn(speed,45), nxt_ultrasonic_sensor(port,Value7), assert(node(X,7,0,Value7), nxt_turn(speed,45), nxt_ultrasonic_sensor(port,Value8), assert(node(X,8,0,Value8) assert(messured_nodes(X)). %--------------------------- % szukanie najlepszej drogi %--------------------------- start :- current_node(X,M), node(X,Y,Next,U), messured_nodes(X), Next = 0, U <= number, % wartość progowa, ponad którą dokonywane jest przeszukiwanie retract(node(X,Y,_,_)), assert(node(X,Y,1000,U), % blokowanie połączenia current_messure(A), B is A + 1, retractall(current_messure(_)), assert(current_messure(B)). start :- current_node(X,M), node(X,Y,Next,U), messured_nodes(X), Next = 0, U > number, % wartość progowa, ponad którą dokonywane jest przeszukiwanie retract(node(X,Y,_,_)), nxt_ultrasonic_sensor(port,Value), assert(node(X,Y,X+1,Value), abs(M-Y,N), nxt_turn(speed,45*N), % przemieszczenie do nowego punktu nxt_go(1), retractall(current_node(X,_)), W is X + 1, assert(current_node(W,1)), assert(current_messure(1)). start :- current_node(X,M), node(X,Y,Next,U), messured_nodes(X), Next > 0, U > number, % wartość progowa, ponad którą dokonywane jest przeszukiwanie current_messure(D), D <= 8, % pominięcie punktu, zwiększenie licznika o 1 B is A + 1, retractall(current_messure(_)), assert(current_messure(B)). start :- current_node(X,M), node(X,Y,Next,U), messured_nodes(X), Next > 0, U > number, % wartość progowa, ponad którą dokonywane jest przeszukiwanie current_messure(D), D > 8, abs(M-Y,N), nxt_turn(speed,45*N), % przejscie do punktu, który już był wcześniej badany nxt_go(1), node(Next,P,X,U2), retractall(current_node(X,_)), assert(current_node(Next,P)), retractall(node(Next,P,_,_)), assert(node(Next,P,1000,U2)). % zablokowanie powrotu do poprzedniego punktu start :- go(0).