Um algoritmo para a solução de labirintos com uso de um

Transcrição

INSTITUTO FEDERAL DO ESPÍRITO SANTO
PÓS GRADUAÇÃO LATO SENSU EM ENGENHARIA ELÉTRICA COM ÊNFASE EM
SISTEMAS INTELIGENTES APLICADOS À AUTOMAÇÃO
ANDERSON SACHETTO ROSA
UM ALGORITMO PARA A SOLUÇÃO DE LABIRINTOS COM USO DE UM ROBÔ
MÓVEL
VITÓRIA
2015
ANDERSON SACHETTO ROSA
UM ALGORITMO PARA A SOLUÇÃO DE LABIRINTOS COM USO DE UM ROBÔ
MÓVEL
Trabalho apresentado ao curso de Pós-Graduação Lato
Sensu em Engenharia Elétrica com Ênfase em Sistemas
Inteligentes Aplicados à Automação do Instituto Federal
do Espírito Santo como requisito parcial para obtenção
do certificado de Especialista em Sistemas Inteligentes
Aplicados à Automação.
Orientador: Prof. Dr. Luis Eduardo Martins de Lima
VITÓRIA
2015
(Biblioteca Nilo Peçanha do Instituto Federal do Espírito Santo)
R788a
Rosa, Anderson Sachetto.
Um algoritmo para a solução de labirintos com uso de um robô móvel /
Anderson Sachetto Rosa. – 2015.
49 f. : il. ; 30 cm
Orientador: Luis Eduardo Martins de Lima.
Monografia (especialização) – Instituto Federal do Espírito Santo,
Coordenadoria de Pós-Graduação em Engenharia Elétrica, Curso PósGraduação Lato Sensu em Engenharia Elétrica com Ênfase em Sistemas
Inteligentes Aplicados à Automação, 2015.
1. Engenharia elétrica. 2. Sistemas de controle inteligente. 3. Robótica.
4. Algorítmos computacionais. 5. Labirintos. I. Lima, Luis Eduardo Martins
de. II. Instituto Federal do Espírito Santo. III. Título.
CDD 21 – 621.3
AGRADECIMENTOS
Agradeço a Deus, pelas
oportunidades
proporcionadas a mim.
E aos meus pais e
amigos pelo incentivo e
paciência.
RESUMO
O desenvolvimento de sistemas inteligentes autônomos tem sido um campo de inúmeros
estudos há vários anos. Atualmente, a robótica móvel assitiva tem se caracterizado como um
ramo dos sistemas autônomos de grande apelo social, nos que diz respeito ao suporte dado a
pessoas com deficiência física. Os robôs capazes de se locomoverem de forma autônoma por
ambientes, podem auxiliar deficientes visuais a se locomoverem dentro de suas residências, por
exemplo. A resolução de labirintos com o uso de robôs móveis de forma autônoma, viabiliza a
elaboração de técnicas/ algoritmos que podem vir a ser extrapolados para ambientes físicos
reais. Este trabalho tem por objetivo desenvolver um algoritmo, embarcado em uma plataforma
robótica, capaz de explorar um labirinto desconhecido em busca de um ponto demarcado como
destino, em busca de um caminho que o leve diretamente de um ponto de origem a outro de
destino. Ao encontrar o ponto de destino, o robô retorna ao ponto de partida executando o trajeto
que o leva diretamente de volta, para então navegar novamente rumo ao destino, executando o
trajeto que o leva diretamente ao destino. Estas constituem as três etapas do algoritmo proposto,
denominadas exploração, volta a origem e navegação.
Palavras-chaves: Robótica móvel. Resolução de Labirintos. Algoritmo. Micromouse.
LISTA DE FIGURAS
Figura 1 - Exemplo de labirinto tradicional.............................................................................. 13
Figura 2 – Exemplo de labirinto utilizado. ............................................................................... 14
Figura 3 - Estrutura de hardware utilizada. .............................................................................. 14
Figura 4 - Robô Zumo. a) Vista em perspectiva; b) Principais módulos. ................................ 15
Figura 5 - Barra de sensores IR. ............................................................................................... 16
Figura 6 - Modelo de encoder utilizado. .................................................................................. 17
Figura 7 - Plataforma de desenvolvimento Arduino Mega 2560. ............................................ 17
Figura 8 - Bifurcações possíveis de serem encontradas nos labirintos..................................... 19
Figura 9 - Percepção e diferenciação de bifurcações. a) e c) Momento em que uma bifurcação
é percebida; b) Leitura que caracteriza a bifurcação percebida em a); d) Leitura que
caracteriza a bifurcação percebida em c). ................................................................ 20
Figura 10 - Comportamento do robô para uma dada situação. a) Robô se aproximando da
bifurcação; b) Identifica-se o sentido e segue por ele; c) Não há caminhos para
prosseguir nesta direção; d) Segue por um novo caminho. ..................................... 21
Figura 11 - Código em "C" que define a estrutura Crossing. ................................................... 22
Figura 12 - Inserção de uma nova bifurcação à lista. ............................................................... 23
Figura 13 - Etapa de exploração. Eliminando bifurcações que não contribuem para a resolução.
.................................................................................................................................. 24
Figura 14 - Fluxograma que representa o funcionamento do algoritmo na fase de exploração.
.................................................................................................................................. 25
Figura 15 – Exemplo de composição da lista de bifurcações. .................................................. 27
Figura 16 - Exemplo de trajeto oval, utilizado na primeira etapa de testes. ............................. 29
Figura 17 - Módulo Bluetooth HC-06 ...................................................................................... 30
Figura 18 - Labirinto de teste do sistema de coordenadas. ....................................................... 31
Figura 19 – Trajetos resultantes do labirinto de testes. ............................................................ 32
Figura 20 - Trajetos resultantes do labirinto de validação........................................................ 34
Figura 21 - Problema referente a labirintos circulares. ............................................................ 35
LISTA DE SIGLAS
IEEE – Institute of Electrical and Electronics Engineers (Instituto de Engenheiros Eletricistas
e Eletrônicos);
IR – Infra Red (Infravermelho);
PC - Personal Computer (Computador Pessoal);
PCI – Placa de Circuito Impresso;
PWM – Pulse Width Modulation (Modulação por Largura de Pulso);
SPD – Seguidor de Parede à Direita;
SPE – Seguidor de Parede à Esquerda.
SUMÁRIO
1
INTRODUÇÃO .............................................................................................................. 9
1.1
TRABALHOS RELACIONADOS ................................................................................ 11
2
MATERIAIS ................................................................................................................. 13
3
MÉTODOS .................................................................................................................... 19
3.1
ETAPA DE EXPLORAÇÃO ......................................................................................... 20
3.2
ETAPAS “VOLTA À ORIGEM” E NAVEGAÇÃO DO LABIRINTO ...................... 26
4
TESTES E RESULTADOS ......................................................................................... 29
4.1
TESTES DE DESENVOLVIMENTO ........................................................................... 29
4.2
TESTES E VALIDAÇÃO DO ALGORITMO .............................................................. 32
5
CONCLUSÃO............................................................................................................... 36
REFERÊNCIAS ............................................................................................................ 38
APÊNDICE A – Algoritmo desenvolvido ...................................................................... 39
9
1 INTRODUÇÃO
Investigações acerca de métodos para resolução de labirintos com o uso de robôs móveis vêm
sendo feitas a mais de 30 anos (MISHRA e BANDE, 2008), mas sua importância para a
robótica, e em particular a robótica móvel, continua muito presente nos dias atuais, uma vez
que se busca cada vez mais o desenvolvimento de sistemas autônomos inteligentes capazes de
interagir com o “mundo real”, e desenvolver tarefas não previstas, algo que simule o
comportamento humano.
Esta “inteligência” projetada sobre os robôs móveis, e proporcionada pelas novas tecnologias
disponíveis no mercado (sensores, microcontroladores, atuadores de precisão, drivers de
potência miniaturizados), tem exercido papel fundamental no desenvolvimento de soluções de
muita relevância para a sociedade. Por exemplo, pode-se citar a redução das limitações impostas
às pessoas portadoras de algum tipo de deficiência física. Pessoas com dificuldades motoras
e/ou visuais têm conseguido com a robótica móvel assistiva o auxílio para a realização de
tarefas como, por exemplo, andar por pequenos trajetos dentro de suas residências. Pessoas com
dificuldades motoras podem precisar de auxílio para a própria locomoção, já pessoas com
deficiência visual necessitam de sistemas que lhes proporcionem a visão do ambiente em que
se encontram. (MOREIRA, VASCONCELOS e FILHO, 2010). Neste contexto, robôs móveis
que tenham a capacidade de guiar o indivíduo por entre obstáculos presentes no percurso são
de grande relevância no auxílio às atividades cotidianas deste indivíduo, e consequentemente
contribui para o aumento do grau de autonomia diária destas pessoas.
A navegação autônoma de robôs móveis, em ambientes com configuração dinâmica e
desconhecida, ou seja, ambientes não estruturados, é tratada na literatura como uma
caracterização do problema de SLAM (Simultaneous Localization and Mapping - Localização
e Mapeamento Simultâneos) (CHANG, LEE, et al., 2007). Neste contexto, os robôs percorrem
o ambiente executando um processo de auto-localização e mapeamento do ambiente com o uso
dos sensores neles embarcados e algoritmos para a construção topológica do ambiente. Após o
mapeamento, os robôs tornam-se capazes de locomoverem-se pelo ambiente realizando
trajetórias de interesse, livres de colisões.
A estruturação de um ambiente, com sinalização sob a forma de um labirinto, facilita o
sensoriamento exigido para que o robô possa “perceber” o ambiente, permitindo assim que os
esforços no desenvolvimento da pesquisa estejam focados na implantação da “inteligência” do
robô, representada pelos algoritmos de solução dos labirintos.
10
Ao longo dos anos, diversas competições de robótica (Micromouse, Robot Soccer) têm
proporcionado a estudantes e pesquisadores a oportunidade de apresentarem seus trabalhos de
pesquisa em grandes fóruns e com grande cobertura da mídia (BRAUNL, 1999). Desde 1986 o
IEEE (Institute of Electrical and Electronics Engineers – Instituto de Engenheiros Eletricistas
e Eletrônicos) formula um conjunto de regras para competições de robóticas, que envolvem o
problema de resolução de labirintos por robôs móveis.
Estas competições têm também a função de estimular nos participantes a criatividade de cada
um, proporcionando o aprendizado no trabalho em equipe. Os projetos desenvolvidos para estas
competições oferecem aos participantes o desafio de trabalhar em problemas com solução em
aberto, complementando o aprendizado da teoria convencional acadêmica (MISHRA e
BANDE, 2008).
Robôs de pequeno porte capazes de locomoverem-se no interior de labirintos, de um ponto de
partida até outro demarcado com a chegada, são conhecidos como Micromouses. Estes robôs
utilizam sistemas de sensoriamento e controle embarcados, e devem ser dotados de
“inteligência” que os possibilitem realizar esta tarefa com o menor custo possível. Onde o custo
pode estar vinculado ao tempo gasto na etapa de mapeamento, quantidade de movimentos, etc
(CAI et al., 2010).
Os algoritmos presentes na literatura atual, e que apresentam os melhores resultados na
resolução de labirintos, necessitam de alguma informação prévia a respeito dos labirintos (CAI,
HUO, et al., 2010). Quando são extrapolados os limites definidos pelos labirintos utilizados
nestas competições, e se passa a observar este problema pela ótica do mundo real, não se pode
garantir a passagem de informações previas aos robôs, uma vez que a cena na qual ele será
inserido tende a ser dinâmica.
Neste trabalho é apresentado um estudo de análise e adaptação de algoritmos para a resolução
de labirintos aplicados ao controle de locomoção de um robô móvel. Este trabalho tem por
objetivo desenvolver um algoritmo para solução de labirintos de modo que não haja a
necessidade de conhecimento prévio das dimensões do labirinto onde o robô estiver inserido, e
de forma que o robô não se perca em loops que o impossibilitem de encontrar o caminho até o
fim do labirinto.
11
1.1 TRABALHOS RELACIONADOS
Em busca da melhor forma de se obter o mapeamento de labirintos com o uso de robôs, Mishra
and Bande, 2008 discorrem acerca de três algoritmos utilizados na etapa de mapeamento. Os
métodos mais simples levam o robô a seguir sempre por uma parede que esteja mais a sua
esquerda ou à direita, estes algoritmos são chamados Left Wall Follower (Seguidor de Parede à
Esquerda – SPE) e Right Wall Follower (Seguidor de Parede à Direita - SPD), eles são ditos
como "ausentes de inteligência", uma vez que não se sabe a posição e direção do robô a cada
instante.
Outra forma é utilizar uma variação do algoritmo de Dijkstra (Skiena, 1990), para o qual faz-se
a identificação dos cruzamentos presentes no caminho para a resolução do labirinto e computa
o custo necessário para a locomoção da origem ao destino, com isso pode-se escolher o caminho
de menor custo para a solução do labirinto (Mishra and Bande, 2008). Como desvantagem desta
solução há a necessidade da locomoção por todo o labirinto, para que todos os cruzamentos
possíveis sejam identificados para que se possa definir o caminho de menor custo.
A terceira proposta de solução para labirintos, considerada como sendo a melhor dentre as já
citadas (Mishra and Bande, 2008), é o Flood Fill Algorithm. Neste método o labirinto tem suas
células divididas e enumeradas de acordo com sua distância até a célula destino, com o
conhecimento que é adquirido pelo robô na sua navegação ele consegue identificar qual e o
número da célula em questão, de forma a evitar navegar por rotas que o leve a células com uma
numeração crescente.
Ao considerar o tamanho padrão dos labirintos, (Cai et al., 2010) faz a divisão do labirinto em
12 partes ao redor do ponto de chegada, localizado no centro do labirinto. A partir de então,
técnicas de locomoção distintas são utilizas pelo robô a medida que sua posição muda de uma
partição para a outra. A divisão reduz a possibilidade do afastamento do robô na medida em
que se aproxima do ponto de chegada, mas, limita sua utilização a labirintos que tenham suas
dimensões conhecidas, assim como apresentado por (Mishra and Bande, 2008).
Diversos hardwares de robôs móveis são utilizados no desenvolvimento de soluções para
labirintos. Hualong et al., 2011 propõe uma arquitetura composta por cinco elementos
principais, a citar: fonte de energia, unidade de detecção de distância, unidade de controle de
movimento, unidade de conversão de sinal e uma central de controle. O teste do hardware
desenvolvido é feito com base nos algoritmos SPE e SPD, primeiro o robô percorre o labirinto
utilizando o SPE e retorna para a origem. Logo após o robô percorre novamente, desta vez
12
utilizando o SPD, e então retorna novamente até a origem, então é feita a comparação dos
percursos feitos em busca do menor, o qual será executado como caminho ótimo.
Cooperative Labyrinth Discovery (Descoberta Cooperativa de Labirinto) (Rahnama et al.,
2013), onde dois ou mais robôs são postos a mapear de forma independente um mesmo
labirinto, compartilhando entre si as informações adquiridas individualmente.
13
2 MATERIAIS
Tipicamente os labirintos nos quais os robôs são inseridos, são formados por espaços vazios
por onde o robô é capaz de navegar - os caminhos - e barreiras intransponíveis, que contornam
os caminhos ditando os trajetos que podem ser seguidos pelo robô, Figura 1. Nestes casos, o
sistema de sensoriamento do robô deve ser capaz de perceber obstáculos ao seu redor,
mensurando as distâncias entre o robô e estes obstáculos, tipicamente são utilizados sensores
ultrassônicos ou sensores de reflexão de infravermelho (HUALONG, YONGHONG e
HONGQI, 2011).
Figura 1 - Exemplo de labirinto tradicional.
Fonte: (MISHRA e BANDE, 2008)
Para facilitar o desenvolvimento do sistema de sensoriamento do robô neste trabalho, optou-se
pelo uso de um labirinto que não contivesse barreiras físicas, e sim trajetos demarcados na
superfície de locomoção, Figura 2.
14
Figura 2 – Exemplo de labirinto utilizado.
Fonte: Elaborado pelo autor, 2014.
A estrutura de hardware utilizada para que o robô possa interagir com o ambiente, é dividida
em três partes principais, o conjunto de sensores, o controlador embarcado na plataforma
robótica e os atuadores Figura 3. O conjunto de sensores é formado por uma matriz de sensores
IR, capaz de identificar os trajetos do labirinto, e um par de encoders utilizados medir a distância
percorrida pelo robô. O controlador embarcado é composto por um microcontrolador capaz de
armazenar o algoritmo desenvolvido. Os atuadores são os próprios motores da plataforma
robótica utilizada.
Figura 3 - Estrutura de hardware utilizada.
15
Assim como exposto por Hualong et al., 2011, diversas plataformas de robôs móveis são
utilizadas no desenvolvimento da tarefa de resolução de labirintos. A plataforma utilizada neste
trabalho foi o robô de esteiras Zumo, Figura 4. Projetado para competições de sumô
(PASSOLD, 2006), este robô utiliza o sistema de tração diferencial para acionamento das
esteiras. O robô Zumo foi desenvolvido para trabalhar em compatibilidade com a placa
microcontrolada Arduino Uno, ou outra placa compatível, sendo que, a possibilidade do uso de
sensores infravermelhos em sua parte frontal, permite que ele identifique as linhas dos
labirintos.
Figura 4 - Robô Zumo. a) Vista em perspectiva; b) Principais módulos.
Fonte: (POLOLU, 2014).
Cada esteira do robô Zumo é movimentada por uma roda dentada acoplada a um motor,
possibilitando assim a tração diferencial, o que possibilita a execução de curvas com grandes
raios, ou giros em seu próprio eixo, dentre outras manobras. Com o uso de motores com caixa
de redução 75:1 HP para se movimentar, pode atingir uma velocidade máxima de 60 cm/s
(POLOLU, 2014).
Esta plataforma robótica é composta por um shield para Arduino Uno Figura 4 (b), composta
por um controlador duplo de motores, um acelerômetro com bússola triaxial, um buzzer
(campainha) para emitir sons e um botão para o usuário, através do qual o usuário pode enviar
comandos ao robô utilizado, por exemplo, para comando de start ao robô. Como hardware de
controle, utiliza-se um Arduino Uno, ou outra plataforma compatível.
O acelerômetro e a bússola podem ser utilizados no posicionamento do robô, bem como na
detecção de impactos. Apesar de sofrer interferências dos motores, das pilhas e do PCI – (Placa
16
de Circuito Impresso) a bússola, se bem calibrada, pode ser utilizada como indicador de direção
em diversos ambientes (POLOLU, 2014).
As demarcações são identificadas como trajetos com o auxílio de um sistema de sensoriamento
infravermelho, contendo oito pares emissor-receptor de IR (Infra Red – Infravermelho), Figura
5 (POLOLU, 2014). Quando o sensor passa pela superfície que representa o caminho a ser
percorrido, linha preta, há o bloqueio do recebimento do reflexo do infravermelho por parte do
receptor. A sequência de sensores bloqueados será avaliada pela lógica do sistema,
possibilitando a identificação dos vários tipos de bifurcação considerados neste trabalho.
Figura 5 - Barra de sensores IR.
Fonte: (POLOLU, 2014).
Além dos sensores utilizados na identificação do caminho, dois encoders são utilizados para
medir a distância percorrida pelo robô no interior do labirinto. Cada encoder, Figura 6, é
colocado em uma esteira do robô, a fim de coletar os pulsos produzidos pela roda dentada que
causa a tração das esteiras no robô.
17
Figura 6 - Modelo de encoder utilizado.
Fonte: (POLOLU, 2014)
Para processar as informações oriundas dos sensores IR, encoders e executar todas as rotinas
desenvolvidas para o algoritmo, foi utilizada a plataforma de desenvolvimento Arduino Mega
2560, Figura 7.
Figura 7 - Plataforma de desenvolvimento Arduino Mega 2560.
Fonte: (ARDUINO, 2014).
As principais características do Arduino Mega são:

Microcontrolador ATmega 2560 – 8 bits;

Velocidade de Clock: 16 MHz;

Tensão de operação: 5V;

Tensões de entrada recomendadas: 7 – 12V;
18

54 entradas e saídas digitais, das quais 15 podem fornecer sinais de PWM (Pulse Width
Modulation – Modulação por Largura de Pulso) como saída.

16 Entradas analógicas;

256KB de memória Flash;

8KB de memória SRAM;

4KB de memória EEPROM.
19
3 MÉTODOS
O processo de resolução de labirintos, com uso de robôs móveis, exige o cumprimento de, pelo
menos, duas etapas, exploração e navegação. A primeira exige que o robô passe pelo labirinto
realizando a busca dos caminhos que o levem à saída do mesmo, nesta etapa o robô pode, ou
não, passar por todos os caminhos possíveis, durante o desenvolvimento desta etapa o robô
precisa obter e armazenar informações que o permita identificar de fato qual é o caminho que
o levará à saída do labirinto. Na segunda etapa, o robô irá executar a navegação, com base nas
informações coletadas durante a etapa de exploração.
No algoritmo desenvolvido neste trabalho, o robô parte de um ponto de origem em busca de
um ponto de destino, sem nenhuma informação a respeito do tamanho do labirinto (ambiente).
Quando o ponto de destino é encontrado, o robô volta para o ponto de origem fazendo o menor
caminho possível daqueles pelo qual ele passou na etapa de mapeamento. Então, para confirmar
que ele aprendeu o caminho, ele o executa novamente, até chegar ao ponto de destino, onde
ficará parado.
As únicas informações do labirinto que são de conhecimento prévio do robô são os tipos de
bifurcações que ele poderá encontrar durante sua exploração. A Figura 8 apresenta os tipos de
bifurcações que podem ser encontradas pelo robô, e a forma geométrica utilizada para
identificar o início e fim do labirinto. Estas bifurcações são baseadas nos modelos formulados
pelo IEEE para suas competições (IEEE, 2013), que levam em consideração apenas bifurcações
perpendiculares.
Figura 8 - Bifurcações possíveis de serem encontradas nos labirintos.
20
A diferenciação entre as bifurcações apresentadas na Figura 8, é feita pelo algoritmo de acordo
com as informações provenientes da barra de sensores IR, e é feita em dois estágios. No
primeiro estágio, durante o seguimento da linha, o robô percebe que há uma bifurcação
conforme ilustram as Figuras 8a e 8c, então ele seguirá em linha reta por uma distância que
equivale à de metade do comprimento do robô, medida por meio dos encoders. Neste ponto
será feita uma nova leitura, como ilustram as Figuras 8b e 8d, neste ponto o algoritmo será
capaz de identificar o tipo de bifurcação, considerando as duas leituras.
Figura 9 - Percepção e diferenciação de bifurcações. a) e c) Momento em que uma bifurcação é percebida; b)
Leitura que caracteriza a bifurcação percebida em a); d) Leitura que caracteriza a bifurcação percebida em c).
3.1 ETAPA DE EXPLORAÇÃO
Sempre que o robô iniciar a exploração do labirinto, ele deverá reconhecer e diferenciar cada
uma das bifurcações mostradas na Figura 8, e será função do algoritmo implementado decidir
por qual direção seguir. Este algoritmo foi desenvolvido de forma que possa ser escolhida a
“preferência” do robô no momento da gravação do programa no microcontrolador.
21
Esta “preferência” será considerada sempre que ele chegar a uma das bifurcações apresentadas
na Figura 8 (e) ou Figura 8 (f). Outra característica é que, sempre que o robô chegar a uma nova
bifurcação seja ela (b), (c), (d) ou (g) da Figura 8, o algoritmo levará sempre o robô a seguir
pela direção da bifurcação, e não a passar direto. Como mostra a Figura 10, na qual demonstrase o comportamento do robô para o caso da Figura 8 (c).
Figura 10 - Comportamento do robô para uma dada situação. a) Robô se aproximando da bifurcação; b)
Identifica-se o sentido e segue por ele; c) Não há caminhos para prosseguir nesta direção; d) Segue por um novo
caminho.
A partir no início da exploração, sempre que o robô chegar a uma nova bifurcação, esta será
inserida em uma lista de bifurcações. Para o algoritmo, cada bifurcação é tratada como uma
estrutura de dados denominada Crossing, composta pelos seguintes parâmetros:

