Revert "Revert "Tuned PD Wall follower and fixed sign issue in PD Controller""

This reverts commit 17f0ffb251acec4a09de7d0801ade5fec436795a.