Index: trunk/demos/TwoWheelRobotCircleFollower/simulator/resources/core2-64/simulator.xml
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/simulator/resources/core2-64/simulator.xml	(revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/simulator/resources/core2-64/simulator.xml	(revision 379)
@@ -5,5 +5,5 @@
       <TabWidget name="tabs">
         <Tab name="sampling">
-          <DoubleSpinBox name="Tech (s):" value="0.010"/>
+          <DoubleSpinBox name="Tech (s):" value="0.01"/>
         </Tab>
         <Tab name="optitrack">
@@ -11,11 +11,11 @@
         </Tab>
         <Tab name="init">
-          <Vector3DSpinBox name="position" value_x="1" value_y="0" value_z="-1"/>
-          <SpinBox name="yaw (deg):" value="0"/>
+          <Vector3DSpinBox name="position" value_x="2" value_y="0" value_z="-1"/>
+          <SpinBox name="yaw (deg):" value="90"/>
         </Tab>
         <Tab name="model">
           <DoubleSpinBox name="mass (kg):" value="0.2"/>
           <DoubleSpinBox name="size (m):" value="0.2"/>
-        <DoubleSpinBox name="translational speed (m/s):" value="1"/><DoubleSpinBox name="rotational speed (deg/s):" value="2"/></Tab>
+        <DoubleSpinBox name="translational speed (m/s):" value="2"/><DoubleSpinBox name="rotational speed (deg/s):" value="4"/></Tab>
         <Tab name="visual">
           <SpinBox name="arm color (R):" value="255"/>
@@ -38,6 +38,6 @@
         </Tab>
         <Tab name="model">
-          <DoubleSpinBox name="translational speed (m/s):" value="0"/>
-          <DoubleSpinBox name="rotational speed (deg/s):" value="0"/>
+          <DoubleSpinBox name="translational speed (m/s):" value="1.30"/>
+          <DoubleSpinBox name="rotational speed (deg/s):" value="60.00"/>
         </Tab>
       </TabWidget>
Index: trunk/demos/TwoWheelRobotCircleFollower/simulator/src/main.cpp
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/simulator/src/main.cpp	(revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/simulator/src/main.cpp	(revision 379)
@@ -96,11 +96,9 @@
   gui = new Parser(960, 480, 960, 480, media_path, scene_file);
 #endif
-
   
   robot = new TwoWheelRobot(name, 0);
 
-
 #ifdef GL
-  //man = new Man("target",1);
+  man = new Man("target",1);
 #endif
 
Index: trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/setup_simu.xml
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/setup_simu.xml	(revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/setup_simu.xml	(revision 379)
@@ -40,15 +40,15 @@
           <GroupBox name="circle">
             <DoubleSpinBox name="period, 0 for auto" value="0"/>
-            <DoubleSpinBox name="R" value="0"/>
-            <DoubleSpinBox name="velocity" value="0"/>
-            <DoubleSpinBox name="acceleration (absolute)" value="0"/>
+            <DoubleSpinBox name="R" value="2"/>
+            <DoubleSpinBox name="velocity" value="1"/>
+            <DoubleSpinBox name="acceleration (absolute)" value="0.2"/>
           </GroupBox>
         </Tab>
         <Tab name="Mesures ugv_0">
           <DataPlot1D name="x" period="100" enabled="1"/>
-          <DataPlot1D name="y" period="100" enabled="0"/>
+          <DataPlot1D name="y" period="100" enabled="1"/>
           <DataPlot1D name="z" period="100" enabled="0"/>
-          <DataPlot1D name="vx" period="100" enabled="0"/>
-          <DataPlot1D name="vy" period="100" enabled="0"/>
+          <DataPlot1D name="vx" period="100" enabled="1"/>
+          <DataPlot1D name="vy" period="100" enabled="1"/>
           <DataPlot1D name="vz" period="100" enabled="0"/>
         </Tab>
@@ -72,9 +72,9 @@
       <GridLayout name="buttons">
         <PushButton name="kill"/>
-        <PushButton name="start_circle"/>
-        <PushButton name="stop_circle"/>
+        <PushButton name="start_circle" value="1"/>
+        <PushButton name="stop_circle" value="1"/>
       </GridLayout>
     </Tab>
-  </TabWidget>
+  <Tab name="control laws"><TabWidget name="laws"><Tab name="Setup"><GroupBox name="u_x"><DoubleSpinBox name="period, 0 for auto" value="0"/><DoubleSpinBox name="kp:" value="0.8"/><DoubleSpinBox name="ki:" value="0.1"/><DoubleSpinBox name="sat i:" value="0"/><DoubleSpinBox name="kd:" value="0.3"/><DoubleSpinBox name="sat:" value="1"/></GroupBox><GroupBox name="u_y"><DoubleSpinBox name="period, 0 for auto" value="0"/><DoubleSpinBox name="kp:" value="0.8"/><DoubleSpinBox name="ki:" value="0.1"/><DoubleSpinBox name="sat i:" value="0"/><DoubleSpinBox name="kd:" value="0.3"/><DoubleSpinBox name="sat:" value="1"/></GroupBox><DoubleSpinBox name="L" value="1"/></Tab><Tab name="Graphes"><DataPlot1D name="u_x" period="100" enabled="0"/><DataPlot1D name="u_y" period="100" enabled="0"/></Tab></TabWidget></Tab></TabWidget>
   <PushButton name="save config on target (ugv_0)" value="1"/>
 </root>
Index: trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp
===================================================================
--- trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp	(revision 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp	(revision 379)
@@ -24,7 +24,12 @@
 #include <Matrix.h>
 #include <Tab.h>
+#include <TabWidget.h>
+#include <DoubleSpinBox.h>
 #include <Pid.h>
+#include <Quaternion.h>
+#include <Euler.h>
 #include <Ugv.h>
 #include <UgvControls.h>
+#include <math.h>
 
 using namespace std;
@@ -36,6 +41,6 @@
 using namespace flair::actuator;
 
-CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
-   this->controller=controller;
+CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) {
+    this->controller=controller;
     controller->Start();
     
@@ -57,7 +62,7 @@
     vrpnclient->Start();
 	
-	Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
+    Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
     GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
-	button_kill = new PushButton(buttonslayout->NewRow(), "kill");
+    button_kill = new PushButton(buttonslayout->NewRow(), "kill");
     startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
     stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
@@ -69,9 +74,15 @@
     ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
     ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
-/*
+
+    Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
+    TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
+    Tab *setupLawTab = new Tab(tabWidget, "Setup");
+    Tab *graphLawTab = new Tab(tabWidget, "Graphes");
     uX=new Pid(setupLawTab->At(1,0),"u_x");
     uX->UseDefaultPlot(graphLawTab->NewRow());
     uY=new Pid(setupLawTab->At(1,1),"u_y");
-    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());*/
+    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
+    
+    l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
 }
 
@@ -88,9 +99,23 @@
 
     while (!ToBeStopped()) {
-        float speed=-controller->GetAxisValue(3);
-        float turn=controller->GetAxisValue(0);
-       
-        //sumoControl->move(x, rot);
-        GetUgv()->GetUgvControls()->SetControls(speed,turn);
+        if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
+        if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
+        
+        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)) {
+            StopCircle();
+        }
+    
         if (button_kill->Clicked() == true)
             SafeStop();
@@ -100,13 +125,55 @@
 }
 
-void CircleFollower::StartCircle(void) {/*
-    if (SetOrientationMode(OrientationMode_t::Custom)) {
-        Thread::Info("CircleFollower: start circle\n");
-    } else {
-        Thread::Warn("CircleFollower: could not start circle\n");
-        return;
-    }
-    Vector3Df uav_pos,target_pos;
-    Vector2Df uav_2Dpos,target_2Dpos;
+void CircleFollower::ComputeManualControls(void) {
+  float speed=-controller->GetAxisValue(3);
+  float turn=controller->GetAxisValue(0);
+  GetUgv()->GetUgvControls()->SetControls(speed,turn);
+}
+
+void CircleFollower::ComputeCircleControls(void) {
+
+  Vector3Df ugv_pos,ugv_vel,target_pos; // in VRPN coordinate system
+  Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
+  Vector2Df pos_error,vel_error;
+  Vector2Df circle_pos,circle_vel;
+    
+  ugvVrpn->GetPosition(ugv_pos);
+  ugvVrpn->GetSpeed(ugv_vel);
+
+  ugv_pos.To2Dxy(ugv_2Dpos);
+  ugv_vel.To2Dxy(ugv_2Dvel);
+  
+  targetVrpn->GetPosition(target_pos);
+  target_pos.To2Dxy(target_2Dpos);
+  circle->SetCenter(target_2Dpos);
+
+  //circle reference
+  circle->Update(GetTime());
+  circle->GetPosition(circle_pos);
+  circle->GetSpeed(circle_vel);
+
+  //error in optitrack frame
+  pos_error=ugv_2Dpos-circle_pos;
+  vel_error=ugv_2Dvel-circle_vel;
+    
+  uX->SetValues(pos_error.x, vel_error.x);
+  uX->Update(GetTime());
+  uY->SetValues(pos_error.y, vel_error.y);
+  uY->Update(GetTime());
+  
+  //get yaw from vrpn
+  Quaternion vrpnQuaternion;
+  ugvVrpn->GetQuaternion(vrpnQuaternion);
+  float yaw=vrpnQuaternion.ToEuler().yaw;
+  float L=1;
+  float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
+  float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
+  GetUgv()->GetUgvControls()->SetControls(-v,-w);
+}
+
+
+void CircleFollower::StartCircle(void) {
+    Vector3Df ugv_pos,target_pos;
+    Vector2Df ugv_2Dpos,target_2Dpos;
 
     targetVrpn->GetPosition(target_pos);
@@ -114,18 +181,20 @@
     circle->SetCenter(target_2Dpos);
 
-    uavVrpn->GetPosition(uav_pos);
-    uav_pos.To2Dxy(uav_2Dpos);
-    circle->StartTraj(uav_2Dpos);
+    ugvVrpn->GetPosition(ugv_pos);
+    ugv_pos.To2Dxy(ugv_2Dpos);
+    circle->StartTraj(ugv_2Dpos);
 
     uX->Reset();
     uY->Reset();
-    behaviourMode=BehaviourMode_t::Circle;*/
+    behaviourMode=BehaviourMode_t::Circle;
+    Thread::Info("CircleFollower: start circle\n");
 }
 
 void CircleFollower::StopCircle(void) {
-	/*
+	
     circle->FinishTraj();
     //GetJoystick()->Rumble(0x70);
-    Thread::Info("CircleFollower: finishing circle\n");*/
-}
-
+    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 378)
+++ trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h	(revision 379)
@@ -20,4 +20,5 @@
     namespace gui {
         class PushButton;
+        class DoubleSpinBox;
     }
     namespace filter {
@@ -41,22 +42,22 @@
 
 	enum class BehaviourMode_t {
-            Default,
+            Manual,
             Circle
         };
 
+        void Run(void);
+        void StartCircle(void);
+        void StopCircle(void);
+        void ComputeManualControls(void);
+        void ComputeCircleControls(void);
+
+        flair::filter::Pid *uX, *uY;
+        flair::gui::PushButton *startCircle,*stopCircle,*button_kill;
+        flair::gui::DoubleSpinBox *l;
+        flair::meta::MetaVrpnObject *targetVrpn,*ugvVrpn;
+        flair::filter::TrajectoryGenerator2DCircle *circle;
         BehaviourMode_t behaviourMode;
         bool vrpnLost;
-		flair::sensor::TargetController *controller;
-
-		void Run(void);
-        void StartCircle(void);
-        void StopCircle(void);
-
-        flair::filter::Pid *uX, *uY;
-
-        flair::gui::PushButton *startCircle,*stopCircle,*button_kill;
-        flair::meta::MetaVrpnObject *targetVrpn,*ugvVrpn;
-        flair::filter::TrajectoryGenerator2DCircle *circle;
-
+        flair::sensor::TargetController *controller;
 };
 
