% vehicle is heavier and harder to control, but more stable\r
% at higher speeds.\r
+variables:\r
- lSensor, rSensor, fSensor, bSensor (object).\r
- lWheel,rWheel (object).\r
- tleft,tright (int). \r
+ lSensor, rSensor, fSensor, bSensor (object).\r
+ lfWheel,rfWheel,lbWheel,rbWheel (object).\r
+ tleft,tright (int). \r
iterate(int).\r
-\r
teorias (list).\r
sa (object).\r
teoria (object).\r
entorno (hash).\r
datos-finales (hash).\r
- \r
- \r
+ avanzando,retrocediendo,girando(int). \r
\r
- to get-density:\r
return 20.0.\r
- to get-wheel-radius:\r
return 0.8.\r
\r
- + to set-global-velocity to velocity=5 (float):\r
- rWheel set-velocity to velocity.\r
- lWheel set-velocity to velocity.\r
+ + to set-global-velocity to velocity (float):\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: \r
tright++.\r
+ to get-sensor-value:\r
return (fSensor get-sensor-value).\r
\r
+ +to update-entorno:\r
+ entorno{"sensor_f"} = (fSensor get-data).\r
+ entorno{"sensor_b"} = (bSensor get-data).\r
+ entorno{"sensor_r"} = (rSensor get-data).\r
+ entorno{"sensor_l"} = (lSensor get-data).\r
+ entorno{"movido"} = 0. # TODO\r
+ sa update-entorno entorno entorno. \r
+\r
+to init:\r
sa = new SistemaAutonomo.\r
iterate=0.\r
\r
- fSensor = (self add-sensor at (2.0, .4, 0)). \r
- fSensor set-direction to (1,0,0).\r
- #fSensor set-direction to (0,0,1).\r
- fSensor set-id at 1.\r
- fSensor set-body at self.\r
- bSensor = (self add-sensor at (-2.0, .4, 0)).\r
- bSensor set-direction to (-1,0,0).\r
- #bSensor set-direction to (0,0,1).\r
- bSensor set-id at 2.\r
- bSensor set-body at self.\r
- lSensor = (self add-sensor at (0, .4, 1.5)).\r
- lSensor set-direction to (0,0,1).\r
- #lSensor set-direction to (1,0,0).\r
- lSensor set-id at 3.\r
- lSensor set-body at self.\r
-\r
-\r
- rSensor = (self add-sensor at (0, .4, -1.5)).\r
- rSensor set-direction to (0,0,-1).\r
- #rSensor set-direction to (-1,0,0).\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
- tleft=tright=0. #Debe ser inicializado en 0 esta asi para probar!!!!!!!!!!!!!!!!!!!!!!!!\r
-\r
- entorno{"sensor_f"} = 0.\r
- entorno{"sensor_b"} = 0.\r
- entorno{"sensor_r"} = 0.\r
- entorno{"sensor_l"} = 0.\r
-\r
- entorno{"movido"} = 0.\r
- sa update-entorno entorno entorno. \r
- \r
-\r
teorias = 2 new Teorias.\r
- teorias{0} init named "avanzar" with-action "avanza".\r
- (teorias{0}) set-dato-inicial name "sensor_f" value ANY.\r
- (teorias{0}) set-dato-inicial name "sensor_b" value ANY.\r
- (teorias{0}) set-dato-inicial name "sensor_r" value ANY.\r
- (teorias{0}) set-dato-inicial name "sensor_l" value ANY.\r
- (teorias{0}) set-dato-inicial name "movido" value ANY.\r
- (teorias{0}) set-dato-final name "sensor_f" value ANY.\r
- (teorias{0}) set-dato-final name "sensor_b" value ANY.\r
- (teorias{0}) set-dato-final name "sensor_r" value ANY.\r
- (teorias{0}) set-dato-final name "sensor_l" value ANY.\r
- (teorias{0}) set-dato-final name "movido" value ANY.\r
-\r
- teorias{1} init named "avanzar2" with-action "avanza".\r
- (teorias{1}) set-dato-inicial name "sensor_f" value ANY.\r
- (teorias{1}) set-dato-inicial name "sensor_b" value ANY.\r
- (teorias{1}) set-dato-inicial name "sensor_r" value ANY.\r
- (teorias{1}) set-dato-inicial name "sensor_l" value ANY.\r
- (teorias{1}) set-dato-inicial name "movido" value ANY.\r
- (teorias{1}) set-dato-final name "sensor_f" value ANY.\r
- (teorias{1}) set-dato-final name "sensor_b" value ANY.\r
- (teorias{1}) set-dato-final name "sensor_r" value ANY.\r
- (teorias{1}) set-dato-final name "sensor_l" value ANY.\r
- (teorias{1}) set-dato-final name "movido" value ANY.\r
-\r
-\r
- sa add teoria (teorias{0}).\r
- sa add teoria (teorias{1}).\r
+ teorias{0} init named "Avanzar" with-action "avanzar".\r
+ teorias{0} set-dato-inicial name "sensor_f" value 0.\r
+ teorias{0} set-dato-inicial name "sensor_b" value ANY.\r
+ teorias{0} set-dato-inicial name "sensor_r" value ANY.\r
+ teorias{0} set-dato-inicial name "sensor_l" value ANY.\r
+ teorias{0} set-dato-inicial name "movido" value ANY.\r
+ teorias{0} set-dato-final name "sensor_f" value ANY.\r
+ teorias{0} set-dato-final name "sensor_b" value ANY.\r
+ teorias{0} set-dato-final name "sensor_r" value ANY.\r
+ teorias{0} set-dato-final name "sensor_l" value ANY.\r
+ teorias{0} set-dato-final name "movido" value 1.\r
+\r
+ teorias{1} init named "Rotar a derecha" with-action "derecha".\r
+ teorias{1} set-dato-inicial name "sensor_f" value 1.\r
+ teorias{1} set-dato-inicial name "sensor_b" value ANY.\r
+ teorias{1} set-dato-inicial name "sensor_r" value ANY.\r
+ teorias{1} set-dato-inicial name "sensor_l" value ANY.\r
+ teorias{1} set-dato-inicial name "movido" value ANY.\r
+ teorias{1} set-dato-final name "sensor_f" value 0.\r
+ teorias{1} set-dato-final name "sensor_b" value ANY.\r
+ teorias{1} set-dato-final name "sensor_r" value ANY.\r
+ teorias{1} set-dato-final name "sensor_l" value ANY.\r
+ teorias{1} set-dato-final name "movido" value 0.\r
+\r
+ sa add teoria teorias{0}.\r
+ sa add teoria teorias{1}.\r
\r
- datos-finales{"movido"} = 1.\r
-\r
- sa update-datos-finales datos-finales datos-finales.\r
- sa plan.\r
+ fSensor = (self add-sensor at (2.0, .4, 0)). \r
+ fSensor set-direction to (1,0,0).\r
+ #fSensor set-direction to (0,0,1).\r
+ fSensor set-id at 1.\r
+ fSensor set-body at self.\r
+ bSensor = (self add-sensor at (-2.0, .4, 0)).\r
+ bSensor set-direction to (-1,0,0).\r
+ #bSensor set-direction to (0,0,1).\r
+ bSensor set-id at 2.\r
+ bSensor set-body at self.\r
+ lSensor = (self add-sensor at (0, .4, 1.5)).\r
+ lSensor set-direction to (0,0,1).\r
+ #lSensor set-direction to (1,0,0).\r
+ lSensor set-id at 3.\r
+ lSensor set-body at self.\r
+\r
+\r
+ rSensor = (self add-sensor at (0, .4, -1.5)).\r
+ rSensor set-direction to (0,0,-1).\r
+ #rSensor set-direction to (-1,0,0).\r
+ rSensor set-id at 4.\r
+ rSensor set-body at self.\r
+\r
+\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
- \r
+to iterate:\r
- #+ to post-iterate:\r
valuef,valueb,valuer,valuel (float).\r
fl, fr(float).\r
\r
+ valuef=fSensor get-data.\r
+ valueb=bSensor get-data.\r
+ valuel=lSensor get-data.\r
+ valuer=rSensor get-data.\r
\r
\r
- if(iterate==0): {\r
- print "iteracion 0".\r
- if (sa has-next-theory):\r
- {\r
+ datos-finales{"movido"} = 1.\r
+ print "senforl: $valuel".\r
\r
- print "hay teoria".\r
- teoria = sa get-next-theory.\r
- if ((teoria get-accion) == "avanza"): {\r
- #entorno{"sensor"} = 1.\r
- #entorno{"movido"} = 1.\r
- self set-global-velocity.\r
- \r
- }\r
- if ((teoria get-accion) == "retrocede"): {\r
- self set-global-velocity to -5.\r
\r
+ sa update-datos-finales datos-finales datos-finales.\r
+ sa plan.\r
+\r
+ if (iterate==0): {\r
+ print "iteracion 0".\r
+ if (sa has-next-theory):\r
+ {\r
+\r
+ print "hay teoria".\r
+ teoria = sa get-next-theory.\r
+ if ((teoria get-accion) == "avanza"): {\r
+ #entorno{"sensor"} = 1.\r
+ #entorno{"movido"} = 1.\r
+ self set-global-velocity.\r
+\r
+ }\r
+ if ((teoria get-accion) == "retrocede"): {\r
+ self set-global-velocity to -5.\r
+\r
+ }\r
}\r
}\r
- }\r
\r
- if (iterate==100) && (teoria):\r
+ if ((iterate==100) && (teoria)):\r
{\r
\r
- entorno{"sensor_f"} = (fSensor get-data).\r
- entorno{"sensor_b"} = (bSensor get-data).\r
- entorno{"sensor_r"} = (rSensor get-data).\r
- entorno{"sensor_l"} = (lSensor get-data).\r
-\r
- sa update-entorno entorno entorno.\r
+ self update-entorno.\r
if (sa validate theory teoria): {\r
print "valida".\r
}\r
}\r
\r
}\r
- \r
- iterate++.\r
- if(iterate==101):\r
- iterate=0.\r
\r
+ iterate++.\r
+ if(iterate==101):\r
+ iterate=0.\r
}\r
\r
Stationary : CeldasObstacle (aka CeldasObstacles) {\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 "begin: $wallBegin end: $wallEnd ".\r
- #print "begin: $wallBegin end: $wallEnd".\r
-\r
\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 "begin: $wallBegin end: $wallEnd ".\r
- \r
+ draw clear.\r
\r
dist=|obsLoc - (self get-location)|.\r
if( (j==0) || (min>dist) ):\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
\r
\r
}\r
-
\ No newline at end of file
+ \r
projects / z.facultad / 75.68 / celdas.git / blobdiff