From 6566954a731b998269a9f17108ece63b17ccbbbf Mon Sep 17 00:00:00 2001
From: Marcelo Benitez <cmarcelobenitez@gmail.com>
Date: Sun, 17 Dec 2006 23:55:12 +0000
Subject: [PATCH]

---
 trunk/src/breve/Celdas.tz | 12 ++++++++++++
 1 file changed, 12 insertions(+)

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