First commit :)
The controller is decreasing gradually the velocity while it advances to towards a wall that is closer to inf_thres until it fully stop.
Some small details were corrected. The general behavior is the same.
Updated the function for following the wall. Now we are decreasing the velocity proportional to the velocity
Some bugs were fixed. Such as reading values of wrong IR sensors.

Some comments to be discussed later are added

The next step is to control while following the wall
To get the most up to date code before cleanup

Merge branch 'logic_controldraft' into logic_cleanup