@use Drawing.\r
@use SistemaAutonomo.\r
\r
-@define CELDAS_MAX_VELOCITY 30.\r
-@define CELDAS_TURNO 80.\r
-@define CELDAS_SENSOR_THRESHOLD 9.\r
-@define CELDAS_VECTOR_ERROR 0.5.\r
+@define CELDAS_MAX_VELOCITY 5.\r
+@define CELDAS_TURNO 30.\r
+@define CELDAS_SENSOR_THRESHOLD 6.\r
\r
PhysicalControl : CeldasControl {\r
% This class is used for building simple vehicle \r
teoria (object).\r
entorno (hash).\r
datos-finales (hash).\r
- plan_finished (int).\r
+ plan-finished (int).\r
+ posicion-inicial (vector).\r
+ posicion-final (vector).\r
\r
- to get-density:\r
return 20.0.\r
- to get-wheel-radius:\r
return 0.8.\r
\r
- - to compare-vectors compare-vector vector1(vector) with-vector vector2(vector):\r
+ - to near position thePosition (vector) with-error error (float):\r
vectorAux(vector).\r
- vectorAux=vector1-vector2.\r
+ vectorAux = (self get-location) - thePosition.\r
\r
- if ((|vectorAux::x|<CELDAS_VECTOR_ERROR) && (|vectorAux::y|<CELDAS_VECTOR_ERROR) && (|vectorAux::z|<CELDAS_VECTOR_ERROR))\r
+ #print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error.\r
+\r
+ if ((|vectorAux::x| < error) && (|vectorAux::z| < error)):\r
return 1.\r
\r
return 0.\r
- \r
+\r
+ to set-global-velocity to velocity (float):\r
rfWheel set-velocity to velocity.\r
lfWheel set-velocity to velocity.\r
\r
+ to turn-right: \r
tright++.\r
-\r
self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. \r
- \r
- if (tright == CELDAS_TURNO): tright=0.\r
+#!\r
+ if (tright == CELDAS_TURNO):\r
+ {\r
+ fSensor set-direction to (-1,0,0).\r
+ bSensor set-direction to (1,0,0).\r
+ lSensor set-direction to (0,0,-1).\r
+ rSensor set-direction to (0,0,1).\r
+ }\r
+!#\r
\r
\r
+ to turn-left:\r
tleft++.\r
-\r
self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. \r
- \r
- if (tleft == CELDAS_TURNO): tleft=0.\r
-\r
+#!\r
+ if (tleft == CELDAS_TURNO):\r
+ {\r
+ fSensor set-direction to (-1,0,0).\r
+ bSensor set-direction to (1,0,0).\r
+ lSensor set-direction to (0,0,-1).\r
+ rSensor set-direction to (0,0,1).\r
+ }\r
+!#\r
\r
+ to get-sensor-value:\r
return (fSensor get-sensor-value).\r
entorno{"sensor_b"} = (bSensor get-sensor-value).\r
entorno{"sensor_r"} = (rSensor get-sensor-value).\r
entorno{"sensor_l"} = (lSensor get-sensor-value).\r
- entorno{"movido"} = 0. # TODO\r
sa update-entorno with entorno. \r
\r
+to init:\r
girando_izq=0. \r
girando_der=0. \r
\r
+ posicion-inicial = (self get-location).\r
+ posicion-final = (0, 0, 0).\r
+\r
# Configuracion de sistema autonomo\r
sa = new SistemaAutonomo.\r
sa init with-max-pasos 4 with-max-teorias 15.\r
iterate = 0.\r
- plan_finished = 1. # así planificamos apenas empezamos\r
+ plan-finished = 1. # así planificamos apenas empezamos\r
\r
teorias = 4 new Teorias.\r
teorias{0} init named "Avanzar" with-action "adelante".\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 "Retroceder" with-action "atras".\r
+ teorias{1} init named "Retroceder" with-action "atras".# executed 2.\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{2} set-dato-final name "sensor_l" value 1.\r
teorias{2} set-dato-final name "movido" value 0.\r
\r
- teorias{3} init named "Rotar a izquierda" with-action "izquierda".\r
+ teorias{3} init named "Rotar a izquierda" with-action "izquierda". # executed 2.\r
teorias{3} set-dato-inicial name "sensor_f" value 1.\r
teorias{3} set-dato-inicial name "sensor_b" value ANY.\r
teorias{3} set-dato-inicial name "sensor_r" value ANY.\r
sa update-datos-finales with datos-finales.\r
\r
+to iterate:\r
+\r
+ # Movimiento del robot\r
+ if (avanzando):\r
+ {\r
+ self set-global-velocity to (CELDAS_MAX_VELOCITY).\r
+ }\r
+ if (retrocediendo):\r
+ {\r
+ self set-global-velocity to (-CELDAS_MAX_VELOCITY).\r
+ }\r
+ if (girando_izq):\r
+ {\r
+ self set-global-velocity to 0.\r
+ self turn-left.\r
+ #if (iterate): self rotate around-axis (0,1,0) by (1.570796/CELDAS_TURNO)*iterate.\r
+ print "izq: ", (1.570796/CELDAS_TURNO)*iterate.\r
+ }\r
+ if (girando_der):\r
+ {\r
+ self set-global-velocity to 0.\r
+ self turn-right.\r
+ #if (iterate): self rotate around-axis (0,1,0) by (-1.570796/CELDAS_TURNO)*iterate. \r
+ print "der: ", (-1.570796/CELDAS_TURNO)*iterate.\r
+ }\r
+ #print "vel: ", (bodyLink get-velocity).\r
+\r
+ # Actualiza entorno\r
self update-entorno.\r
\r
- if (0): # TODO posicion_final == posicion_actual\r
+ # Chequeo de objetivo\r
+ if (self near position posicion-final with-error 5.0):\r
{\r
print "Llegamos al FINAL!!!".\r
self set-global-velocity to 0.\r
return.\r
}\r
\r
- if (plan_finished):\r
+ # Planificación\r
+ if (plan-finished):\r
{\r
+ # Actualiza entorno indicando que no se movió para que\r
+ # el planificador actue\r
+ sa set-entorno value 0 with-name "movido".\r
sa plan. # Si no tenemos plan, lo hacemos\r
- plan_finished = 0.\r
- # TODO posicion_inicial = posicion_actual\r
+ plan-finished = 0.\r
+ iterate = 0.\r
if (! (sa has-next-theory)):\r
{\r
- plan_finished = 1.\r
+ plan-finished = 1.\r
print "El planificador no encuentra PLAN!!!".\r
return.\r
}\r
}\r
\r
+ # Ejecución de teoría\r
if (iterate == 0):\r
{\r
+ posicion-inicial = (self get-location).\r
if (sa has-next-theory):\r
{\r
teoria = sa get-next-theory.\r
girando_der = 1.\r
}\r
}\r
+ else\r
+ {\r
+ plan-finished = 1.\r
+ }\r
}\r
\r
+ # Validación de teoría\r
if (iterate == CELDAS_TURNO):\r
{\r
- # TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true.\r
- if (sa validate theory teoria):\r
- {\r
+ # Actualiza entorno segun si se movio o no\r
+ if (self near position posicion-inicial with-error 1.0):\r
+ {\r
+ sa set-entorno value 0 with-name "movido".\r
}\r
else\r
{\r
- plan_finished = 1.\r
+ sa set-entorno value 1 with-name "movido".\r
+ }\r
+ print iterate.\r
+ if (!(sa validate theory teoria)):\r
+ {\r
+ plan-finished = 1.\r
}\r
- }\r
-\r
- iterate++.\r
- if (iterate == CELDAS_TURNO + 1):\r
iterate = 0.\r
-\r
- # Movimiento del robot\r
- if (avanzando):\r
- self set-global-velocity to (15).\r
- if (retrocediendo):\r
- self set-global-velocity to (-15).\r
- if (girando_izq):\r
- self turn-left.\r
- if (girando_der):\r
- self turn-right.\r
+ }\r
+ else\r
+ {\r
+ iterate++.\r
+ }\r
\r
}\r
\r
des2=0.\r
if(dot(direction,(1,1,1))<0):\r
{ \r
- if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):\r
- des2=1. \r
+ if(dot((body get-location),positiveDirection) > dot((self get-location),positiveDirection)):\r
+ {\r
+ if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):\r
+ des2=1.\r
+ }\r
+ else\r
+ if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):\r
+ des2=1. \r
}\r
else\r
{\r
- if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):\r
+ if(dot((body get-location),positiveDirection) < dot((self get-location),positiveDirection)):\r
+ {\r
+ if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):\r
des2=1. \r
+ }\r
+ else\r
+ if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))):\r
+ des2=1.\r
+ \r
} \r
\r
\r
projects / z.facultad / 75.68 / celdas.git / blobdiff