Xuan0319
/
DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'protobuf' of github.com:Xuan0319/DroneControl into protobuf

Browse Source
protobuf
RangeOfGlitching 3 years ago
parent f8812afb5a eec2609f88
commit d2f433c80f
5 changed files with 58 additions and 23 deletions
Show Stats Download Patch File Download Diff File

1
class_model/include/class_model/formation.h
Unescape Escape View File

@ -55,6 +55,7 @@ private:
float pre_alt,cur_alt; float pre_alt,cur_alt;
float leader_pid[3] = {0.5 , 0.000000000001 ,0.5}; float leader_pid[3] = {0.5 , 0.000000000001 ,0.5};
float follower_pid[3] = {1 , 0.00000000001 ,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 calculate_position(float k,float theta,int direction=0);
void spherical_coordinate(float k,float theta,float psi); void spherical_coordinate(float k,float theta,float psi);

2
class_model/src/PID.cpp
Unescape Escape View File

@ -12,7 +12,7 @@ float PIDClass::update(float current ,float target ,float pidvals[3]){
current_time = std::clock(); current_time = std::clock();
// pTime = current_time - // pTime = current_time -
double t = (current_time - pTime); double t = (current_time - pTime);
float error = target - current; float error = (target - current)/1.1;
double P,D,result; double P,D,result;
P = pidvals[0] * error; P = pidvals[0] * error;

27
class_model/src/formation.cpp
Unescape Escape View File

@ -109,10 +109,11 @@ void FormationClass::leader1(float x,float y){
void FormationClass::follower1(int type){ void FormationClass::follower1(int type){
std::string command,pre_command = ""; std::string command = "";
std::string pre_command = receiver_object.check_command();
int message; int message;
while(true){ while(true){
if(type == 0){ if(type == 0){
calculate_position(4,30); calculate_position(4,30);
}else if(type == 1){ }else if(type == 1){
@ -130,12 +131,13 @@ void FormationClass::follower1(int type){
command = receiver_object.check_command(); command = receiver_object.check_command();
// message = request_object.get_formation_message(); // message = request_object.get_formation_message();
// ROS_INFO("%d",message); // ROS_INFO("%d",message);
// std::cout<<command<<std::endl;
// std::cout<<"/"<<pre_command<<std::endl;
if(command != pre_command ){ if(command != pre_command ){
// ROS_INFO("change formation"); ROS_INFO("change formation");
break; break;
} }
pre_command = command;
} }
@ -437,8 +439,8 @@ void FormationClass::calculate_position(float k,float theta,int direction){
// float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] ); //caculate error // 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] ); // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
// float error_x = Pf[0] - (follower_location.lon*1e+5); //caculate error float error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
// float error_y = Pf[1] - (follower_location.lat*1e+5); float error_lat = Pf[1] - (follower_location.lat*1e+5);
float error_degree = deg_phi - heading + direction; float error_degree = deg_phi - heading + direction;
float error_yaw = 0.2; //CCW float error_yaw = 0.2; //CCW
@ -452,11 +454,11 @@ void FormationClass::calculate_position(float k,float theta,int direction){
error_yaw = check_direction(error_degree)*error_yaw; //check yaw direction error_yaw = check_direction(error_degree)*error_yaw; //check yaw direction
if (error_x < 0.3 && error_x > -0.3){ //ignore small errors if (error_lon < ignore_small && error_lon > -ignore_small){ //ignore small errors
error_x = 0; error_lon = 0;
} }
if (error_y < 0.3 && error_y > -0.3){ if (error_lat < ignore_small && error_lat > -ignore_small){
error_y = 0; error_lat = 0;
} }
if (error_degree < 5 && error_degree > -5 ){ if (error_degree < 5 && error_degree > -5 ){
error_yaw = 0; 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",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
// ROS_INFO("%f,%f",Pf[0],Pf[1]); // 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("%f,%f",error_lon,error_lat);
// ROS_INFO("deg:%f",deg_phi); // ROS_INFO("deg:%f",deg_phi);
// ROS_INFO("heading:%f",heading); // ROS_INFO("heading:%f",heading);
// ROS_INFO("error_degree:%f",error_degree); // ROS_INFO("error_degree:%f",error_degree);
// ROS_INFO("error_yaw:%f",error_yaw); // ROS_INFO("error_yaw:%f",error_yaw);
// ROS_INFO("************************************"); // 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",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",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
// ROS_INFO("%f,%f",Pf[0],Pf[1]); // 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("deg:%f",deg_phi);
// ROS_INFO("heading:%f",heading); // ROS_INFO("heading:%f",heading);
// ROS_INFO("error_degree:%f",error_degree); // ROS_INFO("error_degree:%f",error_degree);

49
class_model/src/main.cpp
Unescape Escape View File

@ -22,26 +22,57 @@ int main(int argc, char **argv) {
// SelectionClass selection_object; // 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"); mode_object.set_Mode("GUIDED");
control_object.arm(); control_object.arm();
control_object.takeoff(4.5); control_object.takeoff(2);
sleep(2); sleep(2);
// selection_object.choose_leader(); // selection_object.choose_leader();
// mission_object.start(); //default hover in goose formation // mission_object.start(); //default hover in goose formation
INFO msg; // INFO msg;
msg.type = "V_formation"; // msg.type = "V_formation";
msg.mission = "fly2target"; // 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(); ros::waitForShutdown();

2
class_model/src/select.cpp
Unescape Escape View File

@ -70,7 +70,7 @@ void SelectClass::line_formation(float x ,float y){
counter = 1; counter = 1;
if(param_object.getID() == 1){ if(param_object.getID() == 1){
formation_object.leader1(); formation_object.leader1(x,y);
}else if(param_object.getID() == 2){ }else if(param_object.getID() == 2){
formation_object.follower1(counter); formation_object.follower1(counter);
}else if(param_object.getID() == 3){ }else if(param_object.getID() == 3){

Write Preview
Loading…
Cancel
Save