% at higher speeds.\r
+variables:\r
lSensor, rSensor, fSensor, bSensor (object).\r
- lWheel,rWheel (object). \r
+ lfWheel,rfWheel,lbWheel,rbWheel (object).\r
+ tleft,tright (int). \r
\r
+ avanzando,retrocediendo,girando(int). \r
+ \r
+ \r
- to get-density:\r
return 20.0.\r
\r
return 0.8.\r
\r
+ to set-global-velocity to velocity (float):\r
- rWheel set-velocity to velocity.\r
- lWheel set-velocity to velocity.\r
+ rfWheel set-velocity to velocity.\r
+ lfWheel set-velocity to velocity.\r
+ rbWheel set-velocity to velocity.\r
+ lbWheel set-velocity to velocity.\r
\r
+ to get-global-velocity:\r
- return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.\r
+ return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.\r
\r
- + to turn-right with-velocity velocity (float): \r
- lWheel set-velocity to velocity.\r
- rWheel set-velocity to -velocity.\r
+ + to turn-right: \r
+ tright++.\r
+\r
+ self rotate around-axis (0,1,0) by (-1.5709/10)*tright. \r
+ \r
+ if(tright==10): tright=0.\r
+\r
+\r
+ + to turn-left:\r
+ tleft++.\r
+\r
+ self rotate around-axis (0,1,0) by (1.5709/10)*tleft. \r
+ \r
+ if(tleft==10): tleft=0.\r
\r
- + to turn-left with-velocity velocity (float):\r
-# vehicle rotate around-axis (0,1,0) by 1. \r
- lWheel set-velocity to -velocity. \r
- rWheel set-velocity to velocity.\r
\r
+ to get-sensor-value:\r
return (fSensor get-sensor-value).\r
rSensor set-id at 4.\r
rSensor set-body at self.\r
\r
- lWheel = (self add-wheel at (0, 0, -1.5)).\r
- rWheel = (self add-wheel at (0, 0, 1.5)).\r
- \r
- +to iterate: \r
- #+ to post-iterate:\r
+ lfWheel = (self add-wheel at (2, 0, -1.5)).\r
+ lbWheel = (self add-wheel at (-2, 0, -1.5)).\r
+ rfWheel = (self add-wheel at (2, 0, 1.5)).\r
+ rbWheel = (self add-wheel at (-2, 0, 1.5)).\r
+\r
+ tleft=tright=0. #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!\r
+ avanzando=1.\r
+ retrocediendo=0.\r
+ girando=0. \r
+ \r
+ +to iterate:\r
valuef,valueb,valuer,valuel (float).\r
fl, fr(float).\r
\r
valueb=bSensor get-data.\r
valuel=lSensor get-data.\r
valuer=rSensor get-data.\r
+ \r
+ print "senforl: $valuel".\r
\r
- self turn-left with-velocity(20).\r
- self set-global-velocity to (15).\r
- if valuef >7: \r
- self set-global-velocity to (15).\r
- else if (valuef <=7) && (valuef > 0):\r
- { \r
- self set-global-velocity to (0).\r
- #self turn-left with-velocity(2).\r
- #self turn-right with-velocity(2).\r
- #self set-global-velocity to (0).\r
- }\r
- #print "sensor valuef: $valuef valueb: $valueb".\r
- \r
- #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.\r
- #else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1).\r
+ if(avanzando):\r
+ if(valuef>15):\r
+ self set-global-velocity to (15).\r
+ else \r
+ { \r
+ avanzando=0.\r
+ retrocediendo=1.\r
+ } \r
+\r
+\r
+ if(retrocediendo):\r
+ if(valueb>15):\r
+ self set-global-velocity to (-15).\r
+ else \r
+ { \r
+ tleft=1.\r
+ retrocediendo=0.\r
+ }\r
+\r
+\r
+ if((tleft) && (valuel>10)): \r
+ {\r
+ #self set-global-velocity to (0).\r
+ self turn-left.\r
+ }\r
+ else\r
+ {\r
+ avanzando=1.\r
+ #tright=1.\r
+ }\r
\r
- #fl = (flWheel get-velocity).\r
- #fr = (frWheel get-velocity).\r
- #print " sensorf: $value sensorb $valueb, fr: $fr, fl: $fl". \r
+#!\r
+ if((tright) && (valuer>10)): \r
+ {\r
+ #self set-global-velocity to (0).\r
+ self turn-right.\r
+ }\r
+ else\r
+ {\r
+ avanzando=1.\r
+ #tright=1.\r
+ } \r
+!# \r
\r
}\r
\r
v,obs(vector).\r
aux(float).\r
j (int).\r
- des1,des2,des3(int).\r
+ des2,des3(int).\r
wallBegin,wallEnd,wallCenter (float).\r
\r
- aux1,aux2,aux3,aux4 (float).\r
- yo,toObstacle, transDir (vector).\r
+ toObstacle(vector).\r
largeWall (float).\r
- source,destiny (vector).\r
+ \r
obsLoc (vector). \r
location (vector).\r
- posObstacle,posSensor (vector).\r
+ posObstacle,destiny,yo(vector).\r
\r
draw clear.\r
value = 0.0.\r
des3=0.\r
\r
} \r
-\r
- aux1=dot((self get-location),(i get-direction)).\r
-\r
- #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
- #print "dist: $aux1 begin: $wallBegin end: $wallEnd ".\r
- #print "begin: $wallBegin end: $wallEnd".\r
-\r
\r
- if ((i get-location)::z==20):\r
- print "------>sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo des2: $des2 des1: $des1". \r
-\r
- #print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".\r
if ((des2) && (des3)):\r
{ \r
- draw clear. \r
- #print " posObstacle: $posObstacle". \r
- \r
- #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".\r
- #print "dist: $aux1 begin: $wallBegin end: $wallEnd ".\r
-\r
-\r
- \r
-\r
+ draw clear. \r
\r
dist=|obsLoc - (self get-location)|.\r
+\r
if( (j==0) || (min>dist) ):\r
{\r
min=dist.\r
obs=obsLoc.\r
+ \r
j++.\r
- print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". \r
+ #print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ". \r
}\r
\r
} \r
#Dibujo el laser\r
draw set-color to (1, 0, 0).\r
draw draw-line from (self get-location) to (obs).\r
- return dist.\r
+ return min.\r
}\r
\r
\r
projects / z.facultad / 75.68 / celdas.git / blobdiff