20100905_linePID.zip (1459Bytes)
little background first... i wanted to try the NXT robot to follow line, BUT:
- the smart brick only has 4 sensor ports
- i only have 1 light sensor (able to detect colors as well - NXT2)
i didnt want to buy any mux or sensor arrays you can get for the NXT to solve these, so i was thinking about how to use what i have to being able to follow the line.
i then noticed, that the sensor uses 3 RGB LEDs to produce light and (what is very important for my findings) those are set to shine on the same spot in closer distance than is the possible detection distance of the sensor resulting in not one "white light disc" but rather 3 overlapping Blue-Red-Green discs (you can see it on the image on top. so there is a portion of the surface, where the sensor detects the whole spectrum of colors, but on the sides, there is just B or G color resulting in detecting B or G respectively if shone upon the White surface.
with that said i tried to detect Black line with senor put in the position further from the surface (to have these color light discs bigger and easier to distinguish) and turned so that the discs are perpendicular to the line. this way i can detect these colors if going from left to the right from the robot view: 6(W), 3(G), 1(B), 2(B), 6(W)
this itself suffices the simple control algorithm or you can ad a little piece of code (i use NXC, but i guess you can do it in NXT-G as well) to transfer these into some sort of values that can be used as input for PID controller e.g. as i did in my omocha project - you can see the videos of the robot using this as well.
my code for transforming color values into "relative position to line" values is here:
SysColorSensorRead(col);
if (col.ColorValue == 2) {
lastLine = 1;
} else if (col.ColorValue == 3) {
lastLine = 2;
}
if (lastLine == 1) {
if (col.ColorValue == 6) y = -4;
else if (col.ColorValue == 2) y = -1;
else if (col.ColorValue == 1) y = 0;
} else {
if (col.ColorValue == 6) y = 4;
else if (col.ColorValue == 3) y = 1;
else if (col.ColorValue == 1) y = 0;
}
where y represents the position and lastLine is used to check on which side of the line we are (as we have white (6) on both sides) and i used the B/G color to detect change of the side. this works well for speeds to about 50% of the maximum of the motors with my big wheels, but there may be situations in which it is not good enough - hitting the B line from side, thus not having B/G at all, just changing from 6 to 1 or moving over the line too fast for the sensor to notice. if you find some cure for this, i will be gratefull :)
as you can see i used values of 1 and 4 for position differece on each side - this is just the rule of the thumb i guess, you can use 1 and 2 to decrease the responsivity or 1 and 5 to increase it.
the PID code i used was greatly inspired byt the code from www.techbricks.nl but those just used it to detect and follow the edge of the line and didnt bother with the color tune-up :) i attach my whole code, just in case it may be of any help to someone. again, if you find something obviously wrong, let me know (as it is, when you spend several nights on something, you tend to overlook elementary errors...)