Added integral gain to wall follow