US Labyrinth Solver

Założenia tego projektu były następujące:

  • Robot miał przejechać nierozgałęziony labirynt
  • Powinien unikać kolizji ze ścianami, a gdy taka nastąpi wykryć ją i skorygować kurs

Trasa labiryntu była nieskomplikowana, jednak wymagała skrętów w różne strony, o różnej długości i promieniu. Szerokość trasy była kilkanaście centymetrów szersza od robota i stała na całej długości.

Aby wykrywać odległość od ścian wybrałem czujnik ultradźwiękowy. Konstrukcja zapewniała jego nieustanny obrót w promieniu 180 stopni. Do detekcji kolizji zastosowałem czujniki dotyku zamocowane pod odpowiednim kątem po obu stronach robota. Pojazd wyposażyłem także w czujnik koloru pracujący jako kolorowa dioda, wskazująca postęp pracy programu w celach diagnostycznych.

Czujnik ultradźwiękowy zastosowany w klockach LEGO MINDSTORMS® ma małą dokładność. Często podaje zawyżone lub zaniżone pomiary, często nie wykrywa ścian, gdy znajduje się blisko nich. Pracuje również bardzo wolno. Potrafi wykonać około 30 odczytów na sekundę, co w poruszającym się robocie jest bardzo mało. Aby zniwelować błędy odczytu, z pomiarów wyciągałem średnią, co sprawdziło się bardzo dobrze, zapewniając płynną jazdę.

Algorytm przejścia był bardzo prosty. Robot skanował teren od lewej do prawej szukając miejsca, gdzie wykrył najmniejszą średnią odległość, i zapamiętywał względny kąt od kierunku na wprost. Pełen skan zajmował pół sekundy. Następnie porównywał odczytany kąt z dwoma poprzednimi skanami, wyciągał średnią z pomiarów i korygował moc silników, aby spowodować skręt w kierunku od przeszkody.

Dostosowanie tego programu do rzeczywistych warunków wymagało wprowadzenia szeregu parametrów, takich jak szybkość poszczególnych silników, aby jazda przebiegała płynnie, a robot nie wpadał na ściany pomiędzy poszczególnymi skanami.

Na filmie przedstawiłem przejazd w dwóch labiryntach. Pierwszy jest mniej skomplikowany, zakręty są mniej ostre. Pojazd przejeżdza go bardzo szybko, dotykając ścian dwukrotnie. Drugi labirynt był dużo dłuższy i trudniejszy. Robot często wpada na ściany, ale szybko koryguje kurs i przejeżdza go względnie poprawnie.

