From e52581599264b9fe120e1aaeaf74124a323f0866 Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Tue, 1 Aug 2023 16:22:04 +0800 Subject: [PATCH] Bio test succeeded --- class_model/include/class_model/Leader.h | 4 +- class_model/include/class_model/PID.h | 11 +++-- .../include/class_model/convert_degree.h | 4 +- class_model/include/class_model/follower.h | 6 +-- class_model/src/Leader.cpp | 3 +- class_model/src/PID.cpp | 29 +++++++++++-- class_model/src/bio_main.cpp | 25 +++++++---- class_model/src/bio_main2.cpp | 25 +++++++---- class_model/src/bio_main3.cpp | 25 +++++++---- class_model/src/bio_main4.cpp | 14 ++++--- class_model/src/bio_main5.cpp | 16 ++++--- class_model/src/config.yaml | 12 +++--- class_model/src/follower.cpp | 42 +++++++++---------- iq_sim/worlds/three_drone.world | 2 +- 14 files changed, 137 insertions(+), 81 deletions(-) diff --git a/class_model/include/class_model/Leader.h b/class_model/include/class_model/Leader.h index 01e5299..2febf8e 100644 --- a/class_model/include/class_model/Leader.h +++ b/class_model/include/class_model/Leader.h @@ -45,8 +45,8 @@ private: int OnTarget = 0; float pre_alt,cur_alt; float error_lon,error_lat; - float leader_pid[3] = {1 , 0.000000000001 ,0}; - float ignore_small = 0.50 ,limit_speed = 0.5; + float leader_pid[3] = {0.5 , 0.000001 ,0.001}; + float ignore_small = 0.50 ,limit_speed = 10; void face2target(float target_heading); }; #endif // Leader_H \ No newline at end of file diff --git a/class_model/include/class_model/PID.h b/class_model/include/class_model/PID.h index e873161..86b9fe1 100755 --- a/class_model/include/class_model/PID.h +++ b/class_model/include/class_model/PID.h @@ -4,16 +4,21 @@ #include #include "ctime" +#include +#include class PIDClass { public: -// PIDClass(); -// virtual ~PIDClass(); + PIDClass(); + virtual ~PIDClass(); float update(float current ,float target ,float pidvals[3]); float update1(float current ,float target ); + std::string droneID; private: // ROS NodeHandle ros::NodeHandle node_handle_; + std::ofstream logFile; + std::string ros_namespace; float pidval1[3] = {0.5 , 0.000000000001 , 0.5}; float limit[2] = {2 , -2}; @@ -23,5 +28,5 @@ private: }; - +//"/home/dodo/ardupilot_ws/src/class_model/src/log/PID"+droneID+".txt" #endif // PID_H \ No newline at end of file diff --git a/class_model/include/class_model/convert_degree.h b/class_model/include/class_model/convert_degree.h index a4d8ef1..21e2cbf 100755 --- a/class_model/include/class_model/convert_degree.h +++ b/class_model/include/class_model/convert_degree.h @@ -29,11 +29,11 @@ float check_direction(float error_degree){ int direction; if(error_degree <= 180 && error_degree >= 0){ //check yaw direction direction = -1; - }else if(error_degree < -180){ + }else if(error_degree < -180 && error_degree < 0){ direction = -1; }else if(error_degree >= -180 && error_degree < 0){ direction = 1; - }else if(error_degree > 180){ + }else if(error_degree > 180 && error_degree >= 0){ direction = 1; } diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index ffb2af1..9f74da2 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -1,7 +1,7 @@ /*Follow the leader in a fixed formation */ #ifndef FOLLOWER_H #define FOLLOWER_H -#define swarmNum 3 +#define swarmNum 5 //The number of drones #define changeLeader // #define NoChangeLeader #include @@ -62,8 +62,8 @@ private: float distance = config["formation"]["distance"].as(); float angle = config["formation"]["angle"].as(); float error_lon,error_lat; - float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize - float ignore_small = 0.50 ,limit_speed = 0.5; + float follower_pid[3] = {0.5 , 0.000001 ,0.001};// D downsize + float ignore_small = 0.50 ,limit_speed = 10; float m,n,k,cosTheta,pre_m,pre_k,u,LTcos; // function direct()'s variable global_location pre_tar,ref_point; // function plane()'s variable float slope,c,y; // function plane()'s variable diff --git a/class_model/src/Leader.cpp b/class_model/src/Leader.cpp index 8b4b244..bf9286c 100644 --- a/class_model/src/Leader.cpp +++ b/class_model/src/Leader.cpp @@ -28,6 +28,7 @@ void LeaderClass::leader(global_location target){ pre_target.lon = target.lon; pre_target.lat = target.lat; OnTarget = 1; + heading_status = 0; ROS_INFO("break"); break; } @@ -122,7 +123,7 @@ void LeaderClass::face2target(float target_heading){ float error_heading; float heading = GPS_object.get_heading(); - float error_yaw = 0.2; + float error_yaw = 0.25; error_heading = target_heading - heading; error_yaw = check_direction(error_heading)*error_yaw*0.5; diff --git a/class_model/src/PID.cpp b/class_model/src/PID.cpp index 792170b..86e80b6 100755 --- a/class_model/src/PID.cpp +++ b/class_model/src/PID.cpp @@ -1,23 +1,37 @@ #include "class_model/PID.h" -// PIDClass::PIDClass() : node_handle_(""){ +PIDClass::PIDClass() : node_handle_("~"){ + + + // if (!node_handle_.hasParam("droneID")) + // { + // }else{ + // node_handle_.getParam("droneID", droneID); + // } + + if (!node_handle_.hasParam("namespace")) + { + }else{ + node_handle_.getParam("namespace", ros_namespace); + } -// } +} -// PIDClass::~PIDClass() { ros::shutdown(); } +PIDClass::~PIDClass() { ros::shutdown(); } float PIDClass::update(float current ,float target ,float pidvals[3]){ current_time = std::clock(); // pTime = current_time - - double t = (current_time - pTime); + double t = (current_time - pTime)/1e6; float error = (target - current); double P,D,result; P = pidvals[0] * error; I = I + (pidvals[1] * error *t); // D = (pidvals[2] * (error - pError))/t; + // I = 0; D = 0; result = P + I + D; @@ -31,6 +45,13 @@ float PIDClass::update(float current ,float target ,float pidvals[3]){ } } + + logFile.open("/home/dodo/ardupilot_ws/src/class_model/src/log"+ros_namespace+"PID.txt", std::ofstream::app); + logFile << P <<","<< I <<","<< D << std::endl; + + logFile.close(); + + pError = error; pTime = current_time; diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index 5e59da9..b548e3e 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -45,10 +45,10 @@ int main(int argc, char **argv) { M4 = request_object.get_M4_GPS(); - // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; - // global_location* member[] = {&M1,&M2,&M3,&M4}; - global_location* pos[] = {&target,&self,&M1,&M2}; - global_location* member[] = {&M1,&M2}; + global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; + global_location* member[] = {&M1,&M2,&M3,&M4}; + // global_location* pos[] = {&target,&self,&M1,&M2}; + // global_location* member[] = {&M1,&M2}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -95,7 +95,7 @@ int main(int argc, char **argv) { // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; - while(errorX > 100 || errorY > 100){ + while(errorX > 150 || errorY > 150){ self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); @@ -103,6 +103,12 @@ int main(int argc, char **argv) { M3 = request_object.get_M3_GPS(); M4 = request_object.get_M4_GPS(); + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + if(errorX < 150 && errorY < 150){ + break; + } + if(checkLeader == 1){ std::cout << "Drone1 is Leader" << std::endl; @@ -114,20 +120,21 @@ int main(int argc, char **argv) { } // OnTarget = leader_object.isOnTarget(); // std::cout <<"tar: "<< OnTarget << std::endl; - errorX = abs(pos[leaderIndex]->lon - target.lon) ; - errorY = abs(pos[leaderIndex]->lat - target.lat) ; // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; } sleep(3); // while(type != "n"){ // type = receiver_object.check_command(); - if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){ routeIndex++; checkLeader = 0; - std::cout <<"Drone1routeIndex: "<= routeNum){ diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp index 60085bb..6c81920 100644 --- a/class_model/src/bio_main2.cpp +++ b/class_model/src/bio_main2.cpp @@ -45,10 +45,10 @@ int main(int argc, char **argv) { M4 = request_object.get_M4_GPS(); - // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; - // global_location* member[] = {&M1,&M2,&M3,&M4}; - global_location* pos[] = {&target,&self,&M1,&M2}; - global_location* member[] = {&M1,&M2}; + global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; + global_location* member[] = {&M1,&M2,&M3,&M4}; + // global_location* pos[] = {&target,&self,&M1,&M2}; + // global_location* member[] = {&M1,&M2}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -95,7 +95,7 @@ int main(int argc, char **argv) { // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; - while(errorX > 100 || errorY > 100){ + while(errorX > 150 || errorY > 150){ self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); @@ -103,6 +103,12 @@ int main(int argc, char **argv) { M3 = request_object.get_M3_GPS(); M4 = request_object.get_M4_GPS(); + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + if(errorX < 150 && errorY < 150){ + break; + } + if(checkLeader == 1){ std::cout << "Drone2 is Leader" << std::endl; @@ -114,20 +120,21 @@ int main(int argc, char **argv) { } // OnTarget = leader_object.isOnTarget(); // std::cout <<"tar: "<< OnTarget << std::endl; - errorX = abs(pos[leaderIndex]->lon - target.lon) ; - errorY = abs(pos[leaderIndex]->lat - target.lat) ; // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; } sleep(3); // while(type != "n"){ // type = receiver_object.check_command(); - if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){ routeIndex++; checkLeader = 0; - std::cout <<"Drone2routeIndex: "<= routeNum){ diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp index fcf0970..53e6d52 100644 --- a/class_model/src/bio_main3.cpp +++ b/class_model/src/bio_main3.cpp @@ -45,10 +45,10 @@ int main(int argc, char **argv) { M4 = request_object.get_M4_GPS(); - // global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; - // global_location* member[] = {&M1,&M2,&M3,&M4}; - global_location* pos[] = {&target,&self,&M1,&M2}; - global_location* member[] = {&M1,&M2}; + global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4}; + global_location* member[] = {&M1,&M2,&M3,&M4}; + // global_location* pos[] = {&target,&self,&M1,&M2}; + // global_location* member[] = {&M1,&M2}; size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); @@ -95,7 +95,7 @@ int main(int argc, char **argv) { // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; - while(errorX > 100 || errorY > 100){ + while(errorX > 150 || errorY > 150){ self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); @@ -103,6 +103,12 @@ int main(int argc, char **argv) { M3 = request_object.get_M3_GPS(); M4 = request_object.get_M4_GPS(); + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + if(errorX < 150 && errorY < 150){ + break; + } + if(checkLeader == 1){ std::cout << "Drone3 is Leader" << std::endl; @@ -114,20 +120,21 @@ int main(int argc, char **argv) { } // OnTarget = leader_object.isOnTarget(); // std::cout <<"tar: "<< OnTarget << std::endl; - errorX = abs(pos[leaderIndex]->lon - target.lon) ; - errorY = abs(pos[leaderIndex]->lat - target.lat) ; // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; } sleep(3); // while(type != "n"){ // type = receiver_object.check_command(); - if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){ routeIndex++; checkLeader = 0; - std::cout <<"Drone3routeIndex: "<= routeNum){ diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp index c5fb6b3..c35ca1f 100644 --- a/class_model/src/bio_main4.cpp +++ b/class_model/src/bio_main4.cpp @@ -93,7 +93,7 @@ int main(int argc, char **argv) { // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; - while(errorX > 100 || errorY > 100){ + while(errorX > 150 || errorY > 150){ self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); @@ -101,6 +101,12 @@ int main(int argc, char **argv) { M3 = request_object.get_M3_GPS(); M4 = request_object.get_M4_GPS(); + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + if(errorX < 150 && errorY < 150){ + break; + } + if(checkLeader == 1){ std::cout << "Drone4 is Leader" << std::endl; @@ -112,18 +118,16 @@ int main(int argc, char **argv) { } // OnTarget = leader_object.isOnTarget(); // std::cout <<"tar: "<< OnTarget << std::endl; - errorX = abs(pos[leaderIndex]->lon - target.lon) ; - errorY = abs(pos[leaderIndex]->lat - target.lat) ; // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; } // while(type != "n"){ // type = receiver_object.check_command(); - if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){ routeIndex++; checkLeader = 0; - std::cout <<"routeIndex: "< 100 || errorY > 100){ + while(errorX > 150 || errorY > 150){ self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); @@ -101,6 +101,12 @@ int main(int argc, char **argv) { M3 = request_object.get_M3_GPS(); M4 = request_object.get_M4_GPS(); + errorX = abs(pos[leaderIndex]->lon - target.lon) ; + errorY = abs(pos[leaderIndex]->lat - target.lat) ; + if(errorX < 150 && errorY < 150){ + break; + } + if(checkLeader == 1){ std::cout << "Drone5 is Leader" << std::endl; @@ -112,18 +118,16 @@ int main(int argc, char **argv) { } // OnTarget = leader_object.isOnTarget(); // std::cout <<"tar: "<< OnTarget << std::endl; - errorX = abs(pos[leaderIndex]->lon - target.lon) ; - errorY = abs(pos[leaderIndex]->lat - target.lat) ; // std::cout <<"x: "<< errorX << std::endl; // std::cout <<"y: "<< errorY << std::endl; } - + sleep(3); // while(type != "n"){ // type = receiver_object.check_command(); - if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){ + if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){ routeIndex++; checkLeader = 0; - std::cout <<"routeIndex: "<lon + c; // std::cout<< follower.ID << ","<< follower.lat <<","<< y <lat <= y && phi < 180) || (follower->lat > y && phi >= 180)){ // std::cout <<"drone :"<ID <<", plane: 1" <ID){ - // self = drone4; - // }else{ - // memberDrone[j] = drone4; - // j+=1; - // // std::cout << "drone4" <ID){ - // self = drone5; - // }else{ - // memberDrone[j] = drone5; - // j+=1; - // // std::cout << "drone4" <ID){ + self = drone4; + }else{ + memberDrone[j] = drone4; + j+=1; + // std::cout << "drone4" <ID){ + self = drone5; + }else{ + memberDrone[j] = drone5; + j+=1; + // std::cout << "drone4" <lon/100) - (leader->lon/100); y = (target->lat/100) - (leader->lat/100); target_heading = ConvertDeg(x,y); diff --git a/iq_sim/worlds/three_drone.world b/iq_sim/worlds/three_drone.world index 23f162c..25a0921 100644 --- a/iq_sim/worlds/three_drone.world +++ b/iq_sim/worlds/three_drone.world @@ -85,7 +85,7 @@ 0 0 0 0 0 0 - model://drone1 + model://drone_with_camera