First commit

.classpath

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="con" path="org.lejos.nxt.ldt.LEJOS_LIBRARY_CONTAINER/nxt"/>
+	<classpathentry kind="src" path="src"/>
+	<classpathentry kind="output" path="bin"/>
+</classpath>

.gitignore

@@ -0,0 +1,83 @@
+
+.metadata
+bin/
+tmp/
+*.tmp
+*.bak
+*.swp
+*~.nib
+local.properties
+.settings/
+.loadpath
+.recommenders
+
+# External tool builders
+.externalToolBuilders/
+
+# Locally stored "Eclipse launch configurations"
+*.launch
+
+# PyDev specific (Python IDE for Eclipse)
+*.pydevproject
+
+# CDT-specific (C/C++ Development Tooling)
+.cproject
+
+# CDT- autotools
+.autotools
+
+# Java annotation processor (APT)
+.factorypath
+
+# PDT-specific (PHP Development Tools)
+.buildpath
+
+# sbteclipse plugin
+.target
+
+# Tern plugin
+.tern-project
+
+# TeXlipse plugin
+.texlipse
+
+# STS (Spring Tool Suite)
+.springBeans
+
+# Code Recommenders
+.recommenders/
+
+# Annotation Processing
+.apt_generated/
+
+# Scala IDE specific (Scala & Java development for Eclipse)
+.cache-main
+.scala_dependencies
+.worksheet
+
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+*.nxj
+*.nxd

.project

@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>CoNCISa Labyrinth</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.lejos.nxt.ldt.leJOSBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.lejos.nxt.ldt.leJOSNature</nature>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+	</natures>
+</projectDescription>

LICENSE

@@ -0,0 +1,19 @@
+Copyright (c) 2018 Carlos Sanguña Henriques and Miguel A. Astor
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -0,0 +1,8 @@
+# CoNCISa Labyrinth Solver
+
+Proyecto consiste en un robot capaz de resolver un laberinto en el menor tiempo posible
+en las reglas, el robor parte de una esquina y debe desplazarse a la esquina que se encuentra en su diagonal
+para ello se utilizó una pila, en la cual se agregaba el estado de cada casilla en el laberinto, y las posibles direcciones en las que puede ir,
+estas direcciones se obtenian mediante el uso de ultrasonidos que mostraban la distancia hasta la siguiente pared
+y junto con una brújula se orientó la posición del robot y la ubicación en cordenadas polares de cada pared
+Finalmente se busca que el robot recorriera cualquier casilla que lo acercara los más posíble a la ubicación final deseada. 

src/ve/ucv/ciens/cicore/concisa/LabyrinthSolver.java

@@ -0,0 +1,41 @@
+package ve.ucv.ciens.cicore.concisa;
+
+import lejos.nxt.SensorPort;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.subsumption.Arbitrator;
+import lejos.robotics.subsumption.Behavior;
+import ve.ucv.ciens.cicore.concisa.subsumption.SensorCalibrationBehavior;
+import ve.ucv.ciens.cicore.concisa.subsumption.SolveLabyrinthBehavior;
+
+public class LabyrinthSolver {
+	private UltrasonicSensor m_usFront;
+	private UltrasonicSensor m_usRight;
+	private UltrasonicSensor m_usLeft;
+	private static final CompassHTSensor m_csCompass = new CompassHTSensor(SensorPort.S3);
+	
+	
+	private Behavior[] m_aBehaviors;
+	private Arbitrator m_saArbitrator;
+	
+	public LabyrinthSolver() {
+		m_usFront = new UltrasonicSensor(SensorPort.S2);
+		m_usRight = new UltrasonicSensor(SensorPort.S1);
+		m_usLeft  = new UltrasonicSensor(SensorPort.S4);
+		// m_csCompass = new CompassHTSensor(SensorPort.S3);
+		
+		m_usFront.setMode(UltrasonicSensor.MODE_PING);
+		m_usRight.setMode(UltrasonicSensor.MODE_PING);
+		m_usLeft.setMode(UltrasonicSensor.MODE_PING);
+		
+		m_aBehaviors = new Behavior[2];
+		m_aBehaviors[0] = new SolveLabyrinthBehavior(m_usFront, m_usRight, m_usLeft, m_csCompass);
+		m_aBehaviors[1] = new SensorCalibrationBehavior(m_usFront, m_usRight, m_usLeft, m_csCompass);
+		
+		m_saArbitrator = new Arbitrator(m_aBehaviors, true);
+	}
+	
+	public void start() {
+		m_saArbitrator.start();
+	}
+}

src/ve/ucv/ciens/cicore/concisa/Main.java

@@ -0,0 +1,13 @@
+package ve.ucv.ciens.cicore.concisa;
+
+import lejos.nxt.Button;
+import ve.ucv.ciens.cicore.concisa.utils.QuitButtonListener;
+
+public class Main {	
+	public static void main(String[] args) {
+		Button.ESCAPE.addButtonListener(new QuitButtonListener());
+		
+		LabyrinthSolver solver = new LabyrinthSolver();
+		solver.start();
+	}
+}