Projekt zakończony został wielkim sukcesem. Zbudowany pojazd spełniał wszystkie postawione przed nim wymagania. Był szybki i względnie dokładny.

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 /* S1 - US Sensor S2 - Left Touch S3 - Right Touch S4 - Color Sensor as a Diode OUT_A - Left OUT_B - Right OUT_C - Scanner Right side of the robot is heavier than the left so the motors are not configured in the same way. */ #define NEUTRAL 30 // motor power #define DELTA 55 // change in motor power #define SAFE_ZONE 10 // acceptable difference between left and right wall #define SCAN_AREA 130 // angle of scanned teritory #define SCAN_SPEED 80 // speed of the US Sensor - the less the more accurate #define RELAX_TIME 600 #define TOO_CLOSE 15 int DIR; mutex DIR_MUTEX; void init() { SetSensor(S2, SENSOR_TOUCH); SetSensor(S3, SENSOR_TOUCH); SetSensor(S1, SENSOR_LOWSPEED_9V); OnRev(OUT_C, 10); int count = MotorRotationCount(OUT_C); Wait(100); while (count != MotorRotationCount(OUT_C)) { count = MotorRotationCount(OUT_C); Wait(40); } Off(OUT_C); RotateMotor(OUT_C, 30, 180); ResetAllTachoCounts(OUT_C); ResetRotationCount(OUT_C); Wait(300); } mutex WAIT_MUTEX, RUNNING_MUTEX; bool waiter; task scan() { int max_dist, max_ang = 0, scanned, prev_max; int min_dist, min_ang = 0, prev_min; int counter = 0; RotateMotor(OUT_C, 30, -SCAN_AREA); while (true) { max_dist = 0; min_dist = SCAN_AREA; int prev_sc; if (counter == 50) { Acquire(WAIT_MUTEX); waiter = true; Release(WAIT_MUTEX); Acquire(RUNNING_MUTEX); init(); Release(RUNNING_MUTEX); counter = 0; } OnFwd(OUT_C, SCAN_SPEED); while (MotorRotationCount(OUT_C) < SCAN_AREA) { prev_sc = scanned; scanned = SensorUS(S1); if (scanned == 255) scanned = 70; scanned = (prev_sc*2 +scanned)/3; if (scanned > max_dist) { max_ang = MotorRotationCount(OUT_C); max_dist = scanned; } if (scanned < min_dist) { min_ang = MotorRotationCount(OUT_C); min_dist = scanned; } } Off(OUT_C); max_ang = (prev_max*1+max_ang)/2; min_ang = -min_ang; Acquire(DIR_MUTEX); if (min_dist <= TOO_CLOSE) { DIR = (min_ang*3+max_ang)/4; if (DIR < 0) DIR -= 10; else DIR += 10; } else DIR = max_ang; if (abs(DIR) <= SAFE_ZONE) DIR = 0; Release(DIR_MUTEX); prev_max = max_ang; prev_min = min_ang; max_dist = 0; min_dist = SCAN_AREA; OnRev(OUT_C, SCAN_SPEED); while (MotorRotationCount(OUT_C) > -SCAN_AREA) { prev_sc = scanned; scanned = SensorUS(S1); if (scanned == 255) scanned = 70; scanned = (prev_sc*2 +scanned)/3; if (scanned > max_dist) { max_ang = MotorRotationCount(OUT_C); max_dist = scanned; } if (scanned < min_dist) { min_ang = MotorRotationCount(OUT_C); min_dist = scanned; } } Off(OUT_C); max_ang = (prev_max*1+max_ang)/2; min_ang = -min_ang; Acquire(DIR_MUTEX); if (min_dist <= TOO_CLOSE) { DIR = (min_ang*3+max_ang)/4; if (DIR < 0) DIR -= 10; else DIR += 10; } else DIR = max_ang; if (abs(DIR) <= SAFE_ZONE) DIR = 0; Release(DIR_MUTEX); prev_max = max_ang; prev_min = min_ang; Off(OUT_C); counter++; } } task main() { int dir_read; int prev_read; int power; init(); Acquire(RUNNING_MUTEX); StartTask(scan); while (true) { OnRev(OUT_AB, NEUTRAL); while (!SensorBoolean(S2) && !SensorBoolean(S3)) { Acquire(WAIT_MUTEX); if (waiter) { Release(RUNNING_MUTEX); Off(OUT_AB); Yield(); Wait(500); Acquire(RUNNING_MUTEX); waiter = false; } Release(WAIT_MUTEX); Acquire(DIR_MUTEX); prev_read = dir_read; dir_read = DIR; power = DELTA*abs(dir_read)/SCAN_AREA; if (power > 100-NEUTRAL) power = 100-NEUTRAL; Release(DIR_MUTEX); if (dir_read == 0) { OnRev(OUT_AB, NEUTRAL); SetSensorColorGreen(S4); } else if (dir_read < 0) { // right if (dir_read*prev_read < 0) Wait(RELAX_TIME); OnRev(OUT_A, 0.5*NEUTRAL+power); OnRev(OUT_B, NEUTRAL); SetSensorColorBlue(S4); } else { // left if (dir_read*prev_read < 0) Wait(RELAX_TIME); OnRev(OUT_B, NEUTRAL+0+power); OnRev(OUT_A, NEUTRAL); SetSensorColorRed(S4); } } Off(OUT_AB); if (SensorBoolean(S2)) { // left RotateMotor(OUT_AB, NEUTRAL*2, 100); RotateMotor(OUT_B, NEUTRAL*2, 200); } else if (SensorBoolean(S3)) { // right RotateMotor(OUT_AB, NEUTRAL*2, 100); RotateMotor(OUT_A, NEUTRAL*2, 200); } } }

Pokaż/ukryj cały kod

Komentarze

Jaj wgrać kod programu do robota?

Dodane przez: Anonymous | 2015-09-19

fajny ale nie mogę go zbudować

Dodane przez: 123kubaj321 | 2013-12-05

fajny ale nie mogę go zbudować

Dodane przez: 123kubaj321 | 2013-12-05

Dodaj swój komentarz