(no commit message)

diff --git a/trunk/src/breve/Celdas.tz b/trunk/src/breve/Celdas.tz
index 6ed43f40714beba737928c83e6fdf1d7bf4edf9b..ed8532c9c249389560274cf0ad8bc6a3611a3bd0 100644 (file)
--- a/trunk/src/breve/Celdas.tz
+++ b/trunk/src/breve/Celdas.tz
@@ -9,6 +9,7 @@
 @define CELDAS_MAX_VELOCITY 30.\r
 @define CELDAS_TURNO 80.\r
 @define CELDAS_SENSOR_THRESHOLD 9.\r
+@define CELDAS_VECTOR_ERROR 0.5.\r
 \r
 PhysicalControl : CeldasControl {\r
        % This class is used for building simple vehicle \r
@@ -182,6 +183,15 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
        - to get-wheel-radius:\r
                return 0.8.\r
 \r
+       - to compare-vectors compare-vector vector1(vector) with-vector vector2(vector):\r
+               vectorAux(vector).\r
+               vectorAux=vector1-vector2.\r
+\r
+               if ((|vectorAux::x|<CELDAS_VECTOR_ERROR) && (|vectorAux::y|<CELDAS_VECTOR_ERROR) && (|vectorAux::z|<CELDAS_VECTOR_ERROR))\r
+                       return 1.\r
+\r
+               return 0.\r
+               \r
         + to set-global-velocity to velocity (float):\r
                rfWheel set-velocity to velocity.\r
                lfWheel set-velocity to velocity.\r
@@ -210,6 +220,8 @@ CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
         + to get-sensor-value:\r
                return (fSensor get-sensor-value).\r
 \r
+\r
+\r
        +to update-entorno:\r
                entorno{"sensor_f"} = (fSensor get-sensor-value).\r
                entorno{"sensor_b"} = (bSensor get-sensor-value).\r