diff --git a/real_robot/coordination_formation_control_pkg/README.md b/real_robot/coordination_formation_control_pkg/README.md
index 03ff56ea2b6f6ab7aa0bcb5c73289cfe8e3461fd..9d2b239e8df4242c846cf0d581ffb50cfd685f3b 100644
--- a/real_robot/coordination_formation_control_pkg/README.md
+++ b/real_robot/coordination_formation_control_pkg/README.md
@@ -16,7 +16,7 @@ implement modules
 - [X] send command should include conversion to velocity
 - [X] test omniwheels node
 - [X] Implement swam controller
--
+- [ ] filter received normal velocity and use it for the integrator 
 
 
 
diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index ba8e2d2a49590c7e6586d9a47762a9230a3d11d6..56f5bc9b9f73e67a8139c605ab26b6e331c43391 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -6,7 +6,7 @@ uav_c2_beta: 0.1 # this is recomputed
 uav_c1_gamma: 0.25
 uav_c2_gamma: 0.2
 uav_c1_theta: 0.2
-uav_c1_delta: 0.0
+uav_c1_delta: 0.05
 
 # uav_c1_alpha: 0.035
 # uav_c2_alpha: 0.04 # this is recomputed
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index 855b2d5e0ad80ca516b446c7264407a4a254774a..9a222a018eadafb6c8d4eefca2d2be42ed3c1682 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -4,7 +4,7 @@ hz_freq: 50 # controller frquency
 include_obstacle: false
 include_orientation: false
 n_obs: 0
-log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test"
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_14_working_with_integral/test"
 enable_log: true
 
 # waypoint
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..56f5bc9b9f73e67a8139c605ab26b6e331c43391
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/notes.txt
@@ -0,0 +1,61 @@
+# Uav
+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_c2_gamma: 0.2
+uav_c1_theta: 0.2
+uav_c1_delta: 0.05
+
+# uav_c1_alpha: 0.035
+# uav_c2_alpha: 0.04 # this is recomputed
+# uav_c1_beta: 0.04
+# uav_c2_beta: 0.02 # this is recomputed
+# uav_c1_gamma: 0.02
+# uav_c2_gamma: 0.03
+# uav_c1_theta: 0.3
+# uav_c1_delta: 0.01
+
+uav_d: 0.8
+uav_formation_t:  2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio:  0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+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.01 #/* maximum integral*/
+
+# ugv
+
+# Uav
+ugv_c1_alpha: 0.2
+ugv_c2_alpha: 1 # this is recomputed
+ugv_c1_beta: 0.3
+ugv_c2_beta: 1 # this is recomputed
+ugv_c1_gamma: 0.25
+ugv_c2_gamma: 0.2
+ugv_c1_theta: 0.2
+ugv_c1_delta: 0.09
+
+
+ugv_d: 0.5
+ugv_formation_t: 2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ugv_k: 7
+ugv_ratio:  0.8 # /* ratio between dist inter and obstacle*/
+ugv_eps: 0.1
+ugv_a: 5
+ugv_b: 5
+ugv_h_alpha: 0.2
+ugv_h_beta: 0.9
+ugv_d_obs: 0
+ugv_nav_type:  1 #/* convergence approach -1, parallel approach 2*/
+ugv_integrator: 1 #/* single/ double integrator*/
+ugv_dt:  0.02 #/* sample time*/
+ugv_int_max:  0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/test_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..2cb6f6e2c31780438b0ce1177b49f657db41e109
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/test_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/test_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..4af15211212924e5c19fa1179248d48d1cc8b211
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/test_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..56f5bc9b9f73e67a8139c605ab26b6e331c43391
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/notes.txt
@@ -0,0 +1,61 @@
+# Uav
+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_c2_gamma: 0.2
+uav_c1_theta: 0.2
+uav_c1_delta: 0.05
+
+# uav_c1_alpha: 0.035
+# uav_c2_alpha: 0.04 # this is recomputed
+# uav_c1_beta: 0.04
+# uav_c2_beta: 0.02 # this is recomputed
+# uav_c1_gamma: 0.02
+# uav_c2_gamma: 0.03
+# uav_c1_theta: 0.3
+# uav_c1_delta: 0.01
+
+uav_d: 0.8
+uav_formation_t:  2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio:  0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+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.01 #/* maximum integral*/
+
+# ugv
+
+# Uav
+ugv_c1_alpha: 0.2
+ugv_c2_alpha: 1 # this is recomputed
+ugv_c1_beta: 0.3
+ugv_c2_beta: 1 # this is recomputed
+ugv_c1_gamma: 0.25
+ugv_c2_gamma: 0.2
+ugv_c1_theta: 0.2
+ugv_c1_delta: 0.09
+
+
+ugv_d: 0.5
+ugv_formation_t: 2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+ugv_k: 7
+ugv_ratio:  0.8 # /* ratio between dist inter and obstacle*/
+ugv_eps: 0.1
+ugv_a: 5
+ugv_b: 5
+ugv_h_alpha: 0.2
+ugv_h_beta: 0.9
+ugv_d_obs: 0
+ugv_nav_type:  1 #/* convergence approach -1, parallel approach 2*/
+ugv_integrator: 1 #/* single/ double integrator*/
+ugv_dt:  0.02 #/* sample time*/
+ugv_int_max:  0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/test_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..fea51aca9fe23e423c4687b7c900faece12c006b
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/test_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/test_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..9c5c931c22068cd16e5e83fab64c20d11b189fc1
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/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 17e8c69fdc2cc492ad82ba47f740610923745965..4a49db2763a523d1c4fcb6c5e2d101c51bb9948d 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -319,7 +319,7 @@ int main(int argc, char **argv) {
           current_command.vx = 0;
           current_command.vy = 0;
         }
-        else if(abs(uav_state_q(0, robot_id-1)) > 1.7 || abs(uav_state_q(1, robot_id-1)) > 1.))
+        else if((abs(uav_state_q(0, robotid-1)) > 1.7) || (abs(uav_state_q(1, robotid-1)) > 1.0))
         {
           current_command.vx = 0;
           current_command.vy = 0;