diff --git a/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg b/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
index 153ee988640c1762fcc35044a13c429d70a2e9bb..dc6d89e0cd7d22c2feb5b9054fdea96cf45e65e7 100644
--- a/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
+++ b/real_robot/coordination_formation_control_pkg/cfg/missionController.cfg
@@ -11,11 +11,11 @@ demo.add("start_formation",                   bool_t,    0,
 demo.add("stop",                   bool_t,    0,                               "stop formation ",      False)
 demo.add("mantain_position",                   bool_t,    0,                               "mantain goal centroid",      False)
 demo.add("update_waypoint",                   bool_t,    0,                               "update waypoint",      False)
-demo.add("x",       double_t,  0,                               "desired goal",               0.0,     -3,-3)
-demo.add("y",       double_t,  0,                               "desired goal",               0.0,     -3,-3)
-demo.add("vx",         double_t,   0,                              "desired goal",                     0.0,-3,-3)
-demo.add("vy",         double_t,   0,                              "desired goal",                     0.0, -3,-3)
-demo.add("orientation",         double_t,   0,                              "desired goal",                     0, 0,360)
+demo.add("x",       double_t,  0,                               "desired goal x",               0.0,     -3,3)
+demo.add("y",       double_t,  0,                               "desired goal y",               0.0,     -3,3)
+demo.add("vx",         double_t,   0,                              "desired goal vx",                     0.0,-3,3)
+demo.add("vy",         double_t,   0,                              "desired goal vy",                     0.0, -3,3)
+demo.add("orientation",         double_t,   0,                              "desired goal orientation",                     0, 0,360)
 
 
 exit(gen.generate(PACKAGE, "coordination_formation_control_pkg", "missionController"))
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index d9a7a3d6904517a740a1dfdb4c814a6acb63a140..9acedfd4b3bd1f66c853e75fb11b89a32da5b82c 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,5 +1,5 @@
 n_drones: 4
-n_ugvs: 2
+n_ugvs: 1
 hz_freq: 50 # controller frquency
 include_obstacle: false
 include_orientation: false
diff --git a/real_robot/coordination_formation_control_pkg/include/mission_controller.h b/real_robot/coordination_formation_control_pkg/include/mission_controller.h
index 1a5005a76af70e1228e4c85e9e659201c3e48d9e..8b3dedeac3d900c2f601cad3eb5d62fe22e873e1 100644
--- a/real_robot/coordination_formation_control_pkg/include/mission_controller.h
+++ b/real_robot/coordination_formation_control_pkg/include/mission_controller.h
@@ -34,7 +34,7 @@ class missionController
     /* execute main process*/
     void executeMainProcess();
     void updateCofig(coordination_formation_control_pkg::missionControllerConfig &config, uint32_t level);
-    Eigen::MatrixXf compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs);
+    Eigen::MatrixXf compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs,Mat &previous_labels, Eigen::MatrixXf & previous_centroids);
 
   private:
     fsm_mc_t state;
@@ -44,6 +44,7 @@ class missionController
     bool stop;
     bool mantain_position;
     bool start_publishing_waypoint;
+    bool cluster_initialized;
     ros::NodeHandle nh;
 
       /*dynamic reconfigure*/
diff --git a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
index bc882f0afe3ec1673506e2bffbc756424ccd555a..04954831b9fcc33481daf2f328286a8f96cdd237 100644
--- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
@@ -5,7 +5,7 @@
   <rosparam command="load" file="$(find coordination_formation_control_pkg)/config/formationConfig.yaml" />
 
   <!-- Initialize all the drones first -->
-  <group ns="uav_1">
+  <!-- <group ns="uav_1">
        <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="1" launch-prefix="xterm -e">
        </node>
    </group>
@@ -24,7 +24,13 @@
      <group ns="uav_4">
           <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="4" launch-prefix="xterm -e">
           </node>
-      </group>
+      </group> -->
+-->
+
+      <group ns="omniwheel_1">
+           <node name="omniwheel_node" pkg="coordination_formation_control_pkg" type="omniwheel_node" output="screen" args="1" launch-prefix="xterm -e">
+           </node>
+       </group>
 
 
 
diff --git a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
index d59a890f479dffe905e95d73a38fc4798d95f343..427b97082cce9c6a89b43a26cc19e1dd8db96d58 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -29,6 +29,7 @@ nh(nh)
 
   /* execute main process*/
   this->executeMainProcess();
+  this->cluster_initialized = false;
 
 }
 /*destructor*/
@@ -39,11 +40,12 @@ missionController::~missionController()
 /* main process*/
 void missionController::executeMainProcess()
 {
-  ros::Rate loop_rate(100);
+  ros::Rate loop_rate(50);
   /* create a list of drones*/
   int n_drones, n_ugvs;
   this->nh.getParam("/n_drones", n_drones);
   this->nh.getParam("/n_ugvs", n_ugvs);
+  this->current_waypoint.cluster_pos.resize(n_ugvs*2);
 
 
   std::vector<robotState *> uavs;
@@ -82,17 +84,44 @@ void missionController::executeMainProcess()
     }
   }
 
+  Eigen::MatrixXf uav_state_q(2,n_drones);
+  Eigen::MatrixXf uav_state_p(2,n_drones);
+  Eigen::MatrixXf ugv_state_q(2,n_ugvs);
+  Eigen::MatrixXf ugv_state_p(2,n_ugvs);
   Eigen::MatrixXf current_cluster(2,n_ugvs);
   row_vector_2d_t current_centroid_vel,goal_centroid,current_centroid, error;
   float dist;
