X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/09715c02260f695a67bda6667961b6736bb0a7a8..4564a91af300e428600a997b3da82bf4abf50011:/trunk/src/breve/robot/Celdas-2-5.tz

diff --git a/trunk/src/breve/robot/Celdas-2-5.tz b/trunk/src/breve/robot/Celdas-2-5.tz
index 8534014..ee56a6e 100644
--- a/trunk/src/breve/robot/Celdas-2-5.tz
+++ b/trunk/src/breve/robot/Celdas-2-5.tz
@@ -199,7 +199,6 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
             bSensor set-direction to (-1,0,0).
             bSensor set-id at 2.
 	    bSensor set-body at self.
-            #bSensor set-sensor-angle to (-1.6).
             lSensor = (self add-sensor at (0, .4, 1.5)).
             lSensor set-direction to (0,0,1).
             lSensor set-id at 3.
@@ -223,12 +222,10 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		valueb=bSensor get-data.
                 valuel=lSensor get-data.
                 valuer=rSensor get-data.
-                
-		#value = sensor get-data.
-                #value = self get-sensor-value.
-                #valueb = sensor2 get-sensor-value.
 
-		if valuef >7: 
+		self turn-left with-velocity(20).
+                self set-global-velocity to (15).
+		if valuef >7: 		
                     self set-global-velocity to (15).
                 else if (valuef <=7) && (valuef > 0):
                 {   
@@ -370,9 +367,10 @@ Link : CeldasSensor (aka CeldasSensors) {
 	+ to get-data:
 		i (object).
                 x,y,z (float).
-		a,b (float).
-                v(vector).
+		min,dist (float).
+                v,obs(vector).
 		aux(float).
+		j (int).
 		des1,des2,des3(int).
 		wallBegin,wallEnd (float).
                 
@@ -383,22 +381,22 @@ Link : CeldasSensor (aka CeldasSensors) {
                 obsLoc (vector).                
                 location (vector).
                 posObstacle,posSensor (vector).
-                angulo(double).
-                
-                               
-		transDir = (self get-rotation) * direction.
+                                             
            	draw clear.
 		value = 0.0.
-		foreach i in (all CeldasObstacles): {			                
-                                                
-                        posObstacle=i get-location.
-			#print "$i".
-			
-			
+		j=0.
+		min=0.
+		foreach i in (all CeldasObstacles): 
+			{
+			 posObstacle=i get-location.
+			 
+                                                		
+			#!
 			if(dot((i get-direction),direction)==0):
 				des1=1.
 			else
 				des1=0.
+			!#
 
 			des2=0.
 			if(dot(direction,(1,1,1))<0):
@@ -412,6 +410,7 @@ Link : CeldasSensor (aka CeldasSensors) {
 					des2=1.         
 			}			
 
+
 			#Compruebo que el robot este frente a la pared
 			wallBegin=dot((i get-location),(i get-direction) )- (i get-large)/2.
 			wallEnd=dot((i get-location),(i get-direction) )+ (i get-large)/2.		
@@ -421,10 +420,22 @@ Link : CeldasSensor (aka CeldasSensors) {
 			yo=self get-location.
 			destiny=i get-direction.
 
-			
+			v = (body get-location) - (self get-location ).
+			obsLoc::y=y=posObstacle::y.								
+				
+			if (dot((i get-direction),(1,0,0))):
+			{
+	        	 obsLoc::x=x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).
+                         obsLoc::z=z=posObstacle::z.
+			}				
+			else
+			{
+        	         obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).
+                         obsLoc::x=x=posObstacle::x.
+			}                       				
 
 
-			if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd):
+			if (dot((obsLoc),(i get-direction)) > wallBegin) && (dot((obsLoc),(i get-direction)) < wallEnd):
 			    	des3=1.
 			else
 			{
@@ -434,41 +445,41 @@ Link : CeldasSensor (aka CeldasSensors) {
 
  			aux1=dot((self get-location),(i get-direction)).
 
-			print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".
-			print "dist: $aux1 begin: $wallBegin end: $wallEnd".
-
-			v = (body get-location) - (self get-location ).
+			#print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".
+			#print "dist: $aux1 begin: $wallBegin end: $wallEnd".
 
                        
-
 			print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".
-			if ((des1) && (des2) && (des3)):
-                        {                                    
+			if ((des2) && (des3)):
+                         {                                    
                         	draw clear.        	                
-				#print " posObstacle: $posObstacle".                        
-				
-				obsLoc::y=y=posObstacle::y.								
-				
-				if (dot((i get-direction),(1,0,0))):
-				{
-	        	         obsLoc::x=x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).
-                        	 obsLoc::z=z=posObstacle::z.
-				}				
-				else
-				{
-        	        	 obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).
-                        	 obsLoc::x=x=posObstacle::x.
-				}                       	
-
-				#Dibujo el laser
-       
-	                        draw set-color to (1, 0, 0).
-        	                draw draw-line from (self get-location) to obsLoc.
-			}
+				#print " posObstacle: $posObstacle".                        				
+							
+				dist=|obsLoc - (self get-location)|.
+				if( (j==0) || (min>dist) ):
+				 {
+ 				 	min=dist.
+				 	obs=obsLoc.
+				 	j++.	
+				 }
+
+			 }						
+
                         
-		}
+		} #end for
+
+		if(j!=0):
+ 			{
+		  	  #Dibujo el laser
+       		  	  draw set-color to (1, 0, 0).
+        	  	  draw draw-line from (self get-location) to (obs).
+			  return dist.
+		 	}
+		
+
                 value = -1.
                 return value.
 
 
 }
+		
\ No newline at end of file