LightFollower

Nadrzędnym celem konstrukcji tego robota była chęć sprawdzenia dokładności czujnika światła dołączonego do zestawu Lego. Poza tym konstrukcja robota sterowanego światłem latarki wydała mi się ciekawa i oryginalna.

Pojazd składał się z podstawy umieszczonej na dwóch gąsienicach, na której umieszczone były dwa mechanizmy. Pierwszy i zarazem najważniejszy to oczywiście ruchoma podstawa obrotowego czujnika światła. Do jego konstrukcji wykorzystałem kilka elementów nie dołączonych do oryginalnego zestawu Lego, ale pomysłowy konstruktor na pewno znajdzie zamienniki w zestawie. Czujnik koloru został umieszczony na lekko pochylonym wysięgniku i obudowany dookoła, aby wykrywać tylko światło przed sobą. Całość konstrukcji uzupełnia czujnik dotyku wykrywający aktualne położenie czujnika.

Drugim mechanizmem był wykrywacz kolizji zamontowany przed robotem. Był to długi klocek na dwóch prowadnicach i zamocowany do czujnika dotyku. Taka budowa zapewniła, że czujnik dotyku wykrywał kolizje na całej szerokości pojazdu.

Oprogramowanie robota jest dość skomplikowane. Kilka wątków obsługuje każde z zadań z osobna. Najpierw wykonywana jest kalibracja wszystkich czujników. Czujnik światłą zostaje obrócony na początkowe położenie skanując w tym czasie natężenie światła w środowisku robota i zapisując średnią wartość. Następnie ruszają poszczególne podprogramy. Jeden odpowiedzialny jest za skanowanie otoczenia. Na każdy pełny obrót zwraca do pamięci kierunek na którym zostało wykryte światło o największym natężeniu. Jeżeli nie znaleziono światła znacząco silniejszego od jasności otoczenia zwraca błąd (robot zatrzymuje się).

Kolejny podprogram odpowiada za poruszanie się pojazdu. Odbiera on informacje od czujnika światła i odpowiednio modyfikuje moc silników. W przypadku, gdy zamontowany z przodu ultradźwiękowy czujnik odległości wykryje przeszkodę, robot cofnie się parę centymetrów do tyłu i będzie kontynuował jazdę. Jeżeli natomiast robot wjedzie na przeszkodę, to program zatrzyma awaryjnie robota (taka sytuacja nigdy nie miała miejsca).

Ciekawym dodatkiem do programu jest opcja zatrzymania gdy pojazd zbliży się zanadto do światła. Efekt ten jest dobrze widoczny na filmie.

