O código não faz grande sentido. Utilizar funções que devolvem valores e ao mesmo tempo usar variáveis globais não faz o mais pequeno sentido.
Exactamente o que é que o programa faz e o que é que devia fazer? É que dizer: "não funcionam o sensor ultrassônico e o servo motor" é muito vago e perde-se mais tempo a tentar perceber o que é que isto significa que a procurar o erro em si.