Removed 'break' in one of if branches.

The 'break' caused unintended escaping from the for loop in
AvoidObstacle mode (found by Gareth and Augustine).