Sonar PID working