Obstacle Avoidance Development

 For this practice we have our car again, this time it has to be able to not crash with anything. This is getting interesting, How we are going to be able to programate this? The VFF algorithm will help us to do this.

This algorithm would help us to reach the objective at the same time the car avoids th obstacles. This consist of the sum of two forces:

-Atractive force, this makes our car want to reach the objecive. (blue arrow)

-Repulsive force, that woul avoid the obstacles (red arrow).

The sum of this forces give us the resultant force (the green arrow). This is the force wich the car would follow to complete the objective of this practice.

So lets start calculating this forces:

ATRACTIVE FORCE

This is the easiest force to calculate, we have only to calculate the diference between our point and the goal point (the goal point is given by getNextTarget() function). This is easy done by the function absolute2relative() that is in the JdeRobot page.

But what happens when the objective is too far from the actual position of the car?, the arrow would leave the window. So we have to reduce the arrow without changing the direction of it. I manage to do this applying trigonometry, knowing the x and y we can know the hipotenuse, with all this we can calculate the angle and with the angle calculated is the turn of reducing the hipotenuse (which is the distance to the goal). The result:

**Notice the target point, this point (2000,100) would be really far from the car.
But the arrow doesnt go out of window.

  

REPULSIVE FORCE

Without doubt this was more difficult to calculate. This time we have to get the sum of the 180 points of the laser (which give us the average of where are the obstacles) as we dont want the car go to that zone we have to make it go to the oposite side, we need the inverse of that point.

But that wasnt all, unlike the atractive force (which has to increase when the goal is far), the repulsive force has to decrementate when the obstacle is far and increase when it is too close. To make this i use an exponential function so the objects wich are really close would produce a bigger repulsive force and the furthest would not aport almost anything. 

My exponential functions looks something like this:

Notice that the 90 degrees of the laser doesnt match with the 90 degrees of our car so in order to calculate correctly the repulsive force is necesary to have this in mind. The position 0 of the laser correspond to the angle - 90 of the car.

**Be carefull some points given by the laser goes to infinity

Also, to improve the functioning of the car the points wich are closest to the car (the objects closer) have more wight than the others laser points, it would help to react better to any obstacle.

RESULTANT FORCE

Finally the last force. This force is the sum of the other two forces each one multiplying by a number. The atractive force would be multiply by alpha and the repulsive force by beta. So with differents values for our beta and alpha we will have differents behaviours in our car. 

If we assign alpha a really big number what we will have is a car that doesnt have fear of anything so any object between him and its objective wont stop our car, so it will crash with anything. But if we equal beta to a big number our car will have fear of everything so doesnt matter if the obstacle is still really far, the car wont move. So we have to find the equilibrium between both constants to reach the best behaviour.

Once we have both we have calculate the resultant force:

resultant_x= alfa*atractive_x + beta*repulsive_x

resultant_y= alfa*atractive_y+ beta*repulsive_y

CALCULATE VELOCITIES

The last part but not the least important, what we have to do with the resultante force?

Well now that we have really clear what are our forces, how we have to calculate them, we have to apply the lineal and angular velocities so that our car can move and achieve the targets.

The way i found the easest to do this is applying a proportional force to the case in which the car is.

So taking the front of the car for reference, we have the resultant force (a x and y) with this we can calculate the angle, and with the angle we can see how much is the resultant force desviated from the front of the car.

For the angular velocity i use the cos (Why? becouse after convert the coordenates from absolute to reatives the reference system changes and now we would have x in the axis of abcissae and y on the axis of ordinates). As the cos is closest to 1 (or -1) it means that the resultant force is really desviated from the front of the car, so we would have to apply more angular velocity (if it is close to 0 it means that the resultant force it is almost alineated with the front line).

In the case of the lineal velocity is the same but using sin so if it return a number close to 0 the car dont have to  apply so much lineal velocity becouse is really desviated. Instead if it is almost 1 is well positioned.

For example in this case the resultant force (red arrow) would be alineated with the front of the car:

                                             

But despite all of this, my car has really dificulties with the objects which where right in the middle, and the target positioned behind of it. In this case the repulsive force sends the car to the oposite direction and as the car approached to the obstacle it end stopping, but what i need was that the car could turn a little bit so it dont detect the obstacle in the middle.

So i had to made a special case for this. In case the repulsive arrow is big and almost pointing to the middle it enters on a special case in wich the angular velocity will be increased during an amount of time (a number of iterations), with this the car has the necesary to react correctly.

Finally my car could do a complete turn, here are some examples of the functioning of my car:

The complete circuit: