Micromouse

Ciągle zastanawiam się, czy nazwa Micromouse ma swoje uzasadnienie dla tego robota. Nie jest to micromouse ze ścisłej definicji, ale wykonuje dokładnie takie same zadania. Po pierwsze labirynt nie ma ścian, a możliwy tor jazdy jest oznaczony. Po drugie każde pole labiryntu jest oznaczone dokładnie tak samo jak skrzyżowanie (choć to akurat można by pominąć). I po trzecie labirynt nie jest pełnowymiarowy. Na zawodach labirynt ma wymiary 15x15. Ja ograniczyłem się do 10x10 ze względu na ogrom zajmowanego przez nie miejsca.

Podsumowując, robot jedynie zachowuje się jak regulaminowy micromouse. Niemniej jednak nie chodziło mi o zniewalającą wydajność i zgodność z zasadami (co wymuszałoby kupno drogich czujników laserowych), ale raczej o wyzwanie programistyczne i radość patrzenia, jak zabawka z Lego znajduje wyjście z dość skomplikowanego labiryntu. A teraz do szczegółów:

Robot został zbudowany na podstawie na dwóch kołach z przodu. Posiada jedno koło tylne obracające się swobodnie. Nie pierwszy raz używałem trójkołowych konstrukcji i wiem, że zapewniają ogromną zwrotność i dokładność. Przed robotem umieszczony jest na obrotnicy czujnik koloru, mający możliwość skanowania planszy w zakresie -90° - +90°. Nad całością góruje kostka NXT.

Zadanie postawione przed robotem nie było łatwe. Miał on przejść przez labirynt od znanego punktu A do B znając jedynie współrzędne tych punktów. Chcę zaznaczyć, że konfiguracja labiryntu NIE ZNAJDOWAŁA SIĘ w pamięci robota przed startem. Wymuszało to potrzebę ciągłego skanowania trasy po dotarciu do nowego pola.

Mapa labiryntuTo jest mapa labiryntu z narysowanymi wszystkimi możliwymi ścieżkami. Każde pole zostało oznaczone czerwonym kwadratem, a trasy zaznaczone czarną linią. Pojedyncze pole miało rozmiary 20x20 cm. W poniższym filmie robot przechodził od lewego dolnego do prawego górnego rogu.

Teraz trochę o zastosowanym algorytmie. Użyłem dość popularnego algorytmu propagacji fali, lub jak kto woli rozlewania wody. Obrazowo można go sobie wyobrazić w następujący sposób: W miejscu startu wlewamy do labiryntu wodę, która może płynąć dowolnie, ale nie przez przekątne. Czekamy aż woda dotrze do mety. Oczywiste jest, że najkrótsza trasa to ta, po której woda płynęła. Nie chcę tutaj zagłębiać się w szczegóły algorytmiczne. W zamian odsyłam tutaj i tutaj po więcej informacji.

Oczywiście warto do program zmodyfikować tak, aby korzystał z zebranych wcześniej informacji. Po pierwsze nie należy skanować pól na których się już było wcześniej. Jest to raczej oczywiste. Dodatkowo nie ma potrzeby skanowania w poszukiwaniu dróg do pół w których się już wcześniej było i wykonywało skanowanie "z drugiej strony". Dlatego warto przechowywać informacje nie tylko o dostępnych drogach, ale i o zeskanowanych ścianach. Zostało to pokazane na filmie.

Krok 1Krok 2Krok 3Krok 4Do kodu dodałem również funkcję rysującą na ekranie to, co robot już wie. Na tych zrzutach widać wyraźnie, jak robot trafia w ślepe zaułki, oraz jak dojeżdża do mety nie będąc we wszystkich polach.

Robot postępuje w następujący sposób: skanuje pole na którym jest (jeśli trzeba) i oblicza optymalną trasę (jeżeli wykonywał skanowanie), następnie obraca się w odpowiednim kierunku i używa czujnika do podążania za czarną linią aż do następnego pola, aż trafi na metę. Wydaje się proste, a jednak napisanie tego wszystkiego zajęło mi kilka dni i wynikiem jest ponad 400 linii kodu.

Nie ukrywam, że nie optymalizowałem kodu pod szybkość działania. Ważniejsza była dokładność i bezbłędny przejazd za każdym razem. Testowy labirynt robot rozwiązywał w czasie 15 minut. Wiem, że długo. Zapraszam do obejrzenia filmu (przyspieszonego oczywiście). Kiedyś może wrócę do tego projektu, a tymczasem chętnie zobaczę lepsze konstrukcje.

Wideo



Galeria

Galeria 3D

Obrazy w tej galerii możesz oglądać jedynie przy użyciu specjalnych czerwono-niebieskich okularów. Można je nabyć w wielu sklepach internetowych za bardzo małe pieniądze.


Kod programu

