Use US to help navigate forwards

Use US to help navigate forwards.
Add dumb reverse for stall detect.