src/ve/ucv/ciens/cicore/concisa/Prototype/LaberintoPrototipo.cpp

@@ -0,0 +1,312 @@
+#include <iostream>
+#include <stdlib.h>
+#include <stack> 
+#include <stdio.h> 
+using namespace std;
+
+	class Casilla{
+	    public:
+	   	bool Pmovimiento[4];
+	    int PosXCa,PosYca;
+	    char Orientacion;
+	    
+	    Casilla (bool norte, bool oeste, bool este, bool sur, int x, int y, char direccion){
+	         
+	         Pmovimiento[0] = norte;
+	         Pmovimiento[1] = oeste;
+	         Pmovimiento[2] = este;
+	         Pmovimiento[3] = sur;
+	         PosXCa = x;
+	         PosYca = y;
+	         Orientacion = direccion;
+	    }
+  	};	
+	
+  	stack <Casilla> Pila;	
+
+	int PosX = 1;
+	int PosY = 1;
+	int FN =0;
+	int FO =0;
+	char laberinto [21][11] = { {'0','0','0','0','0','0','0','0','0','0','0'},
+								{'0',' ','0',' ','0',' ',' ',' ','0',' ','0'},//1
+								{'0',' ','0',' ','0',' ','0','0','0',' ','0'},//1
+								{'0',' ','0',' ','0',' ','0',' ',' ',' ','0'},//2																
+								{'0',' ','0',' ','0',' ','0',' ','0','0','0'},//2							
+								{'0',' ',' ',' ',' ',' ','0',' ',' ',' ','0'},//3
+								{'0',' ','0','0','0','0','0',' ','0','0','0'},//3
+								{'0',' ','0',' ',' ',' ',' ',' ',' ',' ','0'},//4
+								{'0',' ','0',' ',' ',' ','0','0','0','0','0'},//4
+								{'0',' ','0',' ','0',' ',' ',' ',' ',' ','0'},//5
+								{'0',' ','0',' ','0','0','0','0','0',' ','0'},//5
+								{'0',' ',' ',' ','0',' ','0',' ','0',' ','0'},//6
+								{'0','0','0',' ','0',' ','0',' ','0',' ','0'},//6
+								{'0',' ',' ',' ',' ',' ',' ',' ','0',' ','0'},//7
+								{'0',' ','0','0','0',' ','0','0','0','0','0'},//7
+								{'0',' ','0',' ','0',' ','0',' ',' ',' ','0'},//8
+								{'0',' ','0',' ','0','0','0',' ','0',' ','0'},//8
+								{'0',' ','0',' ',' ',' ',' ',' ','0',' ','0'},//9
+								{'0',' ','0',' ','0','0','0','0','0','0','0'},//9
+								{'0',' ',' ',' ',' ',' ',' ',' ',' ',' ','0'},//10
+								{'0','0','0','0','0','0','0','0','0','0','0'}
+							};
+
+	void Imprimir() {
+
+		for (int i=0; i<21; i++){
+			for (int j = 0; j < 11; j++)
+			{
+				cout << laberinto[i][j] << " ";
+			}
+			cout << "\n";
+		}
+	}
+
+	bool Frente(char Or){
+		if(Or == 'N'){
+			if(laberinto[PosX+1][PosY]==' '){
+				return true;
+			} else {
+				return false;
+			}
+		} else {
+			if(Or == 'O'){
+				if(laberinto[PosX][PosY+1]==' '){
+					return true;
+				}else{
+					return false;
+				}
+
+			} else{
+				if (Or=='E'){
+					if(laberinto[PosX][PosY-1]==' '){
+					return true;
+				}else{
+					return false;
+				}
+
+				}else{
+					if(Or=='S'){
+						if(laberinto[PosX-1][PosY]==' '){
+				return true;
+			} else {
+				return false;
+			}
+
+					}
+				}
+			}
+		}
+	}
+
+	bool Izquierda (char Or){
+		if(Or == 'N'){
+			if (laberinto[PosX][PosY+1]==' ')
+			{
+				return true;
+			}else {
+				return false;
+			}
+		} else {
+			if(Or == 'O'){
+				if(laberinto[PosX-1][PosY]==' '){
+					return true;
+				}else{
+					return false;
+				}
+
+			} else{
+				if (Or=='E'){
+					if(laberinto[PosX+1][PosY]==' '){
+					return true;
+				}else{
+					return false;
+				}
+
+				}else{
+					if(Or=='S'){
+						if(laberinto[PosX][PosY-1]==' '){
+				return true;
+			} else {
+				return false;
+			}
+
+					}
+				}
+			}
+		}
+	}
+
+
+
+	bool Derecha (char Or){
+		if(Or == 'N'){
+			if(laberinto[PosX][PosY-1]==' '){
+				return true;
+			}else {
+				return false;
+			}
+		} else {
+			if(Or == 'O'){
+				if(laberinto[PosX+1][PosY]==' '){
+					return true;
+				}else{
+					return false;
+				}
+
+			} else{
+				if (Or=='E'){
+					if(laberinto[PosX-1][PosY]==' '){
+					return true;
+				}else{
+					return false;
+				}
+
+				}else{
+					if(Or=='S'){
+						if(laberinto[PosX][PosY+1]==' '){
+				return true;
+			} else {
+				return false;
+			}
+
+					}
+				}
+			}
+		}
+	}
+
+void Mover(char Or){
+		if (Or == 'N'){
+			PosX +=2;
+			laberinto[PosX][PosY]= 'V';
+			FN++;
+		}else {
+			if(Or == 'O'){
+				PosY +=2;
+				laberinto[PosX][PosY]= '>';
+				FO++;
+			}else{
+				if(Or == 'E'){
+					PosY -=2;
+					laberinto[PosX][PosY]= '<';
+					FO--;
+				}else{
+					if(Or == 'S'){
+						PosX -=2;
+						laberinto[PosX][PosY]= 'A';
+						FN--;
+					}
+				}
+			}
+		}
+
+	}
+
+void Regresar(int x, int y, char direccion){
+	PosX=x;
+	PosY=y;
+	laberinto[PosX][PosY]='R';
+	if(direccion=='N'){
+		FN--;
+	}
+	if(direccion=='O'){
+		FO--;
+	}
+	if(direccion=='E'){
+		FO++;
+	}
+	if(direccion=='S'){
+		FN++;
+	}
+	
+
+}
+
+
+int main (){
+
+	laberinto[PosX][PosY]='V';
+	char Orientacion = 'N'; // solo puede tomar valores de N S E O
+	Casilla Actual (Frente(Orientacion), Izquierda(Orientacion), Derecha(Orientacion), false, PosX, PosY, Orientacion);
+	Casilla Siguiente (Frente(Orientacion), Izquierda(Orientacion), Derecha(Orientacion), false, PosX, PosY, Orientacion);
+	Pila.push(Actual);
+	Imprimir();
+	getchar();
+	cout << endl << endl;
+	while (FN!=9 || FO!=4){
+
+		Actual = Pila.top();
+		
+		if(Actual.Pmovimiento[0]){
+			laberinto[PosX][PosY]=' ';
+			Mover('N');
+			Orientacion ='N';
+			Pila.top().Pmovimiento[0] = false;
+			Siguiente.Pmovimiento[0] = Frente(Orientacion);
+			Siguiente.Pmovimiento[1] = Izquierda(Orientacion);
+			Siguiente.Pmovimiento[2] = Derecha(Orientacion);
+			Siguiente.Pmovimiento[3] =  false;
+			Siguiente.PosXCa = PosX;
+			Siguiente.PosYca = PosY;
+			Siguiente.Orientacion = Orientacion;
+			Pila.push(Siguiente);
+		}else {
+			if(Actual.Pmovimiento[1]){
+				laberinto[PosX][PosY]=' ';
+				Mover('O');
+				Orientacion ='O';
+				Pila.top().Pmovimiento[1] = false;
+				Siguiente.Pmovimiento[0] = Derecha(Orientacion);
+				Siguiente.Pmovimiento[1] = Frente(Orientacion);
+				Siguiente.Pmovimiento[2] = false;
+				Siguiente.Pmovimiento[3] = Izquierda(Orientacion);
+				Siguiente.PosXCa = PosX;
+				Siguiente.PosYca = PosY;
+				Siguiente.Orientacion = Orientacion;
+				Pila.push(Siguiente);
+
+			} else{
+				if(Actual.Pmovimiento[2]){
+					laberinto[PosX][PosY]=' ';
+					Mover('E');
+					Orientacion ='E';
+					Pila.top().Pmovimiento[2] = false;
+					Siguiente.Pmovimiento[0] = Izquierda(Orientacion);
+					Siguiente.Pmovimiento[1] = false;
+					Siguiente.Pmovimiento[2] = Frente(Orientacion);
+					Siguiente.Pmovimiento[3] = Derecha(Orientacion);
+					Siguiente.PosXCa = PosX;
+					Siguiente.PosYca = PosY;
+					Siguiente.Orientacion = Orientacion;
+					Pila.push(Siguiente);
+				}else{
+					if(Actual.Pmovimiento[3]){
+						laberinto[PosX][PosY]=' ';
+						Mover('S');
+						Orientacion ='S';
+						Pila.top().Pmovimiento[3] = false;
+						Siguiente.Pmovimiento[0] = false;
+						Siguiente.Pmovimiento[1] = Derecha(Orientacion);
+						Siguiente.Pmovimiento[2] = Izquierda(Orientacion);
+						Siguiente.Pmovimiento[3] = Frente(Orientacion);
+						Siguiente.PosXCa = PosX;
+						Siguiente.PosYca = PosY;
+						Siguiente.Orientacion = Orientacion;
+						Pila.push(Siguiente);
+					} else {
+						laberinto[Actual.PosXCa][Actual.PosYca]= ' ';
+						Orientacion = Actual.Orientacion;
+						Pila.pop();
+						Regresar(Pila.top().PosXCa,Pila.top().PosYca, Orientacion);
+							
+					}
+				}
+			}
+		}
+		Imprimir();
+	cout << endl << endl;
+	getchar();
+	}
+	Imprimir();
+return 0;}

