diff --git a/real_robot/coordination_formation_control_pkg/CMakeLists.txt b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
index 436337c7362a6402e104405a5bc00b90307f5f58..03ac881826186e3d4d026f657ae00da57ecb0d4e 100644
--- a/real_robot/coordination_formation_control_pkg/CMakeLists.txt
+++ b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
@@ -79,3 +79,7 @@ install(DIRECTORY
launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
+
+catkin_install_python(PROGRAMS scripts/model_ugv_data.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
index ccb63c8d161409967bd8c20b9074d7bdda110517..8d7912f8688fa833d2f7f81712501c026589cbef 100644
--- a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
@@ -7,10 +7,10 @@ crazyflies:
channel: 90
initialPosition: [0.0, 0.0, 0.0]
type: default
-# - id: 3
-# channel: 80
-# initialPosition: [0.0, 0.0, 0.0]
-# type: default
+- id: 3
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
# - id: 4
# channel: 80
# initialPosition: [0.0, 0.0, 0.0]
diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index 56f5bc9b9f73e67a8139c605ab26b6e331c43391..92700003cc8bc762f7cd2cbb61febf33ef05fdea 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -18,7 +18,7 @@ uav_c1_delta: 0.05
# 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_formation_t: 3 #/* 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
@@ -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.01 #/* maximum integral*/
+uav_int_max: 0.1 #/* 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 9a222a018eadafb6c8d4eefca2d2be42ed3c1682..643f908d27c2080d448d320e9627563133790e91 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,10 +1,10 @@
-n_drones: 2
+n_drones: 3
n_ugvs: 0
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_14_working_with_integral/test"
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result"
enable_log: true
# waypoint
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 b302d49cfaf497d06d278fd18c1285197e6fbc52..8e386d9d1ed06b4ddc230b25cf17119e9c68ebf5 100644
--- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
@@ -17,10 +17,10 @@
<node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="2" launch-prefix="xterm -e">
</node>
</group>
- <!-- <group ns="uav_3">
+ <group ns="uav_3">
<node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="3" launch-prefix="xterm -e">
</node>
- </group> -->
+ </group>
<!-- <group ns="uav_4">
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/config_files/crazyfliesConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_1/config_files/crazyfliesConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/config_files/crazyfliesConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/config_files/formationConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_1/config_files/formationConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/config_files/formationConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/config_files/missionConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_1/config_files/missionConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/config_files/missionConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/notes.txt b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_1/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_1/notes.txt
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/dummy_test/experiment_10_working_without_integral/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_10_working_without_integral/notes.txt
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/dummy_test/experiment_10_working_without_integral/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_10_working_without_integral/test_crazyflie_1_logdata.bag
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/dummy_test/experiment_10_working_without_integral/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_10_working_without_integral/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_10_working_without_integral/test_crazyflie_2_logdata.bag
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/dummy_test/experiment_11_working_without_integral/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_11_working_without_integral/notes.txt
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/dummy_test/experiment_11_working_without_integral/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_11_working_without_integral/test_crazyflie_1_logdata.bag
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/dummy_test/experiment_11_working_without_integral/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_11_working_without_integral/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_11_working_without_integral/test_crazyflie_2_logdata.bag
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/dummy_test/experiment_12_working_with_integral/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_12_working_with_integral/notes.txt
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/dummy_test/experiment_12_working_with_integral/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_12_working_with_integral/test_crazyflie_1_logdata.bag
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/dummy_test/experiment_12_working_with_integral/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_12_working_with_integral/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_12_working_with_integral/test_crazyflie_2_logdata.bag
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/dummy_test/experiment_12_working_without_integral_goodresut_presentation/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_12_working_without_integral_goodresut_presentation/notes.txt
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/dummy_test/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_1_logdata.bag
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/dummy_test/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_12_working_without_integral_goodresut_presentation/test_crazyflie_2_logdata.bag
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/dummy_test/experiment_13_working_with_integral_good_result/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_13_working_with_integral_good_result/notes.txt
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/dummy_test/experiment_13_working_with_integral_good_result/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_13_working_with_integral_good_result/test_crazyflie_1_logdata.bag
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/dummy_test/experiment_13_working_with_integral_good_result/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_13_working_with_integral_good_result/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_13_working_with_integral_good_result/test_crazyflie_2_logdata.bag
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/dummy_test/experiment_14_working_with_integral_working_result/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_14_working_with_integral_working_result/notes.txt
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/dummy_test/experiment_14_working_with_integral_working_result/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_14_working_with_integral_working_result/test_crazyflie_1_logdata.bag
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/dummy_test/experiment_14_working_with_integral_working_result/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_14_working_with_integral_working_result/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_14_working_with_integral_working_result/test_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/config_files/crazyfliesConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/crazyfliesConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/config_files/crazyfliesConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/config_files/formationConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/formationConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/config_files/formationConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/config_files/missionConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/missionConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/config_files/missionConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/experiment_2_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/experiment_2_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/experiment_2_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/experiment_2_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/experiment_2_trianglular_no_formation_crazyflie_3_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_3_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/experiment_2_trianglular_no_formation_crazyflie_3_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/notes.txt b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_2/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_2/notes.txt
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/config_files/crazyfliesConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/crazyfliesConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/config_files/crazyfliesConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/config_files/formationConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/formationConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/config_files/formationConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/config_files/missionConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/missionConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/config_files/missionConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/experiment_3_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/experiment_3_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/experiment_3_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/experiment_3_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/notes.txt b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_3/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_3/notes.txt
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/config_files/crazyfliesConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/crazyfliesConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/config_files/crazyfliesConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/config_files/formationConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/formationConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/config_files/formationConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/config_files/missionConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/missionConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/config_files/missionConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_4_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_4_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_4_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/experiment_4_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/notes.txt b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_4/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_4/notes.txt
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/config_files/crazyfliesConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/crazyfliesConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/config_files/crazyfliesConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/config_files/formationConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/formationConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/config_files/formationConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/config_files/missionConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/missionConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/config_files/missionConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_5_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_5_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_5_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/experiment_5_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/notes.txt b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_5/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_5/notes.txt
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/config_files/crazyfliesConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/config_files/crazyfliesConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/config_files/crazyfliesConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/config_files/formationConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/config_files/formationConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/config_files/formationConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/config_files/missionConfig.yaml
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/config_files/missionConfig.yaml
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/config_files/missionConfig.yaml
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_6_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_6_trianglular_no_formation_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_6_trianglular_no_formation_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_6_trianglular_no_formation_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_6_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_6_trianglular_no_formation_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/experiment_6_trianglular_no_formation_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/experiment_6_trianglular_no_formation_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_working/notes.txt b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/notes.txt
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_6_working/notes.txt
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_6_working/notes.txt
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_7_working/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_7_working/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_7_working/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_7_working/test_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_7_working/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_7_working/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_7_working/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_7_working/test_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_8_working/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_8_working/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_8_working/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_8_working/test_crazyflie_1_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_8_working/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_8_working/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_8_working/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_8_working/test_crazyflie_2_logdata.bag
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/dummy_test/experiment_9_working/test_crazyflie_1_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_1_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_9_working/test_crazyflie_1_logdata.bag
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/dummy_test/experiment_9_working/test_crazyflie_2_logdata.bag
similarity index 100%
rename from real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_2_logdata.bag
rename to real_robot/coordination_formation_control_pkg/results/dummy_test/experiment_9_working/test_crazyflie_2_logdata.bag
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..967b4eb469ba25990c51a816446e264337bfd3c7
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/notes.MD
@@ -0,0 +1,49 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+
+Experiment premises
+Formation control of 3 crazyflies center at 0,0 position and 0,0 velocity without orientation
+
+# comment on the results obtained during test
+They was a lot of oscillation, this might be due to the fact that the localitzation system has a lot of noise and also due to the error between velocity reference and current velocity. Important to state that I added noise to each of the robot, this can be seen in the distance graph, mark those positions
+
+
+#Paramters
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frquency
+include_obstacle: false
+include_orientation: false
+n_obs: 0
+## Formation configuration
+### 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_d: 0.8
+uav_formation_t: 3 #/* 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*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..b129a403380b2d0b1b34618b232c09ff5ee1efb9
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..ebaa52ee3379469ce1dc8e1b9ec92194abec02a1
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..f751a311bf4593ce58ece5f3c34aa1f8c0de914e
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_1_free_space_maintain_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..62216c13885e7585cf808aad9e795aff34750ee0
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/notes.MD
@@ -0,0 +1,50 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+
+Experiment premises
+Formation control of 3 crazyflies center at 0,0 position and 0,0 velocity without orientation
+
+# comment on the results obtained during test
+We increased the maximum integration action to the system to see whether it recuded the oscillations in the formation
+
+
+
+#Paramters
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frquency
+include_obstacle: false
+include_orientation: false
+n_obs: 0
+## Formation configuration
+### 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_d: 0.8
+uav_formation_t: 3 #/* 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.1 #/* sample time*/
+uav_int_max: 0.01 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..b129a403380b2d0b1b34618b232c09ff5ee1efb9
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..ebaa52ee3379469ce1dc8e1b9ec92194abec02a1
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..f751a311bf4593ce58ece5f3c34aa1f8c0de914e
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_2_free_space_maintain_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/scripts/model_ugv_data.py b/real_robot/coordination_formation_control_pkg/scripts/model_ugv_data.py
old mode 100644
new mode 100755
index 4265cc3e6c16c09774190fa55d609cd9fe0808e4..07d19171b60f6aec3a771bdd40d79e4e1cf52359
--- a/real_robot/coordination_formation_control_pkg/scripts/model_ugv_data.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/model_ugv_data.py
@@ -1 +1,95 @@
#!/usr/bin/env python
+import rospy
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Header
+import time
+import numpy as np
+import matplotlib.pyplot as plt
+
+vel_cmd_vector = [[0.5 ,0]];
+N = 1;
+
+data_xy = []
+c = 0
+t = []
+
+def velCallback(msg):
+ global data_xy
+ global c
+ global t
+ t = rospy.Time.now()
+ tp = t.secs + t.nsecs*1e-9
+ # print(c)
+ if c < N:
+ data_xy.append([tp, vel_cmd_vector[c][0], vel_cmd_vector[c][1],msg.linear.x, msg.linear.y])
+
+def main():
+ global data_xy
+ global c
+ global t
+ rospy.init_node('model_ugv_data', anonymous =True)
+ t = Header()
+ pub = rospy.Publisher('omniwheel1/vel_cmd', Twist, queue_size=1);
+ rospy.Subscriber("omniwheel1/vel", Twist, velCallback)
+ rate = rospy.Rate(100)
+ command = Twist()
+ command.linear.x = vel_cmd_vector[c][0]
+ command.linear.y = vel_cmd_vector[c][1]
+ command.linear.z =0
+ command.angular.x = 0
+ command.angular.y = 0
+ command.angular.z = 0
+ t0 = time.time()
+ timer = 0
+ print(vel_cmd_vector[c][:])
+
+ while not rospy.is_shutdown():
+
+ if (timer > 30): #every 3 seconds
+ c = c +1;
+ if c == N:
+ print("Stopping test")
+ break;
+ else:
+ t0 = time.time()
+ timer = 0;
+ print(vel_cmd_vector[c][:])
+ command.linear.x = vel_cmd_vector[c][0]
+ command.linear.y = vel_cmd_vector[c][1]
+ t1 = time.time()
+ timer = t1-t0;
+ # print(timer)
+ pub.publish(command)
+ rate.sleep()
+
+ # convert array to number
+ data_xy_np = np.array(data_xy);
+ if (data_xy_np.size != 0):
+ data_xy_np[:,0] = data_xy_np[:,0]- data_xy_np[0,0]
+ # write it to a file
+ print("Saving data to file")
+ np.savetxt("data_omniwheelsx.csv",data_xy_np, delimiter="," )
+ # plot results
+ ####################### Direct command ###################################
+ fig, (ax1, ax2) = plt.subplots(1, 2)
+ fig.suptitle('Velocity omniwheels command vs current state')
+ ax1.plot(data_xy_np[:,0], data_xy_np[:,1])
+ ax1.plot(data_xy_np[:,0], data_xy_np[:,3])
+ ax2.plot(data_xy_np[:,0], data_xy_np[:,2])
+ ax2.plot(data_xy_np[:,0], data_xy_np[:,4])
+ ax2.set_xlim([0,data_xy_np[-1,0] ])
+ ax2.set_ylim([-0.1, 0.1])
+ ax1.set_xlim([0,data_xy_np[-1,0] ])
+ ax1.set_ylim([-0.1, 0.1])
+ plt.show()
+
+
+
+
+
+
+if __name__ == '__main__':
+ try:
+ main()
+ except rospy.ROSInterruptException:
+ pass
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 4a49db2763a523d1c4fcb6c5e2d101c51bb9948d..667af43f17dd63e6b0bdd68a04d1196e12d746f1 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -343,7 +343,7 @@ int main(int argc, char **argv) {
in << vxl, vyl;
row_vector_2d_t range;
// range << -0.15, 0.15;
- range << -0.5, 0.5;
+ range << -3, 3;
row_vector_2d_t out = saturate(in, range);
current_command.vx = out(0);
diff --git a/simulation/models/drone_modelling_vx_vy_second_order.mlx b/simulation/models/drone_modelling_vx_vy_second_order.mlx
index 0f141c457c124c9dc02455f7978a2e0770b24684..874198acfd8b16573f2434b10e867a770442acd2 100644
Binary files a/simulation/models/drone_modelling_vx_vy_second_order.mlx and b/simulation/models/drone_modelling_vx_vy_second_order.mlx differ
diff --git a/simulation/models/obtain_discrete_model_second_order.m b/simulation/models/obtain_discrete_model_second_order.m
index f0492732f1b50e5df421b16254b3f85e62893d53..b3a17d6914ec406d0ee8ca0af474731279e9c8b4 100644
--- a/simulation/models/obtain_discrete_model_second_order.m
+++ b/simulation/models/obtain_discrete_model_second_order.m
@@ -1,4 +1,4 @@
-function [d_sys, std_error] = obtain_discrete_model_second_order(sys,plot_test_control, dt,t, input, output )
+function [d_sys, std_error] = obtain_discrete_model_second_order(sys,plot_test_control, dt,t, input, output,title_name )
[A, B, C, D] = tf2ss(sys.numerator, sys.denominator);
sys = ss(A,B,C,D);
d_sys = c2d(sys,dt);
@@ -6,6 +6,10 @@ d_sys = c2d(sys,dt);
%% dummy plot
N = max(size(input)); % Size of input data
+ N = cast((N/2)+(N/4),"uint32");
+ input = input(1:N,1);
+ output = output(1:N,1);
+ t = t(1:N,1);
y = zeros(N,1);
x = [0;0];
@@ -29,7 +33,9 @@ if plot_test_control == true
hold on
plot(t, output)
hold off
- title("Dummy test")
+ title(cast((join(["System indentification ", title_name])), "char"))
+ xlabel("t(s)")
+ ylabel("v(m/s)")
legend("System response", "system input", "Experimental data")
end