Add test code for sonar servo