Exploração da plataforma de programação leJOS

Universidade do Minho
Conselho de Cursos de Engenharia
Licenciatura em Engenharia Informática
3ºAno
Disciplina de Laboratórios de Informática IV
Ano Lectivo de 2007/2008
Exploração da plataforma de programação
leJOS para robôs Lego MindStorms:
Uma Abordagem à Robótica Evolucionária
47109 – Tiago Costa Oliveira
47119 – Nélson Manuel Almeida Gonçalves
47131 – João Manuel Fernandes da Silva Ribeiro
Julho, 2008
i
Data de Recepção
Responsável
Avaliação
Observações
Exploração da plataforma de programação leJOS
para robôs Lego MindStorms:
Uma Abordagem à Robótica Evolucionária
47109 – Tiago Costa Oliveira
47119 – Nélson Manuel Almeida Gonçalves
47131 – João Manuel Fernandes da Silva Ribeiro
Julho, 2008
ii
Resumo
O objectivo deste trabalho foi a exploração da plataforma leJOS que permite a programação
dos robôs LEGO MindStorms em JAVA (mais exactamente, num pequeno subconjunto de
JAVA), assim como o uso da mesma numa abordagem à robótica evolucionária, contudo
acabamos também por testar a linguagem NXT-G e fizemos uma pequena comparação entre
ambas, comparação essa onde concluímos que ambas as linguagens têm tanto pontos fortes
como pontos fracos..
Quanto à abordagem à robótica evolucionária, usamos um modelo baseado em
programação por comportamentos, no qual o robô deveria percorrer um circuito e no fim do
percurso calcular o seu desempenho através de uma função de fitness que tem em conta o
tempo e a distância gastos a percorrer esse mesmo circuito.
Concluindo conseguimos implementar, embora de forma algo rudimentar, um processo
evolucionário de aprendizagem para satisfazer o objectivo pretendido.
Área de Aplicação:
Plataformas de Programação NXT-G e leJOS.
Robótica Evolucionária.
Palavras-Chave:
Lego MindStorm, NXT-G, leJOS, JAVA, robótica evolucionária,
genótipo, programação baseada em comportamentos, cromossomas.
iii
Índice
Índice
iv
1. Introdução
1
1.1. Contextualização
1
1.2. Apresentação do Caso de Estudo
3
1.3. Motivação e Objectivos
5
2. Implementação da Solução
6
3. Conclusões e Trabalho Futuro
8
Anexos
11
I. Programa Foward em Código NXT-G
12
II. Programa Foward em Código JAVA
13
III. Programa Rotate em Código NXT-G
14
IV. Programa Rotate em Código JAVA
15
V. Programa RandomMove em Código NXT-G
16
VI. Programa RandomMove em Código JAVA
17
VII. Programa KeepDistance em Código NXT-G
18
VIII. Programa KeepDistance em Código JAVA
19
IX. Programa LineFollow em Código NXT-G
21
X. Programa LineFollow em Código JAVA
22
XI. Classe FowardBehavior em Código JAVA
24
XII. Classe FindBestDirBehavior em Código JAVA
25
XIII. Classe EndDriveBehavior em Código JAVA
27
XIV. Classe Arbitrator3 em Código JAVA
28
XV. Classe RobotDrive em Código JAVA
30
XVI. Controlador do Robô a Partir do PC
33
iv
1. Introdução
1.1. Contextualização
Como base para a elaboração deste trabalho usamos as plataformas de programação leJOS e
NXT-G, ambas orientadas à programação dos robôs LEGO Mindstorms.
Lego MindStorms é o resultado de uma parceria de mais de uma década entre o Media LAB do
M.I.T e a LEGO e consiste num conjunto de peças lego tradicionais, sensores de toque,
intensidade luminosa, som, distancia entre outros e um bloco programável onde se encontra o
processador.
O nome MindStorms foi inspirado no título do livroMindStorms: Children, Computers,
andPowerfullIdeas escrito porSeymourPapert, que foi um dos fundadores do M.I.T.
A primeira versão lego MindStorms foi lançada em 1998 e era conhecida por Robot
InventionSystem(RIS),
a
versão
actual
chama-se
MindStorms
NXT
(sucessora
do
MindStormsRCX) e foi lançada em 2006.
Os robôs LEGO MindStorms podem ser usados tanto para fins lúdicos como para fins
educacionais como por exemplo introdução à robótica, automação, física, programação, etc.
leJOS é um de vários firmwares alternativos desenvolvidos pela comunidade entusiasta dos
robôs LEGO MindStorms para tirar proveito das capacidades oferecidas pelos mesmos e teve
como principais contribuidores BrianBagnall, JurgenStuber, Paul Andrews e José Solórzano. O
leJOS surgiu como uma ramificação do TinyVM, e consiste num firmware que permite correr
uma maquina virtual de JAVA nos blocos RCX e NXT, ou seja, vai permitir o uso da linguagem
JAVA no desenvolvimento de programas para robôs. O leJOS põe ao dispor do utilizador várias
1
bibliotecas que permitem o controlo de todos os sensores e motores do robô, assim como a
programação baseada em comportamentos, entre outras funções de alto nível.
NXT-G é a linguagem visual de programação que vem por predefinição com os robôs LEGO e
é muito simples e intuitiva para uso das funções básicas do robô como o controlo dos motores
ou leitura dos valores sensores, tornando-se por isso adequada para quem está a ter contacto
pela primeira vez com os robôs MindStorms e quer ver quais as suas potencialidades. É fácil
de usar devido ao seu interface gráfico com menus que são bastante esclarecedores quanto às
várias opções disponíveis, no entanto toda esta facilidade não impede a existência de aspectos
mais avançadas como o uso de variáveis, implementação de ciclos com as respectivas
condições de paragem, o uso de valores lógicos, a possibilidade do encadeamento de blocos
de código mais simples para obter blocos mais complexos, etc. É portanto uma linguagem
bastante completa que permite desenvolver tanto programas básicos, como programas mais
complexos.
Finda que está a contextualização relativamente às plataformas usadas neste trabalho
podemos passar a uma breve explicação acerca da Robótica Evolucionária, o que trata e em
que consiste.
A possibilidade da evolução de robôs inteligentes através de processos evolucionários surgiu
pela primeira vez em 1984 pelo neurofisiologista Valentino Braitenberg quando este falou da
possibilidade da criação de robôs inteligentes a partir de processos evolucionários no livro
Vehicles: Experiments in Synthetic Psychology.
O conceito de robótica evolucionária apareceu em 1993 por CliffHarvey e Husbands na
Universidade de Sussex. Em 1992/1993, duas equipas uma composta por Floreano e
Mondada, a outra constituída por um grupo de investigadores da universidade de Sussex
anunciaram as primeiras experiências de evolução artificial com robôs. O sucesso desta
pesquisa gerou uma enorme série de actividades pelo mundo fora na tentativa de explorar todo
potencial deste novo conceito.
A ideia básica por trás da robótica evolucionária é a de aplicar o conceito de evolução natural
de Darwin aos robôs, conceito esse no qual existe uma reprodução selectiva na qual apenas
participam os seres mais aptos. Neste conceito de robótica evolucionária os robôs são vistos
como organismos artificiais e autónomos que desenvolvem as duas aptidões apenas através
2
da interacção com o meio que os rodeia, sem qualquer tipo intervenção humana. Da mesma
maneira que nos seres vivos a informação genética está codificada nos cromossomas, nos
robôs essa informação está codificada também sob a forma de cromossomas, no caso dos
robôs, designamos por cromossomas o conjunto de parâmetros que controlam o
comportamento do robô. Depois de termos uma população de robôs com diferentes
cromossomas, a cada robô (físico ou simulado) é lhe permitido agir livremente, mas as suas
acções são monitorizadas automaticamente por um mecanismo de controlo específico que
avalia a sua performance tendo em conta os critérios definidos pelo responsável da
experiência. Aos robôs que obterem melhores performances é-lhes permitido reproduzirem-se
(sexuada/assexuadamente) através da geração de copias dos seus genótipos com a adição de
modificações introduzidas por mutações, combinações ou duplicações dos valores seus
parâmetros(ou operadores genéticos). Este processo é depois repetido durante várias
gerações até aparecer um indivíduo que satisfaça os critérios de performance previamente
considerados como aceitáveis.
1.2. Apresentação do Caso de Estudo
Exploração da plataforma de programação leJOS para robôs Lego MindStorms: uma
abordagem à Robótica Revolucionária.
No âmbito de projectos de investigação em curso existem disponíveis robôs Lego MindStorms
NXT. Pretende-se neste projecto explorar a plataforma Lejos que permite programar estes
robôs utilizando a linguagem Java.
O objectivo final é a construção de uma aplicação, onde o robô seja capaz de evoluir o seu
comportamento numa dada tarefa ao longo do tempo, apenas fornecendo feedback pelo seu
desempenho e não programando explicitamente a sua resolução.
Pretende-se neste trabalho a análise da plataforma leJOS assim como implementação de
conceitos de robótica evolucionária usando esta mesma plataforma como base.
A plataforma leJOS surge como ferramenta de trabalho no âmbito deste projecto, tendo dois
objectivos em vista, sendo o primeiro chegar à conclusão se vale a pena usar a plataforma
leJOS em detrimento da linguagem NXT-G que é a linguagem que vem por predefinição, o
3
segundo dos objectivos é servir de suporte à utilização de conceitos tanto de robótica
evolucionária como de programação baseada em comportamentos. Para a resolução do
primeiro objectivo, foi necessária a criação de alguns programas de diversos níveis de
complexidade, uns mais simples, outros mais complexos, os programas foram desenvolvidos
em cada uma das plataformas para depois podermos proceder à comparação da complexidade
obtida na criação dos mesmos.
A conclusão foi então que para projectos de pequena
dimensão e baixo nível de complexidade a linguagem NXT-G é mais simples de usar que a
linguagem JAVA, embora sejam ambas fáceis de utilizar, contudo no caso de um projecto mais
complexo acaba por ser mais fácil programar usando Java, isto porque a linguagem visual
desenvolvida pela LEGO se torna demasiado complexa, em suma podemos concluir que a
linguagem NXT-G é a mais adequada à resolução de problemas simples, enquanto que JAVA
é mais indicada para a resolução de problemas mais complexos. (O código dos programas
criados tendo em vista a comparação entre as plataformas pode ser visto nos anexos.)
A abordagem à robótica evolucionária está presente neste trabalho na medida em que tanto
conceitos de robótica evolucionária como conceitos de
programação baseada em
comportamentos foram utilizados na programação do robô. Apesar da programação baseada
em comportamentos não ser exactamente o mesmo que robótica evolucionária, ambas têm
pontos em comum, tais como a importância desempenhada pelo ambiente no qual decorrem as
experiências. Já o principal ponto de divergência entre ambas é que enquanto que na
programação baseada em comportamentos a resolução do problema é obtida através de um
processo de tentativa e erro no qual o operador modifica os comportamentos actuais do robô
enquanto vê a repercussão destas alterações no comportamento global do mesmo, na
programação baseada em robótica evolucionária o processo de tentativa e erro é feito
automaticamente através de um algoritmo de avaliação de resultados obtidos que funciona
automaticamente, esse algoritmo faz uso de uma função que mede o desempenho do robô
tendo em conta os resultados pretendidos, função essa a que iremos chamar função de
desempenho mas que também pode ser chamada de fitnessfunction. Outra das diferenças
entre ambos os processos é que enquanto que na aproximação à robótica baseada no uso de
comportamentos, a divisão de um comportamento do robô em comportamentos mais simples é
desempenhada de forma intuitiva pelo operador, na robótica evolucionária esta divisão não
éexecutada pelo operador, é sim resultado de um processo de auto organização.
A programação baseada em comportamentos baseia-se num pressuposto no qual os robôs são
munidos de um conjunto de comportamentos básicos. À reacção resultante da interacção
desses comportamentos básicos com o ambiente da experiência dá-se o nome de
comportamento global, o que nós fizemos foi então usar esta característica, ou seja, a
existência de comportamentos básicos, e depois aplicamos um algoritmo de avaliação de
4
resultados (como o que existe em robótica evolucionária), em vez de fazermos a avaliação de
resultados obtidos manualmente. É por isto que no nosso trabalho se verifica o uso tanto de
conceitos de robótica evolucionária como de conceitos de programação baseada em
comportamentos.
1.3. Motivação e Objectivos
Sabemos que hoje em dia os robôs e a inteligência artificial são temas muito discutidos e o seu
desenvolvimento e utilização está a ganhar terreno rumo ao futuro e sendo a robótica
evolucionária uma técnica para automatizar a criação de robôs autónomos, achamos que era
bastante interessante descobrirmos um pouco mais acerca deste tema.
Quando se houve falar em Legos, somos levados ao nosso tempo de crianças em que
juntávamos blocos para construir o que nos passava pela imaginação. Explorar o modelo mais
avançado de robô programável desta marca internacional, juntamente com a possibilidade de
trabalhar aplicações e programas relativos à nossa área de estudo tais como programação
lejos baseada em Java é uma oportunidade única.
Explorar e descobrir muitos conceitos da robótica revolucionária e inteligência artificial são
também desafios que nos movem na realização deste projecto.
Como base para a elaboração deste trabalho iremos explorar as plataformas de programação
leJOS e NXT-G, ambas orientadas à programação dos robôs LEGO Mindstorms, utilizando
exemplos e tratando problemas e tarefas iguais para ambos os tipos de programação.
5
2. Implementação da Solução
Na implementação deste projecto foram usadas as ferramentas Lego MindStorms NXT,
o IDE Eplipse com um plugin específico para leJOS e o IDE NetBeans para criação de um
ambiente em SWING para a aplicação.
Tal como já foi dito previamente, um dos objectivos do trabalho foi comparar as
plataformas leJOS e LEGO MindStorms NXT-G. Para tal foram concebidos cinco programas na
linguagem gráfica da Lego, usando a ferramenta Lego MindStorms NXT e outros cinco
desenvolvidos em java para correrem na máquina virtual disponibilizada pelo firmware leJOS,
programas esses que são por ordem crescente de complexidade: Forward que é programa que
apenas consiste em fazer o robô andar em frente até encontrar um obstáculo, Rotate que é um
programa que consiste em fazer o robô rodar sobre o seu próprio eixo até captar um ruído
acima de um determinado nível, Random_Move que consiste em fazer com que o robô tenha
movimentos aleatórios até se deparar com um obstáculo, Keep_Distance que faz com que o
robô mantenha uma certa distância (neste caso 35cm) de um objecto qualquer, isto é, caso o
objecto ande na direcção oposta ao robot, este avança de modo a encurtar a distancia até
35cm, caso o objecto se mova na direcção do robô este recua até atingir a distancia de 35cm
do objecto, por último temos o programa mais complexo que se chama Line_Follow e consiste
em fazer com que o robô siga uma determinada linha preta no chão. Depois de termos
implementado todos estes programas, através da análise do código (presente na secção
anexos do relatório) é-nos possível verificar que à medida que aumenta a complexidade dos
programas, a interpretação do código dos programas em Java não se torna tão complicada
como a interpretação do código dos mesmos programas em NXT-G, ou seja, JAVA é a
linguagem mais indicada para a programação destes robôs, uma vez que para programas
simples, é fácil de usar (embora não tanto como a NXT-G), e para programas mais complexos
não se torna muito confusa tal como a sua rival, a linguagem gráfica desenvolvida pela LEGO.
A segunda parte do projecto foi implementada usando alguns conceitos de robótica
evolucionária e a programação do robô foi feita segundo um modelo baseado em
comportamentos. O modelo baseado em comportamentos que nós criamos para este caso em
específico, baseia-se em 3 comportamentos básicos sendo eles:
6
Forwardbehavior (Anexo XI) que é o comportamento permite andar em frente;
EndDriveBehavior (Anexo XIII) que é o comportamento que permite identificar o final de
percurso e calcular o desempenho obtido pelo robô, tendo em conta o tempo demorado e a
distância percorrida;
FindBestDirBehavior (Anexo XII) que é o comportamento que permite identificar qual a melhor
direcção a tomar.
Estes três comportamentos são depois codificados através do uso de quatrovariáveis Drive
Speed que controla a velocidade do robô em linha recta, RotatitionSpeed que controla a
velocidade de rotação do robot, RotationAngle que controla o ângulo de rotação do robô em
graus e Stop Margin que codifica a distância de paragem perante um obstáculo. O envio destas
quatro variáveis é efectuado via bluetooth através de um PC com um programa também por
nós desenvolvido (ver anexo XVI). Por uma questão de simplicidade o algoritmo evolucionário
(implementado do lado do PC) procede à optimização de cada um destes parâmetros de cada
vez até alcançarem o seu valor óptimo.
Depois de tomadas todas estas decisões quanto à maneira como iríamos implementar os
conceitos de robótica evolucionária, partimos para a produção de código em JAVA (ver anexo
XI).
7
3. Conclusões e Trabalho Futuro
Neste relatório encontra-se a maior parte da pesquisa efectuada por nós para uma melhor
elaboração deste projecto de grupo, pesquisa essa que cobre vários assuntos, desde a origem
dos robôs LEGO MindStorms passando pela historia de como surgiu o leJOS, até finalmente
fazermos uma breve introdução à Robótica Evolucionária e a programação baseada em
comportamentos.
Relativamente ao que nos foi pedido, pensamos ter alcançado de forma satisfatória os
objectivos a que nos propusemos de início, contudo outro tipo de abordagem poderia ter sido
utilizada, nomeadamente um tipo de evolução mais sofisticado o que é possível alcançar sem
alterar o software desenvolvido para o robô dado que este apenas recebe parâmetros e
devolve um valor relativo ao desempenho, ficando a evolução dos parâmetros a cargo de um
qualquer software que possa efectuar uma ligação bluetooth ao robô.
8
Bibliografia
Bagnall, Brian
“Building Robots with Java Brains”
Variant Press, Canada, 2007
Floreano, Dario
Nolfi, Stefano
“Evolutionary Robotics”
MIT Press, EUA, 2000
9
Referências WWW
[01]
http://mindstorms.lego.com
Página do fabricante do robô MindStorm da Lego. Aqui podemos encontrar informação
referente ao robô assim como projectos já realizados.
[02]
http://en.wikipedia.org/wiki/Lego_Mindstorms
Página da conhecida Wikipédia com conceitos interessantes sobre os robôs da Lego,
MindStorms.
[03]
http://lejos.sourceforge.net/
Página principal do firmware leJOS que nos permite introduzir a programação JAVA
para controlar o robot.
[04]
http://en.wikipedia.org/wiki/Evolutionary_robotics
Página da Wikipédia que aborda o tema da Robótica Evolucionária.
[05]
http://lis.epfl.ch/index.html?content=resources/documentation/EvolutionaryRobo
tics/index.php
Website sobre robótica revolucionária.
[06]
http://www.bartneck.de/2008/03/04/java-lego-nxt-eclipse-tutorial/
Página com Tutoriais de programação NXT-G.
[07]
http://www.ortop.org/NXT_Tutorial/
Página com Tutoriais de programação NXT-G.
[08]
http://lejos.sourceforge.net/tutorial/index.html
Página com Tutoriais de programação lejos.
10
Anexos
11
I. Programa Foward em Código NXT-G
Figura 1 - Ilustração do Programa Foward em Código NXT-G
12
II. Programa Foward em Código JAVA
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
public class Forward {
public static void main(String[] args) throws Exception {
UltrasonicSensor us = new UltrasonicSensor(SensorPort.S3);
for(int i = 0; i < 3; i++){
Sound.beep();
Thread.sleep(700);
}
Motor.A.setPower(75);
Motor.C.setPower(75);
while(us.getDistance() > 50){
Motor.A.forward();
Motor.C.forward();
}
Motor.A.stop();
Motor.C.stop();
}
13
III. Programa Rotate em Código NXT-G
Figura 2 - Ilustração do Programa Foward em Código NXT-G
14
IV. Programa Rotate em Código JAVA
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.SoundSensor;
public class Rotate {
public static void main(String[] args) throws Exception {
SoundSensor ss = new SoundSensor(SensorPort.S4);
for(int i = 0; i < 3; i++){
Sound.beep();
Thread.sleep(700);
}
Motor.A.setPower(75);
Motor.C.setPower(75);
while(ss.readValue() < 50){
Motor.A.forward();
Motor.C.backward();
}
Motor.A.stop();
Motor.C.stop();
}
15
V. Programa RandomMove em Código NXT-G
Figura 3 - Ilustração do Programa Foward em Código NXT-G
16
VI. Programa RandomMove em Código JAVA
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
public class RandomMove {
public static void main(String[] args) throws Exception {
UltrasonicSensor us = new UltrasonicSensor(SensorPort.S3);
for(int i = 0; i < 3; i++){
Sound.beep();
Thread.sleep(700);
}
Motor.A.setSpeed(0);
Motor.C.setSpeed(0);
Motor.A.forward();
Motor.C.forward();
while(us.getDistance() > 50){
Motor.A.setSpeed(200 + (int) (Math.random()*400));
Motor.C.setSpeed(200 + (int) (Math.random()*400));
Thread.sleep(500);
}
Motor.A.flt();
Motor.C.flt();
}
17
VII. Programa KeepDistance em Código NXT-G
Figura 4 - Ilustração do Programa Foward em Código NXT-G
18
VIII. Programa KeepDistance em Código JAVA
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
public class keepDistance {
public static void main(String[] args) throws Exception {
UltrasonicSensor us = new UltrasonicSensor(SensorPort.S3);
int distance;
for(int i = 0; i < 3; i++){
Sound.beep();
Thread.sleep(700);
}
Motor.A.setSpeed(500);
Motor.C.setSpeed(500);
while(Button.readButtons() == 0){
us.ping();
Thread.sleep(20);
distance = us.getDistance();
LCD.clear();
LCD.drawInt(Motor.A.getActualSpeed(), 0, 0);
LCD.drawInt(distance, 0, 1);
LCD.refresh();
(continua na página seguinte)
19
if(40 < distance && distance < 80){
while(35 < distance && distance < 85){
Motor.A.forward();
Motor.C.forward();
LCD.clear();
LCD.drawInt(Motor.A.getActualSpeed(), 0, 0);
LCD.drawInt(distance, 0, 1);
us.ping();
Thread.sleep(20);
distance = us.getDistance();
LCD.refresh();
}
Motor.A.stop();
Motor.C.stop();
}
us.ping();
Thread.sleep(20);
distance = us.getDistance();
if(distance < 30){
while(distance < 35){
Motor.A.backward();
Motor.C.backward();
LCD.clear();
LCD.drawInt(Motor.A.getActualSpeed(), 0, 0);
LCD.drawInt(distance, 0, 1);
LCD.refresh();
us.ping();
Thread.sleep(20);
distance = us.getDistance();
}
Motor.A.stop();
Motor.C.stop();
}
}
}
}
20
IX. Programa LineFollow em Código NXT-G
Figura 5 - Ilustração do Programa Foward em Código NXT-G
21
X. Programa LineFollow em Código JAVA
import lejos.nxt.Button;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
public class LineFollow {
public static void main(String[] args) throws Exception {
LightSensor ls = new LightSensor(SensorPort.S1);
for(int i = 0; i < 3; i++){
Sound.beep();
Thread.sleep(700);
}
while(Button.readButtons() == 0){
Motor.A.setPower(50);
Motor.C.setPower(50);
while(ls.readValue() < 35){
Motor.A.forward();
Motor.C.forward();
}
Motor.A.stop();
Motor.C.stop();
if(ls.readValue() > 35){
Motor.A.setPower(20);
Motor.C.setPower(20);
22
Motor.A.resetTachoCount();
Motor.C.resetTachoCount()
Thread.sleep(200);
while(Motor.C.getTachoCount() < 90 && ls.readValue() > 35){
Motor.A.backward();
Motor.C.forward();
}
Motor.A.stop();
Motor.C.stop();
}
if(ls.readValue() > 35){
Motor.A.setPower(20);
Motor.C.setPower(20);
Motor.A.resetTachoCount();
Motor.C.resetTachoCount();
Thread.sleep(200);
while(Motor.A.getTachoCount() < 180 && ls.readValue() > 35){
Motor.A.forward();
Motor.C.backward();
}
Motor.A.stop();
Motor.C.stop();
}
if(ls.readValue() > 35){
Motor.A.setPower(20);
Motor.C.setPower(20);
Motor.A.resetTachoCount();
Motor.C.resetTachoCount();
Thread.sleep(200);
while(Motor.C.getTachoCount() < 90 && ls.readValue() > 35){
Motor.A.backward();
Motor.C.forward();
}
Motor.A.stop();
Motor.C.stop();
}
}
}
}
23
XI. Classe FowardBehavior em Código JAVA
package li4v4;
import lejos.navigation.Pilot;
import lejos.subsumption.Behavior;
public class ForwardBehavior implements Behavior {
private Pilot pilot;
int driveSpeed;
public ForwardBehavior(Pilot pilot, int driveSpeed){
this.pilot = pilot;
this.driveSpeed = driveSpeed;
}
public boolean takeControl() {
return this.pilot.getSpeed() > 0;
}
public void suppress() {
this.pilot.stop();
}
public void action() {
this.pilot.setSpeed(this.driveSpeed);
this.pilot.forward();
}
24
XII. Classe FindBestDirBehavior em Código JAVA
package li4v4;
import lejos.navigation.Pilot;
import lejos.nxt.UltrasonicSensor;
import lejos.subsumption.Behavior;
public class FindBestDirBehavior implements Behavior {
private Pilot pilot;
private UltrasonicSensor ultrasonicSensor;
private int stopMargin;
private int rotationSpeed;
private int rotationAngle;
public
FindBestDirBehavior(Pilot
pilot,
UltrasonicSensor
stopMargin, int rotationSpeed, int rotationAngle){
this.pilot = pilot;
this.ultrasonicSensor = ultrasonicSensor;
this.stopMargin = stopMargin;
this.rotationSpeed = rotationSpeed;
this.rotationAngle = rotationAngle;
}
25
ultrasonicSensor,
int
private int getDistance(){
this.ultrasonicSensor.ping();
try{
Thread.sleep(20);
}catch(Exception e){
/*nothing to do*/
}
return this.ultrasonicSensor.getDistance();
}
public boolean takeControl() {
return this.getDistance() < this.stopMargin && this.pilot.getSpeed() > 0;
}
public void suppress() {
this.pilot.stop();
}
public void action() {
int bestAngle = -Math.round(this.rotationAngle/2);
int bestDistance = 0;
int distance;
int initRotationCount;
this.pilot.setSpeed(this.rotationSpeed);
this.pilot.rotate(-Math.round(this.rotationAngle/2));
//wait until rotation is complete
initRotationCount = this.pilot.getAngle();
this.pilot.rotate(this.rotationAngle, true);
//return immediately
while(this.pilot.isMoving()){
//rotation monitoring
distance = this.getDistance();
if(distance > bestDistance && distance < 255){
bestAngle = initRotationCount - this.pilot.getAngle();
bestDistance = distance;
}
}
this.pilot.rotate(-this.rotationAngle - bestAngle);
}
}
26
XIII. Classe EndDriveBehavior em Código JAVA
package li4v4;
import lejos.navigation.Pilot;
import lejos.nxt.LightSensor;
import lejos.subsumption.Behavior;
public class EndDriveBehavior implements Behavior {
Pilot pilot;
LightSensor lightSensor;
float startTime;
public EndDriveBehavior(Pilot pilot, LightSensor lightSensor, float startTime){
this.pilot = pilot;
this.lightSensor = lightSensor;
this.startTime = startTime;
}
public boolean takeControl() {
return (lightSensor.readValue() < 28 && this.pilot.getSpeed() > 0) ||
(System.currentTimeMillis() - startTime > 120000);
}
public void suppress() {
/*nothing to do*/
}
public void action() {
this.pilot.setSpeed(0);
}
}
27
XIV. Classe Arbitrator3 em Código JAVA
package li4v4;
import lejos.subsumption.Behavior;
/**
* @see Behavior
* @author <a href="mailto:[email protected]">Tiago Oliveira</a>
* @version 1 01-06-2008
*/
public class Arbitrator3 {
private final int NONE = -1;
private Behavior[] behaviorList;
private boolean returnWhenInactive;
/**
* @param behaviorList an array of Behavior objects.
* @param returnWhenInactive if <B>true</B>, the <B>start()</B> method return when
no Behavior is active.
*/
public Arbitrator3(Behavior[] behaviorList, boolean returnWhenInactive){
this.behaviorList = behaviorList;
this.returnWhenInactive = returnWhenInactive;
}
28
/**
* Same as Arbitrator3(behaviorList, true).
* @param behaviorList An array of Behavior objects.
*/
public Arbitrator3(Behavior[] behaviorList){
this(behaviorList, true);
}
public void start(){
int currentBehavior = this.NONE;
int maxPriority = this.behaviorList.length - 1;
int highestActive;
do{
highestActive = this.NONE;
/*FIND HIGHEST PRIORITY ACTIVE BEHAVIOR*/
for(int i = maxPriority; i >= 0; i--){
if(behaviorList[i].takeControl()){
highestActive = i;
break;
}
}
if(currentBehavior != highestActive && currentBehavior != this.NONE){
/*SUPRESS CURRENT BEHAVIOR*/
behaviorList[currentBehavior].suppress();
}
if(highestActive != this.NONE){
/*EXECUTE HIGHEST PRIORITY ACTIVE BEHAVIOR*/
behaviorList[highestActive].action();
}
/*UPDATE CURRENT BEHAVIOR*/
currentBehavior = highestActive;
}
while(currentBehavior != this.NONE || !returnWhenInactive);
}
}
29
XV. Classe RobotDrive em Código JAVA
package li4v4;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import lejos.navigation.Pilot;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.subsumption.Behavior;
public class RobotDrive {
private static final float WHEEL_DIAMETER = 5.5f;
private static final float TRACK_WIDTH = 11.1f;
private static final Motor LEFT_MOTOR = Motor.C;
private static final Motor RIGHT_MOTOR = Motor.A;
public static void main(String[] args) throws Exception {
int stopMargin = 0;
//stop margin
int rotationAngle = 0;
//rotation angle, for best direction search
int rotationSpeed = 0;
//rotational speed, when searching
int driveSpeed = 0;
//forward drive speed
float startTime; //timer, for fitness calculation
boolean btError = false;
boolean endEvolution = false;
30
/*bluetooth connection*/
BTConnection btc;
/*data input stream for bluetooth*/
DataInputStream is;
/*data output stream for bluetooth*/
DataOutputStream os;
/*drive control*/
Pilot
pilot
=
new
Pilot(WHEEL_DIAMETER,
TRACK_WIDTH,
LEFT_MOTOR,
RIGHT_MOTOR);
/*distance reading*/
UltrasonicSensor ultrasonicSensor = new UltrasonicSensor(SensorPort.S3);
/*end-line detection*/
LightSensor lightSensor = new LightSensor(SensorPort.S1);
Behavior forwardBehavior;
Behavior shortDistanceBehavior;
Behavior endDriveBehavior;
Behavior[] behaviorList = new Behavior[3];
Arbitrator3 arbitrator;
try{
Thread.sleep(20);
}catch(Exception e){
/*nothing to do*/
}
LCD.clear();
LCD.drawString("waiting", 0, 0);
LCD.drawString("BT connection..", 0, 1);
LCD.refresh();
/*wait for bluetooth connection*/
btc = Bluetooth.waitForConnection();
LCD.clear();
is = btc.openDataInputStream();
os = btc.openDataOutputStream();
while(!btError && ! endEvolution){
LCD.clear();
LCD.drawString("waiting values..", 0, 0);
LCD.refresh();
if(is.available() >= 16){
31
try{
driveSpeed = is.readInt();
rotationSpeed = is.readInt();
rotationAngle = is.readInt();
stopMargin = is.readInt();
LCD.clear();
}catch(Exception e){
btError = true;
}
if(!btError){
for(int i = 0; i < 3; i++){
Sound.beep();
Thread.sleep(700);
}
startTime = (float) System.currentTimeMillis();
forwardBehavior = new ForwardBehavior(pilot, driveSpeed);
shortDistanceBehavior
=
new
FindBestDirBehavior(pilot,
ultrasonicSensor, stopMargin, rotationSpeed, rotationAngle);
endDriveBehavior = new EndDriveBehavior(pilot, lightSensor,
startTime);
behaviorList[0] = forwardBehavior;
behaviorList[1] = shortDistanceBehavior;
behaviorList[2] = endDriveBehavior;
arbitrator = new Arbitrator3(behaviorList);
pilot.resetTachoCount();
arbitrator.start();
/*wait arbitrator to return*/
float distance = pilot.getTravelDistance();
float
driveTime
=
((float)
System.currentTimeMillis()
startTime)/1000;
float fitness = 1000/(distance + driveTime);
os.writeFloat(fitness);
os.flush();
}
}
else if(is.available() == 1){
Sound.buzz();
endEvolution = true;
}
}
btc.close();
}
}
32
-
XVI. Controlador do Robô a Partir do PC
33
34