Criar Robot para percorrer labirinto sem nunca o ter visto

[Junior Frota]

Nossa que tópico antigo, me desculpem por estar desenterrando-o!

Já que o Pedro Almeida não relatou suas dificuldades com o projeto (que espero ter concluído com êxito), nem mesmo um feedback sobre a conclusão do mesmo, vim aqui para postar minhas dúvidas e sugestões.

Para o projeto tenho:
- um Arduino Duemilanove
- um sensor ultrassom
- um servo 180°
- dois servos modificados para motor contínuo com redução
- um chassi para montar tudo

Obs.: tenho também sensores IR

Criei um algorítimo que julgo funcionar para que o Robô saia do labirinto sozinho:

- posição do servo igual a 90°
- ler distância frontal
- se distância frontal for menor ou igual 15 cm:
   - parar
   - posição do servo igual a 0°
   - ler distância esquerda
   - posição do servo igual a 180°
   - ler distância direita
   - se distância esquerda for maior que 15 cm e distância direita for maior que 15 cm faça:
      - virar à direita
     senão se distância esquerda for maior que 15 cm e distância direita for menor ou igual a 15 cm faça:
      - virar à esquerda
     senão se distância esquerda for menor ou igual a 15 cm e distância direita for maior que 15 cm faça:
      - virar à direita
     senão (distância esquerda for mmenor ou igual a 15 cm e distância direita for menor ou igual a 15 cm) faça:
      - girar 180° à direita
     fim se
  senão (distância frontal form maior que 15 cm) faça:
   - seguir em frente
  fim se

Penso que está bem enxuto e funcional, porém, sugestões serão bem vindas.

Obs.: sempre que o robô se deparar com a situação em que tanto a distância da direita quanto a distância da esquerda em relação as paredes do labirinto forem maiores que 15 cm, este escolherá o caminho da direita!

Segue em anexo o esquema de montagem do meu robô que sai de um labirinto sozinho.

Futuramente quero implementá-lo com um sistema de bússola e um sistema de salvamento com garra, onde o mesmo percorreria o labirinto e salvaria as vítimas, representadas por latas de refrigerante envoltas em papel alumínio.

Alguma sugestão?!

Obrigado e desculpa por reabrir o tópico!