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

fix the error of change leader

Browse Source
protobuf
Xuan0319 3 years ago
parent 53456c157e
commit 0bf03420d1
10 changed files with 74 additions and 59 deletions
Show Stats Download Patch File Download Diff File

2
class_model/include/class_model/Leader.h
Unescape Escape View File

@ -46,7 +46,7 @@ private:
float pre_alt,cur_alt; float pre_alt,cur_alt;
float error_lon,error_lat; float error_lon,error_lat;
float leader_pid[3] = {1 , 0.000000000001 ,0}; 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); void face2target(float target_heading);
}; };
#endif // Leader_H #endif // Leader_H

2
class_model/include/class_model/follower.h
Unescape Escape View File

@ -60,7 +60,7 @@ private:
float angle = config["formation"]["angle"].as<float>(); float angle = config["formation"]["angle"].as<float>();
float error_lon,error_lat; float error_lon,error_lat;
float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize 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 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 global_location pre_tar,ref_point; // function plane()'s variable
float slope,c,y; // function plane()'s variable float slope,c,y; // function plane()'s variable

21
class_model/src/Leader.cpp
Unescape Escape View File

@ -16,8 +16,11 @@ void LeaderClass::leader(global_location target){
if(pre_target.lon != target.lon || pre_target.lat != target.lat){ if(pre_target.lon != target.lon || pre_target.lat != target.lat){
heading_status = 0; heading_status = 0;
ROS_INFO("heading_flag"); 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){ while(true){
if(flag==0){ if(flag==0){
OnTarget = 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){ if (drone_speed.y < ignore_small && drone_speed.y > -ignore_small){
drone_speed.y = 0; drone_speed.y = 0;
} }
if (drone_speed.x < -1){ //max velociy if (drone_speed.x < -limit_speed){ //max velociy
drone_speed.x = -1; drone_speed.x = -limit_speed;
} }
if (drone_speed.x > 1){ if (drone_speed.x > limit_speed){
drone_speed.x = 1; drone_speed.x = limit_speed;
} }
if (drone_speed.y < -1){ if (drone_speed.y < -limit_speed){
drone_speed.y = -1; drone_speed.y = -limit_speed;
} }
if (drone_speed.y > 1){ if (drone_speed.y > limit_speed){
drone_speed.y = 1; drone_speed.y = limit_speed;
} }
if (drone_speed.x == 0 && drone_speed.y == 0){ if (drone_speed.x == 0 && drone_speed.y == 0){

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

@ -17,7 +17,8 @@ float PIDClass::update(float current ,float target ,float pidvals[3]){
double P,D,result; double P,D,result;
P = pidvals[0] * error; P = pidvals[0] * error;
I = I + (pidvals[1] * error *t); I = I + (pidvals[1] * error *t);
D = (pidvals[2] * (error - pError))/t; // D = (pidvals[2] * (error - pError))/t;
D = 0;
result = P + I + D; result = P + I + D;

17
class_model/src/bio_main.cpp
Unescape Escape View File

@ -79,12 +79,12 @@ int main(int argc, char **argv) {
leaderID = check_leader(*pos,index).leader_ID; leaderID = check_leader(*pos,index).leader_ID;
leaderIndex = check_leader(*pos,index).leader_index; leaderIndex = check_leader(*pos,index).leader_index;
std::cout << leaderID << std::endl; // std::cout << leaderID << std::endl;
std::cout << target.lon << std::endl; // std::cout << target.lon << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){ while(errorX > 100 || errorY > 100){
@ -119,15 +119,18 @@ int main(int argc, char **argv) {
// std::cout <<"tar: "<< OnTarget << std::endl; // std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
} }
if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
routeIndex++; routeIndex++;
std::cout <<"routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0; // OnTarget = 0;
}else if(routeIndex >= routeNum){ }
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND"); mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
} }
// if(type == "land"){ // if(type == "land"){
// mode_object.set_Mode("LAND"); // mode_object.set_Mode("LAND");

16
class_model/src/bio_main2.cpp
Unescape Escape View File

@ -79,12 +79,12 @@ int main(int argc, char **argv) {
leaderID = check_leader(*pos,index).leader_ID; leaderID = check_leader(*pos,index).leader_ID;
leaderIndex = check_leader(*pos,index).leader_index; leaderIndex = check_leader(*pos,index).leader_index;
std::cout << leaderID << std::endl; // std::cout << leaderID << std::endl;
std::cout << target.lon << std::endl; // std::cout << target.lon << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){ while(errorX > 100 || errorY > 100){
@ -119,15 +119,17 @@ int main(int argc, char **argv) {
// std::cout <<"tar: "<< OnTarget << std::endl; // std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
} }
if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
routeIndex++; routeIndex++;
// OnTarget = 0; // OnTarget = 0;
}else if(routeIndex >= routeNum){ }
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND"); mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
} }
// if(type == "land"){ // if(type == "land"){
// mode_object.set_Mode("LAND"); // mode_object.set_Mode("LAND");

16
class_model/src/bio_main3.cpp
Unescape Escape View File

@ -79,12 +79,12 @@ int main(int argc, char **argv) {
leaderID = check_leader(*pos,index).leader_ID; leaderID = check_leader(*pos,index).leader_ID;
leaderIndex = check_leader(*pos,index).leader_index; leaderIndex = check_leader(*pos,index).leader_index;
std::cout << leaderID << std::endl; // std::cout << leaderID << std::endl;
std::cout << target.lon << std::endl; // std::cout << target.lon << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){ while(errorX > 100 || errorY > 100){
@ -119,15 +119,17 @@ int main(int argc, char **argv) {
// std::cout <<"tar: "<< OnTarget << std::endl; // std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
} }
if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
routeIndex++; routeIndex++;
// OnTarget = 0; // OnTarget = 0;
}else if(routeIndex >= routeNum){ }
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND"); mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
} }
// if(type == "land"){ // if(type == "land"){
// mode_object.set_Mode("LAND"); // mode_object.set_Mode("LAND");

