From 0bf03420d10d426e0fbe583709f91bb1ad3bcc60 Mon Sep 17 00:00:00 2001
From: Xuan0319 <new310049@gmail.com>
Date: Sat, 10 Jun 2023 23:09:04 +0800
Subject: [PATCH] fix the error of change leader

---
 class_model/include/class_model/Leader.h   |  2 +-
 class_model/include/class_model/follower.h |  2 +-
 class_model/src/Leader.cpp                 | 21 +++++++++++--------
 class_model/src/PID.cpp                    |  3 ++-
 class_model/src/bio_main.cpp               | 17 ++++++++-------
 class_model/src/bio_main2.cpp              | 16 ++++++++-------
 class_model/src/bio_main3.cpp              | 16 ++++++++-------
 class_model/src/bio_main4.cpp              | 16 ++++++++-------
 class_model/src/bio_main5.cpp              | 16 ++++++++-------
 class_model/src/follower.cpp               | 24 +++++++++++-----------
 10 files changed, 74 insertions(+), 59 deletions(-)

diff --git a/class_model/include/class_model/Leader.h b/class_model/include/class_model/Leader.h
index 7a6a606..01e5299 100644
--- a/class_model/include/class_model/Leader.h
+++ b/class_model/include/class_model/Leader.h
@@ -46,7 +46,7 @@ private:
     float pre_alt,cur_alt;
     float error_lon,error_lat;
     float leader_pid[3] = {1 , 0.000000000001 ,0};
-    float ignore_small = 0.50;
+    float ignore_small = 0.50 ,limit_speed = 0.5;
     void face2target(float target_heading);   
 };
 #endif // Leader_H
\ No newline at end of file
diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h
index 7672022..dc32323 100755
--- a/class_model/include/class_model/follower.h
+++ b/class_model/include/class_model/follower.h
@@ -60,7 +60,7 @@ private:
     float angle = config["formation"]["angle"].as<float>(); 
     float error_lon,error_lat;
     float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
-    float ignore_small = 0.50;
+    float ignore_small = 0.50 ,limit_speed = 0.5;
     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 a4b2cd3..4e78f22 100644
