X-Git-Url: https://git.llucax.com/z.facultad/75.68/celdas.git/blobdiff_plain/6566954a731b998269a9f17108ece63b17ccbbbf..HEAD:/trunk/src/breve/Celdas.tz diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz index ed8532c..481c28c 100644 --- a/trunk/src/breve/Celdas.tz +++ b/trunk/src/breve/Celdas.tz @@ -6,10 +6,9 @@ @use Drawing. @use SistemaAutonomo. -@define CELDAS_MAX_VELOCITY 30. -@define CELDAS_TURNO 80. -@define CELDAS_SENSOR_THRESHOLD 9. -@define CELDAS_VECTOR_ERROR 0.5. +@define CELDAS_MAX_VELOCITY 5. +@define CELDAS_TURNO 30. +@define CELDAS_SENSOR_THRESHOLD 6. PhysicalControl : CeldasControl { % This class is used for building simple vehicle @@ -172,7 +171,9 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { teoria (object). entorno (hash). datos-finales (hash). - plan_finished (int). + plan-finished (int). + posicion-inicial (vector). + posicion-final (vector). - to get-density: return 20.0. @@ -183,15 +184,17 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { - to get-wheel-radius: return 0.8. - - to compare-vectors compare-vector vector1(vector) with-vector vector2(vector): + - to near position thePosition (vector) with-error error (float): vectorAux(vector). - vectorAux=vector1-vector2. + vectorAux = (self get-location) - thePosition. - if ((|vectorAux::x| (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error. + + if ((|vectorAux::x| < error) && (|vectorAux::z| < error)): return 1. return 0. - + + to set-global-velocity to velocity (float): rfWheel set-velocity to velocity. lfWheel set-velocity to velocity. @@ -203,19 +206,30 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { + to turn-right: tright++. - self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. - - if (tright == CELDAS_TURNO): tright=0. +#! + if (tright == CELDAS_TURNO): + { + fSensor set-direction to (-1,0,0). + bSensor set-direction to (1,0,0). + lSensor set-direction to (0,0,-1). + rSensor set-direction to (0,0,1). + } +!# + to turn-left: tleft++. - self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. - - if (tleft == CELDAS_TURNO): tleft=0. - +#! + if (tleft == CELDAS_TURNO): + { + fSensor set-direction to (-1,0,0). + bSensor set-direction to (1,0,0). + lSensor set-direction to (0,0,-1). + rSensor set-direction to (0,0,1). + } +!# + to get-sensor-value: return (fSensor get-sensor-value). @@ -227,7 +241,6 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { entorno{"sensor_b"} = (bSensor get-sensor-value). entorno{"sensor_r"} = (rSensor get-sensor-value). entorno{"sensor_l"} = (lSensor get-sensor-value). - entorno{"movido"} = 0. # TODO sa update-entorno with entorno. +to init: @@ -265,11 +278,14 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { girando_izq=0. girando_der=0. + posicion-inicial = (self get-location). + posicion-final = (0, 0, 0). + # Configuracion de sistema autonomo sa = new SistemaAutonomo. sa init with-max-pasos 4 with-max-teorias 15. iterate = 0. - plan_finished = 1. # así planificamos apenas empezamos + plan-finished = 1. # así planificamos apenas empezamos teorias = 4 new Teorias. teorias{0} init named "Avanzar" with-action "adelante". @@ -284,7 +300,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { teorias{0} set-dato-final name "sensor_l" value ANY. teorias{0} set-dato-final name "movido" value 1. - teorias{1} init named "Retroceder" with-action "atras". + teorias{1} init named "Retroceder" with-action "atras".# executed 2. teorias{1} set-dato-inicial name "sensor_f" value 1. teorias{1} set-dato-inicial name "sensor_b" value ANY. teorias{1} set-dato-inicial name "sensor_r" value ANY. @@ -308,7 +324,7 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { teorias{2} set-dato-final name "sensor_l" value 1. teorias{2} set-dato-final name "movido" value 0. - teorias{3} init named "Rotar a izquierda" with-action "izquierda". + teorias{3} init named "Rotar a izquierda" with-action "izquierda". # executed 2. teorias{3} set-dato-inicial name "sensor_f" value 1. teorias{3} set-dato-inicial name "sensor_b" value ANY. teorias{3} set-dato-inicial name "sensor_r" value ANY. @@ -329,30 +345,64 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { sa update-datos-finales with datos-finales. +to iterate: + + # Movimiento del robot + if (avanzando): + { + self set-global-velocity to (CELDAS_MAX_VELOCITY). + } + if (retrocediendo): + { + self set-global-velocity to (-CELDAS_MAX_VELOCITY). + } + if (girando_izq): + { + self set-global-velocity to 0. + self turn-left. + #if (iterate): self rotate around-axis (0,1,0) by (1.570796/CELDAS_TURNO)*iterate. + print "izq: ", (1.570796/CELDAS_TURNO)*iterate. + } + if (girando_der): + { + self set-global-velocity to 0. + self turn-right. + #if (iterate): self rotate around-axis (0,1,0) by (-1.570796/CELDAS_TURNO)*iterate. + print "der: ", (-1.570796/CELDAS_TURNO)*iterate. + } + #print "vel: ", (bodyLink get-velocity). + + # Actualiza entorno self update-entorno. - if (0): # TODO posicion_final == posicion_actual + # Chequeo de objetivo + if (self near position posicion-final with-error 5.0): { print "Llegamos al FINAL!!!". self set-global-velocity to 0. return. } - if (plan_finished): + # Planificación + if (plan-finished): { + # Actualiza entorno indicando que no se movió para que + # el planificador actue + sa set-entorno value 0 with-name "movido". sa plan. # Si no tenemos plan, lo hacemos - plan_finished = 0. - # TODO posicion_inicial = posicion_actual + plan-finished = 0. + iterate = 0. if (! (sa has-next-theory)): { - plan_finished = 1. + plan-finished = 1. print "El planificador no encuentra PLAN!!!". return. } } + # Ejecución de teoría if (iterate == 0): { + posicion-inicial = (self get-location). if (sa has-next-theory): { teoria = sa get-next-theory. @@ -385,33 +435,35 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) { girando_der = 1. } } + else + { + plan-finished = 1. + } } + # Validación de teoría if (iterate == CELDAS_TURNO): { - # TODO if (posicion_actual == posicion_inicial): movido = false. else movido = true. - if (sa validate theory teoria): - { + # Actualiza entorno segun si se movio o no + if (self near position posicion-inicial with-error 1.0): + { + sa set-entorno value 0 with-name "movido". } else { - plan_finished = 1. + sa set-entorno value 1 with-name "movido". + } + print iterate. + if (!(sa validate theory teoria)): + { + plan-finished = 1. } - } - - iterate++. - if (iterate == CELDAS_TURNO + 1): iterate = 0. - - # Movimiento del robot - if (avanzando): - self set-global-velocity to (15). - if (retrocediendo): - self set-global-velocity to (-15). - if (girando_izq): - self turn-left. - if (girando_der): - self turn-right. + } + else + { + iterate++. + } } @@ -580,13 +632,26 @@ Link : CeldasSensor (aka CeldasSensors) { des2=0. if(dot(direction,(1,1,1))<0): { - if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))): - des2=1. + if(dot((body get-location),positiveDirection) > dot((self get-location),positiveDirection)): + { + if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))): + des2=1. + } + else + if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))): + des2=1. } else { - if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))): + if(dot((body get-location),positiveDirection) < dot((self get-location),positiveDirection)): + { + if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))): des2=1. + } + else + if((dot((self get-location),positiveDirection))>(dot(obsLoc,positiveDirection))): + des2=1. + }