diff --git a/real_robot/coordination_formation_control_pkg/src/test_controller.cpp b/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
index 7badb8830482358cd980e18151aa003147ba202d..3944eb038fc8332759dd4ed762bf16f630c1bd47 100644
--- a/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
@@ -41,6 +41,20 @@ int main(int argc, char **argv)
 
 
   robot_ind_t robot_ind;
+  Eigen::MatrixXf uav_state_q(2,3);
+  Eigen::MatrixXf uav_state_p(2,3);
+  obstacle_t *current_obs = new obstacle_t[n_obs]();
+  waypoint_t current_waypoint;
+
+  uav_state_q << 1 1 2 -1 -1.1 -2;
+  uav_state_q << 0 0 0 0 0;
+
+
+  current_waypoint ref;
+  ref.q << 0,0;
+  ref.p << 5,1;
+
+
 
   robot_ind.id = 1;
   robot_ind.id_m = 0;
diff --git a/simulation/controller/test_controllers.m b/simulation/controller/test_controllers.m
index 09e7c1c9d6d626390feb320bd94adebdc03b53c9..8848d1e3b6f241899b47b1a038f683e2f5485534 100644
--- a/simulation/controller/test_controllers.m
+++ b/simulation/controller/test_controllers.m
@@ -14,10 +14,12 @@ controller_gains.c2_beta = 1;
 controller_gains.c1_gamma = 1;
 controller_gains.c2_gamma = 1;
 controller_gains.c1_theta = 1;
+controller_gains.c1_delta = 1;
+
 form_param.dist = 0.2;
 form_param.type = 2;
 form_param.k = 1.2;
-form_param.ratio = 0.8; % ratio between dist inter and obstacle
+form_param.ratio = 7; % ratio between dist inter and obstacle
 form_param.eps = 0.1;
 form_param.a = 5;
 form_param.b = 5;
@@ -26,12 +28,13 @@ form_param.h_beta = 0.9;
 form_param.d_obs = 0;
 form_param.nav_type = 1; % triangle
 form_param.integrator = 2;
+form_param.dt = 0.01;
 % form_param.nav_type = 2; 
 % form_param.nav_type = 2; 
 % form_param.nav_type = 2; 
 
 form_param.integrator = 1;
-
+cooperation = 1;
 %% demo state and goal
 
 q0 = [1 1 2;
@@ -48,11 +51,14 @@ obs = [1, 0,0
     -1.15,0,0;
     0.5,2,3];
 
-ori = [];
+ori = 0;
 
 
 
 
 c1 = SwamControllerAlg(robot_size, id,ind_pos,form_param,controller_gains);
 c1.showFormation
-[input, input_vec ] = c1.controller(q0,p0,ref,obs,0);
\ No newline at end of file
+[input, input_vec ] = c1.controller(q0,p0,ref,obs,ori,cooperation);
+
+input
+input_vec