+  int cluster_ind = 0;
+  // std::vector<Point2f> previous_center;
+  // for(int i = 0; i<n_ugvs; i++)
+  // {
+  //   Point2f center;
+  //   center.x = ugv_state_q(0,i);
+  //   center.y = ugv_state_q(1,i);
+  //   previous_center.push_back(center);
+  // }
+  Mat previous_labels;
+  previous_labels = cv::Mat(n_drones,1,CV_32S);
+
+  for (int i = 0; i<n_drones; i++)
+  {
+    previous_labels.at<int>(i,0) = i;
+  }
+  Eigen::MatrixXf previous_centroids(2,n_ugvs);
 
+for (int i = 0; i ++; i < n_ugvs)
+{
+  robot_state_t current_state;
+  current_state = ugvs[i-1]->get_state();
+  previous_centroids.col(i) << current_state.q(0),current_state.q(1);
+}
   while(ros::ok())
   {
 
-    Eigen::MatrixXf uav_state_q(2,n_drones);
-    Eigen::MatrixXf uav_state_p(2,n_drones);
-    Eigen::MatrixXf ugv_state_q(2,n_ugvs);
-    Eigen::MatrixXf ugv_state_p(2,n_ugvs);
+    // std::cout <<"here" << std::endl;
+
+
+
     for(int i = 1; i <=n_drones; i++)
     {
       robot_state_t current_state;
@@ -115,6 +144,7 @@ void missionController::executeMainProcess()
 
     }
 
+
     if(this->shut_down)
     {
       // stop all process here
@@ -172,6 +202,36 @@ void missionController::executeMainProcess()
 
         break;
         case START_FORMATION:
+        /* testint*/
+        uav_state_q(0, 1-1) =0;
+        uav_state_q(1, 1-1) =0;
+        uav_state_p(0, 1-1) =0;
+        uav_state_p(1, 1-1) = 0;
+
+        uav_state_q(0, 2-1) =1;
+        uav_state_q(1, 2-1) =0;
+        uav_state_p(0, 2-1) = 0;
+        uav_state_p(1, 2-1) = 0;
+
+        uav_state_q(0, 3-1) =0;
+        uav_state_q(1, 3-1) =1;
+        uav_state_p(0, 3-1) = 0;
+        uav_state_p(1, 3-1) = 0;
+
+        uav_state_q(0, 4-1) =1;
+        uav_state_q(1, 4-1) =1;
+        uav_state_p(0, 4-1) = 0;
+        uav_state_p(1, 4-1) = 0;
+
+        ugv_state_q(0, 1-1) =-1.5;
+        ugv_state_q(1, 1-1) =-0.5;
+        ugv_state_p(0, 1-1) = 0;
+        ugv_state_p(1, 1-1) =0;
+
+        ugv_state_q(0, 2-1) =1.5;
+        ugv_state_q(1, 2-1) =-0.5;
+        ugv_state_p(0, 2-1) = 0;
+        ugv_state_p(1, 2-1) =0;
 
         /* compute waypoint formation*/
         /* initial waypoint will be the centorid of the current configuration*/
@@ -186,39 +246,62 @@ void missionController::executeMainProcess()
         dist = error.norm();
 
 
+
+
         /* compute cluster*/
-        current_cluster = this->compute_cluster(uav_state_q, ugv_state_q, n_ugvs,n_drones);
+        // std::cout << "Here" << std::endl;
+        if (n_ugvs >0)
+        {
+          // std::cout << "outside before previous lable" << previous_labels << std::endl;
+
+          // current_cluster = this->compute_cluster(uav_state_q, ugv_state_q, n_ugvs,n_drones);
+          current_cluster = this->compute_cluster(uav_state_q, ugv_state_q, n_ugvs,n_drones, previous_labels, previous_centroids);
+          previous_centroids = current_cluster;
+          // std::cout << "outside after previous lable" << previous_labels <<std::endl;
+
+
+          // std::cout << current_cluster << std::endl;
+        }
+
 
         /* assign clusters*/
+        cluster_ind = 0;
+
         for(int i = 0; i < n_ugvs; i++)
         {
-          this->current_waypoint.cluster_pos.push_back(current_cluster(0,i));
-          this->current_waypoint.cluster_pos.push_back(current_cluster(1,i));
+
+          this->current_waypoint.cluster_pos[cluster_ind] = current_cluster(0,i);
+          this->current_waypoint.cluster_pos[cluster_ind+1] = current_cluster(1,i);
+          // previous_center[i].x =  current_cluster(0,i);
+          // previous_center[i].y = current_cluster(1,i);
+          cluster_ind +=2;
         }
         this->current_waypoint.cluster_vel[0] = current_centroid_vel(0);
         this->current_waypoint.cluster_vel[1] = current_centroid_vel(1);
 
 
-        /* send goals*/
-        if (dist < 0.1) // waypoint reached TODO, this is hard coded, not a good idea
-        {
-          if(this->mantain_position == false)
-          {
-            this->current_waypoint.stop = true;
-          }
-          else
-          {
-            this->current_waypoint.stop = false;
-            /* if we want to mantain centroid, vel to 0*/
-            this->current_waypoint.vel[0] = 0;
-            this->current_waypoint.vel[1] = 0;
 
-          }
-        }
 
         /* update waypoints*/
         if (start_publishing_waypoint)
         {
+          // std::cout << dist << std::endl;
+          /* send goals*/
+          if (dist < 0.1) // waypoint reached TODO, this is hard coded, not a good idea
+          {
+            if(this->mantain_position == false)
+            {
+              this->current_waypoint.stop = true;
+            }
+            else
+            {
+              this->current_waypoint.stop = false;
+              /* if we want to mantain centroid, vel to 0*/
+              this->current_waypoint.vel[0] = 0;
+              this->current_waypoint.vel[1] = 0;
+
+            }
+          }
           waypoint_publisher.publish(this->current_waypoint);
         }
 
@@ -292,27 +375,31 @@ void missionController::updateCofig(coordination_formation_control_pkg::missionC
   }
 }
 
-Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs)
+Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen::MatrixXf ugv_q,int num_ugvs, int num_uavs,Mat &previous_labels,Eigen::MatrixXf & previous_centroids)
 {
+
   /* only implemented for two ugv*/
-  if(num_ugvs > 2)
+  if(num_ugvs > 2 || num_ugvs <1)
   {
     ROS_INFO("n_ugv is only implemented for 2 ugv, should modify for more ugvs");
 
     this->stop = true;
 
+
   }
   else
   {
-    std::vector<Point2f> centers;
-    for (int i = 0; i <2; i++)
-    {
-      /* make intial ugv be the centroid */
-      Point2f center;
-      center.x = ugv_q(0,i);
-      center.y =  ugv_q(1,i);
-      centers.push_back(center);
-    }
+
+    std::vector<Point2f> centers;;
+    // for (int i = 0; i <num_ugvs; i++)
+    // {
+    //   /* make intial ugv be the centroid */
+    //   Point2f center;
+    //   center.x = ugv_q(0,i);
+    //   center.y =  ugv_q(1,i);
+    //   centers.push_back(center);
+    // }
+
     /* points*/
     std::vector<Point2f> points;
 
@@ -323,12 +410,18 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
       point.y =  uav_q(1,t);
       points.push_back(point);
     }
-    Mat labels;
+    // ROS_INFO("*****************************************Here");
+
+    Mat labels = previous_labels;
+    /* set up labels*/
     int cluster_cout = num_ugvs;
     double compactness = kmeans(points, cluster_cout, labels,
-            TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 10, 1.0),
+            TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 0, 0.1),
                10,  KMEANS_PP_CENTERS, centers);
 
