Calculate robot heading to follow wall and avoid obstacles

Question

I have a task that involves implementing robot behaviour that will follow wall and avoid obstacles along it's path. The robot must stay at desired distance from the wall but also stick to it so it should not loose sight of it. Robot is sensing it's surrounding with ultrasonic sensor that is oscillating from left to right and filling an array of small length (10 values) with detected distances (every 10 degrees). From this reading I would like to calculate heading vector that will result in robot path similar to one shown in bottom picture:

Black(walls), red(obstacles), blue(robot), green(desired path)

Answer 1

It sounds like you want some sort of bug algorithm.

But the pseudo-code for what you want is simple:

while not at goal:
    if there is a wall in front:
        turn right a little
    else if there is a wall on the left:
        drive straight a little
    else:
        turn left a little
stop

Of course tuning this thing can be tricky. For example, you probably don't want to turn in place when you see no wall. You should drive forward and left in an arc. The specifics of which depend on your robot and environment.

Also, because you have a spinning ultrasonic sensor, you should not drive the robot too fast.

Note that the pseudo code really only needs a single sensor on the left of the robot and in front of the robot. But since you have an array of range values, you can potentially do something more sophisticated, although more complicated. For example, use the few range readings on the left side of the robot to determine the distance and angle of the wall relative to the robot, and act accordingly.