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

diff --git a/trunk/src/breve/robot/Celdas-2-6.tz b/trunk/src/breve/robot/Celdas-2-6.tz
index e0d352c..1e3f9a6 100644
--- a/trunk/src/breve/robot/Celdas-2-6.tz
+++ b/trunk/src/breve/robot/Celdas-2-6.tz
@@ -160,7 +160,9 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 	% at higher speeds.
         +variables:
             lSensor, rSensor, fSensor, bSensor (object).
-            lWheel,rWheel (object).        
+            lWheel,rWheel (object).
+	    tleft,tright (int).	    
+        
         
 	- to get-density:
 		return 20.0.
@@ -178,14 +180,21 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 	+ to get-global-velocity:
 		return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.
 
-	+ to turn-right with-velocity velocity (float):		
-		lWheel set-velocity to velocity.
-                rWheel set-velocity to -velocity.
+	+ to turn-right:		
+		tright++.
+
+		self rotate around-axis (0,1,0) by (-1.5709/10)*tright. 
+			
+		if(tright==10): tright=0.
+
+
+	+ to turn-left:
+		tleft++.
+
+		self rotate around-axis (0,1,0) by (1.5709/10)*tleft. 
+			
+		if(tleft==10): tleft=0.
 
-	+ to turn-left with-velocity velocity (float):
-#                vehicle rotate around-axis (0,1,0) by 1. 
-		lWheel set-velocity to -velocity.	
-		rWheel set-velocity to velocity.
 
         + to get-sensor-value:
 		return (fSensor get-sensor-value).
@@ -214,10 +223,12 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
             rSensor set-id at 4.
 	    rSensor set-body at self.
 
-            lWheel  = (self add-wheel at (0, 0, -1.5)).
-            rWheel  = (self add-wheel at (0, 0, 1.5)).
+            lWheel = (self add-wheel at (0, 0, -1.5)).
+            rWheel = (self add-wheel at (0, 0, 1.5)).
+
+	    tleft=tright=1.	#Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!
                         
-        +to iterate:            
+        +to iterate:
         #+ to post-iterate:
 		valuef,valueb,valuer,valuel (float).
 		fl, fr(float).
@@ -226,18 +237,24 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		valueb=bSensor get-data.
                 valuel=lSensor get-data.
                 valuer=rSensor get-data.
+		
+		
+		if(tright>0):
+			self turn-right.
+		
 
-		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):
-                {   
+
+		#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):
+                #{   
                     self set-global-velocity to (0).
                     #self turn-left with-velocity(2).
                     #self turn-right with-velocity(2).
                     #self set-global-velocity to (0).
-		}
+		#}
                 #print "sensor valuef: $valuef  valueb: $valueb".
                 
                 #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.
@@ -374,16 +391,15 @@ Link : CeldasSensor (aka CeldasSensors) {
                 v,obs(vector).
 		aux(float).
 		j (int).
-		des1,des2,des3(int).
+		des2,des3(int).
 		wallBegin,wallEnd,wallCenter (float).
                 
-		aux1,aux2,aux3,aux4 (float).
-		yo,toObstacle, transDir (vector).
+		toObstacle(vector).
                 largeWall (float).
-                source,destiny (vector).
+                
                 obsLoc (vector).                
                 location (vector).
-                posObstacle,posSensor (vector).
+                posObstacle,destiny,yo(vector).
                                              
            	draw clear.
 		value = 0.0.
@@ -445,15 +461,13 @@ Link : CeldasSensor (aka CeldasSensors) {
                         	 
 			}				
 
- 			aux1=dot((self get-location),(i get-direction)).
+ 			#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 ".
+			#print "begin: $wallBegin end: $wallEnd ".
 			#print "begin: $wallBegin end: $wallEnd".
 
                        
-			if ((i get-location)::z==20):
-				print "------>sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo  des2: $des2 des1: $des1".	
 
 			#print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".
 			if ((des2) && (des3)):
@@ -461,20 +475,17 @@ Link : CeldasSensor (aka CeldasSensors) {
                         	draw clear.        	                
 				#print " posObstacle: $posObstacle".                        				
 				
-			#print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".
-			#print "dist: $aux1 begin: $wallBegin end: $wallEnd ".
-
-
+			print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".
+			print "begin: $wallBegin end: $wallEnd ".
 					
 
-
 				dist=|obsLoc - (self get-location)|.
 				if( (j==0) || (min>dist) ):
 				 {
  				 	min=dist.
 				 	obs=obsLoc.
 				 	j++.
-					print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".	
+					#print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".	
 				 }
 
 			 }