Add velocity watchdog, that adjust velocity when close to a wall.

Fixes issue 69 in robot_ai