Linefollower

This is my first linefollower. I wanted to build a fast and accurate robot which wouldn't lose the line nor keep turning around searching for it.

I assumed I would need 3 light sensors. Since I didn't have them, I decided to make tri-color line. My robot followed the black line in the middle and used the blue and the red one on both sides to to tell wheather it should turn left ar right.

The program was very simple. After 15 minutes of test drives I had the perfect parameters and the robot drove at full speed through sharp turns and right angles.

This construction, though very simple, do not work on just a black line. I had plans for a linefollower with three and two light sensors, and maybe a one with just one. I know it can be done.

Nevertheless, for someone who is new to Lego Mindstorms this project can be a great start. Building, programming and testing takes about half a day. Building instruction and my program can be found below.

Video


Gallery

3D Gallery

To watch pictures in this gallery you need special red-cyan anaglyph glasses. They are to buy in many Internet stores for less than 1$.


Program code

//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(); }

Show/hide all the code

Comments

No comments yet. Be the first one! Post a comment!

Post your comment