diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index 777f40a60887a3d006858f94d746505e35179c3f..04aecd3c6e9657b7656a101ff9c3f6d745632f4e 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -5,7 +5,7 @@ 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_theta: 0.1
uav_c1_delta: 0.07
# uav_c1_alpha: 0.035
@@ -42,10 +42,10 @@ 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_c1_delta: 0.07
-ugv_d: 0.5
+ugv_d: 0.8
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*/
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index ac75ef76dbd115e39af8b5da40ceb5eb46629e40..7e2f797b9124333356696dbbbc2e3a61396d026b 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: 3
-n_ugvs: 0
+n_ugvs: 1
hz_freq: 50 # controller frquency
include_obstacle: false
-include_orientation: true
+include_orientation: false
n_obs: 0
-log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result"
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result"
enable_log: true
# waypoint
diff --git a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
index 279a0cb28bc17f0d2be76804b7179efa6baf8de2..c943c0657cc2c3d232a0a874cf7db82b130f5ae6 100644
--- a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
@@ -10,7 +10,7 @@
<rosparam>
# Logging configuration (Use enable_logging to actually enable logging)
genericLogTopics: ["position", "velocity","acceleration"]
- genericLogTopicFrequencies: [50,50,50]
+ genericLogTopicFrequencies: [30,30,30]
genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z", "stateEstimate.yaw"]
genericLogTopic_velocity_Variables: ["stateEstimateZ.vx", "stateEstimateZ.vy","stateEstimateZ.vz"]
genericLogTopic_acceleration_Variables: ["stateEstimateZ.ax", "stateEstimateZ.ay","stateEstimateZ.az"]
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 8e386d9d1ed06b4ddc230b25cf17119e9c68ebf5..a51133e3878a9acf72894932c331113122c566b0 100644
--- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
@@ -33,11 +33,11 @@
-<!--
+
<group ns="omniwheel_1">
- <node name="omniwheel_node" pkg="coordination_formation_control_pkg" type="omniwheel_node" output="screen" args="1" launch-prefix="xterm -e gdb ">
+ <node name="omniwheel_node" pkg="coordination_formation_control_pkg" type="omniwheel_node" output="screen" args="1" launch-prefix="xterm -e">
</node>
- </group> -->
+ </group>
<!-- launch dynamic reconfigure -->
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag
index 92d5b64b4497d959eb04137f4294f0f30a3314c6..982885079388be659587f39b10b1064bd04aa1c0 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag
index 0ce1a46ab55557e7dca311ab7395c30914a3288f..3d52b1994c10ce790722ca96654b20666b893b6d 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..c80ed1d9d0c65c0e91a1be5821a479e9496fd0f1
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_18_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..a443af573f7b338e3048a3c7042a87d39bdae4f7
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/notes.MD
@@ -0,0 +1,57 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+- Orientation 270 from 90
+
+Experiment premises
+Same as 10 but with 19 but reduced the theta gain
+#comments
+
+Worked like a charm also
+
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+n_obs: 0
+## Formation configuration
+### Uav
+# Uav
+# 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.1
+uav_c1_delta: 0.07
+
+# 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.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..a31e188369c2283a9e4b7d5345cb9a602746ab66
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..ad87a8bc3698d2999d338c992c6fbdf88913a4ed
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..49975028143b9cc0ffb917089db68585c376d17e
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_19_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..39c036674b513d63a4af4b72e161a8268a2552eb
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/notes.MD
@@ -0,0 +1,57 @@
+# Goal
+- Maintain the current centroid
+- X: 0
+- Y: 0
+- Vx: 0
+- Vy: 0
+- Orientation 90 from 0
+
+Experiment premises
+Same as 10 but with 3 drones
+#comments
+
+Worked like a charm also
+
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+n_obs: 0
+## Formation configuration
+### Uav
+# Uav
+# 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.1
+uav_c1_delta: 0.07
+
+# 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.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..70c7588c46b8918d0505597f2cc8839c921039f5
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..f6496dc4ed455ec74f78967a0787b15184109061
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..afc7c53ae4abdbc00cb42191ae16f7cc3e3b0673
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_20_free_space_orientaion_maintain_3_uav/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..1275e92111fe4cad8e291eaf2579a128d5109535
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/notes.MD
@@ -0,0 +1,84 @@
+# Goal
+- Maintain the current centroid
+- X: 1.4
+- Y: 0
+- Vx: 0.001
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+Flockinr g and cooridnation with 1 ugv
+
+#comments
+
+Worked like a charm also
+
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+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.1
+uav_c1_delta: 0.07
+
+# 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: 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.1 #/* 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.07
+
+
+ugv_d: 0.8
+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_21_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..b27a27eb673598b2cb16ab9189bf38726cc50b85
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..9bd0a0ff79c272162332025582fa8fca0b6e9c50
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..68773db1b26b4032a6346249395c2e8d7a5bf398
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..0777792eed60ce029d0cdef14101d33b8e02c87f
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_21_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..1275e92111fe4cad8e291eaf2579a128d5109535
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/notes.MD
@@ -0,0 +1,84 @@
+# Goal
+- Maintain the current centroid
+- X: 1.4
+- Y: 0
+- Vx: 0.001
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+Flockinr g and cooridnation with 1 ugv
+
+#comments
+
+Worked like a charm also
+
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+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.1
+uav_c1_delta: 0.07
+
+# 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: 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.1 #/* 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.07
+
+
+ugv_d: 0.8
+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_22_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..9ef72260a508a2c1319302bb98a3d65a8e919265
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..4be912230bfc062e76080b3546e94b38c0150572
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..932e0cc7718715eea5ce5ef7fb1716c3733b6964
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..d4c9b4ad883ddbc436902ea6798782b7717c6ee8
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_22_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/notes.MD b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/notes.MD
new file mode 100644
index 0000000000000000000000000000000000000000..08287d652b82a3f9dbfdbaeec18727e14f8d18d2
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/notes.MD
@@ -0,0 +1,84 @@
+# Goal
+- Maintain the current centroid
+- X: 1.5
+- Y: 0
+- Vx: 0.001
+- Vy: 0
+- Orientation 0
+
+Experiment premises
+Flockinr g and cooridnation with 1 ugv
+
+#comments
+
+Worked like a charm also
+
+# Mission configuration
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frequency
+include_obstacle: false
+include_orientation: true
+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.1
+uav_c1_delta: 0.07
+
+# 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: 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.1 #/* 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.07
+
+
+ugv_d: 0.8
+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_23_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..7738081e3f4eb2e7f25867727b47fc760f02542f
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..e3f4ed4fdc86b24940891a381d44846f6c114268
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..c3311674b81c6329b2128ca639eb63dfc471cbf2
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..2179b4d559540d54f0c9f45ec7cc3d441df79843
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_23_coord_form_3_uav_1_ugv/result_omniwheel_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
index 65f3a9c7a69b18bfdd83aa1063e6e05f6f91b35c..699bcf2773d4ae9a76b5e1da8dee4ec078dc803c 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
@@ -73,8 +73,11 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]):
bag.close()
t_np = np.array(t_)
+# print(pose_data_np)
+
# remove the offset
t_np = t_np -t_np[0]
+
pose_data_np = np.array(pose_data);
# print(pose_data_np)
vel_data_np = np.array(vel_data);
diff --git a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
index 83e464a0432740251154334a606b34a4e41d3fb7..2c46e6e53504734966f2605d46c072a17318fcbc 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -306,30 +306,29 @@ for (int i = 0; i ++; i < n_ugvs)
current_cluster = this->compute_cluster(uav_state_q, ugv_state_q, n_ugvs,n_drones, previous_labels, previous_centroids);
previous_centroids = current_cluster;
// std::cout << "outside after previous lable" << previous_labels <<std::endl;
+ /* assign clusters*/
+ cluster_ind = 0;
+ // if(n_ugvs > 0)
+ // {
+ for(int i = 0; i < n_ugvs; i++)
+ {
+
+ this->current_waypoint.cluster_pos[cluster_ind] = current_cluster(0,i);
+ this->current_waypoint.cluster_pos[cluster_ind+1] = current_cluster(1,i);
+ // previous_center[i].x = current_cluster(0,i);
+ // previous_center[i].y = current_cluster(1,i);
+ cluster_ind +=2;
+ }
+ this->current_waypoint.cluster_vel[0] = current_centroid_vel(0);
+ this->current_waypoint.cluster_vel[1] = current_centroid_vel(1);
+ // }
- // std::cout << current_cluster << std::endl;
}
}
- /* assign clusters*/
- cluster_ind = 0;
- for(int i = 0; i < n_ugvs; i++)
- {
-
- this->current_waypoint.cluster_pos[cluster_ind] = current_cluster(0,i);
- this->current_waypoint.cluster_pos[cluster_ind+1] = current_cluster(1,i);
- // previous_center[i].x = current_cluster(0,i);
- // previous_center[i].y = current_cluster(1,i);
- cluster_ind +=2;
- }
- if(n_ugvs > 0)
- {
- this->current_waypoint.cluster_vel[0] = current_centroid_vel(0);
- this->current_waypoint.cluster_vel[1] = current_centroid_vel(1);
- }
diff --git a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
index 80ab19825ad95d99c7b5ad357463be2da4c443bf..b23eb3737c8f529ea648ce164df685e7560c5ae0 100644
--- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -18,7 +18,7 @@ std::stringstream ss_log_naming;
rosbag::Bag bag;
bool waypoint_received = false;
std::string robot_name;
-bool enable_log;
+bool enable_log = false;
void configurationCallback(const coordination_formation_control_pkg::coordination::ConstPtr& msg );
@@ -210,6 +210,7 @@ int main(int argc, char **argv) {
ugv_state_p(1, i-1) = current_state.p(1);
ugv_state_a(0, i-1) = current_state.a(0);
ugv_state_a(1, i-1) = current_state.a(1);
+ // std::cout << ugv_state_q(0, i-1) << std::endl;
}
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
index 3df94e842e3e822e6dc190300839dedcf1b75250..eb6bf1db31fd1c77264d8035a37c1f91cfeeeb61 100644
--- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -304,7 +304,11 @@ 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));
+ if(this->form_param.param.integrator == 2)
+ {
+ u_navigation = u_navigation -this->form_param.gain.c2_gamma*(pi-p_ref);
+ }
// ROS_ERROR("Here: %f:%f " (qi-q_ref)(0), (qi-q_ref)(1)l;
}
else