diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index fcb77a7..3cf4bb3 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -37,7 +37,7 @@ public: DroneStatus samePlane[5]; void follower(global_location* member_data, size_t index, int refID); - int find_ref_drone(global_location data[], size_t index, int leaderID); + int find_ref_drone(global_location* data[], size_t index, int leaderID); float lon[3],lat[3]; @@ -62,8 +62,8 @@ private: float ignore_small = 0.50; void calculate_position(float k,float theta,int plane); - void plane(global_location target, global_location leader, global_location follower); - int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member); + void plane(global_location* target, global_location* leader, global_location* follower); + int direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member); void choose_leader(); diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index 3f618c4..6b92554 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -48,23 +48,23 @@ int main(int argc, char **argv) { checkLeader = check_leader(*pos,index).is_leader; if(checkLeader != 1){ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); + ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ - // self = request_object.get_self_GPS(); - // M1 = request_object.get_M1_GPS(); - // M2 = request_object.get_M2_GPS(); - // M3 = request_object.get_M3_GPS(); - // M4 = request_object.get_M4_GPS(); + self = request_object.get_self_GPS(); + M1 = request_object.get_M1_GPS(); + M2 = request_object.get_M2_GPS(); + M3 = request_object.get_M3_GPS(); + M4 = request_object.get_M4_GPS(); // pos[] = {target,self,M1,M2,M3,M4}; // member[] = {M1,M2,M3,M4}; if(checkLeader == 1){ // leader_object.leader(target); }else{ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + 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_main2.cpp b/class_model/src/bio_main2.cpp index 17dedc9..4d3c36a 100644 --- a/class_model/src/bio_main2.cpp +++ b/class_model/src/bio_main2.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { checkLeader = check_leader(*pos,index).is_leader; if(checkLeader != 1){ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); + ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ @@ -64,7 +64,7 @@ int main(int argc, char **argv) { if(checkLeader == 1){ // leader_object.leader(target); }else{ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + 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_main3.cpp b/class_model/src/bio_main3.cpp index 99d099c..ffa56d5 100644 --- a/class_model/src/bio_main3.cpp +++ b/class_model/src/bio_main3.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { checkLeader = check_leader(*pos,index).is_leader; if(checkLeader != 1){ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); + ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ @@ -64,7 +64,7 @@ int main(int argc, char **argv) { if(checkLeader == 1){ // leader_object.leader(target); }else{ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + 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 3789550..0b514e1 100644 --- a/class_model/src/bio_main4.cpp +++ b/class_model/src/bio_main4.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { checkLeader = check_leader(*pos,index).is_leader; if(checkLeader != 1){ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); + ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ @@ -64,7 +64,7 @@ int main(int argc, char **argv) { if(checkLeader == 1){ // leader_object.leader(target); }else{ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + 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_main5.cpp b/class_model/src/bio_main5.cpp index e8da062..4064955 100644 --- a/class_model/src/bio_main5.cpp +++ b/class_model/src/bio_main5.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { checkLeader = check_leader(*pos,index).is_leader; if(checkLeader != 1){ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); + ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); } while(true){ @@ -64,7 +64,7 @@ int main(int argc, char **argv) { if(checkLeader == 1){ // leader_object.leader(target); }else{ - ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); + 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/follower.cpp b/class_model/src/follower.cpp index 378f804..463d96a 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -43,80 +43,80 @@ void FollowerClass::follower(global_location* member_data, size_t index, int ref //} } -void FollowerClass::plane(global_location target, global_location leader, global_location follower){ +void FollowerClass::plane(global_location* target, global_location* leader, global_location* follower){ //caculate drone is locate on R/L plane float slope,c,y; - slope = (target.lat-leader.lat)/(target.lon-leader.lon); - c = leader.lat - slope*leader.lon; - y = slope * follower.lon + c; + slope = (target->lat-leader->lat)/(target->lon-leader->lon); + c = leader->lat - slope*leader->lon; + y = slope * follower->lon + c; // std::cout<< follower.ID << ","<< follower.lat <<","<< y <lat <= y){ // std::cout<< follower.lat <<","<< y <ID){ case 1: - drone1.init_location.lat = follower.lat; - drone1.init_location.lon = follower.lon; - drone1.ID = follower.ID; + drone1.init_location.lat = follower->lat; + drone1.init_location.lon = follower->lon; + drone1.ID = follower->ID; drone1.plane = 1; //right plane break; case 2: - drone2.init_location.lat = follower.lat; - drone2.init_location.lon = follower.lon; - drone2.ID = follower.ID; + drone2.init_location.lat = follower->lat; + drone2.init_location.lon = follower->lon; + drone2.ID = follower->ID; drone2.plane = 1; break; case 3: - drone3.init_location.lat = follower.lat; - drone3.init_location.lon = follower.lon; - drone3.ID = follower.ID; + drone3.init_location.lat = follower->lat; + drone3.init_location.lon = follower->lon; + drone3.ID = follower->ID; drone3.plane = 1; break; case 4: - drone4.init_location.lat = follower.lat; - drone4.init_location.lon = follower.lon; - drone4.ID = follower.ID; + drone4.init_location.lat = follower->lat; + drone4.init_location.lon = follower->lon; + drone4.ID = follower->ID; drone4.plane = 1; break; case 5: - drone5.init_location.lat = follower.lat; - drone5.init_location.lon = follower.lon; - drone5.ID = follower.ID; + drone5.init_location.lat = follower->lat; + drone5.init_location.lon = follower->lon; + drone5.ID = follower->ID; drone5.plane = 1; break; } - }else if(follower.lat > y){ + }else if(follower->lat > y){ // std::cout<< follower.lat <<","<< y <ID){ case 1: - drone1.init_location.lat = follower.lat; - drone1.init_location.lon = follower.lon; - drone1.ID = follower.ID; + drone1.init_location.lat = follower->lat; + drone1.init_location.lon = follower->lon; + drone1.ID = follower->ID; drone1.plane = -1; //left plane break; case 2: - drone2.init_location.lat = follower.lat; - drone2.init_location.lon = follower.lon; - drone2.ID = follower.ID; + drone2.init_location.lat = follower->lat; + drone2.init_location.lon = follower->lon; + drone2.ID = follower->ID; drone2.plane = -1; break; case 3: - drone3.init_location.lat = follower.lat; - drone3.init_location.lon = follower.lon; - drone3.ID = follower.ID; + drone3.init_location.lat = follower->lat; + drone3.init_location.lon = follower->lon; + drone3.ID = follower->ID; drone3.plane = -1; break; case 4: - drone4.init_location.lat = follower.lat; - drone4.init_location.lon = follower.lon; - drone4.ID = follower.ID; + drone4.init_location.lat = follower->lat; + drone4.init_location.lon = follower->lon; + drone4.ID = follower->ID; drone4.plane = -1; break; case 5: - drone5.init_location.lat = follower.lat; - drone5.init_location.lon = follower.lon; - drone5.ID = follower.ID; + drone5.init_location.lat = follower->lat; + drone5.init_location.lon = follower->lon; + drone5.ID = follower->ID; drone5.plane = -1; break; } @@ -125,12 +125,12 @@ void FollowerClass::plane(global_location target, global_location leader, global } -int FollowerClass::direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){ +int FollowerClass::direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member){ 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_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) vector_SM.lat = member.init_location.lat - self.init_location.lat; k = (vector_LT.lon*vector_SM.lon)+(vector_LT.lat*vector_SM.lat); //k = vector_LT dot vector_SM @@ -146,40 +146,40 @@ int FollowerClass::direct(global_location target, global_location leader, DroneS } -int FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){ +int FollowerClass::find_ref_drone(global_location* data[], size_t index, int leaderID){ for(int i=1 ; iID){ + leader_drone = *data[i]; switch(leaderID){ case 1: - drone1.init_location.lat = data[i].lat; - drone1.init_location.lon = data[i].lon; - drone1.ID = data[i].ID; + drone1.init_location.lat = data[i]->lat; + drone1.init_location.lon = data[i]->lon; + drone1.ID = data[i]->ID; drone1.plane = 0; //center break; case 2: - drone2.init_location.lat = data[i].lat; - drone2.init_location.lon = data[i].lon; - drone2.ID = data[i].ID; + drone2.init_location.lat = data[i]->lat; + drone2.init_location.lon = data[i]->lon; + drone2.ID = data[i]->ID; drone2.plane = 0; break; case 3: - drone3.init_location.lat = data[i].lat; - drone3.init_location.lon = data[i].lon; - drone3.ID = data[i].ID; + drone3.init_location.lat = data[i]->lat; + drone3.init_location.lon = data[i]->lon; + drone3.ID = data[i]->ID; drone3.plane = 0; break; case 4: - drone4.init_location.lat = data[i].lat; - drone4.init_location.lon = data[i].lon; - drone4.ID = data[i].ID; + drone4.init_location.lat = data[i]->lat; + drone4.init_location.lon = data[i]->lon; + drone4.ID = data[i]->ID; drone4.plane = 0; break; case 5: - drone5.init_location.lat = data[i].lat; - drone5.init_location.lon = data[i].lon; - drone5.ID = data[i].ID; + drone5.init_location.lat = data[i]->lat; + drone5.init_location.lon = data[i]->lon; + drone5.ID = data[i]->ID; drone5.plane = 0; break; @@ -189,41 +189,41 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead } for(int i=1 ; iID != leader_drone.ID){ //caculate the plane of each drone + plane(data[0],&leader_drone,data[i]); } } int j=0,s_index=0; - if(drone1.ID == data[1].ID){ //seperate self and other + if(drone1.ID == data[1]->ID){ //seperate self and other self = drone1; //problem }else{ memberDrone[j] = drone1; j+=1; // std::cout << "drone1" <ID){ self = drone2; }else{ memberDrone[j] = drone2; j+=1; // std::cout << "drone2" <ID){ self = drone3; }else{ memberDrone[j] = drone3; j+=1; // std::cout << "drone3" <ID){ self = drone4; }else{ memberDrone[j] = drone4; j+=1; // std::cout << "drone4" <ID){ self = drone5; }else{ memberDrone[j] = drone5; @@ -249,7 +249,7 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead // std::cout <<"s_index " <