src/ve/ucv/ciens/cicore/concisa/control/LabyrinthPilot.java

@@ -0,0 +1,229 @@
+package ve.ucv.ciens.cicore.concisa.control;
+
+import lejos.nxt.Motor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.navigation.DifferentialPilot;
+import ve.ucv.ciens.cicore.concisa.subsumption.SolveLabyrinthBehavior;
+import ve.ucv.ciens.cicore.concisa.utils.CompassAbstraction;
+
+@SuppressWarnings("deprecation")
+public class LabyrinthPilot {
+	public static final float WHEEL_DIAMETER_MM = 56.0f;
+	public static final float TRACK_WIDTH_MM = 60.0f;
+	public static final float TRAVEL_DISTANCE_MM = 300.0f;
+	public float Grados;
+	
+	public enum Orientation {
+		NORTH, SOUTH, EAST, WEST, INVALID
+	}
+	
+	//private CompassPilot m_cpPilot;
+	
+	private DifferentialPilot pilot;
+	private CompassAbstraction m_caCompass;
+	//private CompassHTSensor m_csCompass;
+	private Orientation m_eoOrientation;
+	
+	public LabyrinthPilot(CompassHTSensor compass) {
+		/*m_cpPilot = new CompassPilot(compass, WHEEL_DIAMETER_MM, TRACK_WIDTH_MM, Motor.C, Motor.A);
+		m_cpPilot.setRotateSpeed(20);
+		m_cpPilot.setTravelSpeed(600)*/;
+		m_caCompass = new CompassAbstraction(compass);
+		pilot = new DifferentialPilot(WHEEL_DIAMETER_MM, TRACK_WIDTH_MM, Motor.C, Motor.A);
+		pilot.setAcceleration(3);
+		pilot.setTravelSpeed(150.0f);
+		pilot.setRotateSpeed(20);
+		//m_csCompass= compass;
+		
+	}
+	
+	/*public void calibrate() {
+		//m_cpPilot.calibrate();
+	}*/
+	
+	public void setNorth() {
+		//m_cpPilot.resetCartesianZero();
+		m_caCompass.setNorth();
+	}
+
+	public void Mover(Orientation target) {
+		switch (target) {
+		case NORTH:
+			//m_cpPilot.rotate(0, false);
+			Grados =(m_caCompass.getDegreesCartesian());
+			System.out.println(Grados);
+			while(Math.abs(Grados)>5) {
+				System.out.println(Grados);
+				//Button.waitForAnyPress();
+				pilot.rotate(Grados);
+				Grados =(m_caCompass.getDegreesCartesian());
+			}
+			pilot.travel(-300);
+			SolveLabyrinthBehavior.FN++;
+			break;
+		case WEST:
+			//m_cpPilot.rotate(90, false);
+			//m_cpPilot.travel(TRAVEL_DISTANCE_MM);
+			Grados =(m_caCompass.getDegreesCartesian()-90);
+			System.out.println(Grados);
+			if (Grados<-250) {
+				Grados = Grados+360;
+			}
+			
+			while(Math.abs(Grados)>5) {
+				System.out.println(Grados);
+				//Button.waitForAnyPress();
+				pilot.rotate(Grados);
+				Grados =(m_caCompass.getDegreesCartesian()-90);
+			}
+			
+			pilot.travel(-300);
+			SolveLabyrinthBehavior.FO++;
+			break;
+		case SOUTH:
+			//m_cpPilot.rotate(180, false);
+			//m_cpPilot.travel(TRAVEL_DISTANCE_MM);
+			Grados =(m_caCompass.getDegreesCartesian()-180);
+			System.out.println(Grados);
+			while(Math.abs(Grados)>5) {
+				if (Grados<-250) {
+					Grados = Grados+360;
+				}
+				System.out.println(Grados);
+				//Button.waitForAnyPress();
+				pilot.rotate(Grados);
+				Grados =(m_caCompass.getDegreesCartesian()-180);
+			}
+			pilot.travel(-300);
+			SolveLabyrinthBehavior.FN--;
+			break;
+		case EAST:
+			//m_cpPilot.rotate(270, false);
+			//m_cpPilot.travel(TRAVEL_DISTANCE_MM);
+			Grados =(m_caCompass.getDegreesCartesian()+90);
+			System.out.println(Grados);
+			if (Grados>250) {
+				Grados = Grados-360;
+			}
+			while(Math.abs(Grados)>5) {
+				System.out.println(Grados);
+				//Button.waitForAnyPress();
+				pilot.rotate(Grados);
+				Grados =(m_caCompass.getDegreesCartesian()+90);
+			}
+			pilot.travel(-300);
+			SolveLabyrinthBehavior.FO--;
+			break;
+		case INVALID: /* Fall through */
+		default:
+			System.out.println("Inv. Heading");
+			break;
+		}
+		
+	}
+	
+	public void Regresar(Orientation direction, char Corregir) throws Exception {
+		if (direction == Orientation.INVALID)
+			throw new Exception("exPilRev: INV");
+		
+		pilot.travel(300);
+		
+		switch (direction) {
+		case NORTH:
+			SolveLabyrinthBehavior.FN--;
+			break;
+		case WEST:
+			SolveLabyrinthBehavior.FO--;
+			break;
+		case SOUTH:
+			SolveLabyrinthBehavior.FN++;
+			break;
+		case EAST:
+			SolveLabyrinthBehavior.FO++;
+			break;
+		case INVALID: /* Fall through */
+		default:
+			System.out.println("Inv. Heading");
+			break;
+		}
+		switch (Corregir) {
+		case 'N':
+			Grados =(m_caCompass.getDegreesCartesian());
+			System.out.println(Grados);
+			while(Math.abs(Grados)>5) {
+				System.out.println(Grados);
+				//Button.waitForAnyPress();
+				pilot.rotate(Grados);
+				Grados =(m_caCompass.getDegreesCartesian());
+			}
+			break;
+		case 'O':
+			Grados =(m_caCompass.getDegreesCartesian()-90);
+			System.out.println(Grados);
+			if (Grados<-250) {
+				Grados = Grados+360;
+			}
+			
+			while(Math.abs(Grados)>5) {
+				System.out.println(Grados);
+				//Button.waitForAnyPress();
+				pilot.rotate(Grados);
+				Grados =(m_caCompass.getDegreesCartesian()-90);
+			}
+			break;
+		case 'S':
+			//m_cpPilot.rotate(180, false);
+			//m_cpPilot.travel(TRAVEL_DISTANCE_MM);
+			Grados =(m_caCompass.getDegreesCartesian()-180);
+			System.out.println(Grados);
+			while(Math.abs(Grados)>5) {
+				if (Grados<-250) {
+					Grados = Grados+360;
+				}
+				System.out.println(Grados);
+				//Button.waitForAnyPress();
+				pilot.rotate(Grados);
+				Grados =(m_caCompass.getDegreesCartesian()-180);
+			}
+			break;
+		case 'E':
+			Grados =(m_caCompass.getDegreesCartesian()+90);
+			System.out.println(Grados);
+			if (Grados>250) {
+				Grados = Grados-360;
+			}
+			while(Math.abs(Grados)>5) {
+				System.out.println(Grados);
+				//Button.waitForAnyPress();
+				pilot.rotate(Grados);
+				Grados =(m_caCompass.getDegreesCartesian()+90);
+			}
+			break;
+		}
+		
+		
+	}
+	
+	private String orientationToString(Orientation o) {
+		String s = "NONE";
+		switch (o) {
+		case EAST:
+			s = "EAST";
+			break;
+		case INVALID:
+			s = "INVALID";
+			break;
+		case NORTH:
+			s = "NORTH";
+			break;
+		case SOUTH:
+			s = "SOUTH";
+			break;
+		case WEST:
+			s = "WEST";
+			break;
+		}
+		return s;
+	}
+	
+}

