diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index b8b83be81ddc15e82336fb336a2fc1efe2cefe7f..ba8e2d2a49590c7e6586d9a47762a9230a3d11d6 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.09
+uav_c1_delta: 0.0
 
 # uav_c1_alpha: 0.035
 # uav_c2_alpha: 0.04 # this is recomputed
@@ -30,7 +30,7 @@ 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.1 #/* maximum integral*/
+uav_int_max:  0.01 #/* maximum integral*/
 
 # ugv
 
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index bdce025eea94a5ed7a7665676179414deaac73b1..855b2d5e0ad80ca516b446c7264407a4a254774a 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_9_working/test"
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test"
 enable_log: true
 
 # waypoint
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..eaded728f546f53f7292dbb0e4d762384a622131
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/notes.txt
@@ -0,0 +1 @@
+This one uses the vref to integrate
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/test_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..1080a6a5e21728d2139ded466732577ee2e9f38f
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/test_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/test_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..23b24ef6f5eea171b8a906adb4aa5d34f42e1fb0
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/test_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d3b34e57f68561bd57ba274169e841996c286f06
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/notes.txt
@@ -0,0 +1 @@
+This one uses the vnorm mesurrment to integrate
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/test_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..c6ce6b891bee9312ca101234175ebc07d341a79a
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/test_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/test_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..96aeded585f35e56a5e971c22e40eb034def41a8
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/test_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d3b34e57f68561bd57ba274169e841996c286f06
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/notes.txt
@@ -0,0 +1 @@
+This one uses the vnorm mesurrment to integrate
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..05ea459080f5df16ba71d1950f5371b19ac10bb5
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..adff034961b2639d6c99e7873d7e7cf0cc672e5e
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d3b34e57f68561bd57ba274169e841996c286f06
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/notes.txt
@@ -0,0 +1 @@
+This one uses the vnorm mesurrment to integrate
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..e934cb5155f0eb86e230cdecc1858203bf868633
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..b66b47b50129ad2b2c8eaebc1340b76aeae82437
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_2_logdata.bag differ
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
index 77f37da8f05ca924c6a782a54ed022ce84758f88..49063128327fdfa18a0a0c8a7bb7c929d8f836f3 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_1_logdata.bag 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
index e393bfa7ae159d1fc0d8114b742ae058d47f4bb3..40f7d4b26ac428401d937d911a54e715dc0e4dd1 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_2_logdata.bag 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 e5858f16c3c0e2661616ba6ddff54ebf2538bf3f..17e8c69fdc2cc492ad82ba47f740610923745965 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -24,6 +24,7 @@ std::stringstream ss_log_naming;
 rosbag::Bag bag;
 std::string robot_name;
 bool enable_log;
+bool emergency_stop = false;
 
 
 bool waypoint_received = false;
@@ -318,6 +319,12 @@ 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.))
+        {
+          current_command.vx = 0;
+          current_command.vy = 0;
+          emergency_stop = true;
+        }
         else
         {
           /* transform from global velocity to local velocity*/
@@ -335,7 +342,9 @@ int main(int argc, char **argv) {
           row_vector_2d_t in;
           in << vxl, vyl;
           row_vector_2d_t range;
-          range << -0.2, 0.2;
+          // range << -0.15, 0.15;
+          range << -0.5, 0.5;
+
           row_vector_2d_t out = saturate(in, range);
           current_command.vx = out(0);
           current_command.vy = out(1);
@@ -367,7 +376,7 @@ int main(int argc, char **argv) {
     }
 
     // std::cout << "here " << ((coord_config->stop_demo)?1:0) << std::endl;
-    if(coord_config->stop_demo)
+    if(coord_config->stop_demo || emergency_stop)
     {
       // land_drone();
       // current_command.vx = 0;
@@ -418,12 +427,12 @@ int main(int argc, char **argv) {
       logdata->input.y = input(1,0);
       logdata->input_formation.x = input(0,1);
       logdata->input_formation.y = input(1,1);
-      logdata->input_obstacle.x = current_command.vx; //input(0,2);
-      logdata->input_obstacle.y = current_command.vy;//input(1,2);
+      logdata->input_obstacle.x = input(0,2);
+      logdata->input_obstacle.y = input(1,2);
       logdata->input_navigation.x = input(0,3);
       logdata->input_navigation.y = input(1,3);
-      logdata->input_orientation.x =   uav_state_p(0, robotid-1); //input(0,4);
-      logdata->input_orientation.y = uav_state_p(1, robotid-1);// input(1,4);
+      logdata->input_orientation.x =   input(0,4);
+      logdata->input_orientation.y =input(1,4);
       logdata->input_integration.x = input(0,5);
       logdata->input_integration.y = input(1,5);
       logdata->input_to_system.x = current_command.vx;
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
index 80b31cf39dc54d9c67b18071a771d3831cddedab..911acd90345ff2b69a189a944c8dbfccbab7c786 100644
--- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -312,6 +312,8 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
       // 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);
+    // std::cout <<"q***********" <<  centroid_q-q_ref << std::endl;
+    // std::cout <<"p***********" <<  centroid_p-p_ref << std::endl;
 
     }
   }