Index: trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp	(revision 379)
+++ trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp	(revision 389)
@@ -49,12 +49,6 @@
     
     VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
-    
-    if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
-        ugvVrpn = new MetaVrpnObject(name,(uint8_t)0);
-        targetVrpn=new MetaVrpnObject("target",1);
-    } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
-        ugvVrpn = new MetaVrpnObject(name);
-        targetVrpn=new MetaVrpnObject("target");
-    }
+    ugvVrpn = new MetaVrpnObject(name);
+    targetVrpn=new MetaVrpnObject("target");
     
     getFrameworkManager()->AddDeviceToLog(ugvVrpn);
@@ -92,5 +86,5 @@
 void CircleFollower::Run(void) {
     WarnUponSwitches(true);
-    SetPeriodMS(100);
+    SetPeriodMS(20);
     
     if (getFrameworkManager()->ErrorOccured() == true) {
@@ -99,27 +93,49 @@
 
     while (!ToBeStopped()) {
+        SecurityCheck();
+        CheckJoystick();
+        CheckPushButton();
+       
         if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
         if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
+        WaitPeriod();
+    }
+}
+
+void CircleFollower::CheckPushButton(void) {
+  if (startCircle->Clicked() == true)
+      StartCircle();
         
-        if (startCircle->Clicked() == true)
-            StartCircle();
-        
-        if (stopCircle->Clicked() == true)
-            StopCircle();
-        
-         //R1 and Circle
-        if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
-            StartCircle();
-        }
-
-        //R1 and Cross
-        if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
+  if (stopCircle->Clicked() == true)
+      StopCircle();
+
+  if (button_kill->Clicked() == true)
+      SafeStop();
+}
+
+void CircleFollower::CheckJoystick(void) {
+  //R1 and Circle
+  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4)) {
+      StartCircle();
+  }
+
+  //R1 and Cross
+  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5)) {
+      StopCircle();
+  }
+}
+
+void CircleFollower::SecurityCheck(void) {
+    if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::Circle)) {
+        if (!targetVrpn->IsTracked(500)) {
+            Thread::Err("VRPN, target lost\n");
+            vrpnLost=true;
             StopCircle();
         }
-    
-        if (button_kill->Clicked() == true)
-            SafeStop();
-        
-        WaitPeriod();
+        if (!ugvVrpn->IsTracked(500)) {
+            Thread::Err("VRPN, ugv lost\n");
+            vrpnLost=true;
+            StopCircle();
+        }
     }
 }
@@ -174,4 +190,5 @@
 
 void CircleFollower::StartCircle(void) {
+  if(behaviourMode!=BehaviourMode_t::Circle) {
     Vector3Df ugv_pos,target_pos;
     Vector2Df ugv_2Dpos,target_2Dpos;
@@ -189,12 +206,14 @@
     behaviourMode=BehaviourMode_t::Circle;
     Thread::Info("CircleFollower: start circle\n");
+  }
 }
 
 void CircleFollower::StopCircle(void) {
-	
+	if(behaviourMode==BehaviourMode_t::Circle) {
     circle->FinishTraj();
     //GetJoystick()->Rumble(0x70);
     behaviourMode=BehaviourMode_t::Manual;
     Thread::Info("CircleFollower: finishing circle\n");
-}
-
+  }
+}
+
Index: trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h	(revision 379)
+++ trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h	(revision 389)
@@ -51,4 +51,7 @@
         void ComputeManualControls(void);
         void ComputeCircleControls(void);
+        void SecurityCheck(void);
+        void CheckJoystick(void);
+        void CheckPushButton(void);
 
         flair::filter::Pid *uX, *uY;