--- a/class_model/src/Leader.cpp
+++ b/class_model/src/Leader.cpp
@@ -16,8 +16,11 @@ void LeaderClass::leader(global_location target){
     if(pre_target.lon != target.lon || pre_target.lat != target.lat){
         heading_status = 0;
         ROS_INFO("heading_flag");
+        std::cout <<"x: "<< target.lon << std::endl;
+        std::cout <<"y: "<< target.lat << std::endl;
+
     }
-    heading_status = 0;
+    // heading_status = 0;
     while(true){
         if(flag==0){
             OnTarget = 0;
@@ -84,17 +87,17 @@ void LeaderClass::go2target(float target_lon,float target_lat){
     if (drone_speed.y < ignore_small && drone_speed.y > -ignore_small){
         drone_speed.y = 0;
     }
-    if (drone_speed.x < -1){                     //max velociy                 
-        drone_speed.x = -1;
+    if (drone_speed.x < -limit_speed){                     //max velociy                 
+        drone_speed.x = -limit_speed;
     }
-    if (drone_speed.x > 1){                                      
-        drone_speed.x = 1;
+    if (drone_speed.x > limit_speed){                                      
+        drone_speed.x = limit_speed;
     }
-    if (drone_speed.y < -1){
-        drone_speed.y = -1;
+    if (drone_speed.y < -limit_speed){
+        drone_speed.y = -limit_speed;
     }
-    if (drone_speed.y > 1){
-        drone_speed.y = 1;
+    if (drone_speed.y > limit_speed){
+        drone_speed.y = limit_speed;
     }
 
     if (drone_speed.x == 0 && drone_speed.y == 0){
diff --git a/class_model/src/PID.cpp b/class_model/src/PID.cpp
index a8267fa..792170b 100755
--- a/class_model/src/PID.cpp
+++ b/class_model/src/PID.cpp
@@ -17,7 +17,8 @@ float PIDClass::update(float current ,float target ,float pidvals[3]){
     double P,D,result;
     P = pidvals[0] * error;
     I = I + (pidvals[1] * error *t);
-    D = (pidvals[2] * (error - pError))/t;
+    // D = (pidvals[2] * (error - pError))/t;
+    D = 0;
 
     result = P + I + D;
 
diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp
index ce3d7f2..fe6afa1 100644
--- a/class_model/src/bio_main.cpp
+++ b/class_model/src/bio_main.cpp
@@ -79,12 +79,12 @@ int main(int argc, char **argv) {
             leaderID = check_leader(*pos,index).leader_ID;
             leaderIndex = check_leader(*pos,index).leader_index;
          
-            std::cout << leaderID << std::endl;
-            std::cout << target.lon << std::endl;
+            // std::cout << leaderID << std::endl;
+            // std::cout << target.lon << 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
 
         while(errorX > 100 || errorY > 100){
 
@@ -119,15 +119,18 @@ int main(int argc, char **argv) {
             // 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
         }
 
         if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
             routeIndex++;
+            std::cout <<"routeIndex: "<<routeIndex<<std::endl;
             // OnTarget = 0;
-        }else if(routeIndex >= routeNum){
+        }
+        while (routeIndex >= routeNum){
             mode_object.set_Mode("LAND");
+            std::cout <<"LAND"<<std::endl;
         }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp
index 13c15c6..123d84a 100644
--- a/class_model/src/bio_main2.cpp
+++ b/class_model/src/bio_main2.cpp
@@ -79,12 +79,12 @@ int main(int argc, char **argv) {
             leaderID = check_leader(*pos,index).leader_ID;
             leaderIndex = check_leader(*pos,index).leader_index;
          
-            std::cout << leaderID << std::endl;
-            std::cout << target.lon << std::endl;
+            // std::cout << leaderID << std::endl;
+            // std::cout << target.lon << 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
 
         while(errorX > 100 || errorY > 100){
 
@@ -119,15 +119,17 @@ int main(int argc, char **argv) {
             // 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
         }
 
         if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
             routeIndex++;
             // OnTarget = 0;
-        }else if(routeIndex >= routeNum){
+        }
+        while (routeIndex >= routeNum){
             mode_object.set_Mode("LAND");
+            std::cout <<"LAND"<<std::endl;
         }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp
index c20a5cc..3c73937 100644
--- a/class_model/src/bio_main3.cpp
+++ b/class_model/src/bio_main3.cpp
@@ -79,12 +79,12 @@ int main(int argc, char **argv) {
             leaderID = check_leader(*pos,index).leader_ID;
             leaderIndex = check_leader(*pos,index).leader_index;
          
-            std::cout << leaderID << std::endl;
-            std::cout << target.lon << std::endl;
+            // std::cout << leaderID << std::endl;
+            // std::cout << target.lon << 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
 
         while(errorX > 100 || errorY > 100){
 
@@ -119,15 +119,17 @@ int main(int argc, char **argv) {
             // 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
         }
 
         if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
             routeIndex++;
             // OnTarget = 0;
-        }else if(routeIndex >= routeNum){
+        }
+        while (routeIndex >= routeNum){
             mode_object.set_Mode("LAND");
+            std::cout <<"LAND"<<std::endl;
         }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp
index 139271b..5e07cd9 100644
--- a/class_model/src/bio_main4.cpp
+++ b/class_model/src/bio_main4.cpp
@@ -79,12 +79,12 @@ int main(int argc, char **argv) {
             leaderID = check_leader(*pos,index).leader_ID;
             leaderIndex = check_leader(*pos,index).leader_index;
          
-            std::cout << leaderID << std::endl;
-            std::cout << target.lon << std::endl;
+            // std::cout << leaderID << std::endl;
+            // std::cout << target.lon << 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
 
         while(errorX > 100 || errorY > 100){
 
@@ -119,15 +119,17 @@ int main(int argc, char **argv) {
             // 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
         }
 
         if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
             routeIndex++;
             // OnTarget = 0;
-        }else if(routeIndex >= routeNum){
+        }
+        while (routeIndex >= routeNum){
             mode_object.set_Mode("LAND");
+            std::cout <<"LAND"<<std::endl;
         }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
diff --git a/class_model/src/bio_main5.cpp b/class_model/src/bio_main5.cpp
index 4f8cf82..989072f 100644
--- a/class_model/src/bio_main5.cpp
+++ b/class_model/src/bio_main5.cpp
@@ -79,12 +79,12 @@ int main(int argc, char **argv) {
             leaderID = check_leader(*pos,index).leader_ID;
             leaderIndex = check_leader(*pos,index).leader_index;
          
-            std::cout << leaderID << std::endl;
-            std::cout << target.lon << std::endl;
+            // std::cout << leaderID << std::endl;
+            // std::cout << target.lon << 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
 
         while(errorX > 100 || errorY > 100){
 
@@ -119,15 +119,17 @@ int main(int argc, char **argv) {
             // 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;
+            // std::cout <<"x: "<< errorX << std::endl;
+            // std::cout <<"y: "<< errorY << std::endl;
         }
 
         if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
             routeIndex++;
             // OnTarget = 0;
-        }else if(routeIndex >= routeNum){
+        }
+        while (routeIndex >= routeNum){
             mode_object.set_Mode("LAND");
+            std::cout <<"LAND"<<std::endl;
         }
         // if(type == "land"){
         //     mode_object.set_Mode("LAND");
diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp
index 53694e8..92580aa 100755
--- a/class_model/src/follower.cpp
+++ b/class_model/src/follower.cpp
@@ -361,15 +361,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;
@@ -396,17 +396,17 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
         error_yaw = 0;
     }
 
-    if (error_lon < -2){                                      
-        error_lon = -2;
+    if (error_lon < -limit_speed){                                      
+        error_lon = -limit_speed;
     }
-    if (error_lon > 2){                                      
-        error_lon = 2;
+    if (error_lon > limit_speed){                                      
+        error_lon = limit_speed;
     }
-    if (error_lat < -2){
-        error_lat = -2;
+    if (error_lat < -limit_speed){
+        error_lat = -limit_speed;
     }    
-    if (error_lat > 2){
-        error_lat = 2;
+    if (error_lat > limit_speed){
+        error_lat = limit_speed;
     }
 
     // ROS_INFO("%d,%f,%f",refpos.ID,refpos.lon ,refpos.lat );