Extending range if failing.

In tests, a door could sometimes be detected beyond maxRange when the robot was failing once it had got half-way through a door. This change extends the detection range once a half the possible failures have occurred. This could be dangerous as it might send the robot off towards door-like things in the office, but coupled with the previous update (checking delta to target pose) it shouldn't be too bad. Needs testing though.
Tidied up debug output.