Fix bug that made the robot crossing wall in unlikely cases