X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/d90679c0ecd281886d46682ec8831a2ac2e43021..586ad2d03cf73d7e75a1a2522db3e0c4eedd5e9c:/trunk/src/breve/robot/Celdas-2-6.tz?ds=inline

diff --git a/trunk/src/breve/robot/Celdas-2-6.tz b/trunk/src/breve/robot/Celdas-2-6.tz
index 1e3f9a6..9756afe 100644
--- a/trunk/src/breve/robot/Celdas-2-6.tz
+++ b/trunk/src/breve/robot/Celdas-2-6.tz
@@ -160,10 +160,12 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 	% at higher speeds.
         +variables:
             lSensor, rSensor, fSensor, bSensor (object).
-            lWheel,rWheel (object).
+            lfWheel,rfWheel,lbWheel,rbWheel (object).
 	    tleft,tright (int).	    
         
-        
+	    avanzando,retrocediendo,girando(int).	    
+	    
+	            
 	- to get-density:
 		return 20.0.
 
@@ -174,11 +176,13 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 		return 0.8.
 
         + to set-global-velocity to velocity (float):
-		rWheel set-velocity to velocity.
-		lWheel set-velocity to velocity.
+		rfWheel set-velocity to velocity.
+		lfWheel set-velocity to velocity.
+		rbWheel set-velocity to velocity.
+		lbWheel set-velocity to velocity.
 
 	+ to get-global-velocity:
-		return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.
+		return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.
 
 	+ to turn-right:		
 		tright++.
@@ -223,13 +227,17 @@ 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)).
+            lfWheel = (self add-wheel at (2, 0, -1.5)).
+            lbWheel = (self add-wheel at (-2, 0, -1.5)).
+            rfWheel = (self add-wheel at (2, 0, 1.5)).
+            rbWheel = (self add-wheel at (-2, 0, 1.5)).
 
-	    tleft=tright=1.	#Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!
-                        
+	    tleft=tright=0.	#Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!
+	    avanzando=1.
+	    retrocediendo=0.
+	    girando=0.            
+            
         +to iterate:
-        #+ to post-iterate:
 		valuef,valueb,valuer,valuel (float).
 		fl, fr(float).
             	
@@ -238,31 +246,51 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
                 valuel=lSensor get-data.
                 valuer=rSensor get-data.
 		
-		
-		if(tright>0):
-			self turn-right.
-		
+		print "senforl: $valuel".
 
+                if(avanzando):
+			if(valuef>15):
+				self set-global-velocity to (15).
+			else 
+				{		
+				 avanzando=0.
+				 retrocediendo=1.
+				}	
 
-		#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.
-		#else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1).
 
-		#fl = (flWheel get-velocity).
-		#fr = (frWheel get-velocity).
-		#print " sensorf: $value  sensorb $valueb, fr: $fr, fl: $fl".            
+               if(retrocediendo):
+			if(valueb>15):
+				self set-global-velocity to (-15).
+			else 
+				{		
+				 tleft=1.
+				 retrocediendo=0.
+				}
+
+
+		if((tleft) && (valuel>10)):						
+			{
+			 #self set-global-velocity to (0).
+			 self turn-left.
+			}
+		else
+			{
+			 avanzando=1.
+			 #tright=1.
+			}
+
+#!
+		if((tright) && (valuer>10)):						
+			{
+			 #self set-global-velocity to (0).
+			 self turn-right.
+			}
+		else
+			{
+			 avanzando=1.
+			 #tright=1.
+			}	
+!#	
             
 }
 
@@ -460,30 +488,18 @@ Link : CeldasSensor (aka CeldasSensors) {
 				 des3=0.
                         	 
 			}				
-
- 			#aux1=dot((self get-location),(i get-direction)).
-
-			#print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".
-			#print "begin: $wallBegin end: $wallEnd ".
-			#print "begin: $wallBegin end: $wallEnd".
-
                        
-
-			#print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".
 			if ((des2) && (des3)):
                          {                                    
-                        	draw clear.        	                
-				#print " posObstacle: $posObstacle".                        				
-				
-			print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".
-			print "begin: $wallBegin end: $wallEnd ".
-					
+                        	draw clear.        	                			
 
 				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 ".	
 				 }
@@ -498,7 +514,7 @@ Link : CeldasSensor (aka CeldasSensors) {
 		  	  #Dibujo el laser
        		  	  draw set-color to (1, 0, 0).
         	  	  draw draw-line from (self get-location) to (obs).
-			  return dist.
+			  return min.
 		 	}