diff --git a/EscapeTheGhost/Assets/GameScript.cs b/EscapeTheGhost/Assets/GameScript.cs
index 4c7db07..5e12d50 100644
--- a/EscapeTheGhost/Assets/GameScript.cs
+++ b/EscapeTheGhost/Assets/GameScript.cs
@@ -1,54 +1,59 @@
-using System.Collections;
+using System;
+using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
 
 public class GameScript : MonoBehaviour {
         private Cellulo robot1;
         private Cellulo robot2;
 	public GameObject obj1;
 	public GameObject obj2;
         private float start_time;
+        private float update_time_s = 1;
+        private float min_dist_mm = 80;
         private int loop;
 
         private bool isRunning = false;
 
         public void start() {
             isRunning = true;
         }
 
         public void stop() {
             isRunning = false;
         }
 
 	// Use this for initialization
 	void Start () {
             Debug.Log("Start");
             robot1 = new Cellulo("00:06:66:E7:8A:E0", "00:1A:7D:DA:71:03");
             robot2 = new Cellulo("00:06:66:E7:8E:B2", "00:1A:7D:DA:71:03");
             start_time = Time.time;
             loop = 0;
 	}
 	
 	// Update is called once per frame
 	void Update () {
              if(robot1 != null) {
                 obj1.transform.position = new Vector3(robot1.getX(), robot1.getY(), 0);
                 obj2.transform.position = new Vector3(robot2.getX(), robot2.getY(), 0);
-                if(!isRunning) return;
                 float now = Time.time;
-                if(now - start_time > 1) {
+                double distance = Math.Pow(Math.Pow(robot1.getX() - robot2.getX(), 2) + Math.Pow(robot1.getY() - robot2.getY(), 2), 0.5);
+                Debug.Log(distance);
+                if(now - start_time > update_time_s) {
                     if(loop % 2 == 0)
                     {
+                        if(!isRunning) return;
                         robot1.setVisualEffect(0, 255, 0, 0, 255);
-                        robot1.setGoalPose(100, 200, 0, 1000, 1000);
+                        robot1.setGoalPose(robot2.getX(), robot2.getY(), 0, 1000, 1000);
                     }
                     else {
                         robot1.setVisualEffect(0, 0, 255, 0, 255);
-                        robot1.setGoalPose(200, 100, 0, 1000, 1000);
+                        robot1.clearTracking();
                     }
                     loop++;
                     start_time = now;
                 }
              }
 	}
 }