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

move2target will interlock.

Browse Source
This commit is contained in:
Xuan0319
2023-03-27 21:50:02 +08:00
parent ea915b34b8
commit 414c01a502
8 changed files with 14 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/include/class_model/Leader.h
+1 -1
View File
@@ -42,7 +42,7 @@ private:
int flag = 0,heading_status = 0,buf = 0;
float pre_alt,cur_alt;
float error_lon,error_lat;
float leader_pid[3] = {1 , 0.000000000001 ,0.5};
float leader_pid[3] = {1 , 0.000000000001 ,0};
float ignore_small = 0.50;
void face2target(float target_heading);
};
class_model/include/class_model/follower.h
+1
View File
@@ -60,6 +60,7 @@ private:
float error_lon,error_lat;
float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
float ignore_small = 0.50;
int flag = 0;
void calculate_position(float k,float theta,int plane);
void plane(global_location* target, global_location* leader, global_location* follower);
class_model/src/Leader.cpp
+1 -1
View File
@@ -13,7 +13,7 @@ void LeaderClass::leader(global_location target){
std_msgs::String message;
flag = 0;
heading_status = 0;
// heading_status = 0;
while(true){
if(flag==0){
go2target(target.lon,target.lat);
class_model/src/bio_main2.cpp
+1 -1
View File
@@ -62,7 +62,7 @@ int main(int argc, char **argv) {
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(ros::ok()&& flag == 1){
while(ros::ok()){
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
class_model/src/bio_main3.cpp
+2 -2
View File
@@ -62,7 +62,7 @@ int main(int argc, char **argv) {
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(ros::ok()&& flag == 1){
while(ros::ok()){
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
@@ -72,7 +72,7 @@ int main(int argc, char **argv) {
M4 = request_object.get_M4_GPS();
if(checkLeader == 1){
leader_object.leader(target);
// leader_object.l/eader(target);
}else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(member,m_index,ref_ID); //follow reference drone position
class_model/src/bio_main4.cpp
+1 -1
View File
@@ -62,7 +62,7 @@ int main(int argc, char **argv) {
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(ros::ok()&& flag == 1){
while(ros::ok()){
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
class_model/src/bio_main5.cpp
+1 -1
View File
@@ -62,7 +62,7 @@ int main(int argc, char **argv) {
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(ros::ok()&& flag == 1){
while(ros::ok()){
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
class_model/src/follower.cpp
+6 -5
View File
@@ -129,6 +129,7 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
global_location vector_LT,vector_SM;
float m,n,k,cosTheta;
vector_LT.lon = target->lon - leader->lon; //vector_LT (leader->target)
vector_LT.lat = target->lat - leader->lat;
vector_SM.lon = member.init_location.lon - self.init_location.lon; //vector_SM (self->member)
@@ -261,7 +262,7 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
ref_ID = samePlane[check_index].ID;
// std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << std::endl;
std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << std::endl;
return ref_ID;
@@ -291,15 +292,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[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_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_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);
// 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] );
// error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
// error_lat = Pf[1] - (follower_location.lat*1e+5);
error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
error_lat = Pf[1] - (follower_location.lat*1e+5);
float error_degree = deg_phi - heading;