From cf47fee21741c00d17387e1c642056705bc1269b Mon Sep 17 00:00:00 2001
From: Leandro Lucarella <llucax@gmail.com>
Date: Mon, 18 Dec 2006 02:36:26 +0000
Subject: [PATCH] =?utf8?q?Varios=20cambios=20para=20terminar=20de=20integr?=
 =?utf8?q?ar.=20Ya=20est=C3=A1=20casi=20andando=20perfecto,=20s=C3=B3lo=20?=
 =?utf8?q?hay=20algunos=20comportamientos=20raros=20pero=20para=20hacer=20?=
 =?utf8?q?el=20informe=20se=20puede=20usar=20perfecto.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=utf8
Content-Transfer-Encoding: 8bit

---
 trunk/src/breve/Celdas.tz     | 68 +++++++++++++++++++++++------------
 trunk/src/sistemaautonomo.cpp | 10 ++++++
 2 files changed, 55 insertions(+), 23 deletions(-)

diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz
index ed8532c..6ca2d6c 100644
--- a/trunk/src/breve/Celdas.tz
+++ b/trunk/src/breve/Celdas.tz
@@ -7,9 +7,8 @@
 @use SistemaAutonomo.
 
 @define CELDAS_MAX_VELOCITY 30.
-@define CELDAS_TURNO 80.
-@define CELDAS_SENSOR_THRESHOLD 9.
-@define CELDAS_VECTOR_ERROR 0.5.
+@define CELDAS_TURNO 100.
+@define CELDAS_SENSOR_THRESHOLD 10.
 
 PhysicalControl : CeldasControl {
 	% This class is used for building simple vehicle 
@@ -172,7 +171,9 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		teoria (object).
 		entorno (hash).
 		datos-finales (hash).
-		plan_finished (int).
+		plan-finished (int).
+		posicion-inicial (vector).
+		posicion-final (vector).
         
 	- to get-density:
 		return 20.0.
@@ -183,15 +184,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 	- to get-wheel-radius:
 		return 0.8.
 
-	- to compare-vectors compare-vector vector1(vector) with-vector vector2(vector):
+	- to near position thePosition (vector) with-error error (float):
 		vectorAux(vector).
-		vectorAux=vector1-vector2.
+		vectorAux = (self get-location) - thePosition.
 
-		if ((|vectorAux::x|<CELDAS_VECTOR_ERROR) && (|vectorAux::y|<CELDAS_VECTOR_ERROR) && (|vectorAux::z|<CELDAS_VECTOR_ERROR))
+		#print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error.
+
+		if ((|vectorAux::x| < error) && (|vectorAux::z| < error)):
 			return 1.
 
 		return 0.
-		
+
         + to set-global-velocity to velocity (float):
 		rfWheel set-velocity to velocity.
 		lfWheel set-velocity to velocity.
@@ -227,7 +230,6 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		entorno{"sensor_b"} = (bSensor get-sensor-value).
 		entorno{"sensor_r"} = (rSensor get-sensor-value).
 		entorno{"sensor_l"} = (lSensor get-sensor-value).
-		entorno{"movido"} = 0. # TODO
 		sa update-entorno with entorno.            
 
         +to init:
@@ -265,11 +267,14 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		girando_izq=0.            
 		girando_der=0.            
 
+		posicion-inicial = (self get-location).
+		posicion-final = (0, 0, 0).
+
 		# Configuracion de sistema autonomo
 		sa = new SistemaAutonomo.
 		sa init with-max-pasos 4 with-max-teorias 15.
 		iterate = 0.
-		plan_finished = 1. # así planificamos apenas empezamos
+		plan-finished = 1. # así planificamos apenas empezamos
 
 		teorias = 4 new Teorias.
 		teorias{0} init named "Avanzar" with-action "adelante".
@@ -329,30 +334,39 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		sa update-datos-finales with datos-finales.
 
         +to iterate:
+
+		# Actualiza entorno
 		self update-entorno.
 
-		if (0): # TODO posicion_final == posicion_actual
+		# Chequeo de objetivo
+		if (self near position posicion-final with-error 5.0):
 		{
 			print "Llegamos al FINAL!!!".
 			self set-global-velocity to 0.
 			return.
 		}
 
-		if (plan_finished):
+		# Planificación
+		if (plan-finished):
 		{
+			# Actualiza entorno indicando que no se movió para que
+			# el planificador actue
+			sa set-entorno value 0 with-name "movido".
 			sa plan. # Si no tenemos plan, lo hacemos
-			plan_finished = 0.
-			# TODO posicion_inicial = posicion_actual
+			plan-finished = 0.
+			iterate = 0.
 			if (! (sa has-next-theory)):
 			{
-				plan_finished = 1.
+				plan-finished = 1.
 				print "El planificador no encuentra PLAN!!!".
 				return.
 			}
 		}
 
+		# Ejecución de teoría
 		if (iterate == 0):
 		{
+			posicion-inicial = (self get-location).
 			if (sa has-next-theory):
 			{
 				teoria = sa get-next-theory.
@@ -387,21 +401,29 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 			}
 		}
 
+		# Validación de teoría
 		if (iterate == CELDAS_TURNO):
 		{
-			# TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true.
-			if (sa validate theory teoria):
-		       	{
+			# Actualiza entorno segun si se movio o no
+			if (self near position posicion-inicial with-error 1.0):
+			{
+				sa set-entorno value 0 with-name "movido".
 			}
 			else
 			{
-				plan_finished = 1.
+				sa set-entorno value 1 with-name "movido".
+			}
+			print iterate.
+			if (!(sa validate theory teoria)):
+			{
+				plan-finished = 1.
 			}
-		}
-
-		iterate++.
-		if (iterate == CELDAS_TURNO + 1):
 			iterate = 0.
+		}
+		else
+		{
+			iterate++.
+		}
 
 		# Movimiento del robot
 		if (avanzando):
diff --git a/trunk/src/sistemaautonomo.cpp b/trunk/src/sistemaautonomo.cpp
index 54b13cc..2c85e51 100644
--- a/trunk/src/sistemaautonomo.cpp
+++ b/trunk/src/sistemaautonomo.cpp
@@ -69,6 +69,9 @@ CTeoria* CSistemaAutonomo::get_next_theory()
 	}
 	else
 	{
+#ifdef DEBUG
+		std::cout << "SA: ejecuta teoria: " << **(curr_theory) << "\n";
+#endif // DEBUG
 		return *(curr_theory++);
 	}
 }
@@ -82,6 +85,10 @@ bool CSistemaAutonomo::validate_theory(CTeoria* t)
 	// Aumento k (cantidad de veces que se probó la teoría
 	++t->k;
 
+#ifdef DEBUG
+	std::cout << "SA: Entorno de verificacion:\n" << p_entorno->datos;
+#endif // DEBUG
+
 	// Verifico
 	result = verificar_condicion(t->datos_finales) ;
 
@@ -107,6 +114,9 @@ bool CSistemaAutonomo::validate_theory(CTeoria* t)
 		// Aplico heuristicas de correccion
 		this->heuristca_retraccion(*t) ;
 	}
+#ifdef DEBUG
+	else std::cout << "SA: Verifica!\n";
+#endif // DEBUG
 
 	return result;
 }
-- 
2.43.0