16
class_model/src/bio_main4.cpp
Unescape Escape View File

@ -79,12 +79,12 @@ int main(int argc, char **argv) {
leaderID = check_leader(*pos,index).leader_ID; leaderID = check_leader(*pos,index).leader_ID;
leaderIndex = check_leader(*pos,index).leader_index; leaderIndex = check_leader(*pos,index).leader_index;
std::cout << leaderID << std::endl; // std::cout << leaderID << std::endl;
std::cout << target.lon << std::endl; // std::cout << target.lon << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){ while(errorX > 100 || errorY > 100){
@ -119,15 +119,17 @@ int main(int argc, char **argv) {
// std::cout <<"tar: "<< OnTarget << std::endl; // std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
} }
if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
routeIndex++; routeIndex++;
// OnTarget = 0; // OnTarget = 0;
}else if(routeIndex >= routeNum){ }
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND"); mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
} }
// if(type == "land"){ // if(type == "land"){
// mode_object.set_Mode("LAND"); // mode_object.set_Mode("LAND");

16
class_model/src/bio_main5.cpp
Unescape Escape View File

@ -79,12 +79,12 @@ int main(int argc, char **argv) {
leaderID = check_leader(*pos,index).leader_ID; leaderID = check_leader(*pos,index).leader_ID;
leaderIndex = check_leader(*pos,index).leader_index; leaderIndex = check_leader(*pos,index).leader_index;
std::cout << leaderID << std::endl; // std::cout << leaderID << std::endl;
std::cout << target.lon << std::endl; // std::cout << target.lon << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){ while(errorX > 100 || errorY > 100){
@ -119,15 +119,17 @@ int main(int argc, char **argv) {
// std::cout <<"tar: "<< OnTarget << std::endl; // std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ; errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ; errorY = abs(pos[leaderIndex]->lat - target.lat) ;
std::cout <<"x: "<< errorX << std::endl; // std::cout <<"x: "<< errorX << std::endl;
std::cout <<"y: "<< errorY << std::endl; // std::cout <<"y: "<< errorY << std::endl;
} }
if(errorX < 100 && errorY < 100 && routeIndex < routeNum){ if(errorX < 100 && errorY < 100 && routeIndex < routeNum){
routeIndex++; routeIndex++;
// OnTarget = 0; // OnTarget = 0;
}else if(routeIndex >= routeNum){ }
while (routeIndex >= routeNum){
mode_object.set_Mode("LAND"); mode_object.set_Mode("LAND");
std::cout <<"LAND"<<std::endl;
} }
// if(type == "land"){ // if(type == "land"){
// mode_object.set_Mode("LAND"); // mode_object.set_Mode("LAND");

24
class_model/src/follower.cpp
Unescape Escape View File

@ -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[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; 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_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_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_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] );
error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error // error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
error_lat = Pf[1] - (follower_location.lat*1e+5); // error_lat = Pf[1] - (follower_location.lat*1e+5);
float error_degree = deg_phi - heading; float error_degree = deg_phi - heading;
@ -396,17 +396,17 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
error_yaw = 0; error_yaw = 0;
} }
if (error_lon < -2){ if (error_lon < -limit_speed){
error_lon = -2; error_lon = -limit_speed;
} }
if (error_lon > 2){ if (error_lon > limit_speed){
error_lon = 2; error_lon = limit_speed;
} }
if (error_lat < -2){ if (error_lat < -limit_speed){
error_lat = -2; error_lat = -limit_speed;
} }
if (error_lat > 2){ if (error_lat > limit_speed){
error_lat = 2; error_lat = limit_speed;
} }
// ROS_INFO("%d,%f,%f",refpos.ID,refpos.lon ,refpos.lat ); // ROS_INFO("%d,%f,%f",refpos.ID,refpos.lon ,refpos.lat );

Write Preview
Loading…
Cancel
Save