+    // std::cout << "previous lable" << previous_labels << "current labels" << labels << std::endl;
+    // std::cout << "Current center" << centers << std::endl;
+    previous_labels = labels;
     /* decide to change or not*/
     /* we calculate the enegy to mantain the current confguration or change*/
     Eigen::MatrixXf centroids(2,num_ugvs);
@@ -338,25 +431,48 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
       centroids(1,p) = centers[p].y;
     }
 
-    float man1 = (centroids.col(0) - ugv_q.col(0)).norm();
-    float man2 = (centroids.col(1) - ugv_q.col(0)).norm();
-    float cha1 = (centroids.col(1) - ugv_q.col(1)).norm();
-    float cha2 = (centroids.col(0) - ugv_q.col(1)).norm();
+    // std::cout << "Current center set" << centroids << std::endl;
+
+
 
-    if( cha1 < man1)
+
+
+
+    /* this is intended for two ugvs, it computed the energy to mantain leader cluster,
+    and change, if energy in both ugv to change is higher, the switch is perfomred*/
+    if(num_ugvs == 2)
     {
-      if(cha2 <man2)
+      float man1 = (centroids.col(0) - previous_centroids.col(0)).norm();
+      float man2 = (centroids.col(1) - previous_centroids.col(0)).norm();
+      float cha1 = (centroids.col(1) - previous_centroids.col(1)).norm();
+      float cha2 = (centroids.col(0) - previous_centroids.col(1)).norm();
+
+      if( cha1 < man1 && cha2 <man2)
       {
-        Eigen::MatrixXf dummy_centroid(2,num_ugvs);
-        dummy_centroid = centroids;
-        /* switch centroids*/
-        centroids.col(0) = dummy_centroid.col(1);
-        centroids.col(1) = dummy_centroid.col(0);
+
+          Eigen::MatrixXf dummy_centroid(2,num_ugvs);
+          dummy_centroid = centroids;
+          /* switch centroids*/
+          centroids.col(0) = dummy_centroid.col(1);
+          centroids.col(1) = dummy_centroid.col(0);
+
 
 
       }
-    }
+      else
+      {
+        // bad ondding
+        if(this->cluster_initialized)
+        {
+          centroids = previous_centroids;
+
+        }
+      }
 
+    }
+    previous_centroids = centroids;
+    // std::cout << "final centroids" << centroids << std::endl;
+this->cluster_initialized = true;
     return centroids;
 
   }
