diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index c81884d1cfb8df229a9969223db3830f3308130c..3f680fc5c077e922031d90d4c5605555a169ea81 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -3,7 +3,7 @@ uav_c1_alpha: 0.20
 uav_c2_alpha: 0.3 # this is recomputed
 uav_c1_beta: 0.3
 uav_c2_beta: 0.1 # this is recomputed
-uav_c1_gamma: 0.25
+uav_c1_gamma: 0.3
 uav_c2_gamma: 0.2
 uav_c1_theta: 0.2
 uav_c1_delta: 0.09
@@ -27,7 +27,7 @@ uav_b: 5
 uav_h_alpha: 0.2
 uav_h_beta: 0.9
 uav_d_obs: 0
-uav_nav_type:  2 #/* convergence approach -1, parallel approach 2*/
+uav_nav_type:  1 #/* convergence approach -1, parallel approach 2*/
 uav_integrator: 2 #/* single/ double integrator*/
 uav_dt:  0.01 #/* sample time*/
 uav_int_max:  0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index e0455b43ead7b07fb6b390e97a8bd1fc06b51499..9b0637d34543286b538d50305af4856c53e97c24 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -5,7 +5,7 @@ mass: 33 # grams
 include_obstacle: false
 include_orientation: false
 n_obs: 0
-log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_8_working/test"
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_9_working/test"
 enable_log: true
 
 # waypoint
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..11204819d3ce10bca124bed0d83222c58bbd6a6a
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..0de3b9517f341b32bdf3876959c3ffd26ec9ac8e
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
index 0adde6e122582df8bbede8a98f588645d6d94944..733f826171c8b44b53bb956f0a1c382d4e26f93b 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -340,10 +340,11 @@ int main(int argc, char **argv) {
           row_vector_2d_t in;
           in << ax, ay;
           row_vector_2d_t range;
-          range << -0.15, 0.15;
+          // we are saturating the acceleration
+          range << -2, 2;
           row_vector_2d_t out = saturate(in, range);
-          // current_command.roll = out(0)/mass; // conver it to the desired angle
-          // current_command.pitch = out(1)/mass;
+          current_command.roll = -out(1)*180.0/(mass*M_PI); // conver it to the desired angle
+          current_command.pitch = out(0)*180.0/(mass*M_PI);
           // vxx = vxl;
           // vyy = vyl;
         }
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
index bd9157150db320c2d54a630ec6d0bf1a76011b84..8535199541d23a339bc41aa734bc23edcae1a80b 100644
--- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -304,14 +304,14 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
 
     if(this->form_param.param.nav_type == 1)
     {
-      u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne((qi-q_ref));//-this->form_param.gain.c2_gamma*(pi-p_ref);
+      u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne((qi-q_ref))-this->form_param.gain.c2_gamma*(pi-p_ref);
       // ROS_ERROR("Here: %f:%f "  (qi-q_ref)(0), (qi-q_ref)(1)l;
     }
     else
     {
       // std::cout << centroid_q << std::endl;
       // std::cout << centroid_p << std::endl;
-    u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne(centroid_q-q_ref);// -this->form_param.gain.c2_gamma*(centroid_p-p_ref);
+    u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne(centroid_q-q_ref) -this->form_param.gain.c2_gamma*(centroid_p-p_ref);
 
     }
   }