diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml index c81884d1cfb8df229a9969223db3830f3308130c..3f680fc5c077e922031d90d4c5605555a169ea81 100644 --- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml +++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml @@ -3,7 +3,7 @@ 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_c1_gamma: 0.3 uav_c2_gamma: 0.2 uav_c1_theta: 0.2 uav_c1_delta: 0.09 @@ -27,7 +27,7 @@ uav_b: 5 uav_h_alpha: 0.2 uav_h_beta: 0.9 uav_d_obs: 0 -uav_nav_type: 2 #/* convergence approach -1, parallel approach 2*/ +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/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml index e0455b43ead7b07fb6b390e97a8bd1fc06b51499..9b0637d34543286b538d50305af4856c53e97c24 100644 --- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml +++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml @@ -5,7 +5,7 @@ mass: 33 # grams include_obstacle: false include_orientation: false n_obs: 0 -log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_8_working/test" +log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_9_working/test" enable_log: true # waypoint 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 new file mode 100644 index 0000000000000000000000000000000000000000..11204819d3ce10bca124bed0d83222c58bbd6a6a Binary files /dev/null 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 new file mode 100644 index 0000000000000000000000000000000000000000..0de3b9517f341b32bdf3876959c3ffd26ec9ac8e Binary files /dev/null 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 0adde6e122582df8bbede8a98f588645d6d94944..733f826171c8b44b53bb956f0a1c382d4e26f93b 100644 --- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp +++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp @@ -340,10 +340,11 @@ int main(int argc, char **argv) { row_vector_2d_t in; in << ax, ay; row_vector_2d_t range; - range << -0.15, 0.15; + // we are saturating the acceleration + range << -2, 2; row_vector_2d_t out = saturate(in, range); - // current_command.roll = out(0)/mass; // conver it to the desired angle - // current_command.pitch = out(1)/mass; + current_command.roll = -out(1)*180.0/(mass*M_PI); // conver it to the desired angle + current_command.pitch = out(0)*180.0/(mass*M_PI); // vxx = vxl; // vyy = vyl; } diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp index bd9157150db320c2d54a630ec6d0bf1a76011b84..8535199541d23a339bc41aa734bc23edcae1a80b 100644 --- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp +++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp @@ -304,14 +304,14 @@ 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))-this->form_param.gain.c2_gamma*(pi-p_ref); // ROS_ERROR("Here: %f:%f " (qi-q_ref)(0), (qi-q_ref)(1)l; } else { // 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); + u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne(centroid_q-q_ref) -this->form_param.gain.c2_gamma*(centroid_p-p_ref); } }