Tuned PD Wall follower and fixed sign issue in PD Controller

PD wall follower is more or less tuned. Could use more tuning, but the
present gains should suffice for decent wall following.