X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/e800bc42e811087efc35350d5bfaa97cd1926d88..3de9a6adae2fe5a00ec315ba34f08f6d91a5b265:/trunk/src/breve/Celdas.tz

diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz
index ce5d511..ae29a51 100644
--- a/trunk/src/breve/Celdas.tz
+++ b/trunk/src/breve/Celdas.tz
@@ -6,8 +6,9 @@
 @use Drawing.
 @use SistemaAutonomo.
 
-@define CELDAS_MAX_VELOCITY 30.
-@define CELDAS_TURNO 100.
+@define CELDAS_MAX_VELOCITY 5.
+@define CELDAS_TURNO 30.
+@define CELDAS_SENSOR_THRESHOLD 10.
 
 PhysicalControl : CeldasControl {
 	% This class is used for building simple vehicle 
@@ -55,7 +56,6 @@ MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) {
 		bodyLink (object).
 
 		wheels (list).
-		sensors (list).
 
 	+ to init:
 		bodyShape = new Shape.
@@ -171,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.
@@ -182,6 +184,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 	- to get-wheel-radius:
 		return 0.8.
 
+	- to near position thePosition (vector) with-error error (float):
+		vectorAux(vector).
+		vectorAux = (self get-location) - thePosition.
+
+		#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.
@@ -192,30 +205,35 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.
 
 	+ to turn-right:		
-		tright++.
-
+		#tright++.
+		#self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. 
+		#if (tright == CELDAS_TURNO): tright=0.
+		if (tright == 0): self set-global-velocity to 0.
 		self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. 
-			
 		if (tright == CELDAS_TURNO): tright=0.
+		else tright++.
 
 
 	+ to turn-left:
-		tleft++.
-
-		self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. 
-			
+		#tleft++.
+		#self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. 
+		#if (tleft == CELDAS_TURNO): tleft=0.
+		if (tleft == 0): self set-global-velocity to 0.
+		self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft.
 		if (tleft == CELDAS_TURNO): tleft=0.
+		else tleft++.
 
 
         + to get-sensor-value:
 		return (fSensor get-sensor-value).
 
+
+
 	+to update-entorno:
 		entorno{"sensor_f"} = (fSensor get-sensor-value).
 		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:
@@ -253,10 +271,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".
@@ -271,7 +293,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		teorias{0} set-dato-final name "sensor_l" value ANY.
 		teorias{0} set-dato-final name "movido" value 1.
 
-		teorias{1} init named "Retroceder" with-action "atras".
+		teorias{1} init named "Retroceder" with-action "atras" executed 2.
 		teorias{1} set-dato-inicial name "sensor_f" value 1.
 		teorias{1} set-dato-inicial name "sensor_b" value ANY.
 		teorias{1} set-dato-inicial name "sensor_r" value ANY.
@@ -295,7 +317,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		teorias{2} set-dato-final name "sensor_l" value 1.
 		teorias{2} set-dato-final name "movido" value 0.
 
-		teorias{3} init named "Rotar a izquierda" with-action "izquierda".
+		teorias{3} init named "Rotar a izquierda" with-action "izquierda" executed 2.
 		teorias{3} set-dato-inicial name "sensor_f" value 1.
 		teorias{3} set-dato-inicial name "sensor_b" value ANY.
 		teorias{3} set-dato-inicial name "sensor_r" value ANY.
@@ -316,34 +338,41 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		sa update-datos-finales with datos-finales.
 
         +to iterate:
-		fl, fr(float).
 
-		if (0): # TODO posicion_final == posicion_actual
+		# Actualiza entorno
+		self update-entorno.
+
+		# 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):
 		{
-			self update-entorno.
+			# 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):
 		{
-			print "iteracion 0".
+			posicion-inicial = (self get-location).
 			if (sa has-next-theory):
 			{
-				print "hay teoria".
 				teoria = sa get-next-theory.
 				if ((teoria get-accion) == "adelante"):
 				{
@@ -374,32 +403,41 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 					girando_der = 1.
 				}
 			}
+			else
+			{
+				plan-finished = 1.
+			}
 		}
 
+		# Validación de teoría
 		if (iterate == CELDAS_TURNO):
 		{
-			self update-entorno.
-			# TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true.
-			if (sa validate theory teoria):
-		       	{
-				print "valida".
+			# Actualiza entorno segun si se movio o no
+			if (self near position posicion-inicial with-error 2.0):
+			{
+				sa set-entorno value 0 with-name "movido".
 			}
 			else
 			{
-				print "Teoria no valida".
-				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):
-			self set-global-velocity to (15).
+			self set-global-velocity to (CELDAS_MAX_VELOCITY).
 		if (retrocediendo):
-			self set-global-velocity to (-15).
+			self set-global-velocity to (-CELDAS_MAX_VELOCITY).
 		if (girando_izq):
 			self turn-left.
 		if (girando_der):
@@ -526,7 +564,7 @@ Link : CeldasSensor (aka CeldasSensors) {
 		val (float).
 
 		val = self get-data.
-		if (val > 10): return 0.
+		if (val > CELDAS_SENSOR_THRESHOLD): return 0.
 		else           return 1.
         
         #+ to iterate:
@@ -535,16 +573,10 @@ Link : CeldasSensor (aka CeldasSensors) {
 		i (object).
 		min,dist (float).
                 v,obs(vector).
-		aux(float).
 		j (int).
 		des2,des3(int).
 		wallBegin,wallEnd,wallCenter (float).
-                
-		toObstacle(vector).
-                largeWall (float).
-                
                 obsLoc (vector).                
-                location (vector).
                 posObstacle,destiny,yo(vector).
                                              
            	draw clear.