diff --git a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
index 2b935494d60c52587bb2eabc883506965c9d824f..f8ac252c568ab663fb47cc4dd61e495f05700376 100644
--- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -167,96 +167,96 @@ int main(int argc, char **argv) {
     else if(coord_config->initialize_robot && coord_config->init_formation && coord_config->stop_demo == false)
     {
       /* start formation controll*/
-      if (waypoint_received)
-      {
-
-        /*get current state of all the ugv*/
-        for(int i = 1; i <=n_ugvs; i++)
-        {
-          robot_state_t current_state;
-          current_state = ugvs[i-1]->get_state();
-          ugv_state_q(0, i-1) =current_state.q(0);
-          ugv_state_q(1, i-1) =current_state.q(1);
-          ugv_state_p(0, i-1) = current_state.p(0);
-          ugv_state_p(1, i-1) = current_state.p(1);
-
-        }
-
-        /* create a state vector of the cluster and the current agv*/
-        current_state_q(0,0) = next_waypoint->cluster_pos[robotid*2 -2];
-        current_state_q(1,0) = next_waypoint->cluster_pos[robotid*2 -1];
-        current_state_p(0,0) = next_waypoint->cluster_vel[0];
-        current_state_p(1,0) = next_waypoint->cluster_vel[1];
-        current_state_q.col(1) = ugv_state_q.col(robotid-1);
-        current_state_p.col(1) = ugv_state_p.col(robotid-1);
-
-        /* update reference point*/
-        current_waypoint.q(0) =next_waypoint->cluster_pos[robotid*2 -2];
-        current_waypoint.q(1) = next_waypoint->cluster_pos[robotid*2 -1];
-        current_waypoint.p(0) = next_waypoint->cluster_vel[0];
-        current_waypoint.p(1) = next_waypoint->cluster_vel[1];
-
-        /* mantain a 180 degress*/
-        current_waypoint.orientation = 180;
-        if (n_obs_ugv > 0)
-        {
-          /* add obstacle*/
-          if(n_obs != 0)
-          {
-            int indx = 0;
-            for(int n = 0; n < n_obs; n++)
-            {
-
-              current_obs[n].q(0) = next_waypoint->obstacle[indx];
-              current_obs[n].q(1) = next_waypoint->obstacle[indx+1];
-              current_obs[n].radius = next_waypoint->obstacle[indx+2];
-              indx = indx+3;
-            }
-          }
-          /* add other ugv as obstacle*/
-          if((n_ugvs-1) !=0)
-          {
-            int ugv_ind = 0;
-            for(int n = 0; n < (n_ugvs); n++)
-            {
-              if ((n+1) != robotid) // exclude current ugv as it own obstacle
-              {
-                current_obs[n_obs+ugv_ind].q(0) =ugv_state_q(0, n) ;
-                current_obs[n_obs+ugv_ind].q(1) =ugv_state_q(1, n) ;
-                current_obs[n_obs+ugv_ind].radius = robot_ind.safe_ditance + robot_ind.robot_size;
-                ugv_ind = ugv_ind+1;
-              }
-
-            }
-          }
-
-        }
-        else
-        {
-          current_obs = NULL;
-        }
+      // if (waypoint_received)
+      // {
       //
-        /* call controller*/
-        input = swam_controller.controller(current_state_q, current_state_p,current_waypoint,current_obs);
-        /* integrate acceleration*/
-        if(next_waypoint->stop)
-        {
-          current_command.linear.x = 0;
-          current_command.linear.y = 0;
-          current_command.angular.z = 0;
-        }
-        else
-        {
-          current_command.linear.x = input(0,0);
-          current_command.linear.y = input(1,0);
-          current_command.angular.z = 0;
-
-        }
-
-
-
-      }
-      vel_cmd_publisher.publish(current_command);
+      //   /*get current state of all the ugv*/
+      //   for(int i = 1; i <=n_ugvs; i++)
+      //   {
+      //     robot_state_t current_state;
+      //     current_state = ugvs[i-1]->get_state();
+      //     ugv_state_q(0, i-1) =current_state.q(0);
+      //     ugv_state_q(1, i-1) =current_state.q(1);
+      //     ugv_state_p(0, i-1) = current_state.p(0);
+      //     ugv_state_p(1, i-1) = current_state.p(1);
+      //
+      //   }
+      //
+      //   /* create a state vector of the cluster and the current agv*/
+      //   current_state_q(0,0) = next_waypoint->cluster_pos[robotid*2 -2];
+      //   current_state_q(1,0) = next_waypoint->cluster_pos[robotid*2 -1];
+      //   current_state_p(0,0) = next_waypoint->cluster_vel[0];
+      //   current_state_p(1,0) = next_waypoint->cluster_vel[1];
+      //   current_state_q.col(1) = ugv_state_q.col(robotid-1);
+      //   current_state_p.col(1) = ugv_state_p.col(robotid-1);
+      //
+      //   /* update reference point*/
+      //   current_waypoint.q(0) =next_waypoint->cluster_pos[robotid*2 -2];
+      //   current_waypoint.q(1) = next_waypoint->cluster_pos[robotid*2 -1];
+      //   current_waypoint.p(0) = next_waypoint->cluster_vel[0];
+      //   current_waypoint.p(1) = next_waypoint->cluster_vel[1];
+      //
+      //   /* mantain a 180 degress*/
+      //   current_waypoint.orientation = 180;
+      //   if (n_obs_ugv > 0)
+      //   {
+      //     /* add obstacle*/
+      //     if(n_obs != 0)
+      //     {
+      //       int indx = 0;
+      //       for(int n = 0; n < n_obs; n++)
+      //       {
+      //
+      //         current_obs[n].q(0) = next_waypoint->obstacle[indx];
+      //         current_obs[n].q(1) = next_waypoint->obstacle[indx+1];
+      //         current_obs[n].radius = next_waypoint->obstacle[indx+2];
+      //         indx = indx+3;
+      //       }
+      //     }
+      //     /* add other ugv as obstacle*/
+      //     if((n_ugvs-1) !=0)
+      //     {
+      //       int ugv_ind = 0;
+      //       for(int n = 0; n < (n_ugvs); n++)
+      //       {
+      //         if ((n+1) != robotid) // exclude current ugv as it own obstacle
+      //         {
+      //           current_obs[n_obs+ugv_ind].q(0) =ugv_state_q(0, n) ;
+      //           current_obs[n_obs+ugv_ind].q(1) =ugv_state_q(1, n) ;
+      //           current_obs[n_obs+ugv_ind].radius = robot_ind.safe_ditance + robot_ind.robot_size;
+      //           ugv_ind = ugv_ind+1;
+      //         }
+      //
+      //       }
+      //     }
+      //
+      //   }
+      //   else
+      //   {
+      //     current_obs = NULL;
+      //   }
+      // //
+      //   /* call controller*/
+      //   input = swam_controller.controller(current_state_q, current_state_p,current_waypoint,current_obs);
+      //   /* integrate acceleration*/
+      //   if(next_waypoint->stop)
+      //   {
+      //     current_command.linear.x = 0;
+      //     current_command.linear.y = 0;
+      //     current_command.angular.z = 0;
+      //   }
+      //   else
+      //   {
+      //     current_command.linear.x = input(0,0);
+      //     current_command.linear.y = input(1,0);
+      //     current_command.angular.z = 0;
+      //
+      //   }
+      //
+      //
+      //
+      // }
+      // vel_cmd_publisher.publish(current_command);
 
     }
     if(coord_config->stop_demo)
diff --git a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
index ce0145cbaf507b9811ed88cd68adc042c6679c66..6b9ffcff3bdc59f43f914bcc0275eaf34bee2f40 100644
--- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
@@ -24,7 +24,7 @@ t(robot_ind.t)
     this->state_sub_vel = nh.subscribe(ss_vel.str(), 1,&robotState::stateCallbackUavVel, this);
     this->state_sub_acc = nh.subscribe(ss_acc.str(), 1,&robotState::stateCallbackUavAcc, this);
 
-    ROS_INFO("Robot %d state subcribed", this->id );
+    ROS_INFO("Crazyflie %d state subcribed", this->id );
     ros::topic::waitForMessage< crazyswarm::GenericLogData >(ss_vel.str(), ros::Duration(5));
     ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_pose.str(), ros::Duration(5));
     ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_acc.str(), ros::Duration(5));
