[johnyNapalm]

1. Czujnik ultradźwiękowy jest niedokładny z tego powodu, że fala dźwiękowa rozchodzi się kuliście i nie masz pewności że to co mierzysz jest odległością od ściany na przeciwko, czy od ściany obok. Dźwięki odbijają się pod różnymi kątami i nie wiadomo skąd pochodzi ten, który właśnie odebrałeś. Pomyśl o LIDAR.
2. Twój algorytm PID pracuje na liczbach zmiennoprzecinkowych. To jest jeden z najczęstszych błędów popełnianych przez arduinowców. Jak chcesz float/double, to zainteresuj się jakimś STM z FPU. Można oczywiście napisać PID na liczbach całkowitych. Wymaga to trochę innego podejścia do problemu, ale wzrost wydajności jest oszałamiający i myślę, że powinieneś się tym zainteresować.
3. Możesz trochę inaczej zarządzać czasem w programie. Nadaj funkcjom priorytety. Stwórz system przerwań .... a nie, zapomniałem że Arduino nie wspiera przerwań. Ta funkcja attachInterrupt() to jest jakieś nieporozumienie, ale nawet tym jak byś się zainteresował, to może coś wskórasz.
Jak nie, to pomyśl poważnie o STM.
Edit: Bluepill jest tańsze od Ardu!!!

Dzięki za odpowiedź   

1. Stety albo niestety jest to moja praca inżynierska stąd właśnie to tanie rozwiązanie. Nie wiedziałem o LIDAR, bardzo ciekawa sprawa, jeżeli będę robił jakiś projekt dla siebie to na pewno się zainteresuje!
2. Czy zmiana na inty trochę poprawi działanie?
3. Niestety właśnie to jest dużym ograniczeniem w moim projekcie.

Tak jak pisałem wyżej jest to projekt na prace inżynierską więc nie chce wydawać na nią jakiś większych pieniędzy. Myślisz że zmiana liczb na inty i mediana nic tu nie wskórają?