indice – Representa a posição de uma dada bifurcação na lista de bifurcações;

dir_relativa – Representa a direção para a qual o robô virou em uma dada bifurcação, o
valor menos um (-1) representa que o giro do robô foi para a esquerda, mais um (+1)
representa que o giro do robô foi para a direita. Trata-se de direção relativa, uma vez
que com ela não é possível identificar qual a orientação do robô perante o labirinto.

dir_abs – Representa a direção absoluta do robô em relação ao labirinto. Assume-se que
a direção de partida do robô seja o norte, durante a navegação, este parâmetro indicará
qual a direção do robô em relação ao labirinto em uma dada bifurcação. As direções
absolutas são codificadas da seguinte forma, Norte = 1, Leste = 2, Sul = 3 e Oeste = 4.
22
O algoritmo computa uma nova direção absoluta de acordo com a direção absoluta
anterior e a direção relativa do giro efetuado para uma dada bifurcação;

dir_abs_chegada – Sempre que o robô chegar a uma bifurcação, sendo ela uma nova
bifurcação ou não, a direção absoluta atual do robô será armazenada neste parâmetro;

custo – Representa o tamanho do percurso feito pelo robô durante o percurso pelo
labirinto, é medido em número de pulsos do encoder. Neste valor não são computados
os pulsos referentes aos giros feitos pelo robô sempre que precisam mudar de direção,
uma vez que sua posição não muda;

