Follow_line Development

 My first program in this subject is the follow_line. This seems to be a really simple program where you have a car wich has to follow a dark red line along the circuit.

Filter the image

So the first challenge was to be able of filter the line. I have some troubles with the world becouse it was too dark and because of that i couldn´t find the line correctly. What i made to solution it was to apply the solution of the forum of the Academy Jderobot.

 Once i solution this, it was relatively easy to filter because it wasn´t the first time i have done, so knowing the basic of OpenCv this doesn´t took so long.

Reactive System with States

The first approach to solve the problem was create a reactive system with states. It would have three simple states:

• State 1: the line is in the middle of the image so keep going.
• State 2: the line is in the right of the image turn left.
• State 3: the line is in the left so turn right.

So this didn´t work that bad but it i had some problems to resolve.

First of all when i executed the code it seems like the driver has drunk too many glasses. We can apreciate it in this video:

First im going to adjust the range of what is each state (i mean, change the values between the car consider is in one state or not). For example: the state keep going forward would be if the center of the filtered image (x) is between certain values: 

w_center= value

(w_center - num) < x < (w_center + num)

**Let´s notice that if your car is exacly on the middle of the line, your image show that the car is softly at the left of the line that is because the camera is at the left side of the car. You have to keep this in mind if you want the car exctly on the center. To know wich is the real center of the image i put the car centre on the line and see wich was the position on fhe middle point of the filtered image, so thats the real value of the line centre

With this range of each state the car don´t do abrup movements all the time. This is what is called a dead band.

Other error i detected in my code was that when the state was, for example turn right i set:

self.motors.sendW(-0.15) and once it was in the center it didn´t stop turning to the right because i didn´t say it to stop, it was a mistake i didn´t notice. So when the car reach the center it must put the angular velocity to 0. That was the big error that had my code and one of the causes of have a drunk driver.

Also i have to add to the states some adjustments in order to be able to complete the entery circuit. When the car has to turn to one side i have to differenciate two cases:

1. When the line is at the end of the image. In wich case the angular velocity has to be higher because the car has to correct more its trayectory. 

2. When the car is not too deflected so it has to make small corrections and is not necesary to much angular velocity. 

My car now was eable not only of keep going forward on a more or less straight line (it still making some little s but not as big as at the begining). It can also do the curves with out runing into a wall, even the sarps curves.

P Controler

Once the car works with states is time to try to resolve the problem with a P controler.

It´s seems easy, i have only to apply :

 u= -k*error

The error was easy to calculate, the difficult part was adjusting the K. For finding out wich was the best setting i make lot of tries and errors until my car was able to drive the entery circuit.

PD Controler

Now is time to set the Kd and try to be faster. So first of all i add to my code one important case that i didn´t have in mind. What happens when the car loses the line? Until now it was inevitable the collision with one of the walls or lose totally the control, so in order to solution this i made a state. In the case that the car can´t find the line it stops and start to turning around until it sees the line again and can keep with it´s normal drive.

So i have to adjust the variables following the formula: u= -kp*error + kd*de (PD controler). I recalculate again the Kp (wich i find in the last case) becouse i want to set a higher speed, so again i find out with tries and errors. 

After a some tries i finally find also the Kd that able my car to drive the entery circuit as we can see in the next video:

                  This time in the processed image i show you the mask im applying to filter the image.

Although it achive its function i want to be faster (the max velocity i give the car was 8), so let´s make some changes:

Now the lineal velocity wouldn´t be constant. That would bring the car the oportunity of be faster becouse it could increase the lineal velocity on the rect lines but also could slow on the curves. From this i differenciated two cases depending on the position of the middle point of the red line. If it was between de centre of the image +num and -num the car was on a rect line, else it would be on a curve (it is similar to what i made in the case of resolving the problems with states).

 if(w_center-num < x < w_center+num):

        print "Straight line"

else:

        print "Curve"

But i want to be more precise becouse if the car isn´t between two values didn´t mean that was on a straight line so i made some bigger changes on my code:

One big change was the way a filter the image. 

Until now what i made was calculate the middle of all the points filtered using moments, so it get me the middle of the whole line. 

What i made to filtre the image is divide the mask in three parts and calculate the middle point of each segment.

  How i divide the mask in three parts? It´s simple, i draw two lines of other colours at differents heights so when the mask filter the image it won´t took a single line. 

Now the function cv2.findContours() would find three contours and for each contour i calculate it´s middle point using moments and save them into a list. You can see the final result on this image:

In the processed image we can see the mask divided in three and the middle point of each one.

And why is this all made for? With this sistem i can differenciate well two states, when is on a straight line and when is on a curve. How do i do that? well, to know if three points are aligned exists this formula:

                                                            

                                      

So if the difference between this two fractions is 0 we can say that the points are aligned. Obviously i cannot expect that if the car is on a traight line this formula would give 0 all the time so what i made is if the difference between the two fraction is less than 0.05  that is a rect line if not the car is on a curve (basicaly set a dead band) .

Other little change on the filter you can see also in the last screenshot. You don´t see anything? if you pay attension you would notice that the filter don´t filter the whole line, the slice of the line wich is really close to the car is not filtered, why? becouse it made the car oscilate more so in order to stabilice the system i don´t filter that drawing a thicker line on that zone.

The final result is:

    

Now we can easily differenciate the two cases, so i only have to adjust the constants. It took me a while but finnaly my car did the entery circuit.

Also the transition between the higher velocity and the slower wouldn´t be inmediate, while the is on a curve the car the velocity would be decreasing as easy as substract 0.3 each iteraction (it wsill stop decreasing when it reaches the minimum velocity). Same thing when is on a rect it would increment it speed until it reach the hicher speed.

For calculate the error i always use the point wich is on the horizon (the furthes from the car) is the most best point of the three ones becouse it gives us more information than the other ones.

One problem i find during the process of searching the right constants was that some times the filter made some blinks during the travel (you can apreciate it in the video around the minute 0:35) . When i was testing th code with lowers velocities didn´t affect a lot (it made destabilize lightly the car) but when i increase the speed the car start make sharp S so it ends crashing into a wall. Some times only blinked the horizont part so instead os taking the point i want the car to take it took the point of the middle segment so it made the car an abrupt turn.

Other times it blinked the whole filter so becouse there was no point to calculate the error (the point was 0) the error to correct was bigger, also it couldn´t calculate the difference between the points (it was NULL) so the car thought it was on a curve and run into a wall.

So in order to solution these problem i made a second filter cropping the image only to take the horizont part (same segment as the before horizont) and calculate the middle point (the blue point we can see on the video). The whole time it would take this point to calculate the error. This point didn´t blink so it makes the car be the whole time stabiliced.

If the car start without loking to the line:

The best time i get:

    

Hope my development of this exercise have help you. See you in the next practice!