Corso di Robotica A
Prof. G.Gini
Anno Accademico 2001-2002
Definizione di un ambiente
per Robot Lego MindStorms™
Marco Mariotti
[email protected]
INDICE
CAP I TO LO 1 I N T RO D U ZI O N E
O B IE T T IV I
CAP I TO LO 2 I L RO B O T
1
1
2
I L K IT L E GO M IN D S T O R M S ™
R CX
SENSORI
2
3
3
Sensore di contatto
Sensore di luce
Sensore di rotazione
4
4
5
ATTUATORI
C O M U N IC A Z IO N E T R A R C X
C AR A T T E R IS T IC H E D E I R O B O T
5
6
6
Movimento
Sensori di contatto
Sensore di luce
Sensore di rotazione
6
6
7
7
CAP I TO LO 3 I L SO F T WA R E
I L S IS T E M A O P E R A T IV O
RCX Code
LeJOS 2.0
A M B IE N T E D I S V I L U P P O
JKD1.4
Eclipse
LeJOS plugin
I N S T A L L A Z IO N E D E L S O F T W A R E D I S V I L U P P O
LeJOS
Eclipse
LeJOS Plugin
Importare in Eclipse un progetto esistente
CAP I TO LO 4 L ’ A RCH I T E TT UR A
R E Q U IS IT I
S U B S U M P T IO N A R C H IT E C T U R E
Arbitrator
Behaviours
Task switcher
Sensori
Comunicazione
Attuatori
Diagramma di interazione
CAP I TO LO 5 B E H A VI O RS
M O V IM E N T A Z IO N E D I B A S E
Descrizione dell’ambiente
Seguire una linea con un sensore di luce
B e h a vi o r : F o l l o w L i n e
Ruotare agli incroci
B e h a vi o r : R o t a t o r _ t i m e
B e h a vi o r : R o t a t o r _ a n gl e
L O C A L I Z Z A Z IO N E
8
8
8
8
10
10
10
10
11
11
11
11
12
13
13
13
15
15
16
17
17
18
19
20
20
20
21
21
23
23
23
24
I
INDICE
Rilevare la propria posizione - Rilevare gli oggetti
B e h a vi o r : C r o s s F o u n d
P IA N IF IC A Z IO N E D E L T R A C C IA T O
Tracciare un percorso
B e h a vi o r : S o l ve P a t h
Esplorare la mappa - Raggiungere una posizione
24
24
24
24
24
25
B e h a vi o r : E xp l o r e Al l
25
Interazione con l’utente
25
B e h a vi o r : B u t t o n I n p u t
25
CAP I TO LO 6 CO NC L US I O NI
26
APP EN D IC E
27
JOSX.PLATFORM
rcx
27
27
ColorSensor.java
C o l o r S e n s o r Li s t e n e r . j a va
Li gh t S e n s o r . j a v a
27
28
29
J O S X . R O B O T IC S
29
actuator
29
S i mp l e A c t u a t o r . j a v a
R o v e r A c t u a t o r . j a va
R o t a t i o n A c t u a t o r . j a va
S e r vo . j a v a
fsm
29
30
31
31
33
Arbitrator.java
B e h a vi o r . j a v a
behavior
A c t i o n . j a va
ButtonInput.java
C r o s s F o u n d . j a va
E xp l o r e Al l . j a v a
F o l l o w Li n e . j a v a
Rotator_angle.java
R o t a t o r _ t i m e . j a va
SolvePath.java
robots
PathFinder.java
P a t h F i n d e r _ a n gl e . j a v a
va r i a b l e \ R o b C o m m . j a va
world
Direction.java
D i r e c t i o n C o n s t a n t s . j a va
GridMap.java
Path.java
Position.java
PositionConstants.java
33
35
37
37
37
39
42
45
48
50
53
57
57
59
62
63
63
64
65
66
67
68
II
CAPITOLO 1 INTRODUZIONE
CAPITOLO 1
INTRODUZIONE
Obiettivi
L’obiettivo di questo progetto è quello di fornire un ambiente di
sviluppo per il sistema Lego MindStorms. In particolare si vuole
ottenere un sistema modulare di programmazione non orientato verso un
compito specifico ma che dia la possibilità, t ramite la scelta degli
opportuni moduli, di poter ottenere comportamenti diversi del robot
utilizzando sempre la stessa struttura di base.
La modularità offre molti vantaggi, innanzitutto un sistema così
definito porta naturalmente a dividere il problema p rincipale in
sottoproblemi più piccoli e più semplici da risolvere. Inoltre permette di
lavorare facilmente anche con robot dalle caratteristiche fisiche
differenti: è sufficiente ridefinire i moduli che usano in modo diretto
l’hardware mentre si possono r iutilizzare senza modifiche i moduli di
più alto livello (che ad esempio svolgono computazioni e risolvono
algoritmi – i.e. ricerca di un cammino).
Si otterranno così robot diversi ma con gli stessi comportamenti e
che utilizzano moduli (porzioni di codice ) esattamente identici.
1
CAPITOLO 2 IL ROBOT
CAPITOLO 2
IL ROBOT
Dal momento che il progetto riguarda la realizzazione di agenti
embodied (cioè agenti dotati di un “corpo fisico” che gli permette di
interagire con l’ambiente) la struttura e le potenzialità di tale corpo
condizionano le capacità e le strategie di controllo degli agenti stessi.
In questo capitolo dunque vengono presentate le caratteristiche salienti
di tali robot.
Il kit Lego MindStorms ™
I robot sono stati costruiti utilizzando il kit Robotic Invention
System 1.5, ed il kit Ultimate Accessory Set : in Figura 1 sono presentati
i principali componenti, mentre nei paragrafi successivi ne vengono
analizzate le caratteristiche principali..
Figura 1 - Componenti fondamentali del kit
2
CAPITOLO 2 IL ROBOT
RCX
L’RCX, sviluppato presso il
MIT
di
Boston
nel
1987,
costituisce il cuore del sistema
Mindstorms.
Basato su un microcontrollore
HITACHI H8/3293 (della famiglia
H8/300) dotato di 32 Kb di
memoria RAM, l’RCX è dotat o di
tre porte di ingresso a cui
collegare altrettanti sensori e tre
porte di uscita a cui collegare tre
attuatori
(tipicamente motori).
Figura 2 - RCX
Questi componenti sono analizzati
nei paragrafi successivi.
Oltre a questi è dotato di una porta ad infrarossi per la
programmazione da PC (utilizzabile anche per la comunicazione tra più
RCX), un piccolo display a cristalli liquidi che può visualizzare cinque
cifre alfanumeriche, un altoparlante interno e quattro pulsanti per
controllarne le principali funzioni:
On–Off per l’accensione e lo spegnimento
Run per avviare l’esecuzione del programma
Prgm per la selezione del programma da eseguire
View per monitorare lo stato dei sensori
I pulsanti sono controllati dal sistema operativo è possono essere
utilizzati con diverse fu nzionalità (vedi paragrafo comportamenti).
Sensori
L’RCX presenta tre porte di ingresso per il collegamento di sensori
che possono essere alimentati ( modalità attiva ) o meno (modalità
passiva) attraverso tali porte.
La lettura di un sensore in modalità pa ssiva avviene ponendo
sull’ingresso una tensione di 5V e misurando la caduta di tensione
dovuta alla resistenza applicata dal sensore stesso; la tensione
sull’ingresso viene poi discretizzata fornendo un valore in modalità raw
che varia da 0000 (0V) a FFFF (5V), anche se dubito venga utilizzato
un convertitore A/D a 16 bit. Nel caso di un sensore in modalità attiva
si alternano un ciclo di alimentazione in cui l’ingresso viene tenuto ad
una tensione di 8V per circa 3 millisecondi ed un ciclo di lettura dell a
durata di circa 0.1 millisecondi analogo a quella descritta in
precedenza.
3
CAPITOLO 2 IL ROBOT
Sensore di contatto
Costituito sostanzialmente da
un interruttore in serie ad una
resistenza, il sensore di contatto è
l’unico ad essere utilizzato solo in
modalità
passiva.
Qu ando
l’ingresso su cui è montato viene
configurato
come
sensore
di
contatto esso assume il valore 1 se
il sensore è premuto, a 0 se
rilasciato.
Sensore di luce
Figura 3 - Sensore di contatto
Il sensore di luce è costituito da un fotodiodo e da una piccola
sorgente di luce (LED). Quan do questa è spenta il sensore viene
utilizzato in modalità passiva e permette di valutare la luminosità
dell’ambiente (per esempio se si
vuole dirigere il robot verso la zona
più scura della stanza); quando è
accesa invece il sensore viene
utilizzato in modalità attiva e
permette di valutare la quantità di
luce riflessa dalla superficie posta
di fronte al sensore, che sarà tanto
maggiore quanto più il colore è
chiaro (utilizzata ad esempio per
seguire
una
linea).
Quando
l’ingresso su cui è montato viene
configurato come sensore di luce la
il range di valori letti varia
Figura 4 - Sensore di luce
nell’intervallo [0,100] , anche se
sperimentalmente in modalità attiva
i valori non sono mai inferiori a 20 o superiori a 70, richiedendo così
una calibrazione preliminare in base alle caratter istiche dell’ambiente.
Un difetto di tale sensore è che le variazioni di valori sono continue
ossia non si verificano salti ma il valore letto varia sempre di una unità
alla volta.
4
CAPITOLO 2 IL ROBOT
Sensore di rotazione
Il sensore di rotazione, utilizzato
esclusivamente in modalità attiva
permette di misurare la rotazione
dell’asse inserito con la precisione di
1/16
di
giro
(22.5°).
Quando
l’ingresso su cui è montato viene
configurato
come
sensore
di
rotazione si comporta come un
contatore che può essere inizializzato
al
valore
desiderato
e
viene
incrementato o decrementato di una
unità in corrispondenza di una
rotazione dell’asse del sensore in
senso orario o antiorario.
Figura 5 - Sensore di rotazione
Attuatori
L’RCX presenta inoltre tre porte
di uscita utilizzabili per il colle gamento dei motori (semplici motori lineari) che possono essere con trollati definendo la potenza con cui
vengono alimentati che determina la
velocità di rotazione e una dire zione di rotazione:
Forward: il motore gira in avanti
(dipende da come il motore viene
collegato all’RCX)
Backward: stesse considerazioni
del punto precedente.
Float: il motore non ruota, ma
non oppone resistenza.
Stop: il motore è bloccato (viene
alimentato per tenere il rotore
fermo).
Figura 6 - Motore
5
CAPITOLO 2 IL ROBOT
Comunicazione tra RCX
La comunicazione fra RCX avviene tramite la porta a infrarossi
posta frontalmente sull’RCX ad una velocità tipica di 2400 bit/s.
La trasmissione di un byte, dato il grande rumore presente sul mezzo
trasmissivo (luce ambiente), è piuttosto elaborata per evitare errori nel
messaggio. Ogni byt e è prece duto da un bit di start e seguito da un bit
di end ed al termine del pacchetto viene aggiunto un bit di parità (11 bit
di messaggio); viene inoltre trasmesso un pacchetto complementare al
precedente, portando complessivamente ad un pacchetto di 22 bit che
contiene un numero uguale di zeri e di uno, permettendo così al
ricevitore di compensare il segnale costante dovuto alla luce ambiente
(bias) sottraendo il valore medio del segnale trasmesso.
Il rapporto fra bit di informazione e bit di canale porta dunque ad
una velocità di trasmissione che si aggira sui 900 bit/s.
Caratteristiche dei robot
Per meglio analizzare la flessibilità e le potenzialità della
subsumption architecture sono stati utilizzati robot di dimensioni e
caratteristiche diverse, e con compo rtamenti differenti così da rendere
più significativa la flessibilità e la configurabilità data dalla
architettura costruita.
Passiamo ora ad una breve presentazione delle principali
caratteristiche meccaniche dei robot, rimandando alla guida fornita con
il kit MindStorms per una completa descrizione dei passi di costruzione.
Movimento
Il movimento viene realizzato attraverso due cingoli azionati da
motori indipendenti, permettendo così una rotazione del robot attorno al
proprio asse. Allo scopo di contene re il più possibile gli attriti i cingoli
sono azionati tramite una sola coppia d’ingranaggi, mentre esternamente
sono sostenuti da una struttura che mantiene la giusta tensione; in
questo modo si è ottenuta una buona durata delle batterie.
Il robot così r ealizzato riesce a ruotare su se stesso, anche se la
rotazione è accompagnata da una lieve traslazione del robot dovuta all a
superficie leggermente scivolosa del campo, su cui i cingoli non
riescono ad ottenere la massima trazione.
Sensori di contatto
I robot presenta uno o due dei sensori di contatto posti frontalment e
per rivelare l’urto con eventuali ostacoli. Il contatto viene trasmesso al
sensore attraverso degli appositi “paraurti” e dato il particolare compito
da svolgere questi dovranno essere robus ti, per non subire deformazioni
e contemporaneamente anche sensibili per evitare spostamenti degli
oggetti dovuti alla mancata identificazione.
6
CAPITOLO 2 IL ROBOT
Sensore di luce
Ciascun robot presenta un sensore di luce puntato verso il basso che
permette, utilizzato in mod alità attiva, di rilevare il “colore” del campo.
L’esperienza ha mostrato che si riescono ad ottenere delle prestazioni
(in termini di velocità di inseguimento della linea e numero di
smarrimenti della stessa) tanto migliori quanto più il sensore di luce è
lontano dal centro di istantanea rotazione del robot, fermo restando il
fatto che dovrà essere posizionato esattamente sull’asse di simmetria
del robot. Purtroppo il sensore non è molto sensibile né preciso, è
quindi necessaria una calibrazione dei color i prima di utilizzarlo.
Sensore di rotazione
In uno dei due robot, infine, è stato utilizzato anche un sensore di
rotazione per misurare con precisione la rotazione del robot attorno al
proprio asse.
Infatti grazie alla catena di ingranaggi indicata in Figura 7 e
costituita sostanzialmente da una coppia di differenziali, è possibile
sottrarre le rotazioni dei due motori (e quindi dei due cingoli) collegati
agli assi M1 ed M2 ottenendo così una rotazione dell’asse D montato sul
sensore proporzionale alla rotazione del robot attorno al proprio asse.
Figura 7 – Cinematismo del sensore di rotazione
Quando il robot avanza oppure arretra su un percorso rettilineo l e
velocità di rotazione dei motori sono ident iche e dunque la loro
differenza è nulla e l’asse D resta praticamente fermo; viceversa quando
il robot ruota le velocità saranno uguali ed opposte e dunque daranno
una differenza non nulla che porta ad una rotazione dell’asse D e può
essere rilevata trami te il sensore.
7
CAPITOLO 3 IL SOFTWARE
CAPITOLO 3
IL SOFTWARE
In questa sezione verrà analizzata tutta l’architettura software usata
per poter programmare l’ RCX del Lego. Si parlerà quindi in primo
luogo del sistema operativo dell’RCX facendo una breve carrellata sui
S.O. disponibili; poi verranno discusse le AP I disponibili per
controllare il robot; infine si parlerà dell’ambiente di sviluppo usato su
PC.
Il sistema operativo
RCX Code
Il kit Lego MindStorms include il sistema operativo RCX Code
basato sulle librerie spirit.ocx, e’ sostanzialmente un interprete
bytecode in grado di eseguire si programmi residenti sia comandi
impartiti direttamente da PC tramite la torretta a infrarosso. Purtroppo
sia il sistema operativo sia il sistema operativo che l’interfaccia di
programmazione forniti presentano però delle limitazioni piuttosto
pesanti, legate ad esempio al ridotto numero di variabili utilizzabili (al
massimo 32), alla ridotta quantità di memoria utente lasciata libera
dall’interprete (circa 6KB disponibili) ed all’impossibi lità di scrivere
programmi complessi (ad esempio con più cicli innestati). Per superare
le limitazioni imposte dal sistema operativo originale sono state
proposte diverse alternative, come ad esempio l’ NQC il LegOS, oppure
LeJOS, basato su Java e utilizzat o per lo sviluppo del progetto.
LeJOS 2.0
LeJOS nasce dal progetto di una TINY Virtual Machine per il
mattoncino RCX originalmente guidato da Jose Solorzano. Entrambi i
progetti sono ospitati da SourceForge e sono completament e
OpenSource.
TinyVM e’ un sostituto del firmware Lego MindStorms RCX
microcontroller basato su Java. TinyVM occupa circa 10Kb nella
memoria dell’RCX; inoltre i programmi che vengono caricati sono
notevolmente compressi prima del download sull’RCX. Mediamente un
piccolo programma ha a disposizione 16Kb di memoria RAM.
Sostanzialmente si tratta di un’interprete di bytecode scritti in Java.
8
CAPITOLO 3 IL SOFTWARE
Le caratteristiche di TinyVM sono:
Linguaggio di programmazione Object Oriented (Java)
Preemtive thread
Eccezioni
Sincronizzazione
Array multidimensionali
Ricorsione
Accesso ai bottoni RCX
Non e’ necessario un cross -compilatore
Timers
Numeri Random
Caratteri per LCD
Persistenza degli oggetti
LeJOS fondamentalmente utilizza i metodi nativi scritti per la Tiny
Virtual Machine e li estende fornendo oggetti di alto livello per il
controllo degli attuatori e dei sensori. Le classi di LeJOS sono divise in
4 package a seconda della loro utilità:
josx.platform.rcx: è il package fondamentale, contiene tuti gli
oggetti per controllare attuatori, sensori e bottoni, ossia le librerie di
base per far muovere il robot.
josx.rcxcomm : in questo package sono raggruppate tutte le classi
per la comunicazione, a partire da quelle che controllano la porta
infrarossi, a quelle che implementano i protocolli.
josx.robotics : alcune classi per il controllo avanzato del robot
come un navigatore che utilizza motori e sensori di rotazione per
spostare il robot.
josx.util: alcune classi di utilità per gestire le strutture di memoria
come array e vettori, con un occhio di rig uardo all’uso della memoria
(ad esempio usare variabili di tipo short (2 byte) invece che int (4 byte)
Una caratteristica molto importante di Java è che nel programma da
caricare sull’RCX vengono incluse solo le classi che effettivamente
vengono importate, in questo modo si ottimizza in modo trasparente
l’utilizzo di memoria (con gli altri SO è necessario rimuovere dal SO il
codice che non si utilizza e ricompilare lo stesso).
9
CAPITOLO 3 IL SOFTWARE
Ambiente di Sviluppo
L’ambiente di sviluppo che è stato utilizzato è costitui to da quattro
componenti, tutti scaricabili gratuitamente da internet:
LeJOS 2.0 (http://www.lejos.org , http://sourgeforce.net )
JDK 1.4 (http://java.sun.com )
Eclipse (http://www.eclipse.org )
LeJOS Eclipse Plugin : un plugin per Eclipse con delle utilità per
sviluppare codice per RCX MindStorms. ( http://www.eclipse.org)
JKD1.4
Librerie, compilatori, utilità, tutto il necessario per sviluppare in
Java. E’ possibile scaricarlo gratuitamente dal sito di sun.
Eclipse
Eclipse è un IDE per la programmazione in Java sviluppato
inizialmente da IBM, poi donato al mondo OpenSource.
Indubbiamente il fatto di essere gratuito costituisce un fondamentale
vantaggio, inoltre Eclipse è basato su un’architettura a plugin che
permette di estenderne le funzionalità. La scelta è ricaduta su questo
tool soprattutto per il fatto che è disponibile un plugin appositamente
studiato per sviluppare applicazione per LeJOS.
LeJOS plugin
Si tratta di un semplice plugin per Eclipse; è possibile scaricarlo e
trovare le informazioni necessarie per l’installazione al sito ufficiale di
Eclipse sotto la categoria Plugins.
Fondamentalmente questo plugin ha tre funzionalità:
Creazione automatica di progetti Eclipse per LeJOS, che includono
automaticamente tutta le librerie necessarie.
Compilare direttamente in Eclipse i bytecode da inviare al
mattoncino RCX (è necessario un compilatore speciale, non javac)
Inviare direttamente dal tool i bytecode compilati al mattoncino
attraverso la porta infrarossi.
10
CAPITOLO 3 IL SOFTWARE
Installazione del Software di Sviluppo
LeJOS
Creare una cartella sul root del disco rigido (nome c onsigliato:
C:\Robot\ )
Estrarre nella cartella creata il file “ lejos_win32_2_0_0.zip ”
presente sul CD in posizione Software\LeJOS\ .
Per la documentazione eseguire le stesse operazioni con il file
“lejos_win32_2_0_0.doc.zip ” (si consiglia di non sovrascri vere i file).
Verificare la creazione della cartella C:\Robot\lejos\ .
Eclipse
E’ richiesta la presenza della Java Virtual Machine (JVM) di Sun (la
Microsoft JVM non funziona).
Se non presente va installata: è possibile scaricarla gratuitament e
dal sito http://java.sun.com oppure utilizzare la versione 1.3.1 fornita
sul CD lanciando l’eseguibile “ j2sdk-1_3_1_03-win.exe” presente nella
cartella S oftware\JVM\ .
Ora si può passare all’installazione di Ecplise: è suffici ent e
scompattare in una qualsiasi cartella del disco rigido il file “ eclipseSDK-2.0-win32.zip ” presente sul CD in posizione Software\Eclipse\
(cartella consigliata: C:\Robot\ ).
Verificare la creazione della cartella C:\Robot\eclipse\ .
Per lanciare il programma fare doppio clic sull’eseguibil e
Eclipse.exe (Se dovessero esserci errori nell’avvio potrebbero essere
dovuti al fatto che la JVM di default è quella di Microsoft - già presente
nei Windows precedenti a XP – si consiglia di disinstallare la Sun JVM ,
riavviare il computer, installare nuovamente la Sun JVM e riavviare di
nuovo il computer).
LeJOS Plugin
Eclipse deve essere configurato per l’uso che se ne vuole fare
tramite plug-in. In questa fase installiamo il plug -in per LeJOS presente
sul CD nella cartella Software\Eclipse\Plugin\ .
La procedura è molto semplice: chiudere Eclipse, estrarre il file
“org.lejos_1.0.1.zip ”, presente nella cartella cui sopra, nella cartella del
disco rigido C:\Robot\eclipse\plugins\ .
Verificare la creazione della car tella C:\Robot\eclipse\plugins\
org.lejos_1.0.1 .
11
CAPITOLO 3 IL SOFTWARE
Il plugin risulta ora installato, per verificarlo avviare Eclipse, dalla
barra degli strumenti selezionare File…New…Project…: se nella finestra
di destra è possibile selezionare LeJOS Project il plugin è st ato
correttamente riconosciuto.
Importare in Eclipse un progetto esistente
Una vota avviato Eclipse vediamo come caricarvi un progetto tra
quelli forniti sul CD nella cartella Project\ (ad esempio: Arbitrator)
Creare la cartella C:\Robot\project\ e copiarvi la cartella Arbitrator
presente sul CD.
In Eclipse selezionare File…Import… , poi selezionare Existing
Project into WorkSpace e Next>
Tramite il tasto B rowse… cercare la cartella Arbitrator e ciccare su
Finish per terminare.
12
CAPITOLO 4 L’ARCHITETTURA
CAPITOLO 4
L’ARCHITETTURA
Requisiti
Le caratteristiche che richiediamo al robot sono:
Goal multipli, ad esempio il robot dovrà cercare di raggiungere
un certo punto nel mondo, evitando gli ostacoli e nel minor tempo
possibile.
Sensori multipli: il robot può usare contemporaneam ente più
sensori e in modo differente
Robustezza: i sensori non sono precisi e gli eventi del mondo non
sono prevedibili
Estensibilità: deve essere possibile usare più o meno sensori e
robot differenti con caratteristiche differenti sugli attuatori e sui
sensori
Avere la possibilità di cambiare Goal o Robot semplicement e
cambiando la configurazione del sistema.
Per raggiungere tali obiettivi è necessaria una forte modularità e
scalabilità nell’architettura del sistema:
Comportamenti complessi non sono neces sariamente prodotti da
sistemi di controllo complessi, ma possono essere scomposti in molti
comportamenti semplici.
È necessario scomporre il compito globale in comportamenti che
vanno dai più bassi che operano direttamente su attuatori e sensori e in
altri di più alto livello che raccolgono informazioni da questi e
prendono decisioni più complesse. Questa architettura presenta una
robustezza intrinseca distribuendo i compiti via via più complessi su
behavior più ad alto livello.
Subsumption architecture
Si è scelto di implementare un modello che il più possibile rispetti i
principi della Subsumption Architecture
poiché questa soddisfa i
requisiti imposti al nostro sistema.
Il compito globale è visto come una stratificazione di compiti, ad
esempio un caso concreto è quello di un robot che debba esplorare tutt a
la griglia alla ricerca di oggetti che si trovano agli incroci, deve essere
in grado di evitarli e comunque visitare tutte le caselle.
13
CAPITOLO 4 L’ARCHITETTURA
Pianif icare
l’esplorazion e
Decider e il prossimo
punto in cui andare
evitando gli ostacoli
Riconoscere gli
ostacoli e crearne una
mappa
Ruotare agli
incroci
Muoversi lungo
la linea nera
Muoversi sugli
incroci
Ritor nare sulla
linea nera
Figura 8 - Architettura dei Behavior
Attraverso questa architettura è possibile connettere strettamente
percezione e azione, collegando concretamente un robot al suo mondo.
In questo modo abbiamo diversi livelli di competenza, cioè dei livelli
che specificano un a classe desiderata di comportamenti per il robot.
Possiamo osservare ad esempio che fin quando il robot dovrà muoversi
in un mondo costituito da una griglia con righe nere e incroci argentati i
tre comportamenti più bassi, cioè quelli direttamente collega ti con il
mondo saranno sempre validi; se ad esempio dopo che il robot ha
esplorato la mappa e sa dove si trovano gli ostacoli deve tornare
indietro e spostarli, sarà sufficiente sostituire il comportamento che
Pianifica l’esplorazione in modo che dica al robot di andare dove si
trovano gli ostacoli, e il comportamento che si occupa di evitarli con
uno che li spinga fuori dalla griglia.
I principi su cui si basa la Subsumption Architecture :
La computazione è organizzata sotto forma di una rete asincrona
di elementi computazionalmente attivi e indipendenti chiamati
behaviors
I vari moduli si scambiano messaggi lungo una rete prestabilita
Non esiste organizzazione gerarchica, i diversi strati ( behaviors)
funzionano in parallelo.
Esiste un meccanismo di in ibizione tra comportamenti basato
sulle priorità.
Non esiste separazione tra percezione, unità centrale e sistema di
attuazione
14
CAPITOLO 4 L’ARCHITETTURA
Il modello del mondo è distribuito lungo l’intera rete, non esiste
un modello centrale
E’
possibile
includere
nuove
funzionalità
al
sistema
semplicemente aggiungendo nuovi moduli (behaviors)
Passiamo ora alla descrizione dei vari moduli che costituiscono
questa architettura. Ogni modulo è una classe Java con determinate
caratteristiche e metodi. Ora si analizzeranno le caratteris tiche salienti
dei moduli e il loro scopo.
Arbitrator
Questo modulo è il cuore dell’architettura. Il suo compito è quello di
mandare in esecuzione comportamenti che in risposta ad un evento
diventano attivi cioè richiedono di essere eseguiti.
Quando più comportamenti diventano attivi contemporaneamente
l’arbitrator li esegue in ordine di priorità. In questo modo viene
implementato il sistema di inibizione dei comportamenti, ossia solo
quello a priorità più elevata viene effettivamente eseguito. Quando
termina la sua computazione viene di nuovo controllato quali
comportamenti richiedono di essere attivati e di nuovo viene eseguit o
quello a priorità più elevata.
Se durante l’esecuzione di un comportamento un altro con priorità
più elevata richiede l’esecuzio ne il primo viene sospeso e la sua
esecuzione viene ripresa successivamente.
Per fare ciò l’ arbitrator mantiene un array contenente l’elenco dei
behaviors attualmente funzionanti, l’ordine all’interno di tale array ne
determina la priorità (indice minore significa priorità più alta). I
comportamenti possono essere sostituiti a runtime semplicemente
cambiando gli elementi contenuti nell’array (vedi Task Switcher ).
Behaviours
Il behavior nasce come un’entità di alto livello dotata della capacità
di essere attivata o disattivata in maniere selettiva e di racchiudere al
suo interno una machina a stati finiti (AFSM).
Le caratteristiche di un comportamento sono:
Un behavior si occupa di risolvere un compito semplice e preciso
I singoli behaviors sono indipendenti, rispondono ad eventi del
mondo esterno (sensori) e controllano gli attuatori.
Ogni behavior contiene una o più azioni e controlla tramite variabili
interne lo svolgimento delle stesse.
I behaviors si scambiano messaggi lungo delle connessioni che sono
stabilite a priori e costituiscono la struttura del robot, e permettono in
un certo modo di mantenere la rappresentazione del mondo e dello stato
del robot.
15
CAPITOLO 4 L’ARCHITETTURA
Ogni behavior è caratterizzato da una priorità. Tipicamente i
comportamenti di più basso livello, c ioè quelli che si occupano del moto
e comandano direttamente gli attuatori hanno priorità più bassa, mentre
quelli di più alto livello che si occupano di risolvere compiti più astratti
hanno priorità più elevata (bisogna prima pensare cosa fare prima di
farlo!)
Un concetto fondamentale è che i singoli comportamenti decidono
autonomamente quando attivarsi, ossia in seguito ad un evento (sensori)
o ad un segnale (altri comportamenti) richiedono all’ Arbitrator di
essere eseguiti e quando verrà il loro turno s aranno eseguiti.
Abbiamo detto che un behavior contiene una macchina a stati finiti
ossia il compito che deve svolgere viene suddiviso in più azioni che
vengono eseguite in sequenza. Ogni azione può dare un comando agli
attuatori o risolvere un algoritmo, non è importante ciò che fa, la
questione che è garantita l’esecuzione completa di ogni singola azione,
ossia il behavior non può essere interrotto durante l’esecuzione di una
singola azione (cioè di uno stato della AFSM). Il passaggio da
un’azione alla successiva può essere determinato da tre fattori:
Esecuzione temporizzata: l’azione viene eseguita e poi il robot
attende il tempo che l’azione ha deciso prima di passare ad eseguire
la successiva o l’azione di un altro comportamento.
Eventi: l’azione vien e eseguita ripetutamente fino a quando un
determinato evento (scatenato dai sensori) non avviene, allora si
passa ad eseguire l’azione successiva.
Segnali: l’azione viene eseguita ripetutamente fino a quando un
altro comportamento non invia un segnale (mes saggio) che viene
propagato dalla rete a questo comportamento, allora si passa ad
eseguire l’azione successiva.
Task switcher
Questo modulo si occupa di gestire l’insieme dei comportamenti
attivi nel modulo di arbitraggio. Si chiama Task Switcher poiché grazie
a lui è possibile passare ad eseguire differenti TASK ossia avere diversi
Goal durante la vita del robot. Infatti, la particolare architettura studiata
permette di cambiare il comportamento globale del robot semplicement e
sostituendo alcuni dei compor tamenti attivi. Se ad esempio invece che
evitare gli ostacoli li si vuole spingere è sufficiente cambiare il modulo
che si occupa di evitarli con quello che permette di spingerli. Quindi il
robot permette di avere diversi Goal durante la sua vita; il cambi amento
del Task è attivato dal raggiungimento di un Goal (od obiettivo) oppure
da un qualunque evento o qualunque segnale. Sostanzialmente non è
altro che un particolare behavior a priorità più elevata di tutti gli altri
che è in grado di lavorare sulla st ruttura del modulo di arbitraggio.
16
CAPITOLO 4 L’ARCHITETTURA
Sensori
I sensori costituiscono il mezzo con il quale il robot è in grado di
“comprendere” il mondo esterno.
Il modo in cui vengono utilizzati è sostanzialmente un modello ad
eventi ossia quando un sensore registra un cambiamento del valore che
sta leggendo genera un evento.
Un qualunque modulo si può mettere in ascolto di un sensore
dichiarando di estendere un interfaccia SensorListener quindi
implementando un metodo opportuno ( stateChanged ) che il sistema
operativo si occupa di chiamare ogni qualvolta si genera quel
determinato evento. Esiste una unica limitazione che permetto di avere
al massimo 8 ascoltatori per ogni evento dettata dalla limitata capacità
elaborativi del mattoncino RCX.
I sensori sono un componente fondamentale, quindi oltre ai sensori
di base è stato implementato un sistema per cui è possibile definirne di
nuovi e più complessi, basati su un sensore di base o su un insieme di
questi, ma che effettuano una prima interpretazione dell’evento
registrato. Ad esempio il sensore di luce reagisce ad ogni minimo
cambiamento della luce registrata dal sensore, ma se a noi interessa
registrare solo determinati colori come il bianco e il nero basta definire
nel modulo Sensor un nuovo sensore che risveglia tutti i comportamenti
che lo ascoltano solo quando il sensore capta il colore bianco oppure il
nero.
In questo modo si risparmia molta capacità elaborativa e inoltre non
ci si deve preoccupare nei comportamenti di gestire sensori avanzati.
Il sistema per defini re nuovi sensori è molto semplice. E’ necessario
implementare
un
interfaccia
chiamata
ad
esempio
AdvancedSensorListener che obbliga a definire un metodo chiamat o
addListener. Il nuovo sensore deve mantenere un elenco dei componenti
(tipicamente behavior) che vogliono “ascoltarlo” e ogni volta che il
sensore registra un cambiamento chiamare in ordine i metodi
stateChanged di tutti i behavior contenuti nell’elenco. La chiamato al
metodo addListener del sensore non fa altro che aggiungere un elemento
all’elenco degli ascoltatori.
Comunicazione
La comunicazione tra i vari comportamenti è un'altra caratteristica
fondamentale della Subsumtion architecture, è grazie alla comuni cazione che è possibile mantenere una rappresentazione del mondo
esterno e una coscienza dello stato del robot; grazie a queste due
caratteristiche il robot è in grado di risolvere problemi complessi
spezzettandoli in tanti problemi semplici.
Il sistema di comunicazione che è stato implementato è basato sul
concetto di segnale molto simile a quello di Unix. Quando un behavior
17
CAPITOLO 4 L’ARCHITETTURA
lo ritiene necessario può inviare un segnale che può essere un singolo
bit così come un oggetto molto complesso chiamando il metodo
sendSignal(nomeSegnale,valore) del modulo di comunicazione.
Quando invece un comportame nto vuole ricevere un segnale di un
certo tipo deve fare tre cose:
dichiarare di essere in ascolto di quel segnale chiamando il
metodo
addSignalListener(nomeSegnale)
del
modulo
di
comunicazione
implementare l’interfaccia signalListener
dichiarare il metodo receiveSignal(nomeSeganle,valore) che verrà
chiamato quando qualche altro comportamento invia il segnale che si
sta ascoltando
Il modulo di comunicazione si occuperà di propagare un segnale
inviato da un behavior a tutti quelli che lo ascoltano semplicem ente
chiamando i metodi receiveSignal dei comportamenti.
La rete di comunicazione è fissa e decisa a compile time. E’
facilmente configurabile dal momento che i segnali hanno dei nomi
simbolici, quindi può essere modificata in base ai comportamenti
presenti nel sistema.
Attuatori
Gli attuatori sono gli elementi che permettono al robot di muoversi .
Sono direttamente controllati dai comportamenti di basso livello. LeJOS
mette a disposizione delle primitive per comandare direttamente gli
attuatori, e normalmente, i behavior utilizzano direttamente queste. E’
comunque possibile definire attuatori di più alto livello, soprattutto se
si utilizza un robot che deve compiere movimenti complessi che
prevedono l’utilizzo di più motori contemporaneamente. E’ sufficient e
aggiungere i metodi necessari nel modulo che gestisce gli attuatori, si
tratterà comunque di metodi di utilità che compiono funzioni complesse.
Ad esempio è stato definito un metodo per effettuare rotazioni in robot
con due motori e un altro metodo per f ar andare in avanti il robot. In
ogni caso è sconsigliato l’uso di questa tecnica perché piuttosto rigida,
infatti, prevede che il robot abbia determinate caratteristiche.
L’utilizzo degli attuatori in ogni caso dipende dalle caratteristiche
costruttive d el robot (numero di motori, loro disposizione, raggio di
rotazione…) per cui non è possibile avere un modulo in grado di
pilotare robot con caratteristiche molto differenti.
Il modulo degli attuatori e i comportamenti di basso livello (che si
occupano del movimento) sono per forza di cose strettamente dipendenti
dalle caratteristiche del robot.
18
CAPITOLO 4 L’ARCHITETTURA
Diagramma di interazione
Con il seguente diagramma si vuole dare un’idea di come i vari
moduli precedentemente descritti interagiscano tra loro e vadano a
costituire la Subsumption Architecture.
Communicat ion
sendSignal
recei veSignal
Arbitrator
Behavior
Behavior
Behavior
Behavior
Behavior
Behavior
Richiest e di
esecuzione
movimento
stateChanged
Actuator
motori
Sensor
sensori
cpu
Figura 9 – Diagramma di interazione
Le frecce tratteggiate rappresentano delle chiamate di funzione. Si
può notare che mentre per Communication e Sensor è stato poss ibile
indicare il nome della funzione, per il modulo Actuator, a causa della
sua forte dipendenza dalla forma del robot, non è possibile unificare il
modo con cui i comportamenti interagiscono con lui.
I behavior sono collegati con l’Arbitrator con delle f recce continue,
infatti in questo caso non sono i singoli comportamenti a chiamare un
metodo dell’Arbitrator, ma è lui stesso che ciclicamente interroga i
diversi comportamenti alla ricerca di quelli che richiedono di essere
eseguiti (ogni comportamento ha un metodo takeControl che serve a
questo scopo).
I rettangoli con sfondo grigio rappresentano le risorse fisiche del
robot e le frecce grandi indicano da chi sono controllate.
19
Error! Reference source not found.
CAPITOLO 5
BEHAVIORS
In questo capitolo verrà alla luce cosa effettivamente è in grado di
fare il robot attraverso la descrizione dei singoli comportamenti che
concorrono alla capacità elaborativa generale.
Una possibile classificazione legata al tipo di problema risolto è:
Movimentazione di base.
Localizzazione
Pianificazione del movimento
Movimentazione di base
I comportamenti che appartengono a questa categoria sono quelli che
effettivamente controllano gli attuatori. Il loro compito è fondamentale;
infatti nonostante svolgano compiti concettualmente semplici è su di
loro che tutti gli altri si appoggiano per far muovere il robot. Questi
comportamenti dipendono fortemente dalle caratteristiche del mondo in
cui il robot si deve muovere, ma anche dalle caratteristiche fisiche del
robot, anche se queste ultime possono essere inclus e nel modulo
Actuator se le capacità motorie dei robot sono simili.
Descrizione dell’ambiente
La struttura dell’ambiente in cui i robot si spostano è costituita da
una griglia tracciata con del nastro nero su fondo bianco, e con delle
piastrine argentate posizionate in corrispondenza degli incroci. La
struttura del campo così realizzato è mostrata in Figura 10 (la struttura
del campo effettivamente utilizzato si discosta leggermente da quella
qui indicata, non abbiamo infatti cons iderato i semicerchi laterali). In
questo modo il sensore di luce usato registra tre valori sufficientement e
differenti per il fondo, le linee e gli incroci
La griglia è orientata, esistono 4 direzioni fondamentali come
mostrato nella Figura 10. Ogni posizione viene identificata usando un
sistema di coordinate cartesiane con origine in basso a sinistra.
20
Error! Reference source not found.
(0,5)
(5,5)
NORD
OVEST
(0,2)
EST
SUD
(0,1)
(1,1)
home(0,0)
(1,0)
(2,0)
(0,5)
Figura 10 – Struttura dell’ambiente
Seguire una linea con un sensore di luce
Behavior: Follow Line
Questo particolare comportamento è in grado di seguire la linea nera
usando un solo sensore di luce per capire se si è posizionati
correttamente (nero) o si sta uscendo dalla linea.
Analizzando come il robot segue una linea chiusa (ad esempi o
circolare) tracciata su un campo si nota che avanza fino a che il sensore
di luce si trova sopra la riga, e nel momento in cui questa viene smarrit a
(cioè quando il sensore di luce “legge” il colore bianco) il robot viene
fatto ruotare attorno al p roprio asse, sempre dalla stessa parte, fino ad
incontrare nuovamente la linea nera come mostrato nella Figura 11, in
cui la striscia nera rappresenta la linea da seguire, il cerchietto rosso
rappresenta il sensore di luce, mentre la linea rossa rappresenta la
traiettoria seguita dal sensore di luce durante il movimento del robot.
Figura 11 - Prima strategia per l'inseguimento di una linea
21
Error! Reference source not found.
Il robot in definitiva si muove alternando avanzamenti e rotazi oni
sempre nella direzione della curva.
Un secondo problema, seppur meno critico del precedente, nasce
dall’impossibilità di mantenere sempre lo stesso lato di inseguimento
durante una serie di movimenti complessi come quelli richiesti al robot
nello spost amento sulla griglia.
Se invece prendiamo in considerazione una linea retta è necessario
cambiare il lato di inseguimento ad ogni smarrimento della linea, di
modo che questa viene seguita dal robot con una traiettoria a zig -zag
come mostrato in Figura 12.
Figura 12 - Seconda strategia per l'inseguimento di una linea
E’ facile notare che la prossima direzione in cui il robot deve
ruotare per rincontrare la linea è uguale alla penultima rotazione c he ha
effettuato. Si tratta di una sorta di apprendimento: registrando le ultime
due rotazioni che hanno permesso di incontrare la linea si può predire
con una certa sicurezza da che parte si trova la linea rispetto al sensore
di rotazione.
Purtroppo a ca usa di slittamenti o imperfezioni nel piano, è possibile
che tale algoritmo non si riveli vero; il robot in ogni caso commette
errori ogni volta che si passa dall’inseguimento di una linea retta ad una
linea curva e viceversa. Per questi motivi è necessari o limitare la prima
rotazione che potrebbe essere errata e cercare la linea dall’altra parte.
Il comportamento ottiene ciò facendo ruotare il robot prima dalla
parte in cui pensa ci sia la linea per un certo tempo; se la linea non
viene trovata, il robot r uota dalla parte opposta.
L’algoritmo è sviluppato per piccoli allontanamenti dalla linea nera
e deve sicuramente terminare (linea nera trovata) alla seconda
rotazione; nonostante questa apparente limitazione, il behavior
FollowLine si è rivelato abbastanz a robusto.
22
Error! Reference source not found.
Ruotare agli incroci
Behavior: Rotator_t ime
Giunto in prossimità di un incrocio il robot deve decidere in qual e
delle quattro possibili direzioni muoversi dopo averlo superato.
Il movimento consiste in una rotazione del robot su se stesso, fin o a
che il sensore di luce si ritrova sulla linea nera che deve seguire.
Questo comportamento necessità come input la nuova posizione da
raggiungere che deve essere adiacente (uguale a meno di ±1 rispetto alla
coordinata x oppure y) alla posizione corrente e che viene calcolata dai
behavior di esplorazione.
In base quindi a direzione corrente, posizione corrente e nuova
posizione da raggiungere il comportamento calcola se il robot deve
ruotare in senso orario o antiorario e se l’angolo di rotazione deve
essere di 90 º o di 180 º.
Il movimento avviene in due tranche: prima una rotazione a tempo
che ci assicura di portare il sensore di luce sul bianco e
successivamente una rotazione, sempre nello stesso senso che termina
quando il sensore di luce rileva la lin ea nera.
Behavior: Rotator_angle
Questo comportamento è interscambiabile con il precedente se nel
robot è presente un sensore di rotazione collegato al cinematismo
mostrato in Figura 7.
Utilizzando gli stessi input del behavior precedente viene calcolato
se il robot deve ruotare di ± 90° o ± 180°; questo valore viene passato
ad un attuatore avanzato che ne ricava di quanto il valore del sensore di
rotazione deve aumentare o diminuire e fa ruotare il robot fino a
raggiungerlo.
Questo comportamento necessita di una calibrazione preliminare
infatti il fattore di proporzionalità che passa tra l’angolo desiderato e i
valori del sensore di rotazione dipende strettamente dal sistema di
movimentazione del robot e dal tipo di ingranaggi usati nel
cinematismo.
23
Error! Reference source not found.
Localizzazione
Rilevare la propria posizione - Rilevare gli oggetti
Behavior: CrossFound
Il comportamento CrossFound è molto semplice ma fondamentale per
il robot: viene attivato dal passaggio su di un incrocio (il sensore di
luce rileva il colore “silver”) oppure dal rilevamento di un ostacolo
nella posizione verso cui ero diretto.
In entrambi i casi il behavior aggiorna la posizione corrente e si fa
carico di settare alcune variabili che permettono al robot di decidere
come muoversi e quali comportamenti attivare.
Nel caso di rilevamento dell’ostacolo vengono aggiornate la mappa
di esplorazione e la mappa degli oggetti e viene detto al robot di andare
indietro: non posso raggiungere la posizione occupata dall’ostacolo,
quindi torno indietro a quella precedente.
Nel caso di passaggio per un incrocio, aggiorno solo la mappa di
esplorazione e setto una variabile che dà il via al comportamento di
pianificazione che calcola la nuova posizione da raggiungere.
Pianificazione del tracciato
Tracciare un percorso
Behavior: Sol vePat h
Questo comportamento ha come obiettivo il calcolo del percorso
(ovvero dei punti adiacenti, cioè uguali a meno di ±1 rispetto alla
coordinata x oppure y) per raggiungere il punto di destinazione.
La nuova posizione viene selezionata tra quelle adiacenti con i
valori di x o y più vicini a quelli del punto di destinazione. Devono
inoltre essere fatti dei controlli sul nuovo punto selezionato
controllando sulla mappa degli oggetti che non sia occupato da un
ostacolo.
Viene inoltre memorizzata l’ultima posizione occupata dal robot che
deve essere l’ultima scelta possibile tra le posizione adiacenti per poter
uscire dai vicoli ciechi.
24
Error! Reference source not found.
Esplorare la mappa- Raggiungere una posizione
Behavior: Explore All
Il comportamento di esplorazione ha come obiettivo quello di
passare per tutti i punti della mappa; se è presente un ostacolo viene
rilevato e la sua posizione è memorizzata dai comportamenti precedenti
(CrossFound).
Terminata l’esplorazione il robot rimane in attese di nuovi punti da
raggiungere che devono essere inseriti a mano dall’utente; il robot è
quindi in grado di raggiungerli evitando gli ostacoli memorizzato nella
mappa degli oggetti. E’ possibile anche in questa fase inserire nuovi
ostacoli che verranno trovati , memorizzati ed evitati in nuove
esplorazioni.
Interazione con l’utente
Behavior: ButtonInput
Questo comportamento permette all’utente di inserire una nuova
destinazione utilizzando i bottoni presenti sull’RCX.
La procedura di inserimento è molto sempli ce:
1. Premere il tasto PRGM per accedere alla procedure di
programmazione; a questo punto il display dovrebbe mostrare
quattro zeri
2. Premendo ripetutamente il
bottone VIEW
è possibile
incrementare il valore visualizzato sul display; la cifra di sinistra
rappresenta l’ascissa della posizione di destinazione, mentre la
cifra di destra la coordinata, ad ogni pressione del pulsante VIEW
la posizione si incrementa di una unità.
3. Se si vuole memorizzare la posizione e sufficiente premere il
pulsante PRGM a questo pun to si può tornare al punto 2 o
avanzare al punto 4
4. Alla seconda pressione del pulsante PRGM il robot riparte e si
dirigerà alla posizione memorizzata al passa precedente.
E’ possibile memorizzare più posizione fino ad un massimo di
MAX_X *MAX_Y, dove max_x e max_y sono le dimensioni massime
della griglia.
25
CAPITOLO 6 CONCLUSIONI
CAPITOLO 6
CONCLUSIONI
L’architettura sviluppata si è dimostrata adatta alle capacità
dell’hardware disponibile ed efficiente per gli scopi del progetto. In
particolar modo si è riusciti a risolvere com piti complessi riducendoli
ad un insieme di comportamenti semplici; questo facilita notevolmente
la scrittura del codice permettendo di ottenere più facilmente e più
velocemente un prodotto funzionante.
Il lato più interessante è secondo me l’indipendenza che si raggiunge
tra il robot (inteso come dispositivo elettro -meccanico) e il compor tamento (ovvero ciò che si vuol far fare al robot), il tutto ottenuto con
un’architettura robusta e molto versatile.
Contiamo di poterla presto di poterla condividere sul Web e farla
sviluppare dal mondo OpenSource.
26
APPENDICE
APPENDICE
Riportiamo in appendice il codice Java raggruppato per package;
questa struttura rispecchia quella da noi definita e che si ritrova nel
programma Ecplise fornito con il CD.
josx.platform
rcx
ColorSensor.java
package josx.platform.rcx;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class ColorSensor implements ListenerCaller{
private int value;
private int soglia;
private boolean oldColor=false;
private ColorSensorListener[] sListeners;
private short lastListener=0;
private Sensor lightSensor;
public ColorSensor(Sensor lightSensor,int value,int soglia){
this.lightSensor=lightSensor;
this.value=value;
this.soglia=soglia;
}
public boolean isTheColor(){
int aValue = Sensor.readSensorValue(lightSensor.getId(),1);
return ((aValue < value + soglia) && (aValue > value - soglia));
}
public synchronized void addSensorListener(ColorSensorListener
listener){
if(sListeners==null)
sListeners = new ColorSensorListener[3];
sListeners[lastListener++]=listener;
27
APPENDICE
ListenerThread.get().addSensorToMask(lightSensor.getId(),this);
}
public int getID(){
return value;
}
public void callListeners() {
int newValue = Sensor.readSensorValue(lightSensor.getId(),1);
if((newValue < value + soglia) &&
(newValue > value - soglia)){
if(!oldColor){
oldColor=true;
for(int i=0;i<lastListener;i++){
sListeners[i].colorChanged(value);
}
}
}else oldColor=false;
}
}
ColorSensorListener.java
package josx.platform.rcx;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public interface ColorSensorListener {
public void colorChanged(int color);
}
28
APPENDICE
LightSensor. java
package josx.platform.rcx;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class LightSensor {
public static boolean stateLight(int sensValue, int colValue, int
soglia){
if ((sensValue < colValue + soglia) &&
(sensValue > colValue - soglia)){
return true;
}
return false;
}
}
josx.robotics
actuator
Simple Act uator. java
package josx.robotics.actuators;
import josx.platform.rcx.Motor;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public interface SimpleActuator {
public void rotate(int power,boolean clockWise);
public void backward(int power);
public void forward(int power);
public void flt();
public void stop();
}
29
APPENDICE
Rover Act uator. java
package josx.robotics.actuators;
import josx.platform.rcx.Motor;
/**
* @author caimi_mariotti
*
* A simple controller for the motors, very light but need two
* motors connected in an inverse way ad a robot that can rotate
* on its center
*/
public class RoverActuator implements SimpleActuator{
public Motor LEFT_MOTOR = Motor.A;
public Motor RIGHT_MOTOR = Motor.C;
public RoverActuator(Motor leftMotor,Motor rightMotor){
LEFT_MOTOR=leftMotor;
RIGHT_MOTOR=rightMotor;
}
public void rotate(int power,boolean clockWise){
if(clockWise){
RIGHT_MOTOR.setPower(power);
LEFT_MOTOR.setPower(power);
RIGHT_MOTOR.backward();
LEFT_MOTOR.forward();
}else{
RIGHT_MOTOR.setPower(power);
LEFT_MOTOR.setPower(power);
RIGHT_MOTOR.forward();
LEFT_MOTOR.backward();
}
}
public void backward(int power){
RIGHT_MOTOR.setPower(power);
LEFT_MOTOR.setPower(power);
RIGHT_MOTOR.backward();
LEFT_MOTOR.backward();
}
public void forward(int power){
RIGHT_MOTOR.setPower(power);
LEFT_MOTOR.setPower(power);
RIGHT_MOTOR.forward();
LEFT_MOTOR.forward();
}
public void flt(){
RIGHT_MOTOR.flt();
LEFT_MOTOR.flt();
}
public void stop(){
RIGHT_MOTOR.stop();
LEFT_MOTOR.stop();
}
}
30
APPENDICE
Rotation Actuator. java
package josx.robotics.actuators;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public interface RotationActuator extends SimpleActuator {
public void rotate(int power,int angle);
}
Servo. java
package josx.robotics.actuators;
import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class Servo extends RoverActuator implements RotationActuator {
Sensor rotationSensor;
int ratio;
public Servo(Motor leftMotor,Motor rightMotor,Sensor
rotationSensor,int ratio){
super(leftMotor,rightMotor);
this.rotationSensor=rotationSensor;
this.ratio=ratio;
}
public void setRatio(int ratio){
this.ratio=ratio;
}
/**
* @see josx.robotics.actuators.RotationActuator#rotate(int, int)
*/
public void rotate(int power, int angle) {
rotationSensor.setPreviousValue(1000);
31
APPENDICE
if(angle>0){
rotate(power,true);
while(Sensor.readSensorValue(rotationSensor.getId(),1)
<1000+(ratio*angle/360));
stop();
}
else{
rotate(power,false);
while(Sensor.readSensorValue(rotationSensor.getId(),1)
>1000+(ratio*angle/360));
stop();
}
}
}
32
APPENDICE
fsm
Arbitrator. java
package josx.robotics.fsm;
/**
* Arbitrator controls which behavior should currently be active in
* a behavior control system. Make sure to call start() after the
* Arbitrator is instantiated.
* @see robotics.rcx.Behavior
* @author caimi_mariotti
* @version 0.1 18-Sept-2002
*/
public class Arbitrator {
private
private
private
private
Behavior[] behavior;
final int NONE = 99;
int currentBehavior;
BehaviorAction actionThread;
/**
* Allocates an Arbitrator object and initializes it with an array
of
* Behavior objects. The highest index in the Behavior array will
have the
* highest order behavior level, and hence will suppress all lower
level
* behaviors if it becomes active. The Behaviors in an Arbitrator
can not
* be changed once the arbitrator is initialized.<BR>
* <B>NOTE:</B> Once the Arbitrator is initialized, the method
start() must be
* called to begin the arbitration.
* @param behavior An array of Behavior objects.
*/
public Arbitrator(Behavior[] behaviors) {
this.behavior = behaviors;
currentBehavior = NONE;
actionThread = new BehaviorAction();
actionThread.start();
}
/**
* This method starts the arbitration of Behaviors.
* Modifying the start() method is not recomended. <BR>
* Note: Arbitrator does not run in a seperate thread, and hence the
start()
* method will never return.
*/
public void start() {
int totalBehaviors = behavior.length - 1;
while (true) {
// Check through all behavior.takeControl() starting at
highest level behavior
for (int i = 0; i <= totalBehaviors; i++) {
if (behavior[i].takeControl()) {
if (i != actionThread.current) {
33
APPENDICE
// Make currentBehavior this one
actionThread.execute(i);
// Run the currentBehavior.behaviorAction()
Thread.yield();
}
break; // Breaks out of for() loop
}
}
}
}
/**
* This class handles the action() methods of the Behaviors.
*/
private class BehaviorAction extends Thread {
public boolean done = true;
public int current = NONE;
Object synch = new Object();
public void run() {
int curr = current;
while (true) {
if (curr != NONE) {
if (behavior[curr].hasMoreActions()) {
done = false;
int sleepMillis = (behavior[curr].nextAction()).act();
try {
sleep(sleepMillis);
} catch (InterruptedException e) {
}
done = true;
} else {
behavior[curr].reset();
current = NONE;
done = true;
}
}
Thread.yield();
synchronized (synch) {
if (curr != current) {
if (curr != NONE)
behavior[curr].reset();
curr = current;
}
}
}
}
public void execute(int index) {
synchronized (synch) {
current = index;
}
}
}
}
34
APPENDICE
Behavior. java
package josx.robotics.fsm;
import josx.robotics.behaviors.Action;
/**
* The Behavior interface represents an object embodying a specific
* behavior belonging to a robot. Each behavior must define three
things: <BR>
* 1) The circumstances to make this behavior seize control of the
robot.
* e.g. When the touch sensor determines the robot has collided with an
object.<BR>
* 2) The action to exhibit when this behavior takes control.
* e.g. Back up and turn.<BR>
* 3) The actions to perform when another behavior has seized control
from this
* behavior.
* e.g. Stop the current movement and update coordinates.<BR>
* These are represented by defining the methods takeControl(),
action(),
* and suppress() respectively. <BR>
* A behavior control system has one or more Behavior objects. When you
have defined
* these objects, create an array of them and use that array to
initialize an
* Arbitrator object.
*
* @see robotics.rcx.Arbitrator
* @author <a href="mailto:[email protected]">Brian Bagnall</a>
* @version 0.1 27-July-2001
*/
public interface Behavior {
/**
* Returns a boolean to indicate if this behavior should seize
control of the robot.
* For example, a robot that reacts if a touch sensor is pressed:
<BR>
* public boolean takeControl() { <BR>
*
return Sensor.S1.readBooleanValue(); <BR>
* } <BR>
* @return boolean Indicates if this Behavior should seize control.
*/
public boolean takeControl();
/**
* The code in action() represents the actual action of the robot
when this
* behavior becomes active. It can be as complex as navigating
around a
* room, or as simple as playing a tune.<BR>
* <B>The contract for implementing this method is:</B><BR>
* Any action can be started in this method. This method should not
start a
* never ending loop. This method can return on its own, or when the
suppress()
* method is called; but it must return eventually. The action can
run in
35
APPENDICE
* a seperate thread if the designer wishes it, and can therefore
continue
* running after this method call returns.
*/
/**
* Returns the next action to execute in the finite state machine
*
*/
public Action nextAction();
/**
* test if there are more actions to execute in the fsm
*/
public boolean hasMoreActions();
/**
* The code in suppress() should stop the current behavior. This can
include
* stopping motors, or even calling methods to update internal data
(such
* as navigational coordinates). <BR>
* <B>The contract for implementing this method is:</B><BR>
* This method will stop the action running in this Behavior class.
This method
* will <I>not</I> return until that action has been stopped. It is
acceptable for a
* delay to occur while the action() method finishes up.
*/
public void suppress();
/**
* Reset the finite state machine to the initial status
*/
public void reset();
}
36
APPENDICE
behavior
Action. java
package josx.robotics.behaviors;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public interface Action {
public int act();
}
ButtonInput. java
package josx.robotics.behaviors;
import
import
import
import
import
import
import
josx.platform.rcx.Button;
josx.platform.rcx.ButtonListener;
josx.platform.rcx.LCD;
josx.robotics.actuators.SimpleActuator;
josx.robotics.fsm.Behavior;
josx.robotics.world.GridMap;
josx.robotics.world.Path;
/**
* @author caimi_mariotti
*
* This behavior makes the robot rotate until he founds the black line
* rembers the last turns to optimize the following of a straight line
* and a curve line too.
*/
public class ButtonInput implements Behavior, ButtonListener {
Action action;
boolean takeControl = false;
boolean run = true;
GridMap objectMap;
SimpleActuator actuator;
Path path;
byte newpos = 0;
/**
* Constructor for ButtonInput.
*/
public ButtonInput(Path path, SimpleActuator actuator, GridMap
objectMap) {
Button.VIEW.addButtonListener(this);
Button.PRGM.addButtonListener(this);
37
APPENDICE
this.objectMap = objectMap;
this.actuator = actuator;
this.path = path;
createActions();
}
private void createActions() {
action = new Action() {
public int act() {
actuator.stop();
actuator.flt();
return 0;
}
};
}
/**
* @see josx.robotics.fsm.Behavior#takeControl()
*
* da notare che se la current position è
* unguale alla next non mi devo muovere, quindi perde il
controllo.
*
*/
public boolean takeControl() {
return takeControl;
}
/**
* @see josx.robotics.fsm.Behavior#nextAction()
*/
public Action nextAction() {
return action;
}
/**
* @see josx.robotics.fsm.Behavior#hasMoreActions()
*/
public boolean hasMoreActions() {
return true;
}
/**
* @see josx.robotics.fsm.Behavior#suppress()
*/
public void suppress() {
}
public void buttonPressed(Button b) {
}
public void buttonReleased(Button b) {
if (takeControl) {
if (Button.VIEW.getId() == b.getId()) {
run = false;
newpos++;
if (newpos % 10 > (objectMap.maxY() - 1)) {
newpos+=4;
}
38
APPENDICE
if (newpos
> (objectMap.maxX() - 1) * 10 + (objectMap.maxY() - 1))
newpos = 0;
LCD.showNumber(newpos);
} else if (Button.PRGM.getId() == b.getId()) {
if (!run) {
path.addPosition((int) newpos / 10, (int) newpos % 10);
run = true;
} else {
takeControl = false;
run = true;
}
}
} else if (Button.PRGM.getId() == b.getId()) {
LCD.showNumber(0000);
takeControl = true;
}
}
/**
* @see josx.robotics.fsm.Behavior#reset()
*/
public void reset() {
}
}
CrossFound. java
package josx.robotics.behaviors;
import josx.platform.rcx.ColorSensor;
import josx.platform.rcx.ColorSensorListener;
//import josx.platform.rcx.LCD;
import josx.platform.rcx.Sensor;
import josx.platform.rcx.SensorListener;
import josx.robotics.actuators.SimpleActuator;
import josx.robotics.fsm.Behavior;
import josx.robotics.robots.variable.RobComm;
import josx.robotics.world.Direction;
import josx.robotics.world.GridMap;
import josx.robotics.world.Position;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class CrossFound
implements Behavior, ColorSensorListener, SensorListener {
//Action
Position
Position
Position
action;
lastPosition;
currentPosition;
nextPosition;
39
APPENDICE
Direction currentDirection;
GridMap exploreMap;
GridMap objectMap;
//boolean takeControl = false;
SimpleActuator actuator;
ColorSensor silver;
Sensor bumper;
/**
* Constructor for CrossFound
*/
public CrossFound(
ColorSensor silverSensor,
Sensor bumperSensor,
Position lastPosition,
Position currentPosition,
Position nextPosition,
Direction currentDirection,
GridMap exploreMap,
GridMap objectMap,
SimpleActuator actuator) {
this.currentDirection = currentDirection;
this.currentPosition = currentPosition;
this.lastPosition = lastPosition;
this.nextPosition = nextPosition;
this.exploreMap = exploreMap;
this.objectMap = objectMap;
this.actuator = actuator;
silver = silverSensor;
bumper = bumperSensor;
silver.addSensorListener(this);
bumper.addSensorListener(this);
//createActions();
}
/*
private void createActions() {
action = new Action() {
public int act() {
actuator.flt();
return 0;
}
};
}
*/
/**
* @see josx.robotics.fsm.Behavior#takeControl()
*/
public boolean takeControl() {
return false;
//return takeControl;
}
/**
* @see josx.robotics.fsm.Behavior#nextAction()
*/
40
APPENDICE
public Action nextAction() {
//return action;
return null;
}
/**
* @see josx.robotics.fsm.Behavior#hasMoreActions()
*/
public boolean hasMoreActions() {
//takeControl = false;
return false;
//return true;
}
/**
* @see josx.robotics.fsm.Behavior#suppress()
*/
public void suppress() {
}
/**
* @see josx.robotics.fsm.Behavior#reset()
*/
public void reset() {
}
/**
* ColorSensorListener Methods
*/
boolean bumped = false;
public void colorChanged(int sensorID) {
if (RobComm.crossDone && sensorID == silver.getID()) {
if (RobComm.forward) {
lastPosition.setPosition(
currentPosition.getX(),
currentPosition.getY());
currentPosition.add(currentDirection);
} else {
currentPosition.subtract(currentDirection);
actuator.flt();
}
RobComm.forward = true;
//takeControl = true;
/*
LCD.showNumber(
currentPosition.getX() * 1000
+ currentPosition.getY() * 100
+ nextPosition.getX() * 10
+ nextPosition.getY());
*/
exploreMap.set(currentPosition);
RobComm.explore = true;
for(int i=0;i==1000;i++){
}
RobComm.solvePath = true;
RobComm.crossDone = false;
bumped = false;
}
}
41
APPENDICE
/**
* SensorListener Methods
*/
public void stateChanged(Sensor bumper, int aOldValue, int
aNewValue) {
if (!bumped
&& this.bumper.getId() == bumper.getId()
&& aNewValue != 0) {
bumped = true;
if (RobComm.forward)
currentPosition.add(currentDirection);
else
currentPosition.subtract(currentDirection);
RobComm.forward = false;
RobComm.explore = true;
//takeControl = true;
/*
LCD.showNumber(
currentPosition.getX() * 1000
+ currentPosition.getY() * 100
+ nextPosition.getX() * 10
+ nextPosition.getY());
*/
exploreMap.set(RobComm.currentPosition);
objectMap.set(RobComm.currentPosition);
}
}
}
Explore All.java
package josx.robotics.behaviors;
import
import
import
import
import
import
josx.robotics.actuators.SimpleActuator;
josx.robotics.fsm.Behavior;
josx.robotics.robots.variable.RobComm;
josx.robotics.world.GridMap;
josx.robotics.world.Path;
josx.robotics.world.Position;
/**
* @author caimi_mariotti
*
* This behavior makes the robot rotate until he founds the black line
* rembers the last turns to optimize the following of a straight line
* and a curve line too.
*/
public class ExploreAll implements Behavior {
Action[] aArray;
int lastAction = 0;
boolean takeControl = false;
boolean exploreAll = false;
GridMap exploreMap;
Position currentPosition;
Position finalPosition;
Path path;
SimpleActuator actuator;
42
APPENDICE
/**
* Constructor for ExploreAll
*/
public ExploreAll(
SimpleActuator actuator,
Path path,
GridMap exploreMap,
Position finalPosition,
Position currentPosition) {
this.exploreMap = exploreMap;
this.currentPosition = currentPosition;
this.finalPosition = finalPosition;
this.path = path;
this.actuator = actuator;
createActions();
}
private void createActions() {
aArray = new Action[3];
aArray[0] = new Action() {
private int nextPos = 0;
Position pos = new Position(0,0);
int i = 1;
int j = 0;
int incr = 1;
public int act() {
pos=nextPosition();
while(null!=pos){
if(exploreMap.isFree(pos)){
finalPosition.setPosition(pos.getX(),pos.getY());
RobComm.explore = false;
takeControl = false;
return 0;
}
pos=nextPosition();
}
lastAction++;
return 0;
}
private Position nextPosition() {
if (i < 0 || i >= exploreMap.maxX()) {
incr = -incr;
j++;
i += incr;
}
if (j < exploreMap.maxY()) {
pos.setPosition(i,j);
i += incr;
return pos;
}
return null;
}
};
43
APPENDICE
aArray[1] = new Action() {
public int act() {
Position position = path.nextPosition();
if (null != position) {
finalPosition.setPosition(position.getX(),
position.getY());
RobComm.explore = false;
takeControl = false;
return 0;
}
lastAction++;
return 0;
}
};
aArray[2] = new Action() {
public int act() {
actuator.stop();
actuator.flt();
lastAction--;
return 0;
}
};
}
/**
* @see josx.robotics.fsm.Behavior#takeControl()
*/
public boolean takeControl() {
if (RobComm.explore) {
if (currentPosition.equals(finalPosition)) {
takeControl = true;
} else {
RobComm.explore = false;
}
}
return takeControl;
}
/**
* @see josx.robotics.fsm.Behavior#nextAction()
*/
public Action nextAction() {
return aArray[lastAction];
}
/**
* @see josx.robotics.fsm.Behavior#hasMoreActions()
*/
public boolean hasMoreActions() {
return lastAction < aArray.length;
}
/**
* @see josx.robotics.fsm.Behavior#suppress()
*/
public void suppress() {
lastAction = 0;
}
44
APPENDICE
/**
* @see josx.robotics.fsm.Behavior#reset()
*/
public void reset() {
}
}
Follow Line.java
package josx.robotics.behaviors;
import
import
import
import
import
import
import
josx.platform.rcx.ColorSensor;
josx.platform.rcx.ColorSensorListener;
josx.robotics.actuators.SimpleActuator;
josx.robotics.fsm.Behavior;
josx.robotics.robots.variable.RobComm;
josx.util.Timer;
josx.util.TimerListener;
/**
* @author caimi_mariotti
*
* This behavior makes the robot rotate until he founds the black line
* rembers the last turns to optimize the following of a straight line
* and a curve line too.
*/
public class FollowLine implements Behavior, ColorSensorListener,
TimerListener {
Action[] aArray;
int lastAction=0;
boolean clockwise=true;
boolean rightRotation=true;
SimpleActuator actuator;
ColorSensor white;
ColorSensor black;
ColorSensor silver;
int time = 500;
Timer timer;
/**
* Constructor for FollowLine.
*/
public FollowLine(SimpleActuator act,ColorSensor whiteSensor,
ColorSensor blackSensor,ColorSensor silverSensor){
actuator=act;
white = whiteSensor;
black = blackSensor;
silver = silverSensor;
white.addSensorListener(this);
black.addSensorListener(this);
silver.addSensorListener(this);
timer=new Timer(time,this);
createActions();
}
45
APPENDICE
private void createActions(){
aArray = new Action[3];
aArray[0]=new Action(){
public int act(){
time = 500;
if (black.isTheColor())
rightRotation = !clockwise;
if(RobComm.forward)
actuator.forward(4);
else
actuator.backward(2);
if (white.isTheColor())
lastAction=2;
return 0;
}
};
aArray[1]=new Action(){
public int act(){
timer.stop();
timer.setDelay(time);
clockwise = rightRotation;
lastAction++;
return 50;
}
};
aArray[2]=new Action(){
public int act(){
actuator.rotate(3,clockwise);
timer.start();
return 0;
}
};
}
/**
* @see josx.robotics.fsm.Behavior#takeControl()
*
* da notare che se la current position è
* unguale alla next non mi devo muovere, quindi perde il
controllo.
*
*/
public boolean takeControl() {
return true;
}
/**
* @see josx.robotics.fsm.Behavior#nextAction()
*/
public Action nextAction() {
return aArray[lastAction];
}
/**
* @see josx.robotics.fsm.Behavior#hasMoreActions()
*/
public boolean hasMoreActions() {
46
APPENDICE
return lastAction < aArray.length;
}
/**
* @see josx.robotics.fsm.Behavior#suppress()
*/
public void suppress() {
lastAction=0;
}
/**
* @see josx.robotics.fsm.Behavior#reset()
*/
public void reset() {
lastAction=0;
}
public void timedOut(){
clockwise=!clockwise;
timer.stop();
time = time+1000;
timer.setDelay(time);
}
/**
* ColorSensorListener Methods
*/
public void colorChanged (int sensorID){
if (sensorID == white.getID()) {
timer.stop();
lastAction=1;
}
if(sensorID == black.getID()){
timer.stop();
lastAction=0;
}
if(sensorID == silver.getID()){
timer.stop();
lastAction=0;
}
}
}
47
APPENDICE
Rotator_angle.java
package josx.robotics.behaviors;
import
import
import
import
import
import
josx.platform.rcx.ColorSensor;
josx.platform.rcx.ColorSensorListener;
josx.robotics.actuators.RotationActuator;
josx.robotics.fsm.Behavior;
josx.robotics.robots.variable.RobComm;
josx.robotics.world.Direction;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class Rotator_angle implements Behavior, ColorSensorListener {
boolean takeControl = false;
Action[] aArray;
int lastAction=0;
RotationActuator rotAct;
ColorSensor white;
ColorSensor black;
/**
* Constructor for Rotator_angle
*/
public Rotator_angle(RotationActuator actuator,ColorSensor
whiteSensor,
ColorSensor blackSensor){
this.rotAct=actuator;
white=whiteSensor;
black=blackSensor;
white.addSensorListener(this);
black.addSensorListener(this);
createActions();
}
private void createActions(){
aArray = new Action[2];
////ATTENZIONE!!!
aArray[0]=new Action(){
public int act(){
//TextLCD.print("rO_t");
//LCD.showProgramNumber(0);
rotAct.forward(2);
lastAction++;
return 50;
}
};
48
APPENDICE
aArray[1]=new Action(){
public int act(){
//TextLCD.print("rO_t");
//LCD.showProgramNumber(1);
rotAct.flt();
Direction curr = RobComm.currentDirection;
Direction next =
RobComm.currentPosition.compare(RobComm.nextPosition);
int rot = curr.compare(next);
if (rot != 0){
rotAct.rotate(2,rot*90);
if (rot == 1)
RobComm.currentDirection.rotate(true);
else if (rot == -1)
RobComm.currentDirection.rotate(false);
else{
RobComm.currentDirection.rotate(true);
RobComm.currentDirection.rotate(true);
}
}
RobComm.crossDone = true;
lastAction++;
takeControl = false;
return 0;
}
};
}
/**
* @see josx.robotics.fsm.Behavior#takeControl()
*/
public boolean takeControl() {
return takeControl;
}
/**
* @see josx.robotics.fsm.Behavior#nextAction()
*/
public Action nextAction() {
return aArray[lastAction];
}
/**
* @see josx.robotics.fsm.Behavior#hasMoreActions()
*/
public boolean hasMoreActions() {
return lastAction < aArray.length;
}
/**
* @see josx.robotics.fsm.Behavior#suppress()
*/
public void suppress() {
rotAct.flt();
}
/**
49
APPENDICE
* @see josx.robotics.fsm.Behavior#reset()
*/
public void reset() {
lastAction=0;
}
/**
* ColorSensorListener Methods
*/
public void colorChanged (int sensorID){
if(!RobComm.crossDone &&
(sensorID == black.getID() || sensorID == white.getID())){
takeControl=true;
}
}
}
Rotator_time.java
package josx.robotics.behaviors;
import
import
import
import
import
import
josx.platform.rcx.ColorSensor;
josx.platform.rcx.ColorSensorListener;
josx.robotics.actuators.SimpleActuator;
josx.robotics.fsm.Behavior;
josx.robotics.robots.variable.RobComm;
josx.robotics.world.Direction;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class Rotator_time implements Behavior, ColorSensorListener {
boolean takeControl = false;
Action[] aArray;
int lastAction=0;
boolean clockWise=true;
boolean whitePassed=false;
SimpleActuator actuator;
ColorSensor white;
ColorSensor black;
/**
* Constructor for Rotator_time
*/
public Rotator_time(SimpleActuator act,ColorSensor whiteSensor,
ColorSensor blackSensor){
actuator=act;
white=whiteSensor;
black=blackSensor;
white.addSensorListener(this);
black.addSensorListener(this);
50
APPENDICE
createActions();
}
private void createActions(){
aArray = new Action[4];
////ATTENZIONE!!!
aArray[0]=new Action(){
public int act(){
//TextLCD.print("rO_t");
//LCD.showProgramNumber(0);
actuator.flt();
lastAction++;
return 1;
}
};
aArray[1]=new Action(){
public int act(){
//TextLCD.print("rO_t");
//LCD.showProgramNumber(1);
actuator.flt();
Direction curr = RobComm.currentDirection;
Direction next =
RobComm.currentPosition.compare(RobComm.nextPosition);
int rot=curr.compare(next);
if(rot==0){
RobComm.crossDone = true;
lastAction = 4;
takeControl = false;
return 0;
}
else if (rot == 1)
{
clockWise=true;
}
else if (rot == -1)
{
clockWise=false;
}
else{
if(RobComm.currentPosition.getX()==0||
RobComm.currentPosition.getY()+1
==RobComm.exploreMap.maxY()){
if(rot==-2){
clockWise=true;
}else{
clockWise=false;
}
}else{
if(rot==-2){
clockWise=false;
}else{
clockWise=true;
}
}
}
51
APPENDICE
actuator.rotate(2,clockWise);
lastAction++;
return 600;
}
};
aArray[2]=new Action(){
public int act(){
//LCD.showProgramNumber(2);
whitePassed=true;
actuator.rotate(2,clockWise);
return 0;
}
};
aArray[3]=new Action(){
public int act(){
//LCD.showProgramNumber(3);
whitePassed=false;
//aggiorno direzione corrente
RobComm.currentDirection.rotate(clockWise);
lastAction = 1;
return 0;
}
};
}
/**
* @see josx.robotics.fsm.Behavior#takeControl()
*/
public boolean takeControl() {
return takeControl;
}
/**
* @see josx.robotics.fsm.Behavior#nextAction()
*/
public Action nextAction() {
return aArray[lastAction];
}
/**
* @see josx.robotics.fsm.Behavior#hasMoreActions()
*/
public boolean hasMoreActions() {
return lastAction < aArray.length;
}
/**
* @see josx.robotics.fsm.Behavior#suppress()
*/
public void suppress() {
actuator.flt();
}
/**
* @see josx.robotics.fsm.Behavior#reset()
*/
public void reset() {
lastAction=0;
}
52
APPENDICE
/**
* ColorSensorListener Methods
*/
public void colorChanged (int sensorID){
if(whitePassed && sensorID == black.getID()){
lastAction = 3;
}
else
if(!RobComm.crossDone &&
(sensorID == black.getID() || sensorID == white.getID())){
takeControl=true;
}
}
}
Sol vePath. java
package josx.robotics.behaviors;
import
import
import
import
import
import
josx.platform.rcx.LCD;
josx.robotics.fsm.Behavior;
josx.robotics.robots.variable.RobComm;
josx.robotics.world.Direction;
josx.robotics.world.GridMap;
josx.robotics.world.Position;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class SolvePath implements Behavior {
boolean takeControl = false;
Action action;
Position lastPosition;
Position currentPosition;
Position nextPosition;
Position finalPosition;
Direction currentDirection;
GridMap objectMap;
/**
* Constructor for SolvePath
*/
public SolvePath(
Position lastPosition,
Position currentPosition,
Position nextPosition,
Position finalPosition,
Direction currentDirection,
GridMap objectMap) {
this.objectMap = objectMap;
this.currentDirection = currentDirection;
53
APPENDICE
this.lastPosition = lastPosition;
this.currentPosition = currentPosition;
this.nextPosition = nextPosition;
this.finalPosition = finalPosition;
createActions();
}
private void createActions() {
action = new Action() {
public int act() {
RobComm.pathSolved = solvePath();
LCD.showNumber(
currentPosition.getX() * 1000
+ currentPosition.getY() * 100
+ nextPosition.getX() * 10
+ nextPosition.getY());
RobComm.solvePath = false;
return 0;
}
public boolean solvePath() {
if (currentPosition.equals(finalPosition))
return true;
int xDiff = 1;
int yDiff = 1;
if (lastPosition.getX() > currentPosition.getX())
xDiff = -1;
if (lastPosition.getY() > currentPosition.getY())
yDiff = -1;
boolean xIs = false;
boolean yIs = false;
if (currentPosition.getY() < finalPosition.getY()) {
yIs = true;
} else if (currentPosition.getY() > finalPosition.getY()) {
yDiff = -1;
yIs = true;
}
if (currentPosition.getX() < finalPosition.getX()) {
xIs = true;
} else if (currentPosition.getX() > finalPosition.getX()) {
xDiff = -1;
xIs = true;
}
if (currentDirection.equals(Direction.EST)
|| currentDirection.equals(Direction.OVEST)) {
if (yIs)
goTo(
currentPosition.getX(),
currentPosition.getY() + yDiff,
xDiff,
-yDiff,
false);
54
APPENDICE
else if (xIs)
goTo(
currentPosition.getX() + xDiff,
currentPosition.getY(),
xDiff,
-yDiff,
false);
} else if (
currentDirection.equals(Direction.NORD)
|| currentDirection.equals(Direction.SUD)) {
if (xIs)
goTo(
currentPosition.getX() + xDiff,
currentPosition.getY(),
-xDiff,
yDiff,
false);
else if (yIs)
goTo(
currentPosition.getX(),
currentPosition.getY() + yDiff,
-xDiff,
yDiff,
false);
}
return false;
}
private void goTo(
int x,
int y,
int xIncr,
int yIncr,
boolean lastTry) {
if (!objectMap.isFree(currentPosition.getX(), y)
|| (!lastTry
&& (lastTry =
(lastPosition.getX() == currentPosition.getX()
&& lastPosition.getY() == y)
? true
: lastTry))) {
goTo(
currentPosition.getX() + xIncr,
currentPosition.getY(),
-xIncr,
yIncr,
lastTry);
} else if (
!objectMap.isFree(x, currentPosition.getY())
|| (!lastTry
&& (lastTry =
(lastPosition.getX() == currentPosition.getX()
&& lastPosition.getY() == y)
? true
: lastTry))) {
55
APPENDICE
goTo(
currentPosition.getX(),
currentPosition.getY() + yIncr,
xIncr,
-yIncr,
lastTry);
} else
nextPosition.setPosition(x, y);
}
};
}
/**
* @see josx.robotics.fsm.Behavior#takeControl()
*/
public boolean takeControl() {
return RobComm.solvePath;
}
/**
* @see josx.robotics.fsm.Behavior#nextAction()
*/
public Action nextAction() {
return action;
}
/**
* @see josx.robotics.fsm.Behavior#hasMoreActions()
*/
public boolean hasMoreActions() {
return true;
}
/**
* @see josx.robotics.fsm.Behavior#suppress()
*/
public void suppress() {
}
/**
* @see josx.robotics.fsm.Behavior#reset()
*/
public void reset() {
}
}
56
APPENDICE
robots
PathFinder. java
package josx.robotics.robots;
import
import
import
import
import
import
import
import
import
import
import
import
import
import
import
import
import
josx.platform.rcx.ColorSensor;
josx.platform.rcx.Motor;
josx.platform.rcx.Sensor;
josx.robotics.actuators.RoverActuator;
josx.robotics.actuators.SimpleActuator;
josx.robotics.behaviors.CrossFound;
josx.robotics.behaviors.ExploreAll;
josx.robotics.behaviors.FollowLine;
josx.robotics.behaviors.Rotator_time;
josx.robotics.behaviors.SolvePath;
josx.robotics.fsm.Arbitrator;
josx.robotics.fsm.Behavior;
josx.robotics.robots.variable.RobComm;
josx.robotics.world.Direction;
josx.robotics.world.GridMap;
josx.robotics.world.Path;
josx.robotics.world.Position;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class PathFinder {
public static Sensor BUMP = Sensor.S1;
public static Sensor LIGHT = Sensor.S2;
public
public
public
public
public
public
static
static
static
static
static
static
short
short
short
short
short
short
valBlack = 29;
valWhite = 50;
valSilver = 60;
sogliaWhite = 4;
sogliaBlack = 8;
sogliaSilver = 4;
public static void main(String[] args) {
BUMP.setTypeAndMode(1, 0x20);
BUMP.passivate();
LIGHT.setTypeAndMode(3, 0x80);
LIGHT.activate();
ColorSensor wSensor = new ColorSensor(LIGHT, valWhite,
sogliaWhite);
ColorSensor bSensor = new ColorSensor(LIGHT, valBlack,
sogliaBlack);
ColorSensor sSensor = new ColorSensor(LIGHT, valSilver,
sogliaSilver);
57
APPENDICE
RobComm.forward = true;
RobComm.crossDone = true;
RobComm.pathSolved = false;
RobComm.currentPosition = new Position(0, 0);
RobComm.lastPosition = new Position(0, 0);
RobComm.finalPosition = new Position(0, 0);
RobComm.solvePath = true;
RobComm.explore = true;
RobComm.nextPosition = new Position(0, 0);
RobComm.currentDirection = new Direction(Direction.EST);
RobComm.objectMap = new GridMap(6, 6);
RobComm.objectMap.reset();
RobComm.exploreMap = new GridMap(6, 6);
RobComm.exploreMap.reset();
RobComm.exploreMap.set(RobComm.currentPosition);
Path path = new Path(3);
path.addPosition(0, 0);
path.addPosition(5, 5);
//path.addPosition(0, 5);
SimpleActuator act = new RoverActuator(Motor.A, Motor.C);
/* NOTE: low level behaviors should have higher index
* number in the array
*/
Behavior[] bArray = new Behavior[5]; //ATTENZIONE!!
bArray[4] = new FollowLine(act, wSensor, bSensor, sSensor);
bArray[3] = new Rotator_time(act, wSensor, bSensor);
bArray[2] =
new CrossFound(
sSensor,
BUMP,
RobComm.lastPosition,
RobComm.currentPosition,
RobComm.nextPosition,
RobComm.currentDirection,
RobComm.exploreMap,
RobComm.objectMap,
act);
bArray[1] =
new SolvePath(
RobComm.lastPosition,
RobComm.currentPosition,
RobComm.nextPosition,
RobComm.finalPosition,
RobComm.currentDirection,
RobComm.objectMap);
58
APPENDICE
bArray[0] =
new ExploreAll(
act,
path,
RobComm.exploreMap,
RobComm.finalPosition,
RobComm.currentPosition);
//bArray[0] = new ButtonInput(path,act,RobComm.objectMap);
Arbitrator arby = new Arbitrator(bArray);
arby.start();
}
}
PathFinder_angl e.java
package josx.robotics.robots;
import
import
import
import
import
import
import
import
import
import
import
import
import
import
import
import
import
import
josx.platform.rcx.ColorSensor;
josx.platform.rcx.Motor;
josx.platform.rcx.Sensor;
josx.robotics.actuators.RotationActuator;
josx.robotics.actuators.Servo;
josx.robotics.behaviors.ButtonInput;
josx.robotics.behaviors.CrossFound;
josx.robotics.behaviors.ExploreAll;
josx.robotics.behaviors.FollowLine;
josx.robotics.behaviors.Rotator_angle;
josx.robotics.behaviors.SolvePath;
josx.robotics.fsm.Arbitrator;
josx.robotics.fsm.Behavior;
josx.robotics.robots.variable.RobComm;
josx.robotics.world.Direction;
josx.robotics.world.GridMap;
josx.robotics.world.Path;
josx.robotics.world.Position;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class PathFinder_angle {
public static Sensor BUMP = Sensor.S1;
public static Sensor LIGHT = Sensor.S2;
public static Sensor ROT = Sensor.S3;
public
public
public
public
public
public
static
static
static
static
static
static
short
short
short
short
short
short
valBlack = 29;
valWhite = 50;
valSilver = 60;
sogliaWhite = 4;
sogliaBlack = 8;
sogliaSilver = 4;
59
APPENDICE
public static void main(String[] args) {
BUMP.setTypeAndMode(1, 0x20);
BUMP.passivate();
LIGHT.setTypeAndMode(3, 0x80);
LIGHT.activate();
ROT.setTypeAndMode(4, 0xE0);
ROT.activate();
ColorSensor wSensor = new ColorSensor(LIGHT, valWhite,
sogliaWhite);
ColorSensor bSensor = new ColorSensor(LIGHT, valBlack,
sogliaBlack);
ColorSensor sSensor = new ColorSensor(LIGHT, valSilver,
sogliaSilver);
RobComm.forward = true;
RobComm.crossDone = true;
RobComm.pathSolved = false;
RobComm.currentPosition = new Position(0, 0);
RobComm.lastPosition = new Position(0, 0);
RobComm.finalPosition = new Position(0, 0);
RobComm.solvePath = true;
RobComm.explore = true;
RobComm.nextPosition = new Position(0, 0);
RobComm.currentDirection = new Direction(Direction.EST);
RobComm.objectMap = new GridMap(6, 6);
RobComm.objectMap.reset();
RobComm.exploreMap = new GridMap(6, 6);
RobComm.exploreMap.reset();
RobComm.exploreMap.set(RobComm.currentPosition);
Path path = new Path(3);
RotationActuator act = new Servo(Motor.A, Motor.C, ROT, 100);
/* NOTE: low level behaviors should have higher index
* number in the array i.e. b2 is higher level than b1.
*/
Behavior[] bArray = new Behavior[6]; //ATTENZIONE!!
bArray[5] = new FollowLine(act, wSensor, bSensor, sSensor);
bArray[4] = new Rotator_angle(act, wSensor, bSensor);
bArray[3] =
new CrossFound(
sSensor,
BUMP,
RobComm.lastPosition,
RobComm.currentPosition,
RobComm.nextPosition,
RobComm.currentDirection,
RobComm.exploreMap,
RobComm.objectMap,
act);
60
APPENDICE
bArray[2] =
new SolvePath(
RobComm.lastPosition,
RobComm.currentPosition,
RobComm.nextPosition,
RobComm.finalPosition,
RobComm.currentDirection,
RobComm.objectMap);
bArray[1] =
new ExploreAll(
act,
path,
RobComm.exploreMap,
RobComm.finalPosition,
RobComm.currentPosition);
bArray[0] = new ButtonInput(path, act, RobComm.objectMap);
Arbitrator arby = new Arbitrator(bArray);
arby.start();
}
}
61
APPENDICE
variable\ RobComm. java
package josx.robotics.robots.variable;
import
import
import
import
josx.robotics.world.Direction;
josx.robotics.world.GridMap;
josx.robotics.world.Path;
josx.robotics.world.Position;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class RobComm {
public static Position lastPosition;
public static Position nextPosition;
public static Position finalPosition;
public static Position currentPosition;
public static Direction currentDirection;
public static boolean forward;
public static GridMap objectMap;
public static GridMap exploreMap;
public static Path path;
public
public
public
public
static
static
static
static
boolean
boolean
boolean
boolean
crossDone;
pathSolved;
solvePath;
explore;
}
62
APPENDICE
world
Direction. java
package josx.robotics.world;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class Direction extends DirectionConstants {
private byte direction;
Direction(byte direction) {
this.direction = direction;
}
public Direction(Direction direction) {
this.direction = direction.toByte();
}
public int toInt() {
return (int) direction;
}
private byte toByte() {
return direction;
}
public String toString() {
if (equals(Direction.NORD))
return "nOrd";
if (equals(Direction.SUD))
return "SUd ";
if (equals(Direction.EST))
return "ESt ";
if (equals(Direction.OVEST))
return "OVES";
return "0";
}
public char printChDir() {
if (equals(Direction.NORD))
return 'n';
if (equals(Direction.SUD))
return 'S';
if (equals(Direction.EST))
return 'E';
if (equals(Direction.OVEST))
return 'O';
return 'P';
}
63
APPENDICE
public int compare(Direction dir) {
if (((toByte() + 1) % 4) == dir.toByte())
return +1;
else if (((toByte() + 3) % 4) == dir.toByte())
return -1;
else
return (int) toByte() - dir.toByte();
}
public void set(Direction newDirection) {
direction = newDirection.toByte();
}
public boolean equals(Direction dir) {
return this.direction == dir.direction;
}
public void rotate(boolean right) {
if (right)
direction = (byte) ((direction + 1) % 4);
else
direction = (byte) ((direction + 3) % 4);
}
}
DirectionConstant s.java
package josx.robotics.world;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class DirectionConstants {
public
public
public
public
static
static
static
static
Direction
Direction
Direction
Direction
NORD = new Direction((byte) 0);
EST = new Direction((byte) 1);
SUD = new Direction((byte) 2);
OVEST = new Direction((byte) 3);
}
64
APPENDICE
GridMap.java
package josx.robotics.world;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class GridMap {
private boolean[][] map;
public GridMap(int x, int y) {
map = new boolean[x][y];
}
public boolean isFree(int x, int y) {
if (isIn(x, y))
return !map[x][y];
return false;
}
public boolean isFree(Position position) {
return isFree(position.getX(), position.getY());
}
public void reset() {
for (int i = 0; i < map.length; i++)
for (int j = 0; j < map[i].length; j++)
map[i][j] = false;
}
private boolean isIn(int x, int y) {
return (x >= 0 && x < map.length && y >= 0 && y < map[0].length);
}
public void set(int x, int y) {
if (isIn(x, y))
map[x][y] = true;
}
public void set(Position position) {
set(position.getX(), position.getY());
}
public int maxX() {
return map.length;
}
public int maxY() {
return map[0].length;
}
}
65
APPENDICE
Path. java
package josx.robotics.world;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class Path {
private Position[] positions;
private int nextPosition = 0;
private int firstFree = 0;
private boolean full = false;
public Path(int dimension) {
positions = new Position[dimension];
for (int i = 0; i < dimension; i++)
positions[i] = new Position(0, 0);
}
public boolean addPosition(int x, int y) {
synchronized (this) {
if (!full) {
positions[firstFree].setPosition(x, y);
int next = (firstFree + 1) % positions.length;
if (next == nextPosition)
full = true;
else
firstFree = next;
return true;
}
return false;
}
}
public Position nextPosition() {
synchronized (this) {
Position position = null;
if (nextPosition != firstFree) {
position = positions[nextPosition];
nextPosition = (nextPosition + 1) % positions.length;
}
full = false;
return position;
}
}
}
66
APPENDICE
Position. java
package josx.robotics.world;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class Position {
private byte x;
private byte y;
public Position(int x, int y) {
this.x = (byte) x;
this.y = (byte) y;
}
public int getX() {
return (int) x;
}
public int getY() {
return (int) y;
}
public Position setPosition(int X, int Y) {
this.x = (byte) X;
this.y = (byte) Y;
return this;
}
public boolean equals(Position position) {
return (getX() == position.getX() && getY() == position.getY());
}
public void add(Direction direction) {
if (direction.compare(Direction.NORD) == 0)
y++;
else if (direction.compare(Direction.SUD) == 0)
y--;
else if (direction.compare(Direction.OVEST) == 0)
x--;
else if (direction.compare(Direction.EST) == 0)
x++;
}
public void subtract(Direction direction) {
if (direction.compare(Direction.NORD) == 0)
y--;
else if (direction.compare(Direction.SUD) == 0)
y++;
else if (direction.compare(Direction.OVEST) == 0)
x++;
else if (direction.compare(Direction.EST) == 0)
x--;
}
67
APPENDICE
public Direction compare(Position nextPos) {
if (getY() < nextPos.getY())
return Direction.NORD;
if (getY() > nextPos.getY())
return Direction.SUD;
if (getX() > nextPos.getX())
return Direction.OVEST;
if (getX() < nextPos.getX())
return Direction.EST;
return Direction.NORD;
}
}
PositionConstants. java
package josx.robotics.world;
/**
* @author caimi_mariotti
*
* To change this generated comment edit the template variable
"typecomment":
* Window>Preferences>Java>Templates.
* To enable and disable the creation of type comments go to
* Window>Preferences>Java>Code Generation.
*/
public class PositionConstants {
public
public
public
public
static
static
static
static
byte
byte
byte
byte
NORD = 0x00;
EST = 0x01;
SUD = 0x02;
OVEST = 0x03;
}
68
