Fiz o código de andar reto pelo encoder. Consertei o código do HCSR04.
Para andar reto precisaremos usar PID, pois usamos uma roda boba.
O problema no HCSR04 era que precisava de um delay(10).
Biblioteca PID