Autorzy:
Temat:
Podczas testów poszczególnych modułów robota okazało się iż wszystkie poza system podnoszenia platformy z chwytakiem działają bez zarzutu. Duża waga górnej platformy powoduje niestabilność konstrukcji i co za tym idzie małą precyzje w ruchach robota w pionie. Problem ten został rozwiązany poprzez zastosowanie sensora dotyku w chwytaku (robot nie musi już precyzyjnie pozycjonować chwytaka w pionie). Innym rozwiązaniem problemu byłoby użycie części (drugiego „największego” koła zębatego) z kolejnego zestawu NXT, jednak takie rozwiązanie wykraczało poza pierwotne założenia naszego projektu zakładające użycie tylko jednego zestawu NXT.
% % AGH Kraków, 2009 % % Podstawy Inżynierii Wiedzy - PROJEKT % NXT Cases: Problem wieży Hanoi % % Autorzy: Bartosz Beza, Błażej Biesiada % :- consult('plnxt.pl'). :- dynamic(current_rod/1). %jedna z 3 pozycji (left,center,right) w ktorej znajduje sie ramie robota :- dynamic(current_squeeze/1). %aktualny poziom zacisku chwytaka :- dynamic(neutral_squeeze/1). %chwytak w pozycji luznej start :- assert(current_rod(center)), nxt_open, nxt_motor_get_rotations('A',Squeeze_val), %sprawdzenie aktualnej pozycji silnika chwytaka assert(current_squeeze(Squeeze_val)), assert(neutral_squeeze(Squeeze_val)), move_disk(4,left,right,center). stop :- nxt_close. %%%%%%%%% Logika rozwiazujaca problem wiez hanoi: %%%%%%%%% %%move_disk(N,Left,Right,Center) % % Przenosi N krazkow ze stosu Left na stos Right przy pomocy stosu Center. move_disk(1,Left,Right,_) :- current_rod(Cur_rod), move_arm_from_to(Cur_rod,Left), pick_disk, current_rod(Cur_rod), move_arm_from_to(Cur_rod,Right), drop_disk. move_disk(N,Left,Right,Center) :- N>1, M is N-1, move_disk(M,Left,Center,Right), move_disk(1,Left,Right,_), move_disk(M,Center,Right,Left). %%%%%%%%% Kontrola robota: %%%%%%%%% %% pick_disk % % Opuszcza ramie robota dopoki sensor dotyky nie zostanie nacisniety, natepnie zaciska chwytak % i podnosi ramie do pozycji poczatkowej. pick_disk :- nxt_motor_get_rotations('B',Start_count), nxt_motor('B', 50), wait_till(nxt_touch_is_pressed), nxt_motor('B', 0), squeeze, nxt_motor_get_rotations('B',End_count), Back_count is Start_count - End_count, nxt_motor('B', -50, Back_count), wait_till(not(nxt_motor_is_moving('B'))). %% squeeze % % Stopniowo zaciska chwytak na krazku, badajac za kazda iteracja o ile poruszyl sie silnik, tak % dlugo az nie bedzie mozliwe dlasze zaciskanie chwytaka. squeeze :- current_squeeze(Squeeze_val), retract(current_squeeze(Squeeze_val)), squeeze(50,50), nxt_motor_get_rotations('A',End_count), assert(current_squeeze(End_count)). squeeze(Speed,Diff) :- Diff >= Speed, nxt_motor_get_rotations('A',Start_count), nxt_motor('A', Speed, Speed), nxt_motor_get_rotations('A',End_count), wait_till(not(nxt_motor_is_moving('A'))), Diff_new is End_count-Start_count+10, %10 jest stala aby wyeliminowac blad pomiarowy squeeze(Speed,Diff_new). squeeze(Speed,Diff) :- Diff < Speed. %% drop_disk % % Opuszcza ramie robota dopoki sensor dotyky nie zostanie nacisniety, natepnie uwalnia krazek % z chwytaka i podnosi ramie do pozycji poczatkowej. drop_disk :- nxt_motor_get_rotations('B',Start_count), nxt_motor('B', 50), wait_till(nxt_touch_is_pressed), nxt_motor('B', 0), release, nxt_motor_get_rotations('B',End_count), Back_count is Start_count - End_count, nxt_motor('B', -50, Back_count), wait_till(not(nxt_motor_is_moving('B'))). %% release % % Uwalnia krazek z chwytaka, wracajac do pozycji neutralnej. release :- neutral_squeeze(Squeeze_val1), current_squeeze(Squeeze_val2), Rotations is Squeeze_val1 - Squeeze_val2, nxt_motor('A', 50, Rotations), wait_till(not(nxt_motor_is_moving('A'))). %% move_arm_from_to(Cur_rod,Rod) % % Porusza ramieniem robota znad stosu Cur_rod do stosu Rod. move_arm_from_to(Cur_rod,Rod) :- write(Rod), move_arm_to(Rod), retract(current_rod(Cur_rod)), assert(current_rod(Rod)), write(Rod), wait_till(not(nxt_motor_is_moving('C'))), write(Rod). %% move_arm_to(Rod) % % Predykat pomocniczy, porusza ramieniem robota z aktualnego stosu do stosu Rod. move_arm_to(center) :- current_rod(left), nxt_motor('C', 200, 1000). move_arm_to(center) :- current_rod(right), nxt_motor('C', -200, 1000). move_arm_to(center). move_arm_to(left) :- current_rod(center), nxt_motor('C', -200, 1000). move_arm_to(left) :- current_rod(right), nxt_motor('C', -200, 2*1000). move_arm_to(left). move_arm_to(right) :- current_rod(left), nxt_motor('C', 200, 2*1000). move_arm_to(right) :- current_rod(center), nxt_motor('C', -200, 1000). move_arm_to(right). %% nxt_touch_is_pressed % % Predykat pomocniczy, do wygodnego oczekiwania w kodzie na nacisniecie przycisku sensora dotyku. nxt_touch_is_pressed :- nxt_actions_touch_sensor('S1',Value), Value = 1.