src/ve/ucv/ciens/cicore/concisa/control/UltraSoundChecker.java

@@ -0,0 +1,69 @@
+package ve.ucv.ciens.cicore.concisa.control;
+
+import lejos.nxt.UltrasonicSensor;
+
+public class UltraSoundChecker {
+	public static final int WALL_DISTANCE_CM = 20;
+	public static final int WALL_TOL_CM = 2;
+	
+	private UltrasonicSensor m_usFront;
+	private UltrasonicSensor m_usRight;
+	private UltrasonicSensor m_usLeft;
+
+	public UltraSoundChecker(UltrasonicSensor front, UltrasonicSensor right, UltrasonicSensor left) {
+		m_usFront = front;
+		m_usRight = right;
+		m_usLeft = left;
+	}
+
+	public boolean Derecha() throws Exception {
+		int ret = m_usRight.ping();
+		
+		if (ret != 0)
+			throw new Exception("exDER: " + ret);
+		
+		try {
+			Thread.sleep(100);
+		} catch (InterruptedException e) { }
+		
+		//if (Math.abs(WALL_DISTANCE_CM - m_usRight.getDistance()) > WALL_TOL_CM)
+		if (m_usRight.getDistance() < WALL_DISTANCE_CM)
+			return false;
+		else
+			return true;
+	}
+	
+	public boolean Izquierda() throws Exception {
+		int ret = m_usLeft.ping();
+		
+		if (ret != 0)
+			throw new Exception("exIZQ: " + ret);
+		
+		try {
+			Thread.sleep(100);
+		} catch (InterruptedException e) { }
+		
+		//if (Math.abs(WALL_DISTANCE_CM - m_usLeft.getDistance()) > WALL_TOL_CM)
+		if (m_usLeft.getDistance() < WALL_DISTANCE_CM)
+			return false;
+		else
+			return true;
+	}
+	
+	public boolean Frente() throws Exception {
+		int ret = m_usFront.ping();
+		
+		if (ret != 0)
+			throw new Exception("exFRN: " + ret);
+		
+		try {
+			Thread.sleep(100);
+		} catch (InterruptedException e) { }
+		
+		//if (Math.abs(WALL_DISTANCE_CM - m_usFront.getDistance()) > WALL_TOL_CM)
+		if (m_usFront.getDistance() < WALL_DISTANCE_CM)
+			return false;
+		else
+			return true;
+	}
+}

