diff --git a/class_model/include/class_model/formation.h b/class_model/include/class_model/formation.h index dd1beea..c898752 100755 --- a/class_model/include/class_model/formation.h +++ b/class_model/include/class_model/formation.h @@ -55,6 +55,7 @@ private: float pre_alt,cur_alt; float leader_pid[3] = {0.5 , 0.000000000001 ,0.5}; float follower_pid[3] = {1 , 0.00000000001 ,0.5}; + float ignore_small = 0.20; void calculate_position(float k,float theta,int direction=0); void spherical_coordinate(float k,float theta,float psi); diff --git a/class_model/src/PID.cpp b/class_model/src/PID.cpp index 881695b..a8eb07d 100755 --- a/class_model/src/PID.cpp +++ b/class_model/src/PID.cpp @@ -12,7 +12,7 @@ float PIDClass::update(float current ,float target ,float pidvals[3]){ current_time = std::clock(); // pTime = current_time - double t = (current_time - pTime); - float error = target - current; + float error = (target - current)/1.1; double P,D,result; P = pidvals[0] * error; diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp index 21a6144..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< -0.3){ //ignore small errors - error_x = 0; + if (error_lon < ignore_small && error_lon > -ignore_small){ //ignore small errors + error_lon = 0; } - if (error_y < 0.3 && error_y > -0.3){ - error_y = 0; + if (error_lat < ignore_small && error_lat > -ignore_small){ + error_lat = 0; } if (error_degree < 5 && error_degree > -5 ){ error_yaw = 0; @@ -468,13 +470,14 @@ void FormationClass::calculate_position(float k,float theta,int direction){ // 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); // ROS_INFO("error_yaw:%f",error_yaw); // ROS_INFO("************************************"); - command_object.set_velocity(error_x,error_y,0,error_yaw,0.1); + command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1); } @@ -548,7 +551,7 @@ void FormationClass::spherical_coordinate(float k,float theta,float psi){ // 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_x,error_y); // 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 4707e7c..999ab6b 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -22,26 +22,57 @@ int main(int argc, char **argv) { // SelectionClass selection_object; - YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) + // YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) mode_object.set_Mode("GUIDED"); control_object.arm(); - control_object.takeoff(4.5); + control_object.takeoff(2); sleep(2); // selection_object.choose_leader(); // mission_object.start(); //default hover in goose formation - INFO msg; - msg.type = "V_formation"; - msg.mission = "fly2target"; + // INFO msg; + // msg.type = "V_formation"; + // msg.mission = "fly2target"; + + std::string type = "",pre_type ="x"; - while(ros::ok()){ + while(ros::ok()){ + type = receiver_object.check_command(); + + if(pre_type == type){ + type = "alreadly_set"; + ROS_INFO("set"); + } + + + if(type == ""){ + select_formation.goose_formation(); + ROS_INFO("init formation"); + }else if(type == "line"){ + select_formation.line_formation(); + }else if(type == "row"){ + select_formation.row_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"){ + mode_object.set_Mode("LAND"); + } - //json_object.get_command(); //for follower - //select_formation.goose_formation(); - PubInfo_object.pub_info(msg); } ros::waitForShutdown(); 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){