From dd6e987c21309e6ab64d1a4425abc1d340176e6a Mon Sep 17 00:00:00 2001 From: Carlos Henriques Date: Thu, 12 Sep 2019 14:14:11 -0400 Subject: [PATCH] First commit --- .classpath | 6 + .gitignore | 83 +++++ .project | 23 ++ LICENSE | 19 ++ README.md | 8 + .../ciens/cicore/concisa/LabyrinthSolver.java | 41 +++ src/ve/ucv/ciens/cicore/concisa/Main.java | 13 + .../concisa/Prototype/LaberintoPrototipo.cpp | 312 ++++++++++++++++++ .../concisa/control/LabyrinthPilot.java | 229 +++++++++++++ .../concisa/control/UltraSoundChecker.java | 69 ++++ .../concisa/subsumption/BaseBehavior.java | 28 ++ .../SensorCalibrationBehavior.java | 55 +++ .../subsumption/SolveLabyrinthBehavior.java | 171 ++++++++++ .../concisa/utils/CompassAbstraction.java | 29 ++ .../concisa/utils/QuitButtonListener.java | 58 ++++ 15 files changed, 1144 insertions(+) create mode 100644 .classpath create mode 100644 .gitignore create mode 100644 .project create mode 100644 LICENSE create mode 100644 README.md create mode 100644 src/ve/ucv/ciens/cicore/concisa/LabyrinthSolver.java create mode 100644 src/ve/ucv/ciens/cicore/concisa/Main.java create mode 100644 src/ve/ucv/ciens/cicore/concisa/Prototype/LaberintoPrototipo.cpp create mode 100644 src/ve/ucv/ciens/cicore/concisa/control/LabyrinthPilot.java create mode 100644 src/ve/ucv/ciens/cicore/concisa/control/UltraSoundChecker.java create mode 100644 src/ve/ucv/ciens/cicore/concisa/subsumption/BaseBehavior.java create mode 100644 src/ve/ucv/ciens/cicore/concisa/subsumption/SensorCalibrationBehavior.java create mode 100644 src/ve/ucv/ciens/cicore/concisa/subsumption/SolveLabyrinthBehavior.java create mode 100644 src/ve/ucv/ciens/cicore/concisa/utils/CompassAbstraction.java create mode 100644 src/ve/ucv/ciens/cicore/concisa/utils/QuitButtonListener.java diff --git a/.classpath b/.classpath new file mode 100644 index 0000000..ece58ff --- /dev/null +++ b/.classpath @@ -0,0 +1,6 @@ + + + + + + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1c5c9bb --- /dev/null +++ b/.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 diff --git a/.project b/.project new file mode 100644 index 0000000..375e57b --- /dev/null +++ b/.project @@ -0,0 +1,23 @@ + + + CoNCISa Labyrinth + + + + + + org.eclipse.jdt.core.javabuilder + + + + + org.lejos.nxt.ldt.leJOSBuilder + + + + + + org.lejos.nxt.ldt.leJOSNature + org.eclipse.jdt.core.javanature + + diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..ded0533 --- /dev/null +++ b/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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..f48673b --- /dev/null +++ b/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. diff --git a/src/ve/ucv/ciens/cicore/concisa/LabyrinthSolver.java b/src/ve/ucv/ciens/cicore/concisa/LabyrinthSolver.java new file mode 100644 index 0000000..33cf385 --- /dev/null +++ b/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(); + } +} diff --git a/src/ve/ucv/ciens/cicore/concisa/Main.java b/src/ve/ucv/ciens/cicore/concisa/Main.java new file mode 100644 index 0000000..5c8942a --- /dev/null +++ b/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(); + } +} diff --git a/src/ve/ucv/ciens/cicore/concisa/Prototype/LaberintoPrototipo.cpp b/src/ve/ucv/ciens/cicore/concisa/Prototype/LaberintoPrototipo.cpp new file mode 100644 index 0000000..4820705 --- /dev/null +++ b/src/ve/ucv/ciens/cicore/concisa/Prototype/LaberintoPrototipo.cpp @@ -0,0 +1,312 @@ +#include +#include +#include +#include +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 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;} diff --git a/src/ve/ucv/ciens/cicore/concisa/control/LabyrinthPilot.java b/src/ve/ucv/ciens/cicore/concisa/control/LabyrinthPilot.java new file mode 100644 index 0000000..8b646b9 --- /dev/null +++ b/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; + } + +} diff --git a/src/ve/ucv/ciens/cicore/concisa/control/UltraSoundChecker.java b/src/ve/ucv/ciens/cicore/concisa/control/UltraSoundChecker.java new file mode 100644 index 0000000..3cbb88c --- /dev/null +++ b/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; + } +} diff --git a/src/ve/ucv/ciens/cicore/concisa/subsumption/BaseBehavior.java b/src/ve/ucv/ciens/cicore/concisa/subsumption/BaseBehavior.java new file mode 100644 index 0000000..feede55 --- /dev/null +++ b/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(); +} diff --git a/src/ve/ucv/ciens/cicore/concisa/subsumption/SensorCalibrationBehavior.java b/src/ve/ucv/ciens/cicore/concisa/subsumption/SensorCalibrationBehavior.java new file mode 100644 index 0000000..5a4d412 --- /dev/null +++ b/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() { } +} diff --git a/src/ve/ucv/ciens/cicore/concisa/subsumption/SolveLabyrinthBehavior.java b/src/ve/ucv/ciens/cicore/concisa/subsumption/SolveLabyrinthBehavior.java new file mode 100644 index 0000000..9d5943f --- /dev/null +++ b/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 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(); + 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; + } + } +} diff --git a/src/ve/ucv/ciens/cicore/concisa/utils/CompassAbstraction.java b/src/ve/ucv/ciens/cicore/concisa/utils/CompassAbstraction.java new file mode 100644 index 0000000..4883432 --- /dev/null +++ b/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; + } + } + +} diff --git a/src/ve/ucv/ciens/cicore/concisa/utils/QuitButtonListener.java b/src/ve/ucv/ciens/cicore/concisa/utils/QuitButtonListener.java new file mode 100644 index 0000000..5ef3f40 --- /dev/null +++ b/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; + } + } +}