diff --git a/class_model/include/class_model/Leader.h b/class_model/include/class_model/Leader.h index 434406f..c929d25 100644 --- a/class_model/include/class_model/Leader.h +++ b/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); }; diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index 755edbd..f763740 100755 --- a/class_model/include/class_model/follower.h +++ b/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); diff --git a/class_model/src/Leader.cpp b/class_model/src/Leader.cpp index e272ad9..cdbd9a8 100644 --- a/class_model/src/Leader.cpp +++ b/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); diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp index f39b129..a26776e 100644 --- a/class_model/src/bio_main2.cpp +++ b/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(); diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp index 6f5e3cf..db129d6 100644 --- a/class_model/src/bio_main3.cpp +++ b/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 diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp index c51cfc3..e023af1 100644 --- a/class_model/src/bio_main4.cpp +++ b/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(); diff --git a/class_model/src/bio_main5.cpp b/class_model/src/bio_main5.cpp index 84d3eb0..9125931 100644 --- a/class_model/src/bio_main5.cpp +++ b/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(); diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index 4c5740c..34b44fe 100755 --- a/class_model/src/follower.cpp +++ b/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_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_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); // 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_lat = Pf[1] - (follower_location.lat*1e+5); + error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error + error_lat = Pf[1] - (follower_location.lat*1e+5); float error_degree = deg_phi - heading;