Added another p controller for the case in which the robot is too close to a wall