@@ -44,7 +44,7 @@ t(robot_ind.t)
     this->state_sub_vel = nh.subscribe(ss_vel.str(), 1,&robotState::stateCallbackUgvVel, this);
     this->state_sub_acc = nh.subscribe(ss_acc.str(), 1,&robotState::stateCallbackUgvAcc, this);
 
-    ROS_INFO("Robot %d state subcribed", this->id );
+    ROS_INFO("Omniwheels %d state subcribed", this->id );
     ros::topic::waitForMessage<geometry_msgs::Twist>(ss_vel.str(), ros::Duration(5));
     ros::topic::waitForMessage<geometry_msgs::Pose>(ss_pose.str(), ros::Duration(5));
     ros::topic::waitForMessage<geometry_msgs::Vector3>(ss_acc.str(), ros::Duration(5));
diff --git a/simulation/controller/SwamControllerAlg.asv b/simulation/controller/SwamControllerAlg.asv
deleted file mode 100644
index 167694d7606d46da9ef52d918de705fee8e252c8..0000000000000000000000000000000000000000
--- a/simulation/controller/SwamControllerAlg.asv
+++ /dev/null
@@ -1,431 +0,0 @@
-classdef SwamControllerAlg 
-    properties
-        formation_param % information on paramters
-        gain_param % struct gains c1 c2 for alpha(agent) c1 c2(obstacle) c1 c2(gamma)
-        indx %position of the robot in the distance matrix
-        integral_error % store the integrator
-        previous_error % store for integral control
-
-
-    end
-
-    properties (Access = private)
-        robot_size  %impose robot safety circle diameter
-        id % to indxentify the robot
-    end
-
-    properties (Constant)
-        safe_distance = 0.15 % safety distance around the robot in
-    end
-
-
-    methods
-        % -- constructor
-        function thisSwamControllerAlg = SwamControllerAlg(robot_size, id,indx,form_param, gains)
-            % Important, this computation is valindx for 2d positions, 3d not
-            % included
-            thisSwamControllerAlg.robot_size = robot_size+thisSwamControllerAlg.safe_distance;
-            thisSwamControllerAlg.indx = indx;
-            thisSwamControllerAlg.id = id;
-            thisSwamControllerAlg.gain_param  =  gains;
-            thisSwamControllerAlg.integral_error = [0;0];
-            thisSwamControllerAlg.previous_error = [0;0];
-
-            % recompute c2 gain to ensure the following conditions
-            %c1_apha < c1_gamma< c1_beta
-            if false %gains.c1_alpha > gains.c1_gamma || gains.c1_gamma > gains.c1_beta
-                error("Gain conditions not meet: c1_alpha < c1_gamma < c1_beta");
-            end
-            % recompute c2  gain based on the following equation c2 =
-            % c1_gamma = s*sqrt(c) based on original paper
-            thisSwamControllerAlg.formation_param = form_param;
-            % ensure 0 < a < b is met
-            if (0 > form_param.a) && (form_param.a >form_param.b)
-                error('Error. 0 < a <= b not met');
-
-            end
-            % safety
-            if form_param.nav_type <= 0 || form_param.nav_type >2
-                error("Navigational_type should be 1-converge approach or 2 - parallel approach");
-            end
-            if form_param.nav_type ~= 2
-                thisSwamControllerAlg.gain_param.c2_alpha = 2*sqrt(thisSwamControllerAlg.gain_param.c1_alpha);
-                thisSwamControllerAlg.gain_param.c2_beta = 2*sqrt(thisSwamControllerAlg.gain_param.c1_beta);
-                thisSwamControllerAlg.gain_param.c2_gamma = 2*sqrt(thisSwamControllerAlg.gain_param.c1_gamma);
-            end
-            % formation
-            thisSwamControllerAlg.formation_param.r = form_param.k*form_param.dist; % Neighbourhood distance
-
-
-            % obstacle avoindxance parameters
-            thisSwamControllerAlg.formation_param.d_obs = form_param.ratio*form_param.dist;
-            thisSwamControllerAlg.formation_param.r_obs = form_param.k*thisSwamControllerAlg.formation_param.d_obs;
-            if thisSwamControllerAlg.formation_param.d_obs < thisSwamControllerAlg.robot_size/2
-                error("Minimum distance to obstacle is too low and can be dangerous, increase d/d_ob ratio");
-            end
-
-
-            % perform some formation configuration;
-            thisSwamControllerAlg = thisSwamControllerAlg.configureFormation();
-            disp("Robot: "  + id + " Initialized");
-
-
-        end
-        % -- Function responsible forcreating the formation parameters
-        function thisSwamControllerAlg = configureFormation(thisSwamControllerAlg)
-            % ensure distance is more thatn robot safe area
-            dist = thisSwamControllerAlg.formation_param.dist;
-            form_t = thisSwamControllerAlg.formation_param.type;
-            if dist < (thisSwamControllerAlg.robot_size/2)
-                error("Distance between robot will result in collision minimum radius distance: " + thisSwamControllerAlg.robot_size/2);
-            end
-
-            % formation configuration
-            if form_t == 2
-                % 2 man formation following
-                thisSwamControllerAlg.formation_param.iad = [0,dist;    %interagent distance
-                    dist,0];
-                thisSwamControllerAlg.formation_param.N = 2; % number of agent
-                thisSwamControllerAlg.formation_param.node_names = {'d1','d2'};
-
-
-
-            elseif form_t == 3 % triangular formation
-                thisSwamControllerAlg.formation_param.iad = [0,dist, dist;    %interagent distance
-                    dist, 0, dist;
-                    dist, dist, 0];
-                thisSwamControllerAlg.formation_param.N = 3; % number of agent
-                thisSwamControllerAlg.formation_param.node_names = {'d1','d2','d3'};
-
-            elseif form_t == 4 % square formation
-                h = sqrt(2*dist*dist); % diagonal distance
-                thisSwamControllerAlg.formation_param.iad = [0,dist, h, dist;    %interagent distance
-                    dist, 0,dist, h;
-                    h,dist,0,dist;
-                    dist,h, dist,0];
-                thisSwamControllerAlg.formation_param.N = 4; % number of agent
-                thisSwamControllerAlg.formation_param.node_names = {'d1','d2','d3','d4'};
-
-
-            elseif form_t == 5 % pentagon formation
-                h = dist + 2*dist*cos(deg2rad(72)); %diagonal distance
-                thisSwamControllerAlg.formation_param.iad = [0,dist,h,h,dist;    %interagent distance
-                    dist, 0, dist,h,h;
-                    h,dist,0,dist, h;
-                    h,h,dist,0,dist;
-                    dist,h,h,dist,0];
-                thisSwamControllerAlg.formation_param.N = 5; % number of agent
-                thisSwamControllerAlg.formation_param.node_names = {'d1','d2','d3','d4','d5'};
-
-            else
-                error("Wrong formation type: triangle:1 square:2 pentagon: 3");
-            end
-            disp("Interagent distance matrix: ");
-            disp(thisSwamControllerAlg.formation_param.iad);
-        end
-
-        % -- display formation
-        function showFormation(thisSwamControllerAlg)
-            figure
-            if thisSwamControllerAlg.formation_param.type > 2
-            pgon1 = nsindxedpoly(thisSwamControllerAlg.formation_param.N,'Center',[0 0],'SindxeLength',thisSwamControllerAlg.formation_param.dist);
-            plot(pgon1)
-            for i =1:thisSwamControllerAlg.formation_param.N
-                text(pgon1.Vertices(i,1),pgon1.Vertices(i,2),"d" + string(i));
-
-            end
-            elseif  thisSwamControllerAlg.formation_param.type == 2
-                plot([0; thisSwamControllerAlg.formation_param.dist], [0,0])
-                text(0,0,"d1")
-                text(thisSwamControllerAlg.formation_param.dist,0,"d2")
-
-            end
-%             axis equal
-
-        end
-
-        % -- main controller
-        function [input, input_vec,q_obs_vector] = controller(thisSwamControllerAlg,q,p,ref,ob, ori, cooperation)
-            % q is the current position of each of the drone- column vector
-            % p is the current velocitu of each of the drones
-            % ob is the current position of the obstacles [x,y,r]
-            % orientatoin in degs
-            if size(q,1) > 2
-                error("Controller is intended for 2d plane, not 3d, send x y inputs");
-            end
-
-            % ----- formation computation
-            q_rf = q'; % convert to row format
-            [ind,dist] = knnsearch(q_rf,q_rf(thisSwamControllerAlg.indx,:),"K",thisSwamControllerAlg.formation_param.N);
-            I = find(dist <= thisSwamControllerAlg.formation_param.r);
-            if size(I,2) == 1 % only robot % dangerours because in real world the position can be different for the same robot, do not implement this in the real robot
-                warning("Robot does not seem to have any neighbour, make sure this was done intentionally"); % TODO implement better logging
-            end
-            N_agent = ind(1,1:size(I,2));% agent neigbours
-            s_Na = size(N_agent,2);
-            u_formation = [0;0];
-            grad_term = [0;0];
-            cons_term = [0;0];
-            deriv_term = [0;0];
-            a = thisSwamControllerAlg.formation_param.a;
-            b = thisSwamControllerAlg.formation_param.b;
-            h_alpha = thisSwamControllerAlg.formation_param.h_alpha;
-            eps = thisSwamControllerAlg.formation_param.eps;
-            r = thisSwamControllerAlg.formation_param.r;
-            qi =  q(:,thisSwamControllerAlg.indx);
-            pi_ = p(:,thisSwamControllerAlg.indx);
-
-
-
-            for i = 1: s_Na
-                if N_agent(i) ~= thisSwamControllerAlg.indx % only for neigbours
-                    qj = q(:,N_agent(i));
-                    pj = p(:,N_agent(i));
-                    if cooperation == 1
-                    d = thisSwamControllerAlg.formation_param.iad(thisSwamControllerAlg.indx,N_agent(i));
-                    else
-                    d = thisSwamControllerAlg.formation_param.follow_dist;
-                    end
-                    % gradient term
-                    fi = thisSwamControllerAlg.computeFi(qj,qi,a,b,h_alpha,eps,r,d);
-                    nij = SwamControllerAlg.computeNij(qj,qi, eps);
-                    grad_term = grad_term + fi*nij;
-                    % consesus term
-                    if thisSwamControllerAlg.formation_param.integrator == 2
-                        aij = SwamControllerAlg.computeAij(qj,qi,r,eps,h_alpha);
-                        cons_term = cons_term +aij*(pj-pi_);
-
-                        deriv_term = deriv_term + aij*(qj-qi);
-
-                    end
-                     
-            
-                end
-
-            end
-            error_grad_term = grad_term;
-            u_formation = grad_term*thisSwamControllerAlg.gain_param.c1_alpha + cons_term*thisSwamControllerAlg.gain_param.c2_alpha;
-            % obstacle formation
-            % TODO current implementation only consindxers circular
-            % Todo plot gradient and potencial
-            % obstacles, include hyperplane
-
-            if isempty(ob)
-                u_obstacle = [0;0];
-q_obs_vector = [0;0];
-
-            else
-                % compute position and velocity of obstacles
-                % projection of agent onto obstacle
-                ob_s = size(ob,2);
-                r_obs = thisSwamControllerAlg.formation_param.r_obs;
-                d_obs = thisSwamControllerAlg.formation_param.d_obs;
-                h_beta = thisSwamControllerAlg.formation_param.h_beta;
-
-                %                 q_hat = zeros(2,ob_s); % positions of all the obstables
-                %                 p_hat = zeros(2,ob_s); % velocities of all the obstacles
-                %                 N_beta = [];
-                q_obs_vector = zeros(2,ob_s);
-                    grad_term_obs = [0;0];
-                    cons_term_obs = [0;0];
-                    nav_term_obs = [0;0];
-                for o = 1:ob_s
-                    yk = ob(1:2,o); % center of obstacle
-                    Rk = ob(3,o); % radius of obstacle
-                    mu = Rk/norm(qi-yk);
-                    ak = (qi-yk)/norm(qi-yk);
-                    a__ = ak*ak';
-                    sa = size(a__,1);
-                    P = eye(sa)- a__;
-                    q_hat_i = mu*qi + (1-mu)*yk;
-                    p_hat_i = mu *P*pi_;
-                  
-%                     qi = thisSwamControllerAlg.indx;
-%    if thisSwamControllerAlg.indx == 1
-%                     disp("Here");
-%                 end
-                    d2obs = norm(qi- q_hat_i);
-                    q_obs_vector(:,o) = q_hat_i;
-                    if d2obs < r_obs %Neighbourhood obstacles of agent
-                        % gradient term
-                        fi_beta = SwamControllerAlg.computeFiB(q_hat_i,qi,h_beta,d_obs,eps);
-                        nij = SwamControllerAlg.computeNij(q_hat_i,qi, eps);
-                        grad_term_obs = grad_term_obs + fi_beta*nij;
-                        % consesus term
-                        if thisSwamControllerAlg.formation_param.integrator == 2
-
-                            bij = SwamControllerAlg.computeBij(q_hat_i,qi,d_obs,eps, h_beta);
-                            cons_term_obs = cons_term_obs +bij*(p_hat_i-pi_);
-                        end
-% 
-%                          nav_term_obs = nav_term_obs -thisSwamControllerAlg.gain_param.c1_gamma*(SwamControllerAlg.sigmaOne(q_hat_i-ref.q))- ...
-%                         thisSwamControllerAlg.gain_param.c2_gamma*(p_hat_i-ref.p);
-                    end
-                end
-%              
-                u_obstacle = thisSwamControllerAlg.gain_param.c1_beta* grad_term_obs ...
-                    +thisSwamControllerAlg.gain_param.c2_beta*cons_term_obs;
-
-            end
-
-            % navigational term
-            % TODO: implement wait until formation potential has reach a
-            % threshold
-            u_navigation = [0;0];
-            if cooperation == 1
-                if thisSwamControllerAlg.formation_param.nav_type == 1
-                    % convergence approach
-                    u_navigation = -thisSwamControllerAlg.gain_param.c1_gamma*(SwamControllerAlg.sigmaOne(qi-ref.q))- ...
-                        thisSwamControllerAlg.gain_param.c2_gamma*(pi_-ref.p);
-                else
-                    %parallel approach
-                    % compute the centroindx info
-                    mean_q = [mean(q(1,:)); mean(q(2,:))];
-                    mean_p = [mean(p(1,:)); mean(p(2,:))];
-    
-                    u_navigation = -thisSwamControllerAlg.gain_param.c1_gamma*(SwamControllerAlg.sigmaOne((mean_q-ref.q)))- ...
-                        thisSwamControllerAlg.gain_param.c2_gamma*(mean_p-ref.p);
-%                     u_navigation = sat_func(u_navigation,0.05);
-     
-                
-                end
-            end
-
-
-            % orientation term
-            % compute the centroindx vector
-            u_orientation = [0;0];
-            if ~isempty(ori)
-                    desired_angle = deg2rad(ori);
-                    % convert to 3d
-                    desired_vector = [cos(desired_angle); sin(desired_angle);0];
-                    if cooperation == 1
-                    q_centroid = [mean(q,2);0];
-                    q_leader = [q(:,1);0];
-                    else
-                        q_centroid = [q(:,1);0];
-                        q_leader = [q(:,2);0];
-
-                    end
-                    dlc = norm(q_leader-q_centroid); % distance from centroid to leader drone
-                    lcv = q_leader-q_centroid; % vector from centroid to leader
-                    dcv = dlc*desired_vector; % vector from centroid to desired final pose of leader
-                    acl =atan2(lcv(2), lcv(1));%angle of vector from centroid to leadership dron 
-                    acc = atan2(dcv(2), dcv(1)); % angle of vector from fron centroid to final drone pose
-                    if cooperation == 2
-                        acl =atan(lcv(2)/ lcv(1));%angle of vector from centroid to leadership dron 
-                        acc = atan(dcv(2)/dcv(1)); % angle of vector from fron centroid to final drone pose
-                    end
-                    alpha = acc-acl;%angle between vector lcv and dcv
-                    % ---- We want to compute the rotational acceleration
-                    % direction
-                    q_hat = [q(:,thisSwamControllerAlg.indx); 0];% current drone pose
-                    cq_hat = q_hat - q_centroid;% vector from centroid to current drone
-                    rot_d =cross(cq_hat, [0,0,-alpha]) ; %rotaiton direction
-
-                    if norm(rot_d) == 0
-                        rot_d = [0;0];
-                    else
-                    rot_d = rot_d/norm(rot_d); % convert to unit vecor; 
-                    end
-                    u_orientation = thisSwamControllerAlg.gain_param.c1_theta*abs(alpha)*[rot_d(1);rot_d(2)];
-            end
-
-            % integral controld
-            % error obtained from the formation gradient term
-            thisSwamControllerAlg.integral_error = thisSwamControllerAlg.integral_error + error_grad_term*thisSwamControllerAlg.formation_param.dt;
-            % saturate error to prevent anti-winup
-%             thisSwamControllerAlg.integral_error = sat_func( thisSwamControllerAlg.integral_error, thisSwamControllerAlg.formation_param.int_max);
-            u_integral = thisSwamControllerAlg.gain_param.c1_delta * thisSwamControllerAlg.integral_error; % + deriv_term*thisSwamControllerAlg.gain_param.c2_delta;
-%             u_derivative = ((error_grad_term-previous_error)/ thisSwamControllerAlg.formation_param.dt)*thisSwamControllerAlg.gain_param.c3_delta;
-%             thisSwamControllerAlg.previous_error = error_grad_term;
-%                 ce = error_grad_term;
-
-            input = u_formation + u_obstacle + u_navigation+u_orientation + u_integral+ 0;
-            input_vec = [u_formation,u_obstacle,u_navigation,u_orientation,u_integral]; % debug purposes
-%             potential = [potential_formation,potential_obstacle, potential_navigation, potential_orietation];
-        end
-
-    end
-    methods (Static)
-
-        % - computes the sigma norm of a vector
-        function out = computeSigmaNorm(z,eps)
-            n = norm(z);
-            out = (1/eps)*((sqrt(1+eps*n*n))-1);
-        end
-        % compute the fi alpha function
-        function out = computeFi(qj,qi,a,b,h,eps,r,d)
-            % qj is the neighbour
-            % qi is the drone itself
-            % sigma norms of the error-cut off radius- desired distance
-            z_sn = SwamControllerAlg.computeSigmaNorm((qj-qi), eps); % this can be done outsindxe the function
-            r_sn = SwamControllerAlg.computeSigmaNorm(r, eps);
-            d_sn = SwamControllerAlg.computeSigmaNorm(d, eps);
-            out = SwamControllerAlg.ph(z_sn/r_sn,h) * SwamControllerAlg.fi(a,b,(z_sn-d_sn));
-        end
-
-
-        % - computes the fi function
-        function out = fi( a,b,z)
-            c = abs(a-b)/sqrt(4*a*b);
-            out = 0.5 * (((a+b)*SwamControllerAlg.sigmaOne(z+c)) + (a-b));
-        end
-
-        % - computes the ph bumper function
-        function out = ph(z, h)
-            % the bumper function maps R+ to [0,1]
-            if max(size(z)) > 1
-                error("Bumper function: value must be scalar")
-            end
-            if( z >= 0) && ( z<h)
-                out = 1;
-            elseif (z >= h) && (z<=1)
-                out = 0.5*(1+cos(pi*(z-h)/(1-h)));
-            else
-                out = 0;
-            end
-
-        end
-
-        % computes the sigma1
-        function out = sigmaOne(z)
-            n = norm(z);
-            out = z/sqrt(1+(n*n));
-        end
-
-        % --- compute nij function neighbourhood function
-        function out = computeNij(qj,qi, eps)
-            % qj is the neighbour
-            % qi is the drone itself
-            z = qj -qi;
-            n = norm(z);
-            out = z/sqrt(1+(eps*n*n));
-
-
-        end
-
-        % -- compute the adjency matrix element
-        function out = computeAij(qj,qi,r,eps,h)
-            z_sn = SwamControllerAlg.computeSigmaNorm(qj-qi, eps);
-            r_sn = SwamControllerAlg.computeSigmaNorm(r,eps);
-            out = SwamControllerAlg.ph(z_sn/r_sn,h);
-        end
-
-        % -- helper function for obstacle
-        % damping function
-        function out = computeFiB(qj,qi,h_beta,d_obs,eps)
-            z_sn = SwamControllerAlg.computeSigmaNorm(qj-qi,eps);
-            d_obs_sn = SwamControllerAlg.computeSigmaNorm(d_obs,eps);
-            out = SwamControllerAlg.ph(z_sn/d_obs_sn,h_beta) * (SwamControllerAlg.sigmaOne(z_sn-d_obs_sn)-1);
-        end
-        function out = computeBij(qj,qi,d_obs,eps, h)
-            z_sn = SwamControllerAlg.computeSigmaNorm(qj-qi,eps);
-            d_obs_sn = SwamControllerAlg.computeSigmaNorm(d_obs,eps);
-            out = SwamControllerAlg.ph(z_sn/d_obs_sn,h);
-        end
-    end
-
-end
-
-