src/ve/ucv/ciens/cicore/concisa/subsumption/BaseBehavior.java

@@ -0,0 +1,28 @@
+package ve.ucv.ciens.cicore.concisa.subsumption;
+
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.subsumption.Behavior;
+
+public abstract class BaseBehavior implements Behavior {
+	protected UltrasonicSensor m_usFront;
+	protected UltrasonicSensor m_usRight;
+	protected UltrasonicSensor m_usLeft;
+	protected CompassHTSensor  m_csCompass;
+	
+	public BaseBehavior(UltrasonicSensor front, UltrasonicSensor right, UltrasonicSensor left, CompassHTSensor compass) {
+		m_usFront = front;
+		m_usRight = right;
+		m_usLeft = left;
+		m_csCompass = compass;
+	}
+	
+	@Override
+	public abstract boolean takeControl();
+
+	@Override
+	public abstract void action();
+
+	@Override
+	public abstract void suppress();
+}

src/ve/ucv/ciens/cicore/concisa/subsumption/SensorCalibrationBehavior.java

@@ -0,0 +1,55 @@
+package ve.ucv.ciens.cicore.concisa.subsumption;
+
+import lejos.nxt.Button;
+import lejos.nxt.Motor;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import lejos.robotics.navigation.DifferentialPilot;
+import ve.ucv.ciens.cicore.concisa.control.LabyrinthPilot;
+
+public class SensorCalibrationBehavior extends BaseBehavior {
+	private boolean sensorsCalibrated;
+
+	public SensorCalibrationBehavior(UltrasonicSensor front, UltrasonicSensor right, UltrasonicSensor left, CompassHTSensor compass) {
+		super(front, right, left, compass);
+		sensorsCalibrated = false;
+	}
+
+	@Override
+	public boolean takeControl() {
+		return !sensorsCalibrated;
+	}
+
+	@Override
+	public void action() {
+		System.out.println("To calibrate press enter");
+		Button.ENTER.waitForPress();
+		
+		/* Calibrate the compass by turning slowly by 720 degrees. */
+		System.out.println("Calib. compass");
+		DifferentialPilot p = new DifferentialPilot(LabyrinthPilot.WHEEL_DIAMETER_MM, LabyrinthPilot.TRACK_WIDTH_MM, Motor.C, Motor.A);
+		p.setRotateSpeed(20);
+		m_csCompass.startCalibration();
+		p.rotate(855, false);
+		m_csCompass.stopCalibration();
+		
+		System.out.println("Face north and press enter");
+		Button.ENTER.waitForPress();
+		m_csCompass.resetCartesianZero();
+
+		sensorsCalibrated = true;
+		
+		System.out.println("Calib. ready.");
+		Button.waitForAnyPress();
+		
+		for (int i = 0; i < 3; i++) {
+			try {
+				Thread.sleep(1000);
+			} catch (InterruptedException e) { }
+			System.out.println(3 - i);
+		}
+	}
+
+	@Override
+	public void suppress() { }
+}

