For algorithms, consider using a Braitenberg "algorithm" for you avoidance mechanism. This works well as a "background" always on behaviour.
Also consider Potential fields - might work better with your multiple sonar setup:
For algorithms, consider using a Braitenberg "algorithm" for you avoidance mechanism. This works well as a "background" always on behaviour.
Also consider Potential fields - might work better with your multiple sonar setup: