Maze Solver Robot, usando inteligência artificial

Aplicativos e serviços online

	Arduino IDE
	MIT App Inventor 2

Sobre este projeto

Introdução

• (Primeira passagem) :O robô encontra seu caminho para fora de um " labirinto perfeito desconhecido ". Não importa onde você colocá-lo dentro do labirinto, ele encontrará uma" solução ".

• (segunda passagem) :Uma vez que o robô encontrou uma possível solução de labirinto, ele deve otimizar sua solução encontrando o " caminho mais curto do início ao fim ".

Etapa 1:Lista de materiais

• 2 x quadrados de madeira (80x80 mm)

• 3 clipes de encadernação X

• 2 rodas de madeira X (diâmetro:50 mm)

• 1 lançador X Ball

• 9 X elásticos

• Faixa de quadro de comando 3M

• Juntas de plástico para correção do sensor

• BreadBoard e fiação

• 2 X conjuntos de bateria de hidreto de metal 4XNi-Metal (5 V cada conjunto)

• 2 X SM-S4303R de rotação contínua de plástico de 360 ​​graus servo

• Arduino Nano

• Módulo Bluetooth HC-06

• 5 sensores de linha X (Módulo de sensor seguidor de trilha de linha infravermelha TCRT5000 4CH + 1 sensor de trilha independente)

• 2 X ZX03 (baseado em TCRT5000) Sensores infravermelhos reflexivos (saída analógica)

• 1 LED

• 1 botão

Etapa 2:mudanças no corpo

Remova o conjunto original de 5 sensores de seguimento de linha e fixe o novo " Far LEFT" e " Far RIGHT “Sensores reflexivos em cada extremidade da barra de suporte de plástico. Aconselha-se que os 7 sensores sejam o mais alinhados possível.

Etapa 3:Instalando e testando os novos sensores

A nova matriz de agora 7 sensores , é montado de forma que os 5 originais sejam utilizados exclusivamente para controle PID (e detecção da "linha completa", explicada posteriormente) e os 2 novos, deixados para serem utilizados exclusivamente para detecção de interseções ESQUERDA e DIREITA.

Como uma revisão rápida, vamos lembrar como os 5 sensores "digitais" originais funcionam:

Se um sensor estiver centralizado em relação à linha preta, apenas aquele sensor específico produzirá um ALTO. Por outro lado, o espaço entre os sensores deve ser calculado para permitir que 2 sensores possam cobrir toda a largura da linha preta simultaneamente, produzindo também um sinal ALTO em ambos os sensores.

Como funcionam os 2 novos sensores "analógicos":

Se um dos sensores estiver centralizado em relação à linha preta, a saída será um valor analógico, geralmente produzindo uma saída no ADC do Arduino abaixo de "100" (lembre-se que o ADC produz uma saída de 0 a 1023). Com superfícies mais claras, o valor de saída será maior (testei 500 a 600 em papel branco, por exemplo). Este valor deve ser testado em diferentes situações de luz e materiais de superfície para definir a constante THRESHOLD correta a ser usada no seu caso (veja a imagem aqui).

Olhando para o código do Arduino, cada um dos sensores será definido com um nome específico (considere que o Line Follow Sensor original mais à esquerda deve ser atribuído com um rótulo " 0 "):

  const int lineFollowSensor0 =12; // Usando Digital inputconst int lineFollowSensor1 =18; // Usando o pino analógico A4 como entrada digital int lineFollowSensor2 =17; // Usando o pino analógico A3 como entrada digital int lineFollowSensor3 =16; // Usando o pino analógico A2 como entrada digital int lineFollowSensor4 =19; // Usando o pino analógico A5 como entrada digital int farRightSensorPin =0; // Pino analógico A0const int farLeftSensorPin =1; // Pino analógico A1  

Para lembrar, as 5 saídas de matriz de sensor originais possíveis ao seguir uma linha são:

  1 1 1 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 1 1 0 0 0 1 0 0 0 1 1 0 0 0 1 0 0 0 1 1 0 0 0 1 0 0 0 1 1 0 0 0 1 0 0 0 0  

Com a adição dos 2 novos, seus possíveis resultados são:

• Sensor Far LEFT:Saída analógica superior ou inferior a um THRESHOLD

• Sensor Far RIGHT:Saída analógica superior ou inferior a um THRESHOLD

A fim de armazenar os valores de cada sensor, uma variável de matriz é criada para os 5 sensores digitais originais:

  int LFSensor [5] ={0, 0, 0, 0, 0};  

E duas variáveis ​​inteiras para os 2 novos sensores analógicos:

  int farRightSensor =0; int farLeftSensor =0;  

Cada posição do array e variáveis ​​será constantemente atualizada com a saída de cada um dos sensores:

  LFSensor [0] =digitalRead (lineFollowSensor0); LFSensor [1] =digitalRead (lineFollowSensor1); LFSensor [2] =digitalRead (lineFollowSensor2); LFSensor [3] =digitalRead (lineFollowSensor3); =digitalRead (lineFollowSensor4); farRightSensor =analogRead (farRightSensorPin); farLeftSensor =analogRead (farLeftSensorPin);  

Ter 5 sensores, conforme visto no projeto Follower Line Robot, permite a geração de uma "variável de erro" que ajudará a controlar a posição do robô sobre a linha. Além disso, uma variável chamada "modo" será usada para definição se o robô estiver seguindo uma linha , em uma linha contínua , um cruzamento ou sem linha em absoluto.

Esta variável " modo "também será usado com" Extrema ESQUERDA / DIREITA "sensores. Para representação, vamos considerar os sensores da extrema esquerda e direita com 3 estados possíveis:

• H (maior que THRESHOLD),

• L (menor que THRESHOLD) e

• X (irrelevante).

Para as saídas digitais, será o usual 0, 1 e também apresentaremos o X:

• H 0 X X X X L ==> modo =RIGHT_TURN; erro =0; (veja o exemplo na imagem acima)

• L X X X X 0 H ==> modo =LEFT_TURN; erro =0;

• X 0 0 0 0 0 X ==> modo =NO_LINE; erro =0;

• H 0 0 0 0 1 H ==> modo =FOLLOWING_LINE; erro =4;

• H 0 0 0 1 1 H ==> modo =FOLLOWING_LINE; erro =3;

• H 0 0 0 1 0 H ==> modo =FOLLOWING_LINE; erro =2;

• H 0 0 1 1 0 H ==> modo =FOLLOWING_LINE; erro =1;

• H 0 0 1 0 0 H ==> modo =FOLLOWING_LINE; erro =0;

• H 0 1 1 0 0 H ==> modo =FOLLOWING_LINE; erro =-1;

• H 0 1 0 0 0 H ==> modo =FOLLOWING_LINE; erro =-2

• H 1 1 0 0 0 H ==> modo =FOLLOWING_LINE; erro =-3;

• H 1 0 0 0 0 H ==> modo =FOLLOWING_LINE; erro =-4;

• X 1 1 1 1 1 X ==> modo =CONT_LINE; erro =0;

Portanto, implementando a lógica acima na função:

  void readLFSsensors ()  

retornará as variáveis ​​" modo "e" erro "que será usado na lógica do programa. É importante testar a lógica dos sensores antes de seguir com o projeto. A função abaixo está incluída no código e pode ser usada para fins de teste:

  void testSensorLogic (void) {Serial.print (farLeftSensor); Serial.print ("<==LEFT RIGH ==>"); Serial.print (farRightSensor); Serial.print ("modo:"); Serial.print (modo); Serial.print ("erro:"); Serial.println (erro);}  

Etapa 4:Resolvendo o labirinto - A regra da mão esquerda

Conforme discutido na introdução, a maioria dos labirintos, por mais complexos que possam parecer seus projetos, eram formados essencialmente por uma parede contínua com muitas junções e ramificações. Se a parede ao redor da meta de um labirinto estiver conectada ao perímetro do labirinto na entrada, o labirinto sempre pode ser resolvido mantendo uma mão em contato com a parede, independentemente de quantos desvios isso possa envolver. Esses labirintos ‘simples’ são corretamente conhecidos como " Simplesmente conectado . "

Pesquisando na Wikipedia, descobrimos que:

Em suma, a Regra da mão esquerda pode ser descrito como:

Em cada cruzamento e em todo o labirinto, mantenha sua mão esquerda tocando a parede à sua esquerda.

• Coloque sua mão esquerda na parede.

• Comece a andar para frente

• Eventualmente, você chegará ao fim do labirinto. Você provavelmente não irá pelo caminho mais curto e direto, mas chegará lá.

Portanto, a chave aqui é identificar os cruzamentos , definindo o curso a seguir com base nas regras acima. Especificamente em nosso tipo de labirinto 2D, podemos encontrar 8 tipos diferentes de interseções (veja a primeira imagem acima):

Olhando a foto, podemos perceber que as ações possíveis nos cruzamentos são:

1. Em um " Cruzamento ":

• Vá para a esquerda ou

• Vá para a direita ou

• Siga em frente

2. Em um " T ":

• Vá para a esquerda ou

• Vá para a direita

3. Em um " direito apenas ":

• Vá para a direita

4. Em um " Somente esquerda ":

• Vá para a esquerda

5. Em " Reto ou Esquerda ":

• Vá para a esquerda ou

• Siga em frente

6. Na " reta ou direita ":

• Vá para a direita ou

• Siga em frente

7. Em um " beco sem saída ":

• Voltar ("U turn")

8. Em " Fim do Labirinto ":

• Pare

Mas, aplicando a "Regra da Mão Esquerda", as ações serão reduzidas a uma opção cada:

• Em uma "cruz":vá para a esquerda

• Em um "T":vá para a esquerda

• Em um "direito apenas":vá para a direita

• Em um "lado esquerdo apenas":vá para a esquerda

• Em uma "reta ou esquerda":vá para a esquerda

• Na "reta ou na direita":vá direto

• Em um "beco sem saída":volte ("volta em U")

• No "Fim do labirinto":Pare

Estamos quase lá! "Fique calmo!"

Quando o robô chega a um "Beco Sem Saída" ou "Fim de um Labirinto", é fácil identificá-los, pois não existem situações ambíguas (já implementamos essas ações no Robô Seguidor de Linha, lembra?). O problema é quando o robô encontra uma “LINHA” por exemplo, pois uma linha pode ser uma “Cruz” (1) ou um “T” (2). Também quando se atinge uma "VIRAGEM PARA A ESQUERDA ou PARA A DIREITA", essas podem ser uma curva simples (opções 3 ou 4) ou opções para seguir em frente (5 ou 6). Para descobrir exatamente em que tipo de interseção está o robô, um passo adicional deve ser dado:o robô deve correr um "centímetro a mais" e ver o que vem a seguir (veja a segunda foto acima, como exemplo).

Então, em termos de fluxo, as ações possíveis agora podem ser descritas como:

1. Em um "DEAD END":

• Voltar ("U turn")

2. Em uma "LINHA": Executar uma polegada extra

• Se houver uma linha:é uma "Cruz" ==> Vá para a ESQUERDA

• Se não houver linha:é um "T" ==> Vá para a ESQUERDA

• Se houver outra linha:é o "Fim do labirinto" ==> PARAR

3. Em uma "VIRAGEM À DIREITA": Corra uma polegada extra

• se houver uma linha:é uma reta ou direita ==> Vá EM LINHA RETA

• Se não houver linha:é um direito apenas ==> Vá para a DIREITA

