Remove redundant code and rename read_sonar2() to read_sonar() on main.cpp