More changes to wall detection.

Adding more changes, however still buggy.
Can get stuck in goal? Needs tested.