Xuan0319/DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

test bio theory loop caculate

Browse Source
This commit is contained in:
Xuan0319
2023-03-24 23:26:55 +08:00
parent 6854584a5c
commit c6197c5650
7 changed files with 84 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/src/bio_main.cpp
+14 -5
View File
@@ -37,8 +37,8 @@ int main(int argc, char **argv) {
// global_location pos[] = {target,self,M1,M2}; // global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2}; // global_location member[] = {M1,M2};
global_location pos[] = {target,self,M1,M2,M3,M4}; global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location member[] = {M1,M2,M3,M4}; global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]); size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]); size_t m_index = sizeof(member)/sizeof(member[0]);
@@ -46,17 +46,26 @@ int main(int argc, char **argv) {
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(2);
checkLeader = check_leader(pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){ 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){ 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();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
if(checkLeader == 1){ if(checkLeader == 1){
// leader_object.leader(target); // leader_object.leader(target);
}else{ }else{
follower_object.follower(member,m_index,ref_ID); //follow reference drone position 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
} }
} }
ros::waitForShutdown(); ros::waitForShutdown();
class_model/src/bio_main2.cpp
+14 -5
View File
@@ -37,8 +37,8 @@ int main(int argc, char **argv) {
// global_location pos[] = {target,self,M1,M2}; // global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2}; // global_location member[] = {M1,M2};
global_location pos[] = {target,self,M1,M2,M3,M4}; global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location member[] = {M1,M2,M3,M4}; global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]); size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]); size_t m_index = sizeof(member)/sizeof(member[0]);
@@ -46,17 +46,26 @@ int main(int argc, char **argv) {
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(2);
checkLeader = check_leader(pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){ 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){ 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();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
if(checkLeader == 1){ if(checkLeader == 1){
// leader_object.leader(target); // leader_object.leader(target);
}else{ }else{
follower_object.follower(member,m_index,ref_ID); //follow reference drone position 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
} }
} }
ros::waitForShutdown(); ros::waitForShutdown();
class_model/src/bio_main3.cpp
+14 -5
View File
@@ -37,8 +37,8 @@ int main(int argc, char **argv) {
// global_location pos[] = {target,self,M1,M2}; // global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2}; // global_location member[] = {M1,M2};
global_location pos[] = {target,self,M1,M2,M3,M4}; global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location member[] = {M1,M2,M3,M4}; global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]); size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]); size_t m_index = sizeof(member)/sizeof(member[0]);
@@ -46,17 +46,26 @@ int main(int argc, char **argv) {
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(2);
checkLeader = check_leader(pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){ 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){ 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();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
if(checkLeader == 1){ if(checkLeader == 1){
// leader_object.leader(target); // leader_object.leader(target);
}else{ }else{
follower_object.follower(member,m_index,ref_ID); //follow reference drone position 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
} }
} }
ros::waitForShutdown(); ros::waitForShutdown();
class_model/src/bio_main4.cpp
+14 -5
View File
@@ -37,8 +37,8 @@ int main(int argc, char **argv) {
// global_location pos[] = {target,self,M1,M2}; // global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2}; // global_location member[] = {M1,M2};
global_location pos[] = {target,self,M1,M2,M3,M4}; global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location member[] = {M1,M2,M3,M4}; global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]); size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]); size_t m_index = sizeof(member)/sizeof(member[0]);
@@ -46,17 +46,26 @@ int main(int argc, char **argv) {
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(2);
checkLeader = check_leader(pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){ 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){ 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();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
if(checkLeader == 1){ if(checkLeader == 1){
// leader_object.leader(target); // leader_object.leader(target);
}else{ }else{
follower_object.follower(member,m_index,ref_ID); //follow reference drone position 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
} }
} }
ros::waitForShutdown(); ros::waitForShutdown();
class_model/src/bio_main5.cpp
+14 -5
View File
@@ -37,8 +37,8 @@ int main(int argc, char **argv) {
// global_location pos[] = {target,self,M1,M2}; // global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2}; // global_location member[] = {M1,M2};
global_location pos[] = {target,self,M1,M2,M3,M4}; global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location member[] = {M1,M2,M3,M4}; global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]); size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]); size_t m_index = sizeof(member)/sizeof(member[0]);
@@ -46,17 +46,26 @@ int main(int argc, char **argv) {
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(2);
checkLeader = check_leader(pos,index).is_leader; checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){ 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){ 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();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
if(checkLeader == 1){ if(checkLeader == 1){
// leader_object.leader(target); // leader_object.leader(target);
}else{ }else{
follower_object.follower(member,m_index,ref_ID); //follow reference drone position 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
} }
} }
ros::waitForShutdown(); ros::waitForShutdown();
class_model/src/follower.cpp
+7 -7
View File
@@ -30,17 +30,17 @@ void FollowerClass::follower(global_location* member_data, size_t index, int ref
std::string command = ""; std::string command = "";
std::string pre_command = receiver_object.check_command(); std::string pre_command = receiver_object.check_command();
while(true){ // while(true){
calculate_position(distance,angle,self.plane); calculate_position(distance,angle,self.plane);
command = receiver_object.check_command(); // command = receiver_object.check_command();
if(command != pre_command ){ // if(command != pre_command ){
ROS_INFO("change formation"); // ROS_INFO("change formation");
break; // break;
} // }
} //}
} }
void FollowerClass::plane(global_location target, global_location leader, global_location follower){ void FollowerClass::plane(global_location target, global_location leader, global_location follower){
class_model/src/test_lib.cpp
+7 -4
View File
@@ -241,13 +241,13 @@ int main(int argc,char** argv){
global_location M2; global_location M2;
global_location target; global_location target;
target.lon = 3; target.lon = 0;
target.lat = -3; target.lat = 3;
target.heading = 0; target.heading = 0;
self.lon = -2; self.lon = -2;
self.lat = -2; self.lat = -2;
self.ID = 3; self.ID = 1;
M1.lon = 2; M1.lon = 2;
M1.lat = -3; M1.lat = -3;
@@ -255,10 +255,13 @@ int main(int argc,char** argv){
M2.lon = 0; M2.lon = 0;
M2.lat = 0; M2.lat = 0;
M2.ID = 1; M2.ID = 3;
global_location data[]={target,self,M1,M2}; global_location data[]={target,self,M1,M2};
size_t index = sizeof(data)/sizeof(data[0]); size_t index = sizeof(data)/sizeof(data[0]);
int x = 0;
int y = 0;
int* arr[] = {&x,&y};
find_ref_drone(data,index,check_leader(data,index).leader_ID); find_ref_drone(data,index,check_leader(data,index).leader_ID);