Bio test succeeded

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

class_model/include/class_model/PID.h

@@ -4,16 +4,21 @@
 
 #include <ros/ros.h>
 #include "ctime"
+#include <iostream>
+#include <fstream>
 
 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

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;
     }
 

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 <ros/ros.h>
@@ -62,8 +62,8 @@ private:
     float distance = config["formation"]["distance"].as<float>();
     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 ,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