Eu e meu irmão estamos montando um robô explorador.

Até o momento conseguimos fazer ele operar em modo automático  com sensor ultrassom  e no modo manual com controle IR. Porém em códigos distintos.

Estamos tentando integrar esses códigos, porém sem exito. Só conseguimos fazer ele seguir um curso programado.

Nosso objetivo é fazer com que, utilizando o controle IR, possa operar em modo automático ou manual, podendo mudar a qualquer momento.

Fluxograma do modo automático:

- 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 menor ou igual a 15 cm e distância direita for menor ou igual a 15 cm) faça:
      - girar 180° à direita
     fim
  senão (distância frontal form maior que 15 cm) faça:
   - seguir em frente
  fim 

E o modo manual basicamente é controlado assim:

2 => frente

4 => esquerda

6 => direita

8 => ré

cada movimento possui um delay de 350

Nós tentamos fazer com um terceiro modo, com um movimento programado, mas não deu certo, só o movimento programado funcionava. 

Alguém tem alguma dica?

Obs.: Nós já temos os dois códigos, mas não sabemos como integra-los.

Desde já, agradeço.