src/ve/ucv/ciens/cicore/concisa/subsumption/SolveLabyrinthBehavior.java

@@ -0,0 +1,171 @@
+package ve.ucv.ciens.cicore.concisa.subsumption;
+
+import java.util.Stack;
+
+import lejos.nxt.Button;
+import lejos.nxt.UltrasonicSensor;
+import lejos.nxt.addon.CompassHTSensor;
+import ve.ucv.ciens.cicore.concisa.control.LabyrinthPilot;
+import ve.ucv.ciens.cicore.concisa.control.UltraSoundChecker;
+
+
+public class SolveLabyrinthBehavior extends BaseBehavior {
+	private UltraSoundChecker m_scSensors;
+	private LabyrinthPilot m_lpMonkey;
+	private Stack<Casilla> Pila;
+	private byte PosX;
+	private byte PosY;
+	public static int FN;
+	public static int FO;
+	public char R_orientacion;
+	public LabyrinthPilot.Orientation or; 
+
+	public SolveLabyrinthBehavior(UltrasonicSensor front, UltrasonicSensor right, UltrasonicSensor left, CompassHTSensor compass) {
+		super(front, right, left, compass);
+		m_scSensors = new UltraSoundChecker(front, right, left);
+		m_lpMonkey = new LabyrinthPilot(compass);
+		Pila = new Stack<Casilla>();
+		PosX = 1;
+		PosY = 1;
+		FN = 0;
+		FO = 0;
+		R_orientacion='0';
+		or = LabyrinthPilot.Orientation.INVALID;
+	}
+
+	@Override
+	public boolean takeControl() {
+		return true;
+	}
+
+	@Override
+	public void action() {
+		/*try {
+			//System.out.println("Frente " + m_scSensors.Frente());
+			//System.out.println("Izquierda " + m_scSensors.Izquierda());
+			//System.out.println("Derecha " + m_scSensors.Derecha());
+			System.out.println(m_caCompass.getDegreesCartesian());
+		} catch (Exception e) {	
+			e.printStackTrace();
+		}*/
+		try {
+			char Orientacion = 'N';
+			Casilla Actual = new Casilla(m_scSensors.Frente(), m_scSensors.Izquierda(), m_scSensors.Derecha(), false, PosX, PosY, Orientacion);
+			Casilla Siguiente = new Casilla(false, false, false, false, (byte)0, (byte)0, 'N');
+			Pila.push(Actual);
+
+			while (FN != 9 || FO != 4) {
+
+
+				Actual = Pila.peek();
+
+				System.out.println(Actual.Pmovimiento[0]);
+				System.out.println(Actual.Pmovimiento[1]);
+				System.out.println(Actual.Pmovimiento[2]);
+				System.out.println(Actual.Pmovimiento[3]);
+				System.out.println(Actual.Orientacion);
+				Button.waitForAnyPress();
+				if (Actual.Pmovimiento[0]) {
+					m_lpMonkey.Mover(LabyrinthPilot.Orientation.NORTH);
+					Orientacion = 'N';
+					Pila.peek().Pmovimiento[0] = false;
+//					Siguiente.Pmovimiento[0] = m_scSensors.Frente();
+//					Siguiente.Pmovimiento[1] = m_scSensors.Izquierda();
+//					Siguiente.Pmovimiento[2] = m_scSensors.Derecha();
+//					Siguiente.Pmovimiento[3] = false;
+//					Siguiente.PosXCa = PosX;
+//					Siguiente.PosYca = PosY;
+//					Siguiente.Orientacion = Orientacion;
+					Siguiente = new Casilla(m_scSensors.Frente(), m_scSensors.Izquierda(), m_scSensors.Derecha(), false, PosX, PosY, Orientacion);
+					Pila.push(Siguiente);
+				} else {
+					if (Actual.Pmovimiento[1]) {
+						m_lpMonkey.Mover(LabyrinthPilot.Orientation.WEST);
+						Orientacion = 'O';
+						Pila.peek().Pmovimiento[1] = false;
+//						Siguiente.Pmovimiento[0] = m_scSensors.Derecha();
+//						Siguiente.Pmovimiento[1] = m_scSensors.Frente();
+//						Siguiente.Pmovimiento[2] = false;
+//						Siguiente.Pmovimiento[3] = m_scSensors.Izquierda();
+//						Siguiente.PosXCa = PosX;
+//						Siguiente.PosYca = PosY;
+//						Siguiente.Orientacion = Orientacion;
+						Siguiente = new Casilla(m_scSensors.Derecha(), m_scSensors.Frente(), false, m_scSensors.Izquierda(), PosX, PosY, Orientacion);
+						Pila.push(Siguiente);
+					} else {
+						if (Actual.Pmovimiento[2]) {
+							m_lpMonkey.Mover(LabyrinthPilot.Orientation.EAST);
+							Orientacion = 'E';
+							Pila.peek().Pmovimiento[2] = false;
+//							Siguiente.Pmovimiento[0] = m_scSensors.Izquierda();
+//							Siguiente.Pmovimiento[1] = false;
+//							Siguiente.Pmovimiento[2] = m_scSensors.Frente();
+//							Siguiente.Pmovimiento[3] = m_scSensors.Derecha();
+//							Siguiente.PosXCa = PosX;
+//							Siguiente.PosYca = PosY;
+//							Siguiente.Orientacion = Orientacion;
+							Siguiente = new Casilla(m_scSensors.Izquierda(),false, m_scSensors.Frente(), m_scSensors.Derecha(), PosX, PosY, Orientacion);
+							Pila.push(Siguiente);
+						} else {
+							if (Actual.Pmovimiento[3]) {
+								m_lpMonkey.Mover(LabyrinthPilot.Orientation.SOUTH);
+								Orientacion = 'S';
+								Pila.peek().Pmovimiento[3] = false;
+//								Siguiente.Pmovimiento[0] = false;
+//								Siguiente.Pmovimiento[1] = m_scSensors.Derecha();
+//								Siguiente.Pmovimiento[2] = m_scSensors.Izquierda();
+//								Siguiente.Pmovimiento[3] = m_scSensors.Frente();
+//								Siguiente.PosXCa = PosX;
+//								Siguiente.PosYca = PosY;
+//								Siguiente.Orientacion = Orientacion;
+								Siguiente = new Casilla(false,m_scSensors.Derecha(), m_scSensors.Izquierda(), m_scSensors.Frente(), PosX, PosY, Orientacion);
+								Pila.push(Siguiente);
+							} else {
+								Orientacion = Actual.Orientacion;
+								System.out.println(Pila.size());
+								Actual = Pila.pop();
+								//Actual = Pila.pop();
+								System.out.println(Pila.size());
+								R_orientacion = Pila.peek().Orientacion;
+
+								if (Orientacion == 'N') or = LabyrinthPilot.Orientation.NORTH;
+								else if (Orientacion == 'E') or = LabyrinthPilot.Orientation.EAST;
+								else if (Orientacion == 'O') or = LabyrinthPilot.Orientation.WEST;
+								else if (Orientacion == 'S') or = LabyrinthPilot.Orientation.SOUTH;
+								System.out.println("AQUI" + R_orientacion);
+								m_lpMonkey.Regresar(or, R_orientacion);
+							}
+						}
+					}
+				}
+			}
+		} catch (Exception e) {
+			e.printStackTrace();
+			Button.waitForAnyPress();
+			System.exit(1);
+		} 
+	}
+
+	@Override
+	public void suppress() {
+		/* Nothing */
+	}
+
+	class Casilla {
+		public boolean Pmovimiento[];
+		public byte PosXCa, PosYca;
+		public char Orientacion;
+
+		public Casilla(boolean norte, boolean oeste, boolean este, boolean sur, byte x, byte y, char direccion) {
+			Pmovimiento = new boolean[4];
+
+			Pmovimiento[0] = norte;
+			Pmovimiento[1] = oeste;
+			Pmovimiento[2] = este;
+			Pmovimiento[3] = sur;
+			PosXCa = x;
+			PosYca = y;
+			Orientacion = direccion;
+		}
+	}
+}

