made changes to drone_cpp

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*/

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

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;
         }

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);
 
     }
   }