Isso está estranho..
A nossa aborgagem á coisa foi bem mais simples, sabes que começas num dos lados, logo o nosso rato considerava que estava em 0, 22
A partir dai, usa os sensores, se tem tralha á frente mete 0xF0 em 1,22, se tem coisas á esquerda mete em 0,23 se tem á direite mete em 0,21
Os motores andam o que foi considerado na altura a resolução da matrix(10cm), scanna, preenche, rinse, repeat.
Não estou a entender ai essa trigonometria toda, saber se tens obstáculo á frente, esquerda ou direita deve ser tratado após a aquisição dos dados e filtragem dos mesmo, não no meio da rotina de navegação.
