X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/cf47fee21741c00d17387e1c642056705bc1269b..22822a747c21df2f9d99acd8dd2f3f3a9ea54a9a:/trunk/src/breve/Celdas.tz?ds=inline

diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz
index 6ca2d6c..481c28c 100644
--- a/trunk/src/breve/Celdas.tz
+++ b/trunk/src/breve/Celdas.tz
@@ -6,9 +6,9 @@
 @use Drawing.
 @use SistemaAutonomo.
 
-@define CELDAS_MAX_VELOCITY 30.
-@define CELDAS_TURNO 100.
-@define CELDAS_SENSOR_THRESHOLD 10.
+@define CELDAS_MAX_VELOCITY 5.
+@define CELDAS_TURNO 30.
+@define CELDAS_SENSOR_THRESHOLD 6.
 
 PhysicalControl : CeldasControl {
 	% This class is used for building simple vehicle 
@@ -206,19 +206,30 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 
 	+ 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 == 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 == 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).
@@ -289,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".
+		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.
@@ -313,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".
+		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.
@@ -335,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.
 
@@ -399,6 +435,10 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 					girando_der = 1.
 				}
 			}
+			else
+			{
+				plan-finished = 1.
+			}
 		}
 
 		# Validación de teoría
@@ -425,16 +465,6 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 			iterate++.
 		}
 
-		# Movimiento del robot
-		if (avanzando):
-			self set-global-velocity to (15).
-		if (retrocediendo):
-			self set-global-velocity to (-15).
-		if (girando_izq):
-			self turn-left.
-		if (girando_der):
-			self turn-right.
-
 }
 
 Stationary : CeldasObstacle (aka CeldasObstacles) {
@@ -602,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.
+			
 			}