BoeBotTest : Adding sonar servo control