//author: Krzysztof Kapusta, All rights reserved #define MOTORS OUT_BC #define MSEN OUT_A #define SEN S3 #define TOUCH S1 // definitions for the sensor #define IN_LEFT (SensorBoolean(TOUCH)) #define RIGHT_TO_LEFT_DEG (750) #define CENTER_TO_LEFT_DEG (RIGHT_TO_LEFT_DEG/2) // color sensor handling ColorSensorReadType _csr; void readSensor() { _csr.Port = SEN; SysColorSensorRead(_csr); } #define READ readSensor() #define INTENS (_csr.RawArray[INPUT_RED]) #define RED (_csr.ColorValue == INPUT_REDCOLOR) // end color sensor handling typedef byte Dir; #define N 1 #define E 2 #define S 4 #define W 8 struct Point { byte x; byte y; }; Point makePoint(byte x, byte y) { Point ret; ret.x = x; ret.y = y; return ret; } ////////// PHYSICAL PART: THESE FUNCTIONS CONTROLL THE MOVEMENT ONLY ////////// void reset() { if (!IN_LEFT) OnFwd(MSEN, 40); while (!IN_LEFT); Off(MSEN); RotateMotor(MSEN, 50, -CENTER_TO_LEFT_DEG); } void init() { SetSensorTouch(TOUCH); reset(); SetSensorColorFull(SEN); ClearScreen(); } // follows LEFT edge of the line to the RED point (intersection) #define Kp 0.09 // the grater it is the more unstable the robot is #define MED 385 // medium light value between black and white #define FOLL_PWR 25 // speed during linefollowing void followline() { OnFwdSync(MOTORS, FOLL_PWR, 0); char turnpct; do { READ; turnpct = Kp*(INTENS-MED); OnFwdSync(MOTORS, FOLL_PWR, turnpct); } while (!RED); Off(MOTORS); } // turns the robot in the specified direction assuming that robot is facing north #define TURN_PWR 25 // turning speed #define WHOLE_TURN_DEG 850 void turn(Dir dir) { switch (dir) { case N: break; case W: RotateMotorEx(MOTORS, TURN_PWR, WHOLE_TURN_DEG/4, -100, true, true); break; case E: RotateMotorEx(MOTORS, TURN_PWR, WHOLE_TURN_DEG/4-80, 100, true, true); break; case S: RotateMotorEx(MOTORS, TURN_PWR, WHOLE_TURN_DEG/2, -100, true, true); break; } } // scans the area and returns detected walls, assuming the robot is facing North // direction is a bit sum of the directions that need to be scanned (e.x. N & E). Robot can only scan N, W and E. // returns a bit sum representing detected walls in given directions #define SCAN_PWR 60 // scanning speed #define RESET_COUNT_MAX 2 // how often to recalibrate the sensor (grater is less often) byte _RESET_COUNT = 0; byte scan(byte directions) { byte ret = 0; int initpct = MotorRotationCount(MSEN); RotateMotorEx(MOTORS, FOLL_PWR, 155, 0, true, true); // east if (directions & E) { RotateMotor(MSEN, SCAN_PWR, initpct - MotorRotationCount(MSEN) - CENTER_TO_LEFT_DEG/2); OnRev(MSEN, SCAN_PWR); do { READ; } while ((INTENS > MED) & (initpct - MotorRotationCount(MSEN) < CENTER_TO_LEFT_DEG)); Off(MSEN); if (INTENS <= MED) ret |= E; } // north if (directions & N) { RotateMotor(MSEN, SCAN_PWR, initpct - MotorRotationCount(MSEN) - CENTER_TO_LEFT_DEG/3); OnFwd(MSEN, SCAN_PWR); do { READ; } while ((INTENS > MED) & (MotorRotationCount(MSEN) < initpct + CENTER_TO_LEFT_DEG/3)); Off(MSEN); if (INTENS <= MED) ret |= N; } // west if (directions & W) { RotateMotor(MSEN, SCAN_PWR, initpct - MotorRotationCount(MSEN) + CENTER_TO_LEFT_DEG/2); OnFwd(MSEN, SCAN_PWR); do { READ; } while ((INTENS > MED) & (MotorRotationCount(MSEN) - initpct < CENTER_TO_LEFT_DEG)); Off(MSEN); if (INTENS <= MED) ret |= W; } if (directions != 0) { _RESET_COUNT++; if (_RESET_COUNT >= RESET_COUNT_MAX) { reset(); _RESET_COUNT = 0; } else { RotateMotor(MSEN, SCAN_PWR, initpct - MotorRotationCount(MSEN) + 100); RotateMotor(MSEN, SCAN_PWR, -100); } } RotateMotorEx(MOTORS, FOLL_PWR, 100, 0, true, true); return ~ret; } ///////////////////// PHYSICAL PART ENDS HERE ///////////////////////////////// //////// LOGICAL PART: THESE FUNCTIONS CONTROLL THE LABYRINTH LOGIC /////////// #define SIZE 10 // labyrinth size (width and height) byte labyrinth[SIZE*SIZE]; // one field's 8 bits represent: (wall W scanned) (wall S scanned) (wall E scanned) (wall N scanned) (wall W present) (wall S present) (wall E present) (wall N scanned) Point targetPoint, startPoint; Dir heading; Point position; void init_labyrinth(Point initialPosition, Point targetP, Dir initialHeading) { ArrayInit(labyrinth, 0, SIZE*SIZE); for (int i = 0; i<SIZE; i++) { labyrinth[i] |= (S | S<<4); labyrinth[SIZE*SIZE - 1 - i] |= (N | N<<4); labyrinth[i*SIZE] |= (W | W<<4); labyrinth[(i+1)*SIZE - 1] |= (E | E<<4); } targetPoint = targetP; position = initialPosition; startPoint = initialPosition; heading = initialHeading; switch (heading) { case N: labyrinth[position.x+position.y*SIZE] |= (S | S<<4); if (position.y > 0) labyrinth[position.x+(position.y-1)*SIZE] |= (N | N<<4); break; case S: labyrinth[position.x+position.y*SIZE] |= (N | N<<4); if (position.y < SIZE-1) labyrinth[position.x+(position.y+1)*SIZE] |= (S | S<<4); break; case W: labyrinth[position.x+position.y*SIZE] |= (E | E<<4); if (position.x < SIZE-1) labyrinth[position.x+1+position.y*SIZE] |= (W | W<<4); break; case E: labyrinth[position.x+position.y*SIZE] |= (W | W<<4); if (position.x > 0) labyrinth[position.x-1+position.y*SIZE] |= (E | E<<4); break; } } void close_labyrinth() { for(int i = 0; i<SIZE*SIZE; i++) { labyrinth[i] |= (~labyrinth[i]>>4) | 0xF0; } targetPoint = startPoint; } void comp_route(Dir & out [], Point start_p) { int arrsize = SIZE*SIZE; unsigned int queue []; ArrayInit(queue, 0, arrsize); // numbers of fields to visit unsigned int pond []; ArrayInit(pond, UINT_MAX, arrsize); queue[0] = start_p.x + start_p.y * SIZE; pond[queue[0]] = 0; unsigned int act = 0; unsigned int next = 1; unsigned int targetP = targetPoint.x + targetPoint.y * SIZE; unsigned int n; while (act != next) { n = queue[act]; if (n == targetP) break; // check north if (!(labyrinth[n] & N)) { if (pond[n+SIZE] == UINT_MAX) { pond[n+SIZE] = pond[n] + 1; queue[next] = n+SIZE; next++; } } // check south if (!(labyrinth[n] & S)) { if (pond[n-SIZE] == UINT_MAX) { pond[n-SIZE] = pond[n] + 1; queue[next] = n-SIZE; next++; } } // check west if (!(labyrinth[n] & W)) { if (pond[n-1] == UINT_MAX) { pond[n-1] = pond[n] + 1; queue[next] = n-1; next++; } } // check east if (!(labyrinth[n] & E)) { if (pond[n+1] == UINT_MAX) { pond[n+1] = pond[n] + 1; queue[next] = n+1; next++; } } act++; } int len = pond[targetP]; ArrayInit(out, N, len); n = targetP; for (int i = len-1; i>=0; i--) { if (!(labyrinth[n] & N)) { if (pond[n+SIZE] == pond[n] - 1) { n = n+SIZE; out[i] = S; continue; } } if (!(labyrinth[n] & S)) { if (pond[n-SIZE] == pond[n] - 1) { n = n-SIZE; out[i] = N; continue; } } if (!(labyrinth[n] & W)) { if (pond[n-1] == pond[n] - 1) { n--; out[i] = E; continue; } } if (!(labyrinth[n] & E)) { if (pond[n+1] == pond[n] - 1) { n++; out[i] = W; continue; } } // if the program goes here it is error! } } Dir comp_turn(Dir desired) { Dir transition [4][4] = {{N, E, S, W}, // [from][to] {W, N, E, S}, {S, W, N, E}, {E, S, W, N}}; float ln2 = log(2.0); byte from = trunc(log(heading)/ln2 + 0.5); byte to = trunc(log(desired)/ln2 + 0.5); return transition[from][to]; /*trunc(pow(2.0, transition[from][to]) + 0.5); */ } // returns true if any new wall is detected bool comp_scan() { unsigned int pos = position.x + position.y * SIZE; byte rel = 0; if (!(labyrinth[pos] & N<<4)) rel |= comp_turn(N); if (!(labyrinth[pos] & W<<4)) rel |= comp_turn(W); if (!(labyrinth[pos] & S<<4)) rel |= comp_turn(S); if (!(labyrinth[pos] & E<<4)) rel |= comp_turn(E); if (rel & S) { // South should never be not scanned. It should be scanned from previous field PlayTone(300, 500); rel ^= S; } byte result = scan(rel); unsigned int npos; Dir a,b; // north if (rel & N) { switch (heading) { case N: npos = pos + SIZE; a = N; b = S; break; case E: npos = pos + 1; a = E; b = W; break; case S: npos = pos - SIZE; a = S; b = N; break; case W: npos = pos-1; a = W; b = E; break; } labyrinth[pos] |= a << 4; labyrinth[npos] |= b << 4; if (result & N) { labyrinth[pos] |= a; labyrinth[npos] |= b; } } // west if (rel & W) { switch (heading) { case N: npos = pos - 1; a = W; b = E; break; case E: npos = pos + SIZE; a = N; b = S; break; case S: npos = pos + 1; a = E; b = W; break; case W: npos = pos - SIZE; a = S; b = N; break; } labyrinth[pos] |= a << 4; labyrinth[npos] |= b << 4; if (result & W) { labyrinth[pos] |= a; labyrinth[npos] |= b; } } // east if (rel & E) { switch (heading) { case N: npos = pos + 1; a = E; b = W; break; case E: npos = pos - SIZE; a = S; b = N; break; case S: npos = pos - 1; a = W; b = E; break; case W: npos = pos + SIZE; a = N; b = S; break; } labyrinth[pos] |= a << 4; labyrinth[npos] |= b << 4; if (result & E) { labyrinth[pos] |= a; labyrinth[npos] |= b; } } return result; } void graph() { unsigned long pos; ClearScreen(); for (int y = 0; y < SIZE; y++) { for (int x = 0; x < SIZE; x++) { pos = y*SIZE+x; if ((labyrinth[pos] >> 4) == 0xF) { RectOut(x*5, y*5, 2, 2, DRAW_OPT_FILL_SHAPE); if (!(labyrinth[pos] & N) && (labyrinth[pos] & N<<4)) PointOut(x*5+1, y*5+3); if (!(labyrinth[pos] & E) && (labyrinth[pos] & E<<4)) PointOut(x*5+3, y*5+1); if (!(labyrinth[pos] & S) && (labyrinth[pos] & S<<4)) PointOut(x*5+1, y*5-1); if (!(labyrinth[pos] & W) && (labyrinth[pos] & W<<4)) PointOut(x*5-1, y*5+1); } } } } void solve(bool graphOn = false) { Dir route[]; unsigned int i = 0; comp_route(route, position); do { if (comp_scan()) { comp_route(route, position); if (graphOn) graph(); i = 0; } turn(comp_turn(route[i])); heading = route[i]; switch (heading) { case N: position.y++; break; case S: position.y--; break; case W: position.x--; break; case E: position.x++; break; } i++; followline(); } while (position != targetPoint); if (graphOn) graph(); } ///////////////////// LOGICAL PART ENDS HERE ////////////////////////////////// task main() { init(); init_labyrinth(makePoint(0,0), makePoint(9,9), N); solve(true); graph(); PlayTone(600, SEC_1); Wait(SEC_3); }

