PD control follow right wall