Podsumowując ten projekt uważam za wielki sukces. Konstrukcja robota była bardzo prosta i wykorzystywała kilka ciekawych elementów. Czujnik światła okazał się wyjątkowo szybki i dokładny. Ciekawie napisany program zapewnił długie godziny zabawy. Gorąco zapraszam do zbudowania czegoś podobnego! Nie mam instrukcji montażu, ale film z konstruowania robota może pomóc.

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 LIGHT S1 #define HEAD S2 #define SOUND S3 #define BUMPER S4 #define CRASH SensorBoolean(BUMPER) #define OBSTACLE (SensorUS(SOUND) < 30) #define HEAD_STR SensorBoolean(HEAD) #define READ_LIGHT Sensor(LIGHT) #define TURNTABLE OUT_A #define LEFT_MOTOR OUT_B #define RIGHT_MOTOR OUT_C #define MOTORS OUT_BC inline void init() { SetSensorType(LIGHT, IN_TYPE_COLORNONE); SetSensorMode(LIGHT, IN_MODE_PCTFULLSCALE); ResetSensor(LIGHT); SetSensorTouch(HEAD); SetSensorTouch(BUMPER); SetSensorLowspeed(SOUND); } unsigned int FULL_TURN; unsigned int SURROUND_LIGHT; inline void calibrate() { if(!HEAD_STR) { OnFwd(TURNTABLE, 70); while(!HEAD_STR); Off(TURNTABLE); } int beg = MotorRotationCount(TURNTABLE); OnRev(TURNTABLE, 70); Wait(200); while(!HEAD_STR); Off(TURNTABLE); FULL_TURN = abs( MotorRotationCount(TURNTABLE) - beg ); Wait(50); unsigned int count = 0; unsigned long sum = 0; OnFwd(TURNTABLE, 70); while(HEAD_STR) { sum += READ_LIGHT; count++; } Wait(50); while(!HEAD_STR) { sum += READ_LIGHT; count++; } Off(TURNTABLE); SURROUND_LIGHT = sum/count; } //-------------------TASKS--------------------------- string PLAY_F = ""; int REP = 0; mutex PLAY_MUTEX; task play_sound() { while(true) { Acquire(PLAY_MUTEX); if (REP > 0) { PlayFile(PLAY_F); REP--; } Release(PLAY_MUTEX); Wait(1000); } } void CustomPlay(string name, int times) { Acquire(PLAY_MUTEX); PLAY_F = name; REP = times; Release(PLAY_MUTEX); } mutex DIR_MUTEX; int DIRECTION; #define SCAN_SPEED 100 #define MEASURE_ERROR 8 #define LIGHT_FOUND 80 #define SCAN_ERROR INT_MAX /* 0 - FRONT -45 *** 45 *** *** ** ** -90* * 90 * * ** ** -135 *** *** 135 *** 180 */ task follow(); task scan() { int beg_ang, max_ang, max_light, read; bool clockwise = false; while(true) { beg_ang = MotorRotationCount(TURNTABLE); max_ang = INT_MAX; max_light = SURROUND_LIGHT+MEASURE_ERROR; if (clockwise) OnFwd(TURNTABLE, SCAN_SPEED); else OnRev(TURNTABLE, SCAN_SPEED); Wait(200); // wait for the sensor to release while(!HEAD_STR) { read = READ_LIGHT; if (read > max_light) { max_ang = MotorRotationCount(TURNTABLE); max_light = read; } } Off(TURNTABLE); ClearScreen(); Acquire(DIR_MUTEX); if (max_ang == INT_MAX) DIRECTION = SCAN_ERROR; else if (clockwise) DIRECTION = 360*abs(max_ang-beg_ang)/FULL_TURN - 180; else DIRECTION = 180 - 360*abs(max_ang-beg_ang)/FULL_TURN; TextOut(0, LCD_LINE1, "DIRECTION = "); NumOut(70, LCD_LINE1, DIRECTION); Release(DIR_MUTEX); TextOut(0, LCD_LINE2, "LIGHT = "); NumOut(48, LCD_LINE2, max_light); TextOut(0, LCD_LINE3, "SURROUND = "); NumOut(66, LCD_LINE3, SURROUND_LIGHT); if (max_light >= LIGHT_FOUND) { // light source found! StopTask(follow); Off(MOTORS); if (clockwise) RotateMotor(TURNTABLE, -SCAN_SPEED/2, FULL_TURN/2); else RotateMotor(TURNTABLE, SCAN_SPEED/2, FULL_TURN/2); Acquire(DIR_MUTEX); if (DIRECTION < 0) OnRevSync(MOTORS, 80, -100); else OnRevSync(MOTORS, 80, 100); Release(DIR_MUTEX); while(READ_LIGHT < LIGHT_FOUND-10); Off(MOTORS); CustomPlay("Detect.rso", 1); Yield(); while(READ_LIGHT >= LIGHT_FOUND-20); OnFwd(TURNTABLE, 70); until(HEAD_STR); clockwise = false; StartTask(follow); continue; } clockwise = !clockwise; } } #define DRIVE_POWER 100 task follow() { int turnpct; bool error = false; CustomPlay("! Sonar.rso", INT_MAX); while(true) { Acquire(DIR_MUTEX); if (DIRECTION == SCAN_ERROR) error = true; else { if (error) { // previous error error = false; CustomPlay("! Sonar.rso", INT_MAX); } turnpct = DIRECTION * 100 / 180; } Release(DIR_MUTEX); if (!error) { OnRevSync(MOTORS, DRIVE_POWER, turnpct); Wait(100); } else { Off(MOTORS); CustomPlay("Error.rso", INT_MAX); } /*if (OBSTACLE) { Off(MOTORS); CustomPlay("Object Detected.rso", 1); RotateMotorPID(MOTORS, DRIVE_POWER*0.7, 3*360, 20, 40, 80); Yield(); Wait(500); CustomPlay("! Sonar.rso", INT_MAX); } */ } } task crash_monitor() { while (true) { if (CRASH) { StopTask(follow); StopTask(scan); Off(OUT_ABC); StopSound(); Wait(500); CustomPlay("Ouch 02.rso", 1); Wait(1000); Stop(true); } } } task main() { StartTask(play_sound); CustomPlay("Goodmorning.rso", 1); init(); calibrate(); StartTask(crash_monitor); StartTask(scan); Wait(100); StartTask(follow); }

Pokaż/ukryj cały kod

Komentarze

please do you have this code in the NXT-G

Dodane przez: ushe | 2011-11-15

I tried to use the brick command center but when I click on compile program, it gives me some errors like ',' expected or math factor expected, how can I manage it?

Dodane przez: Focus | 2011-09-20

Program code for my robots is available for everyone for free. Good luck!

Dodane przez: Admin | 2011-09-14

Thanks a lot for your explanation, I'm trying to reproduce your robot, can I use also your program code? I'm not so skilled as you :)

Dodane przez: Focus | 2011-09-13

Lego Mindstorms software won't recognize it. You should use Bricx Command Center to compile and upload it to your robot. This software is available in Links section at the bottom of this page.

Dodane przez: Admin | 2011-09-12

Zobacz wszystkie

Dodaj swój komentarz