Pokaż/ukryj cały kod

Komentarze

The first error tells everything. Use enhanced firmware for correct operation. You can download it from Bricx CC website.

Dodane przez: Admin | 2012-12-07

The program still has errors, can you please help me fix it? Errors are the following: line 146: Error: Enhanced firmware is required for this operation line 196: Error: Enhanced firmware is required for this operation line 293: Error: Undefined Identifier log line 293: Error: ';' expected line 294: Error: Undefined Identifier trunc line 294: Error: ';' expected line 294: Error: Undefined Identifier log line 294: Error: Invalid assignment line 295: Error: Undefined Identifier trunc line 295: Error: ';' expected line 295: Error: Undefined Identifier log line 295: Error: Invalid assignment I hope you could help me, thank you.

Dodane przez: Minicinz | 2012-12-07

Unfortunately I don't have any extra pieces same as what you have posted and I have no idea how it would be done with the pieces provided only with the Lego Mindstorm NXT 2.0. If you have any idea on how it would be done please let me know. PLEASE?

Dodane przez: DomzLegoNic | 2012-12-05

I used some extra pieces to build this robot. I'm sorry but I'm unable to provide anything but the building instruction already posted. You may try to use different pieces to achieve the same functionality.

Dodane przez: Admin | 2012-12-05

Do you have another building instructions? My Lego Mindstorm NXT 2.0 seems not fitted to the building instructions seen on the video for the reason that some pieces that you have used is not present on my Lego parts. Please help me, I am hoping for your respond. Thank you!

Dodane przez: DomzLegoNic | 2012-12-05

Zobacz wszystkie

Dodaj swój komentarz