src/ve/ucv/ciens/cicore/concisa/utils/CompassAbstraction.java

@@ -0,0 +1,29 @@
+package ve.ucv.ciens.cicore.concisa.utils;
+
+import lejos.nxt.addon.CompassHTSensor;
+
+public class CompassAbstraction {
+	private CompassHTSensor m_csCompass;
+	
+	public CompassAbstraction(CompassHTSensor compass) {
+		m_csCompass = compass;
+	}
+	
+	public void setNorth() {
+		m_csCompass.resetCartesianZero();
+	}
+	
+	public float getDegreesCartesian() {
+		float measure = m_csCompass.getDegreesCartesian();
+		System.out.println("Comp: " + measure);
+		
+		if (measure >= 0.0f && measure <= 180.0f) {
+			System.out.println("WEST: " + measure);
+			return measure;
+		} else {
+			System.out.println("EAST: " + (measure - 360.0f));
+			return measure - 360.0f;
+		}
+	}
+
+}

src/ve/ucv/ciens/cicore/concisa/utils/QuitButtonListener.java

@@ -0,0 +1,58 @@
+package ve.ucv.ciens.cicore.concisa.utils;
+
+import lejos.nxt.Button;
+import lejos.nxt.ButtonListener;
+
+public class QuitButtonListener implements ButtonListener {
+	private TimeCounter m_tcCounter;
+
+	public QuitButtonListener() {
+		m_tcCounter = null;
+	}
+	
+	@Override
+	public void buttonPressed(Button b) {
+		m_tcCounter = new TimeCounter();
+		m_tcCounter.start();
+	}
+
+	@Override
+	public void buttonReleased(Button b) {
+		if (m_tcCounter != null) {
+			m_tcCounter.finish();
+			m_tcCounter = null;
+		}
+	}
+
+	class TimeCounter extends Thread {
+		private boolean done;
+		private long timeMilisBefore;
+		
+		public TimeCounter() {
+			done = false;
+		}
+		
+		@Override
+		public void run() {
+			long m_lTimeMilisNow;
+			
+			timeMilisBefore = System.currentTimeMillis();
+			
+			while (!done) {
+				try {
+					Thread.sleep(100);
+				} catch (InterruptedException e) {
+				}
+				
+				m_lTimeMilisNow = System.currentTimeMillis();
+				
+				if (m_lTimeMilisNow - timeMilisBefore > 3000)
+					System.exit(0);
+			}
+		}
+		
+		public void finish() {
+			done = true;
+		}
+	}
+}