for experiment(NB give command)

class_model/include/class_model/formation.h

@@ -55,6 +55,7 @@ private:
     float pre_alt,cur_alt;
     float leader_pid[3] = {0.5 , 0.000000000001 ,0.5};
     float follower_pid[3] = {1 , 0.00000000001 ,0.5};
+    float ignore_small = 0.20;
 
     void calculate_position(float k,float theta,int direction=0);
     void spherical_coordinate(float k,float theta,float psi);

class_model/src/PID.cpp

@@ -12,7 +12,7 @@ float PIDClass::update(float current ,float target ,float pidvals[3]){
     current_time = std::clock();
     // pTime = current_time -
     double t = (current_time - pTime);
-    float error = target - current;
+    float error = (target - current)/1.1;
 
     double P,D,result;
     P = pidvals[0] * error;

class_model/src/formation.cpp

@@ -437,8 +437,8 @@ void FormationClass::calculate_position(float k,float theta,int direction){
     // float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] );       //caculate error
     // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
 
-    // float error_x = Pf[0] - (follower_location.lon*1e+5);            //caculate error
-    // float error_y = Pf[1] - (follower_location.lat*1e+5);
+    float error_lon = Pf[0] - (follower_location.lon*1e+5);            //caculate error
+    float error_lat = Pf[1] - (follower_location.lat*1e+5);
     
     float error_degree = deg_phi - heading + direction;
     float error_yaw = 0.2;   //CCW
@@ -452,11 +452,11 @@ void FormationClass::calculate_position(float k,float theta,int direction){
 
     error_yaw = check_direction(error_degree)*error_yaw;              //check yaw direction
 
-    if (error_x < 0.3 && error_x > -0.3){                     //ignore small errors                  
-        error_x = 0;
+    if (error_lon < ignore_small && error_lon > -ignore_small){                     //ignore small errors                  
+        error_lon = 0;
     }
-    if (error_y < 0.3 && error_y > -0.3){
-        error_y = 0;
+    if (error_lat < ignore_small && error_lat > -ignore_small){
+        error_lat = 0;
     }
     if (error_degree < 5 && error_degree > -5 ){
         error_yaw = 0;
@@ -464,17 +464,18 @@ void FormationClass::calculate_position(float k,float theta,int direction){
         error_yaw = 0;
     }
 
-    // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
-    // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
-    // ROS_INFO("%f,%f",Pf[0],Pf[1]);
-    // ROS_INFO("%f,%f",error_x,error_y);
+    ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
+    ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
+    ROS_INFO("%f,%f",Pf[0],Pf[1]);
+    ROS_INFO("%f,%f",error_x,error_y);
+    ROS_INFO("%f,%f",error_lon,error_lat);
     // ROS_INFO("deg:%f",deg_phi);
     // ROS_INFO("heading:%f",heading);
     // ROS_INFO("error_degree:%f",error_degree);
     // ROS_INFO("error_yaw:%f",error_yaw);
     // ROS_INFO("************************************");
     
-    command_object.set_velocity(error_x,error_y,0,error_yaw,0.1);
+    command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1);
 
 }
 
@@ -548,7 +549,7 @@ void FormationClass::spherical_coordinate(float k,float theta,float psi){
     // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
     // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
     // ROS_INFO("%f,%f",Pf[0],Pf[1]);
-    // ROS_INFO("%f,%f",error_x,error_y);
+    ROS_INFO("%f,%f",error_x,error_y);
     // ROS_INFO("deg:%f",deg_phi);
     // ROS_INFO("heading:%f",heading);
     // ROS_INFO("error_degree:%f",error_degree);

class_model/src/main.cpp

@@ -22,26 +22,42 @@ int main(int argc, char **argv) {
     
     // SelectionClass selection_object;
     
-    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
+    // YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
 
     mode_object.set_Mode("GUIDED");
     control_object.arm();
-    control_object.takeoff(4.5);
+    control_object.takeoff(2);
     sleep(2);
 
     // selection_object.choose_leader();
     // mission_object.start();   //default hover in goose formation
 
-    INFO msg;
-    msg.type = "V_formation";
-    msg.mission = "fly2target";
+    // INFO msg;
+    // msg.type = "V_formation";
+    // msg.mission = "fly2target";
 
-    while(ros::ok()){
+    std::string type = "";
 
+    while(ros::ok()){
+        type = receiver_object.check_command();  
+    // ROS_INFO("%s",type.c_str());
+    
+    if(type == "" || type == "init"){
+        select_formation.goose_formation();
+        ROS_INFO("init formation");
+    }else if(type == "line"){
+        select_formation.line_formation();
+        // ROS_INFO("line foemation");
+    }else if(type == "row"){
+        select_formation.row_formation();
+    }else if(type == "circle"){
+        select_formation.circle_formation();
+    }else if(type == "stop"){
+        select_formation.stop();
+    }else if(type == "land"){
+        mode_object.set_Mode("LAND");
+    }
 
-    //json_object.get_command();       //for follower
-    //select_formation.goose_formation();
-    PubInfo_object.pub_info(msg);
     }
 
     ros::waitForShutdown();