From 3597e3d901fa4a35b0a332bffd96a48e451a9643 Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Mon, 27 Mar 2023 22:35:29 +0800 Subject: [PATCH] PI control problem --- class_model/include/class_model/follower.h | 1 + class_model/src/follower.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index f763740..a8ef125 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -40,6 +40,7 @@ public: int find_ref_drone(global_location** data, size_t index, int leaderID); float lon[3],lat[3]; + global_location vector_LT,vector_SM; private: diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index 34b44fe..e718ce3 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -127,11 +127,13 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob int FollowerClass::direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member){ - global_location vector_LT,vector_SM; float m,n,k,cosTheta; + if(flag == 0){ vector_LT.lon = target->lon - leader->lon; //vector_LT (leader->target) vector_LT.lat = target->lat - leader->lat; + flag = 1; + } 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