Exploração da plataforma de programação leJOS para robôs ... · programação baseada em...

i

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

ii

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

Data de Recepção

Responsável

Avaliação

Observações

iii

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.

iv

Í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

1

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

2

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

3

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

4

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

5

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.

6

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:

7

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).

8

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ô.

9

Bibliografia

Bagnall, Brian

“Building Robots with Java Brains”

Variant Press, Canada, 2007

Floreano, Dario

Nolfi, Stefano

“Evolutionary Robotics”

MIT Press, EUA, 2000

10

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.

11

Anexos

12

I. Programa Foward em Código NXT-G

Figura 1 - Ilustração do Programa Foward em Código NXT-G

13

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();

}

14

III. Programa Rotate em Código NXT-G


15

IV. Programa Rotate em Código JAVA




import lejos.nxt.SoundSensor;

public class Rotate {


SoundSensor ss = new SoundSensor(SensorPort.S4);

for(int i = 0; i < 3; i++){

Sound.beep();

Thread.sleep(700);

}



while(ss.readValue() < 50){

Motor.A.forward();

Motor.C.backward();

}

Motor.A.stop();

Motor.C.stop();

}

16

V. Programa RandomMove em Código NXT-G


17

VI. Programa RandomMove em Código JAVA





public class RandomMove {



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();

}

18

VII. Programa KeepDistance em Código NXT-G


19

VIII. Programa KeepDistance em Código JAVA

import lejos.nxt.Button;

import lejos.nxt.LCD;





public class keepDistance {



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)

20

if(40 < distance && distance < 80){

while(35 < distance && distance < 85){

Motor.A.forward();

Motor.C.forward();

LCD.clear();



us.ping();

Thread.sleep(20);


LCD.refresh();

}

Motor.A.stop();

Motor.C.stop();

}

us.ping();

Thread.sleep(20);


if(distance < 30){

while(distance < 35){

Motor.A.backward();

Motor.C.backward();

LCD.clear();



LCD.refresh();

us.ping();

Thread.sleep(20);


}

Motor.A.stop();

Motor.C.stop();

}

}

}

}

21

IX. Programa LineFollow em Código NXT-G


22

X. Programa LineFollow em Código JAVA

import lejos.nxt.Button;

import lejos.nxt.LightSensor;




public class LineFollow {


LightSensor ls = new LightSensor(SensorPort.S1);

for(int i = 0; i < 3; i++){

Sound.beep();

Thread.sleep(700);

}

while(Button.readButtons() == 0){



while(ls.readValue() < 35){

Motor.A.forward();

Motor.C.forward();

}

Motor.A.stop();

Motor.C.stop();

if(ls.readValue() > 35){



Motor.A.resetTachoCount();

Motor.C.resetTachoCount()

23

Thread.sleep(200);

while(Motor.C.getTachoCount() < 90 && ls.readValue() > 35){

Motor.A.backward();

Motor.C.forward();

}

Motor.A.stop();

Motor.C.stop();

}





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();

}





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();

}

}

}

}

24

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();

}

25

XII. Classe FindBestDirBehavior em Código JAVA

package li4v4;




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 ultrasonicSensor, int

stopMargin, int rotationSpeed, int rotationAngle){

this.pilot = pilot;

this.ultrasonicSensor = ultrasonicSensor;

this.stopMargin = stopMargin;

this.rotationSpeed = rotationSpeed;

this.rotationAngle = rotationAngle;

}

26

private int getDistance(){

this.ultrasonicSensor.ping();

try{

Thread.sleep(20);

}catch(Exception e){

/*nothing to do*/

}

return this.ultrasonicSensor.getDistance();

}


return this.getDistance() < this.stopMargin && this.pilot.getSpeed() > 0;

}


this.pilot.stop();

}


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);

}

}

27

XIII. Classe EndDriveBehavior em Código JAVA

package li4v4;




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;

}


return (lightSensor.readValue() < 28 && this.pilot.getSpeed() > 0) ||

(System.currentTimeMillis() - startTime > 120000);

}


/*nothing to do*/

}


this.pilot.setSpeed(0);

}

}

28

XIV. Classe Arbitrator3 em Código JAVA

package li4v4;


/**

* @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;

}

29

/**

* 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);

}

}

30

XV. Classe RobotDrive em Código JAVA

package li4v4;

import java.io.DataInputStream;

import java.io.DataOutputStream;


import lejos.nxt.LCD;






import lejos.nxt.comm.BTConnection;

import lejos.nxt.comm.Bluetooth;


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;


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;

31

/*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);


/*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){

32

try{

driveSpeed = is.readInt();

rotationSpeed = is.readInt();

rotationAngle = is.readInt();

stopMargin = is.readInt();

LCD.clear();


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();

}

}

33

XVI. Controlador do Robô a Partir do PC

Exploração da plataforma de programação leJOS para robôs ... · programação baseada em...

Documents

Transcript of Exploração da plataforma de programação leJOS para robôs ... · programação baseada em...