Coloquei ponte h, continuei o código mas ainda falta.
Obs: PID controller é uma opção valida para resolver o problema de andar torto.