possibilidades – Para cada tipo de bifurcação existe um número de possibilidades de
trajetos a serem seguidos, este parâmetro armazena quantos possíveis trajetos ainda
restam para uma dada bifurcação;

coordenadaX – Este valor representa a quantos pulsos na direção “x” (horizontal), uma
dada bifurcação está do ponto de início do labirinto;

coordenadaY – Assim como o parâmetro anterior, este valor armazena o número de
pulsos na direção “y” (vertical), na qual dada bifurcação se encontra em relação ao ponto
de início do labirinto.
Na Figura 11 é apresentada a estrutura crossing definida em linguagem “C”.
Figura 11 - Código em "C" que define a estrutura Crossing.
23
As listas encadeadas são estruturas flexíveis, pois, para cada elemento inserido na estrutura, um
espaço é alocado para o seu armazenamento. Dessa forma, o espaço de memória utilizado será
sempre compatível com a quantidade de elementos armazenados (VILLAS, FERREIRA, et al.,
1993).
A estrutura da lista é composta por uma sequência encadeada de elementos, geralmente
denominados nós da lista. Os nós de uma lista simplesmente encadeada possuem dois campos:
o dado armazenado e o ponteiro para o próximo elemento da lista, também conhecido como
“link”, visto que é responsável por manter a ligação entre os nós (VILLAS, FERREIRA, et al.,
1993).
Diferentemente de um vetor, não se pode acessar diretamente os elementos da lista encadeada,
pois eles não ocupam um espaço de memória contínuo. Portanto, é necessário percorrer todos
os elementos da lista, sendo que o deslocamento se faz em apenas um sentido: do primeiro para
o último elemento. O último elemento armazena um ponteiro inválido (NULL), sinalizando que
não há um próximo elemento (RANGEL NETO, CERQUEIRA e CELES FILHO, 2004).
As novas bifurcações são sempre inseridas na “cabeça” da lista. Deste modo, a última
bifurcação pela qual o robô passou, sempre será a primeira da lista, Figura 12. Quando uma
bifurcação não é nova, o robô já passou por ela, a estrutura crossing que a representa é
atualizada com os novos dados.
Figura 12 - Inserção de uma nova bifurcação à lista.
À medida que a exploração ocorre, sempre que todas as possibilidades de uma dada bifurcação
são extintas e, percebe-se que a mesma não contribui para a solução do labirinto, esta bifurcação
24
é removida da lista, de forma que apenas as bifurcações que levam o robô ao fim do labirinto,
são mantidas na lista.
Para o caso da Figura 13, o robô inicia sua navegação pela origem, situada à esquerda, ao chegar
à bifurcação 2 ele irá seguir para a direção absoluta “sul” até a bifurcação 3. Nenhuma das
direções presentes na bifurcação 3 levam o robô ao destino, então ele retornará para a bifurcação
2. Como foram exploradas todas as possibilidades para a bifurcação 3, e o robô voltou à
bifurcação 2 através do mesmo trajeto pelo qual a deixou, o algoritmo irá eliminar a bifurcação
3 de sua lista de bifurcações. Este mesmo processo é válido para seguimentos de caminhos,
como os da bifurcação 4. Se por ventura o robô, na etapa de exploração, seguir por um dos dois
seguimentos que não o levam ao destino, o seguimento em questão será eliminado da lista.
Figura 13 - Etapa de exploração. Eliminando bifurcações que não contribuem para a resolução.
Pode-se representar o fluxo do processamento executado na etapa de exploração do labirinto,
em forma de fluxograma, e este é apresentado na Figura 14.
25
Figura 14 - Fluxograma que representa o funcionamento do algoritmo na fase de exploração.
Ao iniciar a etapa de exploração o robô considera sua orientação atual como sendo a direção
“norte” do seu sistema de coordenadas, daí por diante, sempre que ele se deparar com uma
bifurcação, ele deverá armazenar sua orientação atual no parâmetro dir_abs_chegada da
estrutura crossing que representa tal bifurcação. Em seguida, é efetuado o giro para a direção
pretendida, para então calcular e armazenar a nova direção absoluta no parâmetro dir_abs, da
mesma estrutura crossing.
26
3.2 ETAPAS “VOLTA À ORIGEM” E NAVEGAÇÃO DO LABIRINTO
Para efetuar a etapa de “volta à origem” o algoritmo se baseia no parâmetro dir_abs_chegada.
A partir do ponto de destino, o robô iniciará por seguir a linha. Ao chegar à primeira bifurcação,
que foi a última informação acrescentada à lista na etapa de exploração, o algoritmo irá verificar
no campo dir_abs_chegada por onde o robô chegou a esta bifurcação durante a exploração,
com esta informação é possível computar para qual direção absoluta o robô deverá seguir para
encontrar a próxima bifurcação, será efetuado o giro para tal direção, e será observada a posição
imediatamente anterior na lista de bifurcações. Este procedimento será repetido até que todos
os nós da lista tenham sido explorados, e consequentemente, tenha-se chegado ao ponto de
origem do labirinto.
Para a etapa de navegação, são levadas em conta as informações presentes no parâmetro
dir_abs, presente em cada uma das estruturas crossing que levam o robô da origem ao destino.
Da mesma forma como é feita a volta do robô para a origem, na etapa de navegação, sempre
que chegar a uma bifurcação o robô irá seguir para a direção da próxima bifurcação que o leva
ao destino, sem levar em consideração outras direções.
A Figura 15 mostra a composição da lista de bifurcações implementada. Estes resultados foram
obtidos a partir da exploração do robô feita no labirinto apresentado, na qual foi escolhida como
preferência seguir à esquerda. A direção de partida do robô é tomada como Norte (dir_abs = 1),
a partir do giro efetuado para a direita (dir_relativa = 1) na bifurcação de número um, o robô
passa então a navegar para a direção absoluta Leste (dir_abs = 2), e a direção Norte, para a qual
o robô estava indo ao chegar a esta bifurcação é armazenada, (dir_abs_chegada = 1).
27
Figura 15 – Exemplo de composição da lista de bifurcações.
Ao chegar à bifurcação dois, como é caracterizado pelo algoritmo, o robô opta por seguir para
a direita (dir_relativa = 1), o que resulta em uma modificação na sua direção absoluta (dir_abs
= 3) e na sua direção absoluta de chegada (dir_abs_chegada = 2). Mas, este caminho não leva
o robô ao seu destino, e o mesmo acaba por voltar à bifurcação de número 2, bifurcação esta já
conhecida por ele, nesta etapa o robô irá girar para a direita (dir_relativa = 1) por ser uma
possibilidade desta bifurcação ainda não explorada pelo mesmo, modificando assim sua direção
absoluta (dir_abs = 2).
Na bifurcação três ocorre o mesmo. A terceira bifurcação a ser adicionada à lista terá novamente
o parâmetro dir_abs_chegada = 2 e, devido a sua preferência por seguir pela esquerda, o robô
seguirá para a direção Norte (dir_abs = 1) a partir de um giro efetuado para a esquerda
(dir_relativa = -1), caminho este que também não o leva ao destino e ele então retornará à
bifurcação de número três. Desta vez, o giro também será efetuado para a esquerda (dir_relativa
= -1) e a direção absoluta voltará a ser Leste (dir_abs = 2).
28
Chegando à bifurcação de número quatro, a estrutura crossing a ser adicionada à lista terá como
parâmetro dir_abs_chegada = 2, o robô efetuará um giro para a direita (dir_relativa = 1) que é
a única possibilidade que existe, passando o robô a seguir para o Sul (dir_abs = 3). A próxima
bifurcação encontrada pelo robô é seu destino, dando fim assim à etapa de exploração.
Os parâmetros dir_abs_chegada de todas as estruturas adicionadas à lista precisam ser
modificadas de forma que elas passem a representar direções absolutas para as quais o robô
deverá seguir na etapa de retorno à origem. Desta forma, os parâmetros dir_abs_chegada para
as estruturas 1, 2, 3 e 4, armazenadas na lista, passaram a ser iguais a 3, 4, 4 e 4, respectivamente.
29
4 TESTES E RESULTADOS
Os testes foram divididos em cinco etapas durante a fase de desenvolvimento do projeto. Foi
realizado o teste do algoritmo para quatro configurações em dois labirintos distintos, sendo um
destes o mesmo utilizado durante o desenvolvimento do algoritmo, e o segundo labirinto com
o propósito de validar o algoritmo.
4.1 TESTES DE DESENVOLVIMENTO
A primeira etapa de testes teve como objetivo o conhecimento do robô e de seus sensores IR e
encoders, bem como estudar sua biblioteca de programação para a plataforma Arduino,
disponível em GITHUB, 2012. Para estes testes fez-se um traçado fechado (Figura 16),
demarcado em uma superfície de locomoção branca, para que o robô executasse a tarefa de
“seguir a linha”. Ele passa por cima da linha, de forma a percorrer o trajeto repetidas vezes.
Figura 16 - Exemplo de trajeto oval, utilizado na primeira etapa de testes.
Esta etapa foi importante para o aprendizado sobre como lidar com as particularidades da
biblioteca de programação. Uma vez conhecidas as funções da biblioteca de programação do
30
robô, e as particularidades de seus movimentos, pode-se passar aos testes do método para
identificação das bifurcações, descritos na seção 3. Nesta etapa pôde-se verificar a efetividade
do sistema de sensores IR, na identificação das bifurcações apresentadas na Figura 8. Para se
ter um feedback (realimentação) sobre as percepções do robô, utilizou-se um módulo de
comunicação bluetooth HC-06 (GUANGZHOU HC INFORMATION TECHNOLOGY CO.,
LTD, 2011), conectado a uma das portas de comunicação serial do Arduino Mega, e que
transmitia os dados para um PC (Personal Computer – Computador Pessoal).
Figura 17 - Módulo Bluetooth HC-06
Fonte: (GUANGZHOU HC INFORMATION TECHNOLOGY CO., LTD, 2011).
A partir de uma codificação feita para cada uma das bifurcações da Figura 8, durante a segunda
etapa de testes, sempre que o robô encontrava uma bifurcação, o mesmo ficava parado e enviava
uma mensagem para o PC, com isso tornou-se possível validar o reconhecimento das formas
das bifurcações.
Uma vez conhecidas as possibilidades de movimento do robô e sendo o algoritmo capaz de
identificar toda a lista de bifurcações, apresentada na Figura 8, partiu-se para o desenvolvimento
do algoritmo de exploração, responsável por percorrer o labirinto à procura de um caminho que
levasse o robô da origem ao destino. Durante o desenvolvimento do algoritmo foi necessário
executar uma terceira etapa de testes, para tanto, utilizou-se o labirinto mostrado na Figura 12.
Nesta etapa, a cada nova bifurcação uma mensagem era enviada ao PC, na qual era informado
se o robô já havia passado naquela bifurcação, ou não, e qual o índice daquela bifurcação na
lista de bifurcações.
31
Sempre que o robô identificava um trecho que não contribuía para sua chegada ao destino, todas
as bifurcações deste eram removidas da lista e uma mensagem de remoção era enviada ao PC.
Ao chegar ao ponto de destino, o robô enviava para o PC uma mensagem contendo a lista de
bifurcações responsável por levá-lo da origem ao destino. Para o labirinto de testes, Figura 12,
a mensagem representava as bifurcações 1, 2, 4 e 5. Logo, a comunicação bluetooth entre o
robô e o PC, tornou possível identificar se o trajeto que levou o robô da origem ao destino havia
sido realmente o menor trajeto possível dentre os caminhos por onde o robô passou.
Em seguida passou-se aos testes do algoritmo de resolução, quarta etapa de testes, utilizando
ainda o labirinto da Figura 12. Nesta etapa, o robô foi posto no ponto de origem do labirinto e
à medida que encontrava uma bifurcação ele enviava uma mensagem ao PC com as informações
da bifurcação atual, e da bifurcação seguinte, até encontrar o ponto de destino. Este sistema
possibilitou validar as informações que o robô havia coletado do labirinto.
A quinta etapa de testes foi utilizada para validar o uso dos encoders como forma de localização
do robô no interior do labirinto. O robô foi posto a percorrer o mesmo labirinto, mostrado na
Figura 18, por diversas vezes, e a cada nova bifurcação uma mensagem era enviada ao PC com
o valor das variáveis coordenadaX e coordenadaY para aquela bifurcação, tendo como
referencial o sistema de coordenadas na configuração mostrada na Figura 18.
Figura 18 - Labirinto de teste do sistema de coordenadas.
A Tabela 1 demonstra os resultados para 10 voltas dadas pelo robô no labirinto da Figura 18.
Em todos os casos o robô partiu do ponto de origem e foi até o ponto de destino, de onde ele
32
era removido manualmente e posto no ponto de partida novamente, para repetição do
experimento.
Tabela 1 - Testes de localização do robô utilizando encoders.
Coordenadas (x,y)
Bifurcação
Teste 1
Teste 2
Teste 3
Teste 4
Teste 5
Teste 6
Teste 7
Teste 8
Teste 9
Teste 10
1
(0, 10)
(0, 10)
(0, 9)
(0, 8)
(0, 10)
(1, 10)
(0, 10)
(0, 10)
(0, 10)
(0, 11)
2
(10, 10)
(9, 10)
(10, 10)
(11, 9)
(10, 10)
(8, 10)
(12, 10)
(10, 10)
(10, 10)
(10, 12)
3
(10, -4)
(10, -4)
(11, -3)
(10, -4)
(10, -4)
(10, -4)
(9, -3)
(10, -4)
(10, -4)
(9, -4)
4
(20, 10)
(19, 9)
(20, 11)
(20, 10)
(20, 10)
(21, 10)
(21, 11)
(20, 9)
(20, 10)
(19, 12)
5
(30, 10)
(30, 10)
(31, 10)
(28, 11)
(30, 10)
(30, 10)
(32, 10)
(30, 9)
(30, 10)
(31, 11)
6
(30, 0)
(30, -1)
(30, -2)
(30, 01)
(30, 0)
(30, 0)
(30, -2)
(29, 1)
(30, 0)
(32, 0)
4.2 TESTES E VALIDAÇÃO DO ALGORITMO
Os testes do algoritmo por completo foram executados no mesmo labirinto no qual foram feitos
os testes em etapas, a fim de garantir o seu funcionamento. Logo após partiu-se para um
labirinto com um maior número de bifurcações, como forma de validação do algoritmo. As
configurações dos testes foram:
a) Ponto de partida localizado à esquerda, com preferência definida para a esquerda;
b) Ponto de partida localizado à esquerda, com preferência definida para a direita;
c) Ponto de partida localizado à direita, com preferência definida para a esquerda;
d) Ponto de partida localizado à direita, com preferência definida para a direita.
O resultado para as configurações de “a” a “d” são mostradas na Figura 19 de “a” a “d”
respectivamente.
Figura 19 – Trajetos resultantes do labirinto de testes.
33
Após as etapas anteriores, para validar o trabalho, foi utilizado um labirinto mais complexo
conforme apresenta a Figura 20. O caminho percorrido na etapa de volta à origem e de
navegação, foram executadas sempre sem a passagem por caminhos desnecessários.
34
Figura 20 - Trajetos resultantes do labirinto de validação.
O mapeamento das bifurcações do labirinto, de acordo com o sistema de coordenadas X e Y,
feito pelos encoders, foi desenvolvido como pontapé inicial para a resolução do problema de
labirintos circulares, exposto na Figura 21. No labirinto apresentado existem dois caminhos que
levam o robô a uma mesma bifurcação (bifurcação 1), este problema não foi tratado no
algoritmo desenvolvido, mas o mapeamento das coordenadas cartesianas poderá ser utilizado
em trabalhos futuros que busquem resolvê-lo.
35
Figura 21 - Problema referente a labirintos circulares.
Para o labirinto exibido na Figura 21, o problema ficará evidente somente se o robô tiver como
origem o lado esquerdo do labirinto, e estiver com sua preferência configurada para a esquerda.
36
5 CONCLUSÃO
A busca pelo desenvolvimento de sistemas autônomos continua presente nas pesquisas
contemporâneas. Apesar dos mais de 30 anos dedicados à pesquisas sobre o tema, ainda são
fortes os impactos que tecnologias capazes de interagir com o mundo real, sem a necessidade
de intervenções humanas, ocasionam na sociedade. A robótica móvel assistiva é um exemplo
valioso de como a autonomia dada a robôs móveis podem influenciar no bem estar de pessoas
com algum tipo de deficiência.
O avanço no desenvolvimento de sensores cada vez mais precisos e processadores cada vez
mais velozes, com uma grande variedade de periféricos e recursos que possibilitam até o
processamento de imagens, permitem ao desenvolvedor que este se dedique exclusivamente ao
desenvolvimento da “inteligência” do sistema.
A barra de sensores IR utilizada na identificação das demarcações da superfície de locomoção,
possibilitou de fato uma boa estruturação do ambiente, uma vez que a lógica implementada no
algoritmo para percepção dos trajetos, garantiu que o robô se locomovesse sempre dentro dos
caminhos demarcados.
O uso de lista simplesmente encadeada mostrou-se uma solução factível para o uso na resolução
de labirintos, uma vez que o caminho a ser seguido segue uma ordem (sequência) pode-se
implementar o armazenamento das estruturas crossing de forma ordenada sem a necessidade de
se preocupar com a alocação de memória, etapa abstraída pela implementação das funções
referentes à lista encadeada.
A criação da estrutura de dados chamada crossing, possibilitou o armazenamento de todos os
dados/informações necessários para a exploração e solução do labirinto, e permitiu também a
implementação de variáveis que podem ser utilizadas para o aperfeiçoamento futuro do
algoritmos, como é o caso dos parâmetros “custo”, “coordenadasX” e “coordenadasY”
O sistema de sensoriamento utilizado, barra de IR, apresenta-se como uma excelente forma de
percepção de caminhos, uma vez que não apresentam interferência entre sensores, independem
da luminosidade do ambiente e são de baixo custo.
O sistema de coordenadas cartesianas desenvolvido para se obter a localização do robô durante
a etapa de mapeamento, pode ser utilizada em trabalhos futuros para extrair novas informações
a respeito do labirinto como, por exemplo, calcular o caminho que contém o menor
comprimento e, na etapa de resolução do labirinto, fazer com que o robô siga por este caminho.
37
Também com base nas coordenadas cartesianas, já é possível que o robô identifique, na etapa
de exploração, bifurcações pelas quais ele já passou, uma vez que o robô chegará a uma
bifurcação com mesma coordenada que uma pela qual ele já navegou. Com esta informação
pode-se, em trabalhos futuros, agregar rotinas a este algoritmo para que o robô tome decisões
sobre quais providencias tomar neste caso.
38
REFERÊNCIAS
ARDUINO. Arduino - ArduinoBoardMega2560, 2014. Disponivel em:
<http://arduino.cc/en/Main/ArduinoBoardMega2560>. Acesso em: 08 out. 2014.
BRAUNL, T. Research relevance of mobile robot competitions. IEEE Robotics &
Automation Magazine, v. VI, n. 4, p. 32-37, 1999.
CAI, J. et al. An algorithm of micromouse maze solving. Computer and Information
Technology (CIT), 2010 IEEE 10th International Conference on, 2010. 1995-2000.
CHANG, H. J. et al. Simultaneous localization and mapping with environmental-structure
prediction. Robotics, IEEE Transactions on, v. 23, n. 2, p. 281-293, 2007.
GITHUB. Zumo Shield. GitHub, 2012. Disponivel em: <https://github.com/pololu/zumoshield>. Acesso em: 13 set. 2014.
GUANGZHOU HC INFORMATION TECHNOLOGY CO., LTD. Product Data Sheet HC-06. Revisão 2.0. ed. [S.l.]: [s.n.], 2011.
HUALONG, J.; YONGHONG, T.; HONGQI, W. Design and Realization of a Maze Robot.
Consumer Electronics, Communications and Networks (CECNet), 2011 International
Conference on. IEEE, 2011. 201-204.
IEEE. Micromouse Competition Rules. 2013 IEEE Region1 Student Conference,
Cambridge, 5 - 6 Abril 2013.
MISHRA, S.; BANDE, P. Maze Solving Algorithms for Micro Mouse. IEEE International
Conference on Signal Image Technology and Internet Based Systems, 2008. 86 - 96.
MOREIRA, A.; VASCONCELOS, F.; FILHO, J. Uso de Robótica Assistiva no Auxílio de
Pessoas com Deficiências Visuais. V CONNEPI-2010, 2010.
PASSOLD, F. Despertando para a Importância das Competições de Robôs. Anais: do
XXXIV Congresso Brasileiro de Educação em Engenharia, Passo Fundo, 2006.
POLOLU. Pololu - Encoder for Pololu Wheel 42x19mm, 2014. Disponivel em:
<https://www.pololu.com/product/1217>. Acesso em: 30 ago. 2014.
POLOLU. Zumo Robot for Arduino. Pololu Robotics & Eletronics, 2014. Disponivel em:
<https://www.pololu.com/product/2506>. Acesso em: 16 out. 2014.
POLOLU, R. Pololu - QTR-8RC Reflectance Sensor Array. Pololu Robotics & Eletronics,
2014. Disponivel em: <https://www.pololu.com/product/961>. Acesso em: 10 ago. 2014.
RANGEL NETO, J. L. M.; CERQUEIRA, R. F. D. G.; CELES FILHO, W. Introdução a
estrutura de dados: com técnicas de programação em C. 3ª. ed. [S.l.]: Campus, 2004.
VILLAS, M. V. et al. Estrutura de Dados: Conceitos e Técnicas de Implementação. 9ª. ed.
[S.l.]: [s.n.], 1993.
39
APÊNDICE A - Algoritmo desenvolvido
#include <ZumoMotors.h>
#include <Pushbutton.h>
#include "MyQueueList.h"
#include <math.h>
#define interrupt 2
#define passa 5
#define primeiro_pino_ir 33
#define mtrapido 270
#define rapido 170
#define lento 120
#define mtlento 80
#define ultrapass 13
#define possib_cruz 2
#define possib_te 1
#define possib_IK 1
#define possib_KI 1
#define direita 1
#define esquerda -1
#define algoritmo 1
struct crossing {
int indice;
int dir_relativa;
int dir_abs;
int dir_abs_chegada;
int custo;
int possibilidades;
};
int ir[8];
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON);
volatile int pwm_e = 0;
volatile int pwm_d = 0;
int led = 13;
int flag = 0;
int n_flag = 0;
int cont_passa = 0;
int cont_bif = 1;
int dir_abs_ant = 1;
unsigned long num_pulsos = 0;
int habilita_passa;
long coordenadas_xy[2] = {0, 0};
int val_incremento[2] = {0, 1};
const int mat_transf[2][2] = {{0, 1}, {-1, 0}};
QueueList <crossing> list_bif;
QueueList <crossing> list_bif_volta;
QueueList <crossing> list_bif_solve;
void myMain();
void inicializaSensor();
40
byte lerSensores();
void navegaLab(byte ir_array);
int identificaBifurcacao();
int explora(int n_flag);
void praondeVou();
void setaDirabs (int sentido, int reto);
void gira(int sentido, int reto);
void addList(int dir_rel_bif, int possib);
void calculaIncremento(int sent);
void trataListaVolta(crossing noh);
void meiaVolta();
int retornaMase(int bifurca);
void trataListaSolve();
int solveMase(int n_flag);
void piscaLed(int qnt, int vezes_time);
void setup() {
Serial.begin(9600);
Serial1.begin(9600);
attachInterrupt(interrupt, pulsosEncoder, RISING);
pinMode(led,OUTPUT);
digitalWrite(led,LOW);
inicializaSensor();
myMain();
}
void myMain() {
byte ir_array;
int bifurca, end_mase = 0;
crossing noh;
int teste_serial;
piscaLed(8, 3);
while(end_mase == 0){
cont_passa = num_pulsos + passa;
while (num_pulsos <= cont_passa){
ir_array = lerSensores();
navegaLab(ir_array);
}
habilita_passa = 0;
bifurca = identificaBifurcacao();
end_mase = explora(bifurca);
flag = 0;
}
delay(2000);
41
trataListaSolve();
end_mase = 0;
meiaVolta();
}
habilita_passa = 0;
end_mase = retornaMase(bifurca);
flag = 0;
}
delay(2000);
end_mase = 0;
meiaVolta();
}
habilita_passa = 0;
end_mase = solveMase(bifurca);
flag = 0;
}
piscaLed(10,3);
}
void loop(){
}
void inicializaSensor()
{
int i;
for(i=0;i<8;i++)
{
ir[i] = primeiro_pino_ir + (i*2);
pinMode(ir[i], INPUT);
}
42
}
byte lerSensores()
{
int i, ir_val, ir_array = 0;
for(i=7;i>=0;i--)
{
ir_val = digitalRead(ir[i]);
ir_val = ir_val << i;
ir_array += ir_val;
delay(5);
}
return ir_array;
}
void navegaLab(byte ir_array)
{
byte sens = ir_array & B10111101;
if((sens == 1)||(sens == 5)||(sens == 9)||(sens == 13)||(sens == 17)||(sens == 21)||(sens == 25)||(sens ==
29)||(sens == 33)||(sens == 37)||(sens == 41)||(sens == 45)||(sens == 49)||(sens == 53)||(sens == 57)||(sens ==
61))
{
if(flag != 3)
flag = 1;
}
else
if((sens == 4)||(sens == 8)||(sens == 12))
{
pwm_e = rapido;
pwm_d = mtlento;
}
else
if(sens == 24)
{
pwm_e = lento;
pwm_d = lento;
}
else
if((sens == 16)||(sens == 32)||(sens == 48))
{
pwm_e = mtlento;
pwm_d = rapido;
}
else
if((sens == 128)||(sens == 132)||(sens == 136)||(sens == 140)||(sens == 144)||(sens == 148)||(sens ==
152)||(sens == 156)||(sens == 160)||(sens == 164)||(sens == 168)||(sens == 172)||(sens == 176)||(sens ==
180)||(sens == 184)||(sens == 188))
{
if(flag != 3)
flag = 2;
}
else
if((sens == 189))
{
flag = 3;
}
else
if(sens == 0)
43
{
if(flag == 0)
flag = 4;
}
if(flag == 0)
{
motors.setSpeeds(pwm_e, pwm_d);
}
else
{
habilita_passa = 1;
motors.setSpeeds(lento, lento);
}
}
int identificaBifurcacao(){
int n_flag = 0;
if((flag == 3) && (lerSensores() == 0))
n_flag = 1;
else
if((flag == 3) && (lerSensores() == B11111111))
n_flag = 2;
else
if((flag == 1) && (lerSensores() != 0))
n_flag = 3;
else
if((flag == 2) && (lerSensores() != 0))
n_flag = 4;
else
if(flag == 1)
n_flag = 5;
else
if (flag == 2)
n_flag = 6;
else
if (flag == 3)
n_flag = 7;
else
if(flag == 4)
n_flag = 8;
return n_flag;
}
int explora (int bifurca){
int fim_lab = 0;
int dir_giro, reto = 1, n_possibilidades, chegada;
crossing noh;
chegada = dir_abs_ant;
switch (bifurca){
case 1:
if(cont_bif != list_bif.count()){
dir_giro = algoritmo;
setaDirabs(dir_giro, reto);
44
addList(dir_giro,possib_te,chegada);
}
else{
noh = list_bif.pop();
dir_giro = noh.dir_relativa;
if(noh.possibilidades > 0){
n_possibilidades = noh.possibilidades - 1;
addList(dir_giro, n_possibilidades, noh.dir_abs_chegada);
}
}
cont_bif++;
gira(dir_giro, 1);
break;
case 2:
fim_lab = 1;
gira(0,1);
break;
case 3:
dir_giro = direita;
addList(dir_giro,possib_IK,chegada);
reto = 1;
}
else{
reto = 2;
}
else{
cont_bif--;
cont_bif--;
}
}
cont_bif++;
gira(dir_giro, reto);
break;
case 4:
dir_giro = esquerda;
addList(dir_giro,possib_KI,chegada);
reto = 1;
}
else{
45
reto = 2;
}
else{
cont_bif--;
cont_bif--;
}
}
cont_bif++;
break;
case 5:
dir_giro = 1;
gira(dir_giro, 1);
setaDirabs (dir_giro, reto);
break;
case 6:
dir_giro = -1;
gira(dir_giro, 1);
break;
case 7:
addList(dir_giro,possib_cruz,chegada);
}
else{
}
else
cont_bif--;
}
cont_bif++;
gira(dir_giro, 1);
break;
case 8:
gira(esquerda, 1);
setaDirabs(esquerda, reto);
setaDirabs(esquerda, reto);
cont_bif--;
break;
}
46
return fim_lab;
}
void setaDirabs (int sentido, int reto){
int nova_dir_abs;
if(reto != 2){
calculaIncremento(sentido);
switch (sentido){
case (-1):
nova_dir_abs = dir_abs_ant + 3;
break;
case (1):
nova_dir_abs = dir_abs_ant + 1;
break;
}
((nova_dir_abs > 4) ? (dir_abs_ant = (nova_dir_abs - 4)) : (dir_abs_ant = nova_dir_abs));
}
}
void gira(int sentido, int reto){
byte sens_sinal_e = B10000000;
byte sens_sinal_d = B00000001;
byte sens_centro = B00010000;
int pwm_e, pwm_d;
int val_incremento_aux[2] = {val_incremento[0], val_incremento[1]};
val_incremento[0] = 0; val_incremento[1] = 0;
pwm_e = pow(sentido,reto);
pwm_d = pow(-sentido,reto);
motors.setSpeeds((pwm_e * lento), (pwm_d * lento));
while (!((sens_sinal_e & lerSensores()) || (sens_sinal_d & lerSensores()) || (reto == 2)));
while (!((sens_centro & lerSensores()) || (reto == 2)));
val_incremento[0] = val_incremento_aux[0]; val_incremento[1] = val_incremento_aux[1];
}
void addList(int dir_rel_bif, int possib, int dir_chegada_abs){
int custo_lab = 0;
int index = list_bif.count()+1;
crossing noh;
noh = (crossing){index, dir_rel_bif, dir_abs_ant, dir_chegada_abs, custo_lab, possib};
list_bif.myPush(noh);
}
void praondeVou(){
}
void piscaLed(int qnt, int vezes_time){
for(int x = 0; x < 2*qnt; x++){
digitalWrite(led, !digitalRead(led));
47
for(int j = 0; j < vezes_time; j++)
delay(100);
}
}
void pulsosEncoder(){
num_pulsos++;
num_pulsos *= habilita_passa;
coordenadas_xy[0] = coordenadas_xy[0] + 1 * val_incremento[0];
coordenadas_xy[1] = coordenadas_xy[1] + 1 * val_incremento[1];
}
void trataListaVolta(crossing noh){
int dir_abs_volta;
dir_abs_volta = noh.dir_abs_chegada;
if(dir_abs_volta > 2)
dir_abs_volta = dir_abs_volta - 2;
else
dir_abs_volta = dir_abs_volta + 2;
noh.dir_abs_chegada = dir_abs_volta;
list_bif_volta.push(noh);
}
void trataListaSolve(){
crossing noh;
while(list_bif.count() > 0){
trataListaVolta(noh);
list_bif_solve.myPush(noh);
}
}
int solveMase(int bifurca){
crossing noh;
int dir_giro = 1, fim_lab = 0, dif_dir, reto = 1;
switch (bifurca){
case 2:
fim_lab = 1;
dir_giro = 0;
break;
case 5:
dir_giro = 1;
reto = 1;
break;
case 6:
dir_giro = -1;
reto = 1;
break;
48
default:
noh = list_bif_solve.pop();
if(noh.dir_abs == dir_abs_ant)
reto = 2;
else{
dif_dir = dir_abs_ant - noh.dir_abs;
if((dif_dir == -3) || (dif_dir == 1))
dir_giro = -1;
else
dir_giro = 1;
reto = 1;
}
break;
}
if(dir_giro != 0)
return fim_lab;
}
int retornaMase(int bifurca){
crossing noh;
int dir_giro = 1, fim_lab = 0, dif_dir, reto = 1;
switch (bifurca){
case 2:
fim_lab = 1;
dir_giro = 0;
break;
case 5:
dir_giro = 1;
reto = 1;
break;
case 6:
dir_giro = -1;
reto = 1;
break;
default:
noh = list_bif_volta.pop();
if(noh.dir_abs_chegada == dir_abs_ant)
reto = 2;
else{
dif_dir = dir_abs_ant - noh.dir_abs_chegada;
if((dif_dir == -3) || (dif_dir == 1))
dir_giro = -1;
else
dir_giro = 1;
reto = 1;
}
break;
}
if(dir_giro != 0)
49
return fim_lab;
}
void meiaVolta(){
motors.setSpeeds(-lento, -lento);
delay(200);
motors.setSpeeds(0, 0);
delay(200);
motors.setSpeeds(-lento, lento);
while(lerSensores() != 0B00010000);
motors.setSpeeds(0, 0);
setaDirabs (1, 1);
setaDirabs (1, 1);
}
void calculaIncremento(int sent){
int aux;
aux = sent * (mat_transf[0][0] * val_incremento[0] + mat_transf[0][1] * val_incremento[1]);
val_incremento[1] = sent * (mat_transf[1][0] * val_incremento[0] + mat_transf[1][1] * val_incremento[1]);
val_incremento[0] = aux;
}

Um algoritmo para a solução de labirintos com uso de um

Transcrição

Documentos relacionados

xbots

Entrega do Oscar 2011 vai ser em 27 de fevereiro

Aula 09 - Unifei

FILMES SOBRE IA

IMPLEMENTAÇÃO DE UM ROBÔ MÓVEL PARA AUTOMAÇÃO.

Documentação do Projeto Hustle 3000

Programmation complète- Festival Coquetel Molotov

a era da cirurgia robótica no brasil hospital israelita albert

Equipe Robotec - Competição Brasileira de Robótica 2009

MOSTRA PLAY THE MOVIE 2014