X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/5530730f6759a3c449df3e78f122084d16f825d6..HEAD:/trunk/src/breve/Celdas.tz

diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz
index ae29a51..481c28c 100644
--- a/trunk/src/breve/Celdas.tz
+++ b/trunk/src/breve/Celdas.tz
@@ -8,7 +8,7 @@
 
 @define CELDAS_MAX_VELOCITY 5.
 @define CELDAS_TURNO 30.
-@define CELDAS_SENSOR_THRESHOLD 10.
+@define CELDAS_SENSOR_THRESHOLD 6.
 
 PhysicalControl : CeldasControl {
 	% This class is used for building simple vehicle 
@@ -205,24 +205,31 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.
 
 	+ to turn-right:		
-		#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.
+		tright++.
 		self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. 
-		if (tright == CELDAS_TURNO): tright=0.
-		else tright++.
+#!
+		if (tright == CELDAS_TURNO):
+		{
+		  fSensor set-direction to (-1,0,0).
+		  bSensor set-direction to (1,0,0).
+		  lSensor set-direction to (0,0,-1).
+		  rSensor set-direction to (0,0,1).
+		}
+!#
 
 
 	+ to turn-left:
-		#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++.
-
+		tleft++.
+		self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. 
+#!
+		if (tleft == CELDAS_TURNO):
+		{
+	 	  fSensor set-direction to (-1,0,0).
+		  bSensor set-direction to (1,0,0).
+		  lSensor set-direction to (0,0,-1).
+		  rSensor set-direction to (0,0,1).
+		}
+!#
 
         + to get-sensor-value:
 		return (fSensor get-sensor-value).
@@ -293,7 +300,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" executed 2.
+		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.
@@ -317,7 +324,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" executed 2.
+		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.
@@ -339,6 +346,31 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 
         +to iterate:
 
+		# Movimiento del robot
+		if (avanzando):
+		{
+			self set-global-velocity to (CELDAS_MAX_VELOCITY).
+		}
+		if (retrocediendo):
+		{
+			self set-global-velocity to (-CELDAS_MAX_VELOCITY).
+		}
+		if (girando_izq):
+		{
+			self set-global-velocity to 0.
+			self turn-left.
+			#if (iterate): self rotate around-axis (0,1,0) by (1.570796/CELDAS_TURNO)*iterate.
+			print "izq: ", (1.570796/CELDAS_TURNO)*iterate.
+		}
+		if (girando_der):
+		{
+			self set-global-velocity to 0.
+			self turn-right.
+			#if (iterate): self rotate around-axis (0,1,0) by (-1.570796/CELDAS_TURNO)*iterate. 
+			print "der: ", (-1.570796/CELDAS_TURNO)*iterate.
+		}
+		#print "vel: ", (bodyLink get-velocity).
+
 		# Actualiza entorno
 		self update-entorno.
 
@@ -413,7 +445,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		if (iterate == CELDAS_TURNO):
 		{
 			# Actualiza entorno segun si se movio o no
-			if (self near position posicion-inicial with-error 2.0):
+			if (self near position posicion-inicial with-error 1.0):
 			{
 				sa set-entorno value 0 with-name "movido".
 			}
@@ -433,16 +465,6 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 			iterate++.
 		}
 
-		# Movimiento del robot
-		if (avanzando):
-			self set-global-velocity to (CELDAS_MAX_VELOCITY).
-		if (retrocediendo):
-			self set-global-velocity to (-CELDAS_MAX_VELOCITY).
-		if (girando_izq):
-			self turn-left.
-		if (girando_der):
-			self turn-right.
-
 }
 
 Stationary : CeldasObstacle (aka CeldasObstacles) {
@@ -610,13 +632,26 @@ Link : CeldasSensor (aka CeldasSensors) {
 			des2=0.
 			if(dot(direction,(1,1,1))<0):
 			{                        
-                            if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):
-					des2=1.      
+                            if(dot((body get-location),positiveDirection) > dot((self get-location),positiveDirection)):
+				{
+					if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):
+						des2=1.
+				}
+			    else
+			    if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):
+					des2=1.         
                         }
 			else
 			{
-                            if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):
+                            if(dot((body get-location),positiveDirection) < dot((self get-location),positiveDirection)):
+				{
+			     	if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):
 					des2=1.         
+				}
+			   else
+				if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):
+						des2=1.
+			
 			}