4. Em uma "VIRAGEM PARA A ESQUERDA": Corra uma polegada extra

• se houver uma linha:é uma reta ou ESQUERDA ==> Vá para a ESQUERDA

• Se não houver linha:é apenas ESQUERDA ==> Vá para ESQUERDA

Observe que, na verdade, no caso de uma "VIRAGEM PARA A ESQUERDA", você pode pular o teste, porque você fará para a ESQUERDA de qualquer maneira. Deixei a explicação mais genérica apenas para maior clareza. No código real, vou pular este teste. A terceira foto acima mostra um labirinto muito simples no chão do meu laboratório, usado para fins de teste.

Etapa 5:Implementar o algoritmo "Mão esquerda na parede" no código Arduino

Assim que tivermos o readLFSsensors () função modificada para incluir os 2 sensores extras, podemos reescrever a Função de Loop para executar o algoritmo conforme descrito na última etapa:

  void loop () {readLFSsensors (); switch (modo) {case NO_LINE:motorStop (); goAndTurn (LEFT, 180); pausa; case CONT_LINE:runExtraInch (); readLFSsensors (); if (modo ==CONT_LINE) mazeEnd (); senão goAndTurn (LEFT, 90); // ou é um "T" ou "Cruz"). Em ambos os casos, vai para o intervalo ESQUERDO; case RIGHT_TURN:runExtraInch (); readLFSsensors (); if (mode ==NO_LINE) goAndTurn (RIGHT, 90); pausa; case LEFT_TURN:goAndTurn (LEFT, 90); pausa; case FOLLOWING_LINE:followingLine (); pausa; }}  

Algumas novas funções aparecem aqui:

• seguinteLinha () é o mesmo usado com o robô de linha seguinte onde, se estiver apenas seguindo uma linha, deve calcularPID () ; e controlar os motores dependendo dos valores PID: motorPIDcontrol ();

• runExtraInch (): irá empurrar o robô um pouco para a frente. O quanto o robô funcionará dependerá do tempo que você usa na função de retardo, antes de comandar a parada do motor.

  void runExtraInch (void) {motorPIDcontrol (); atraso (extraInch); motorStop ();}  

• goAndTurn (direção, ângulo): esta função especial é importante porque você não pode virar o robô assim que perceber o tipo de interseção em que está. Lembre-se que projetamos um Robô Diferencial que ao fazer curvas, ele "gira em torno de seu machado" e assim, para se mover 90o e seguir continuamente a linha, o centro das rodas deve estar alinhado com o centro de interseção. Assim que a linha de sensores estiver à frente de seu machado, você deve executar o robô para frente para alinhá-los. A constante de tempo adjGoAndTurn deve ser ajustado dependendo da distância entre o machado e a linha do sensor (" d "), velocidade e tamanho das rodas (veja a imagem acima para ilustração).

  void goAndTurn (direção interna, graus internos) {motorPIDcontrol (); atraso (adjGoAndTurn); motorTurn (direção, graus);}  

Neste ponto, o robô está de fato "resolvendo um labirinto"! Você acabou de terminar a "Primeira passagem". Não importa onde você comece dentro de um labirinto, você sempre chegará ao fim.

A seguir, um teste desta fase do projeto:

Etapa 6:Armazenando o Caminho

Vamos considerar o exemplo mostrado na foto acima. No ponto de partida escolhido, o robô encontrará 15 interseções antes de chegar ao final do labirinto:

• Esquerda (L)

• Voltar (B)

• Esquerda (L)

• Esquerda (L)

• Esquerda (L)

• Voltar (B)

• Reto (S)

• Voltar (B)

• Esquerda (L)

• Esquerda (L)

• Voltar (B)

• Reto (S)

• Esquerda (L)

• Esquerda (L)

• Fim

O que deve ser feito em qualquer uma dessas interseções, é salvar cada ação realizada exatamente na mesma sequência em que ocorre. Para isso, vamos criar uma nova variável (array) que irá armazenar o caminho que o robô percorreu:

  char path [100] ="";  

Devemos também criar 2 variáveis ​​de índice para serem usadas junto com a matriz:

  unsigned char pathLength =0; // o comprimento do pathint pathIndex =0; // usado para alcançar um elemento específico da matriz.  

Portanto, se executarmos o exemplo mostrado na imagem, terminaremos com:

  path =[LBLLLBSBLLBSLL] e pathLengh =14  

Etapa 7:Simplificar (otimizar) o caminho

Voltemos ao nosso exemplo. Olhando o primeiro grupo de interseções, percebemos que o primeiro ramo esquerdo é na verdade um "beco sem saída" e então, se o robô em vez de um "Esquerdo-Traseiro-Esquerdo" apenas passasse direto naquele primeiro cruzamento, muita energia e o tempo seria economizado! Em outras palavras, uma sequência "LBL" na verdade seria igual a "S". É exatamente assim que o caminho completo pode ser otimizado. Se você analisar todas as possibilidades onde uma "curva em U" é usada, o conjunto de 3 interseções onde esta "curva em U" ("B") aparece ("xBx") pode ser reduzido a apenas uma.

O acima é apenas um exemplo, abaixo você pode encontrar a lista completa de possibilidades (experimente):

• LBR =B

• LBS =R

• RBL =B

• SBL =R

• SBS =B

• LBL =S

Pegando o caminho completo ou nosso exemplo, podemos reduzi-lo:

  path =[LBLLLBSBLLBSLL] ==> LBL =Spath =[SLLBSBLLBSLL] ==> LBS =Rpath =[SLRBLLBSLL] ==> RBL =Bpath =[SLBLBSLL] ==> LBL =Spath =[SSBSLL ] ==> SBS =Bpath =[SBLL] ==> SBL =Rpath =[RL]  

Surpreendente! Olhando para o exemplo fica muito claro que se o robô pegar a DIREITA na primeira interseção e depois à ESQUERDA, ele alcançará o Fim do Labirinto no caminho mais curto!

O código total do primeiro caminho do Maze Solver será consolidado na função mazeSolve () . Esta função é na verdade a função loop () usada antes, mas incorpora todas as etapas de armazenamento e otimização de caminho. Quando o primeiro caminho terminar, o array path [] terá o caminho otimizado. Uma nova variável é introduzida:

  status int não assinado =0; // resolvendo =0; alcançar Maze End =1  

Abaixo da função do primeiro caminho:

  void mazeSolve (void) {while (! status) {readLFSsensors (); switch (modo) {case NO_LINE:motorStop (); goAndTurn (LEFT, 180); recIntersecção ('B'); pausa; case CONT_LINE:runExtraInch (); readLFSsensors (); if (modo! =CONT_LINE) {goAndTurn (LEFT, 90); recIntersection ('L');} // ou é um "T" ou "Cruz"). Em ambos os casos, vai para LEFT else mazeEnd (); pausa; case RIGHT_TURN:runExtraInch (); readLFSsensors (); if (modo ==NO_LINE) {goAndTurn (RIGHT, 90); recIntersection ('R');} else recIntersection ('S'); pausa; case LEFT_TURN:goAndTurn (LEFT, 90); recIntersecção ('L'); pausa; case FOLLOWING_LINE:followingLine (); pausa; }}}  

Aqui, uma nova função foi introduzida: recIntersection (direction). Esta função será usada para armazenar a interseção e também para chamar outra função simplifyPath () , isso irá reduzir o grupo de 3 cruzamentos envolvendo um "retorno", como vimos antes.

  void recIntersection (char direction) {path [pathLength] =direction; // Armazena a interseção na variável de caminho. pathLength ++; simplifyPath (); // Simplifique o caminho aprendido.}  

O CRÉDITO para o simplifyPath ( ) a função é para Patrick McCabe para o código de solução de caminho (para obter detalhes, visite https://patrickmccabemakes.com)! A estratégia de simplificação do caminho é que sempre que encontrarmos uma sequência xBx, podemos simplificá-la cortando o beco sem saída. Por exemplo, LBL ==> S como vimos no exemplo.

  void simplifyPath () {// apenas simplifica o caminho se a penúltima curva for um 'B' if (pathLength <3 || path [pathLength-2]! ='B') return; int totalAngle =0; int i; para (i =1; i <=3; i ++) {switch (path [pathLength-i]) {case 'R':totalAngle + =90; pausa; caso 'L':totalAngle + =270; pausa; caso 'B':totalAngle + =180; pausa; }} // Obtenha o ângulo como um número entre 0 e 360 ​​graus. totalAngle =totalAngle% 360; // Substitua todas essas curvas por uma única. switch (totalAngle) {case 0:path [pathLength - 3] ='S'; pausa; caso 90:caminho [pathLength - 3] ='R'; pausa; caso 180:caminho [pathLength - 3] ='B'; pausa; caso 270:caminho [pathLength - 3] ='L'; pausa; } // O caminho agora é duas etapas mais curto. pathLength - =2; }  

Etapa 8:Segunda passagem:resolvendo o labirinto o mais rápido possível!

O programa principal: loop () é simples assim:

  void loop () {ledBlink (1); readLFSsensors (); mazeSolve (); // Primeira passagem para resolver o labirinto ledBlink (2); while (digitalRead (buttonPin) {} pathIndex =0; status =0; mazeOptimization (); // Segunda passagem:execute o labirinto o mais rápido possível ledBlink (3); while (digitalRead (buttonPin) {} modo =STOPPED; status =0; // 1ª passagem pathIndex =0; pathLength =0;}  

Portanto, quando o primeiro passo termina, o que devemos fazer é apenas alimentar o robô com o array de caminhos otimizado. Ele começará a funcionar e quando um cruzamento for encontrado, ele agora definirá o que fazer com base no que está armazenado em path [] .

  void mazeOptimization (void) {while (! status) {readLFSsensors (); switch (modo) {case FOLLOWING_LINE:followingLine (); pausa; case CONT_LINE:if (pathIndex> =pathLength) mazeEnd (); else {mazeTurn (path [pathIndex]); pathIndex ++;} break; case LEFT_TURN:if (pathIndex> =pathLength) mazeEnd (); else {mazeTurn (path [pathIndex]); pathIndex ++;} break; case RIGHT_TURN:if (pathIndex> =pathLength) mazeEnd (); else {mazeTurn (path [pathIndex]); pathIndex ++;} break; }}}  

Para comandar o que fazer, uma nova função mazeTurn (path []) foi criado. A função mazeTurn (path []) vai ser:

  void mazeTurn (char dir) {switch (dir) {case 'L':// Vire à esquerda goAndTurn (LEFT, 90); pausa; case 'R':// Vire à direita goAndTurn (RIGHT, 90); pausa; case 'B':// Voltar goAndTurn (RIGHT, 800); pausa; case 'S':// Vá direto runExtraInch (); pausa; }}  

A segunda passagem está concluída! O vídeo abaixo mostra o exemplo completo trabalhado aqui, primeira e segunda passagem. Abaixo do código do Arduino usado neste tutorial:
FV6XNJWINJ45XWM.ino F2FXS8MINJ45XX6.h FX5MHFMINJ45XX7.ino FT2S1WXINJ45XXA.ino F9IC3HQINJ45XXB.ino FU2HRXJINJ45XXV.ino

Etapa 9:Usando o Android para ajuste

O aplicativo Android desenvolvido para o projeto following Line também pode ser usado aqui (se necessário, o aplicativo Android e seu código estão disponíveis em:Line Follower Robot - PID Control - Android Setup. O código do Arduino apresentado na última etapa já inclui a comunicação com o dispositivo Android. Se você não quiser usar o aplicativo Android, não há problema porque o código é " transparente "

Usei muito o Android durante o projeto para enviar dados de teste do robô para o dispositivo usando a " Mensagem recebida "campo. Diversas variáveis ​​devem ser bem definidas para garantir que o robô gire no ângulo correto. As mais importantes estão abaixo (as marcadas em negrito mudei várias vezes):

  const int adj =0; float adjTurn =8; int adjGoAndTurn =800; THRESHOLD =150const int potência =250; const int iniMotorPower =250; int extraInch =200;  

Etapa 10:Conclusão

Esta é a segunda e última parte de um projeto complexo, explorando a potencialidade de um Robô seguidor de linha, onde Inteligência Artificial (AI) conceitos simples foram usados ​​para resolver um labirinto.

Eu não sou um AI especialista e com base em algumas informações que obtive da web, entendi que o que nosso pequeno Rex, o robô fez, resolvendo o labirinto, poderia ser considerado uma aplicação de IA. Vamos dar uma olhada nas 2 fontes abaixo:

Da Wikipedia:

Ou deste artigo da universidade:"Maze Solving Robot Using Freeduino and LSRB Algorithm International Journal of Modern Engineering Research (IJMER)"

Os arquivos atualizados para este projeto podem ser encontrados no GITHUB. Espero poder contribuir para que outros aprendam mais sobre eletrônica, robô, Arduino, etc. Para mais tutoriais, visite meu Blog:MJRoBot.org

Saludos do sul do mundo!

Obrigado

Marcelo

Código

• Snippet de código # 1
• Snippet de código 4
• Snippet de código # 5
• Snippet de código # 6
• Snippet de código # 7
• Snippet de código # 8
• Snippet de código # 12
• Snippet de código # 13
• Snippet de código # 14
• Snippet de código # 15
• Snippet de código # 16
• Snippet de código # 17

Snippet de código # 1 Texto simples

 const int lineFollowSensor0 =12; // Usando Digital inputconst int lineFollowSensor1 =18; // Usando o pino analógico A4 como entrada digital int lineFollowSensor2 =17; // Usando o pino analógico A3 como entrada digital int lineFollowSensor3 =16; // Usando o pino analógico A2 como entrada digital int lineFollowSensor4 =19; // Usando o pino analógico A5 como entrada digital int farRightSensorPin =0; // Pino analógico A0const int farLeftSensorPin =1; // Pino analógico A1 

Snippet de código # 4 Texto simples

 LFSensor [0] =digitalRead (lineFollowSensor0); LFSensor [1] =digitalRead (lineFollowSensor1); LFSensor [2] =digitalRead (lineFollowSensor2); LFSensor [3] =digitalRead (lineFollowSensor1); LFSensor [2] =digitalRead (lineFollowSensor2); LFSensor [3] =digitalRead (lineFollowSensor3); lineFollowSensor4); farRightSensor =analogRead (farRightSensorPin); farLeftSensor =analogRead (farLeftSensorPin); 

Snippet de código # 5 Texto simples

 void testSensorLogic (void) {Serial.print (farLeftSensor); Serial.print ("<==LEFT RIGH ==>"); Serial.print (farRightSensor); Serial.print ("modo:"); Serial.print (modo); Serial.print ("erro:"); Serial.println (erro);} 

Snippet de código # 6 Texto simples

 void loop () {readLFSsensors (); switch (modo) {case NO_LINE:motorStop (); goAndTurn (LEFT, 180); pausa; case CONT_LINE:runExtraInch (); readLFSsensors (); if (modo ==CONT_LINE) mazeEnd (); senão goAndTurn (LEFT, 90); // ou é um "T" ou "Cruz"). Em ambos os casos, vai para o intervalo ESQUERDO; case RIGHT_TURN:runExtraInch (); readLFSsensors (); if (mode ==NO_LINE) goAndTurn (RIGHT, 90); pausa; case LEFT_TURN:goAndTurn (LEFT, 90); pausa; case FOLLOWING_LINE:followingLine (); pausa; }} 

Snippet de código # 7 Texto simples

 void runExtraInch (void) {motorPIDcontrol (); atraso (extraInch); motorStop ();} 

Snippet de código # 8 Texto simples

 void goAndTurn (direção interna, graus internos) {motorPIDcontrol (); atraso (adjGoAndTurn); motorTurn (direção, graus);} 

Snippet de código # 12 Texto simples

 void mazeSolve (void) {while (! status) {readLFSsensors (); switch (modo) {case NO_LINE:motorStop (); goAndTurn (LEFT, 180); recIntersecção ('B'); pausa; case CONT_LINE:runExtraInch (); readLFSsensors (); if (modo! =CONT_LINE) {goAndTurn (LEFT, 90); recIntersection ('L');} // ou é um "T" ou "Cruz"). In both cases, goes to LEFT else mazeEnd(); pausa; case RIGHT_TURN:runExtraInch(); readLFSsensors(); if (mode ==NO_LINE) {goAndTurn (RIGHT, 90); recIntersection('R');} else recIntersection('S'); pausa; case LEFT_TURN:goAndTurn (LEFT, 90); recIntersection('L'); pausa; case FOLLOWING_LINE:followingLine(); pausa; } }}

Code snippet #13Plain text

void recIntersection(char direction){ path[pathLength] =direction; // Store the intersection in the path variable. pathLength ++; simplifyPath(); // Simplify the learned path.}

Code snippet #14Plain text

void simplifyPath(){ // only simplify the path if the second-to-last turn was a 'B' if(pathLength <3 || path[pathLength-2] !='B') return; int totalAngle =0; int i; for(i=1;i<=3;i++) { switch(path[pathLength-i]) { case 'R':totalAngle +=90; pausa; case 'L':totalAngle +=270; pausa; case 'B':totalAngle +=180; pausa; } } // Get the angle as a number between 0 and 360 degrees. totalAngle =totalAngle % 360; // Replace all of those turns with a single one. switch(totalAngle) { case 0:path[pathLength - 3] ='S'; pausa; case 90:path[pathLength - 3] ='R'; pausa; case 180:path[pathLength - 3] ='B'; pausa; case 270:path[pathLength - 3] ='L'; pausa; } // The path is now two steps shorter. pathLength -=2; } 

Code snippet #15Plain text

void loop() { ledBlink(1); readLFSsensors(); mazeSolve(); // First pass to solve the maze ledBlink(2); while (digitalRead(buttonPin) { } pathIndex =0; status =0; mazeOptimization(); // Second Pass:run the maze as fast as possible ledBlink(3); while (digitalRead(buttonPin) { } mode =STOPPED; status =0; // 1st pass pathIndex =0; pathLength =0;}

Code snippet #16Plain text

void mazeOptimization (void){ while (!status) { readLFSsensors(); switch (mode) { case FOLLOWING_LINE:followingLine(); pausa; case CONT_LINE:if (pathIndex>=pathLength) mazeEnd (); else {mazeTurn (path[pathIndex]); pathIndex++;} break; case LEFT_TURN:if (pathIndex>=pathLength) mazeEnd (); else {mazeTurn (path[pathIndex]); pathIndex++;} break; case RIGHT_TURN:if (pathIndex>=pathLength) mazeEnd (); else {mazeTurn (path[pathIndex]); pathIndex++;} break; } } }

Code snippet #17Plain text

void mazeTurn (char dir) { switch(dir) { case 'L':// Turn Left goAndTurn (LEFT, 90); pausa; case 'R':// Turn Right goAndTurn (RIGHT, 90); pausa; case 'B':// Turn Back goAndTurn (RIGHT, 800); pausa; case 'S':// Go Straight runExtraInch(); pausa; }} 

Github

Esquemas