Linefollower

To jest mój pierwszy linefollower. Chciałem zbudować szybkiego i dokładnego robota, który nie będzie gubił trasy, ani obracał się w kółko w poszukiwaniu linii.

Wyszedłem z założenia, że do implementacji będę potrzebował 3 czujniki światła. Jako że dysponowałem jedynie jednym postanowiłem zrobić trójkolorową linię. Robot podążał za linią czarną, a dwie pomocnicze po bokach umożliwiały zorientowanie się po której stronie środka trasy robot się znajduje.

Program był również bardzo prosty. Już po 15 minutach testowania udało mi się ustalić najlepsze parametry i robot przejeżdzał trasę na pełnej prędkości, pokonując ostre zakręty i załamania pod kątem prostym.

Ta bardzo prosta konstrukcja, choć sprawdziła się w praktyce, nie działa na jednokolorowej linii. W planach mam próby stworzenia linefollowera jeżdzącego jedynie po linii czarnej używając trzech czujników koloru, później dwóch, a na końcu może uda mi się osiągnąć cel przy użyciu jednego czujnika. Wiem że to jest możliwe.

Niemniej jednak, dla kogoś poznającego dopiero podstawy Lego Mindstorms, ten projekt jest idealny ze względu na swoją prostotę. Budowa, programowanie i testowanie zajmuje z instrukcją około pół dnia. Instrukcja budowy i program poniżej.

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 M_LEFT OUT_B #define M_RIGHT OUT_C #define MOTORS OUT_BC #define S_COLOR S3 #define S_TOUCH S2 void init() { SetSensorTouch(S_TOUCH); SetSensorColorFull(S_COLOR); } int readColor() { ColorSensorReadType csr; csr.Port = S_COLOR; SysColorSensorRead(csr); if (csr.Result == NO_ERR) return csr.ColorValue; else return 0; } task scan(); task follow(); mutex SCAN_VARS; enum Dir {LEFT = 0; STRAIGHT; RIGHT}; Dir direction; unsigned int factor; task emergencyBrake(); task main() { init(); StartTask(emergencyBrake); StartTask(scan); StartTask(follow); } #define LINE_COLOR INPUT_BLACKCOLOR #define LINE_LEFT INPUT_REDCOLOR #define LINE_RIGHT INPUT_BLUECOLOR #define BACKGROUND INPUT_WHITECOLOR #define SENSITIVITY 1 #define OUT_OF_LINE_MULTIPLIER 3 #define NUMB_TIME 15 task scan() { int prev_color = 0; int color = 0; while (true) { int new_color = readColor(); if (new_color != LINE_COLOR & new_color != LINE_LEFT & new_color != LINE_RIGHT & new_color != BACKGROUND) continue; if (color != new_color) { prev_color = color; color = new_color; Acquire(SCAN_VARS); switch (color) { case LINE_COLOR: factor = 0; direction = STRAIGHT; break; case LINE_LEFT: factor = SENSITIVITY; direction = LEFT; break; case LINE_RIGHT: factor = SENSITIVITY; direction = RIGHT; break; case BACKGROUND: factor += OUT_OF_LINE_MULTIPLIER*SENSITIVITY; break; } Release(SCAN_VARS); } else { switch (color) { case LINE_COLOR: break; case LINE_LEFT: case LINE_RIGHT: Acquire(SCAN_VARS); factor += SENSITIVITY; Release(SCAN_VARS); break; case BACKGROUND: Acquire(SCAN_VARS); factor += OUT_OF_LINE_MULTIPLIER*SENSITIVITY; Release(SCAN_VARS); break; } } Wait(NUMB_TIME); } } #define NEUTRAL_SPEED 100 #define MAX_TURN 40 task follow() { while (true) { int turnpct; Acquire(SCAN_VARS); turnpct = factor; if (turnpct > MAX_TURN) turnpct = MAX_TURN; if (direction == RIGHT) turnpct = -turnpct; Release(SCAN_VARS); OnRevSync(MOTORS, NEUTRAL_SPEED, turnpct); } } task emergencyBrake() { while (true) { if (SensorBoolean(S_TOUCH)) break; } Off(MOTORS); StopAllTasks(); }

Pokaż/ukryj cały kod

Komentarze

Brak komentarzy. Bądź pierwszy! Dodaj komentarz!

Dodaj swój komentarz