move2target will interlock.

class_model/include/class_model/Leader.h

@@ -42,7 +42,7 @@ private:
     int flag = 0,heading_status = 0,buf = 0;
     float pre_alt,cur_alt;
     float error_lon,error_lat;
-    float leader_pid[3] = {1 , 0.000000000001 ,0.5};
+    float leader_pid[3] = {1 , 0.000000000001 ,0};
     float ignore_small = 0.50;
     void face2target(float target_heading);   
 };

class_model/include/class_model/follower.h

@@ -60,6 +60,7 @@ private:
     float error_lon,error_lat;
     float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
     float ignore_small = 0.50;
+    int flag = 0;
 
     void calculate_position(float k,float theta,int plane);
     void plane(global_location* target, global_location* leader, global_location* follower);

class_model/src/Leader.cpp

@@ -13,7 +13,7 @@ void LeaderClass::leader(global_location target){
     std_msgs::String message;
 
     flag = 0;
-    heading_status = 0;
+    // heading_status = 0;
     while(true){
         if(flag==0){
             go2target(target.lon,target.lat);

class_model/src/bio_main2.cpp

@@ -62,7 +62,7 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(ros::ok()&& flag == 1){
+    while(ros::ok()){
 
         // type = receiver_object.check_command();
         self = request_object.get_self_GPS();

class_model/src/bio_main3.cpp

@@ -62,7 +62,7 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(ros::ok()&& flag == 1){
+    while(ros::ok()){
 
         // type = receiver_object.check_command();
         self = request_object.get_self_GPS();
@@ -72,7 +72,7 @@ int main(int argc, char **argv) {
         M4 = request_object.get_M4_GPS();
 
         if(checkLeader == 1){
-            leader_object.leader(target);
+            // leader_object.l/eader(target);
         }else{
             ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
             follower_object.follower(member,m_index,ref_ID);   //follow reference drone position

class_model/src/bio_main4.cpp

@@ -62,7 +62,7 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(ros::ok()&& flag == 1){
+    while(ros::ok()){
 
         // type = receiver_object.check_command();
         self = request_object.get_self_GPS();

class_model/src/bio_main5.cpp

@@ -62,7 +62,7 @@ int main(int argc, char **argv) {
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(ros::ok()&& flag == 1){
+    while(ros::ok()){
 
         // type = receiver_object.check_command();
         self = request_object.get_self_GPS();

class_model/src/follower.cpp

@@ -129,6 +129,7 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
 
     global_location vector_LT,vector_SM;
     float m,n,k,cosTheta;
+
     vector_LT.lon = target->lon - leader->lon;       //vector_LT (leader->target)     
     vector_LT.lat = target->lat - leader->lat;
     vector_SM.lon = member.init_location.lon - self.init_location.lon;       //vector_SM (self->member)
@@ -261,7 +262,7 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
     check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
     ref_ID = samePlane[check_index].ID;
 
-    // std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " <<  ref_ID << std::endl;
+    std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " <<  ref_ID << std::endl;
 
     return ref_ID;   
 
@@ -291,15 +292,15 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
     Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
     Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
     
-    error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error (PID)
+    // error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error (PID)
-    error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
+    // error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
     
 
     // 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] );
 
-    // error_lon = Pf[0] - (follower_location.lon*1e+5);            //caculate error
+    error_lon = Pf[0] - (follower_location.lon*1e+5);            //caculate error
-    // error_lat = Pf[1] - (follower_location.lat*1e+5);
+    error_lat = Pf[1] - (follower_location.lat*1e+5);
 
     
     float error_degree = deg_phi - heading;