From eec2609f88c3e7eeeb7dc0031ff496b7a34647d1 Mon Sep 17 00:00:00 2001
From: Xuan0319 <new310049@gmail.com>
Date: Wed, 4 Jan 2023 17:22:04 +0800
Subject: [PATCH] for experiment

---
 class_model/src/formation.cpp | 20 +++++++++++---------
 class_model/src/main.cpp      | 29 ++++++++++++++++++++++-------
 class_model/src/select.cpp    |  2 +-
 3 files changed, 34 insertions(+), 17 deletions(-)

diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp
index cfdfb27..2a5b106 100755
--- a/class_model/src/formation.cpp
+++ b/class_model/src/formation.cpp
@@ -109,10 +109,11 @@ void FormationClass::leader1(float x,float y){
 
 void FormationClass::follower1(int type){
 
-    std::string command,pre_command = "";
+    std::string command = "";
+    std::string pre_command = receiver_object.check_command();
     int message;
     while(true){
-        
+
         if(type == 0){
             calculate_position(4,30);
         }else if(type == 1){
@@ -130,12 +131,13 @@ void FormationClass::follower1(int type){
         command = receiver_object.check_command();
         // message = request_object.get_formation_message();
         // ROS_INFO("%d",message);
+        // std::cout<<command<<std::endl;
+        // std::cout<<"/"<<pre_command<<std::endl;
 
         if(command != pre_command ){
-            // ROS_INFO("change formation");
+            ROS_INFO("change formation");
             break;
         }
-        pre_command = command;
         
 
     }
@@ -464,11 +466,11 @@ void FormationClass::calculate_position(float k,float theta,int direction){
         error_yaw = 0;
     }
 
-    ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
-    ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
-    ROS_INFO("%f,%f",Pf[0],Pf[1]);
-    ROS_INFO("%f,%f",error_x,error_y);
-    ROS_INFO("%f,%f",error_lon,error_lat);
+    // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
+    // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
+    // ROS_INFO("%f,%f",Pf[0],Pf[1]);
+    // ROS_INFO("%f,%f",error_x,error_y);
+    // ROS_INFO("%f,%f",error_lon,error_lat);
     // ROS_INFO("deg:%f",deg_phi);
     // ROS_INFO("heading:%f",heading);
     // ROS_INFO("error_degree:%f",error_degree);
diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp
index 857505d..999ab6b 100755
--- a/class_model/src/main.cpp
+++ b/class_model/src/main.cpp
@@ -36,22 +36,37 @@ int main(int argc, char **argv) {
     // msg.type = "V_formation";
     // msg.mission = "fly2target";
 
-    std::string type = "";
+    std::string type = "",pre_type ="x";
+
 
     while(ros::ok()){
-        type = receiver_object.check_command();  
-    // ROS_INFO("%s",type.c_str());
+        type = receiver_object.check_command();
+        
+    if(pre_type == type){
+        type = "alreadly_set";
+        ROS_INFO("set");
+    }
+    
     
-    if(type == "" || type == "init"){
+    if(type == ""){
         select_formation.goose_formation();
         ROS_INFO("init formation");
     }else if(type == "line"){
         select_formation.line_formation();
-        // ROS_INFO("line foemation");
     }else if(type == "row"){
         select_formation.row_formation();
-    }else if(type == "circle"){
-        select_formation.circle_formation();
+    }else if(type == "v"){
+        select_formation.goose_formation(0,5);
+        pre_type ="v";
+    }else if(type == "v2"){
+        select_formation.line_formation(-5,0);
+        pre_type ="v2";
+    }else if(type == "v3"){
+        select_formation.goose_formation(0,-5);
+        pre_type ="v3";
+    }else if(type == "v4"){
+        select_formation.row_formation(5,0);
+        pre_type ="v4";
     }else if(type == "stop"){
         select_formation.stop();
     }else if(type == "land"){
diff --git a/class_model/src/select.cpp b/class_model/src/select.cpp
index 4b253e6..c498f5e 100755
--- a/class_model/src/select.cpp
+++ b/class_model/src/select.cpp
@@ -70,7 +70,7 @@ void SelectClass::line_formation(float x ,float y){
     
     counter = 1;
     if(param_object.getID() == 1){
-        formation_object.leader1();
+        formation_object.leader1(x,y);
     }else if(param_object.getID() == 2){
         formation_object.follower1(counter);
     }else if(param_object.getID() == 3){