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

Bio test succeeded

Browse Source
protobuf
Xuan0319 3 years ago
parent bf8f017f95
commit e525815992
14 changed files with 137 additions and 81 deletions
Show Stats Download Patch File Download Diff File

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

@ -45,8 +45,8 @@ private:
int OnTarget = 0;
float pre_alt,cur_alt;
float error_lon,error_lat;
float leader_pid[3] = {1 , 0.000000000001 ,0};
float ignore_small = 0.50 ,limit_speed = 0.5;
float leader_pid[3] = {0.5 , 0.000001 ,0.001};
float ignore_small = 0.50 ,limit_speed = 10;
void face2target(float target_heading);
};
#endif // Leader_H

11
class_model/include/class_model/PID.h
Unescape Escape View File

@ -4,16 +4,21 @@
#include <ros/ros.h>
#include "ctime"
#include <iostream>
#include <fstream>
class PIDClass {
public:
// PIDClass();
// virtual ~PIDClass();
PIDClass();
virtual ~PIDClass();
float update(float current ,float target ,float pidvals[3]);
float update1(float current ,float target );
std::string droneID;
private:
// ROS NodeHandle
ros::NodeHandle node_handle_;
std::ofstream logFile;
std::string ros_namespace;
float pidval1[3] = {0.5 , 0.000000000001 , 0.5};
float limit[2] = {2 , -2};
@ -23,5 +28,5 @@ private:
};
//"/home/dodo/ardupilot_ws/src/class_model/src/log/PID"+droneID+".txt"
#endif // PID_H

4
class_model/include/class_model/convert_degree.h
Unescape Escape View File

@ -29,11 +29,11 @@ float check_direction(float error_degree){
int direction;
if(error_degree <= 180 && error_degree >= 0){ //check yaw direction
direction = -1;
}else if(error_degree < -180){
}else if(error_degree < -180 && error_degree < 0){
direction = -1;
}else if(error_degree >= -180 && error_degree < 0){
direction = 1;
}else if(error_degree > 180){
}else if(error_degree > 180 && error_degree >= 0){
direction = 1;
}

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

@ -1,7 +1,7 @@
/*Follow the leader in a fixed formation */
#ifndef FOLLOWER_H
#define FOLLOWER_H
#define swarmNum 3
#define swarmNum 5 //The number of drones
#define changeLeader
// #define NoChangeLeader
#include <ros/ros.h>
@ -62,8 +62,8 @@ private:
float distance = config["formation"]["distance"].as<float>();
float angle = config["formation"]["angle"].as<float>();
float error_lon,error_lat;
float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
float ignore_small = 0.50 ,limit_speed = 0.5;
float follower_pid[3] = {0.5 , 0.000001 ,0.001};// D downsize
float ignore_small = 0.50 ,limit_speed = 10;
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
float slope,c,y; // function plane()'s variable

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

@ -28,6 +28,7 @@ void LeaderClass::leader(global_location target){
pre_target.lon = target.lon;
pre_target.lat = target.lat;
OnTarget = 1;
heading_status = 0;
ROS_INFO("break");
break;
}
@ -122,7 +123,7 @@ void LeaderClass::face2target(float target_heading){
float error_heading;
float heading = GPS_object.get_heading();
float error_yaw = 0.2;
float error_yaw = 0.25;
error_heading = target_heading - heading;
error_yaw = check_direction(error_heading)*error_yaw*0.5;

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

@ -1,23 +1,37 @@
#include "class_model/PID.h"
// PIDClass::PIDClass() : node_handle_(""){
PIDClass::PIDClass() : node_handle_("~"){
// }
// PIDClass::~PIDClass() { ros::shutdown(); }
// if (!node_handle_.hasParam("droneID"))
// {
// }else{
// node_handle_.getParam("droneID", droneID);
// }
if (!node_handle_.hasParam("namespace"))
{
}else{
node_handle_.getParam("namespace", ros_namespace);
}
}
PIDClass::~PIDClass() { ros::shutdown(); }
float PIDClass::update(float current ,float target ,float pidvals[3]){
current_time = std::clock();
// pTime = current_time -
double t = (current_time - pTime);
double t = (current_time - pTime)/1e6;
float error = (target - current);
double P,D,result;
P = pidvals[0] * error;
I = I + (pidvals[1] * error *t);
// D = (pidvals[2] * (error - pError))/t;
// I = 0;
D = 0;
result = P + I + D;
@ -31,6 +45,13 @@ float PIDClass::update(float current ,float target ,float pidvals[3]){
}
}
logFile.open("/home/dodo/ardupilot_ws/src/class_model/src/log"+ros_namespace+"PID.txt", std::ofstream::app);
logFile << P <<","<< I <<","<< D << std::endl;
logFile.close();
pError = error;
pTime = current_time;

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

@ -45,10 +45,10 @@ int main(int argc, char **argv) {
M4 = request_object.get_M4_GPS();
// global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
// global_location* member[] = {&M1,&M2,&M3,&M4};
global_location* pos[] = {&target,&self,&M1,&M2};
global_location* member[] = {&M1,&M2};
global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location* member[] = {&M1,&M2,&M3,&M4};
// global_location* pos[] = {&target,&self,&M1,&M2};
// global_location* member[] = {&M1,&M2};
size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]);
@ -95,7 +95,7 @@ int main(int argc, char **argv) {
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){
while(errorX > 150 || errorY > 150){
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
@ -103,6 +103,12 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
if(errorX < 150 && errorY < 150){
break;
}
if(checkLeader == 1){
std::cout << "Drone1 is Leader" << std::endl;
@ -114,20 +120,21 @@ int main(int argc, char **argv) {
}
// OnTarget = leader_object.isOnTarget();
// std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
}
sleep(3);
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"Drone1routeIndex: "<<routeIndex<<std::endl;
// std::cout <<"Drone1routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
// else if(routeIndex == routeNum -1){
// routeIndex = 0;
// }
// }
// type = "x";
// while (routeIndex >= routeNum){

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

@ -45,10 +45,10 @@ int main(int argc, char **argv) {
M4 = request_object.get_M4_GPS();
// global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
// global_location* member[] = {&M1,&M2,&M3,&M4};
global_location* pos[] = {&target,&self,&M1,&M2};
global_location* member[] = {&M1,&M2};
global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location* member[] = {&M1,&M2,&M3,&M4};
// global_location* pos[] = {&target,&self,&M1,&M2};
// global_location* member[] = {&M1,&M2};
size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]);
@ -95,7 +95,7 @@ int main(int argc, char **argv) {
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){
while(errorX > 150 || errorY > 150){
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
@ -103,6 +103,12 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
if(errorX < 150 && errorY < 150){
break;
}
if(checkLeader == 1){
std::cout << "Drone2 is Leader" << std::endl;
@ -114,20 +120,21 @@ int main(int argc, char **argv) {
}
// OnTarget = leader_object.isOnTarget();
// std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
}
sleep(3);
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"Drone2routeIndex: "<<routeIndex<<std::endl;
// std::cout <<"Drone2routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
// else if(routeIndex == routeNum -1){
// routeIndex = 0;
// }
// }
// type = "x";
// while (routeIndex >= routeNum){

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

@ -45,10 +45,10 @@ int main(int argc, char **argv) {
M4 = request_object.get_M4_GPS();
// global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
// global_location* member[] = {&M1,&M2,&M3,&M4};
global_location* pos[] = {&target,&self,&M1,&M2};
global_location* member[] = {&M1,&M2};
global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location* member[] = {&M1,&M2,&M3,&M4};
// global_location* pos[] = {&target,&self,&M1,&M2};
// global_location* member[] = {&M1,&M2};
size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]);
@ -95,7 +95,7 @@ int main(int argc, char **argv) {
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){
while(errorX > 150 || errorY > 150){
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
@ -103,6 +103,12 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
if(errorX < 150 && errorY < 150){
break;
}
if(checkLeader == 1){
std::cout << "Drone3 is Leader" << std::endl;
@ -114,20 +120,21 @@ int main(int argc, char **argv) {
}
// OnTarget = leader_object.isOnTarget();
// std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
}
sleep(3);
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"Drone3routeIndex: "<<routeIndex<<std::endl;
// std::cout <<"Drone3routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
// else if(routeIndex == routeNum -1){
// routeIndex = 0;
// }
// }
// type = "x";
// while (routeIndex >= routeNum){

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

@ -93,7 +93,7 @@ int main(int argc, char **argv) {
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){
while(errorX > 150 || errorY > 150){
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
@ -101,6 +101,12 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
if(errorX < 150 && errorY < 150){
break;
}
if(checkLeader == 1){
std::cout << "Drone4 is Leader" << std::endl;
@ -112,18 +118,16 @@ int main(int argc, char **argv) {
}
// OnTarget = leader_object.isOnTarget();
// std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
}
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"routeIndex: "<<routeIndex<<std::endl;
// std::cout <<"routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
// }

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

@ -93,7 +93,7 @@ int main(int argc, char **argv) {
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
while(errorX > 100 || errorY > 100){
while(errorX > 150 || errorY > 150){
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
@ -101,6 +101,12 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
if(errorX < 150 && errorY < 150){
break;
}
if(checkLeader == 1){
std::cout << "Drone5 is Leader" << std::endl;
@ -112,18 +118,16 @@ int main(int argc, char **argv) {
}
// OnTarget = leader_object.isOnTarget();
// std::cout <<"tar: "<< OnTarget << std::endl;
errorX = abs(pos[leaderIndex]->lon - target.lon) ;
errorY = abs(pos[leaderIndex]->lat - target.lat) ;
// std::cout <<"x: "<< errorX << std::endl;
// std::cout <<"y: "<< errorY << std::endl;
}
sleep(3);
// while(type != "n"){
// type = receiver_object.check_command();
if(errorX < 100 && errorY < 100 && routeIndex < routeNum -1 ){
if(errorX < 150 && errorY < 150 && routeIndex < routeNum -1 ){
routeIndex++;
checkLeader = 0;
std::cout <<"routeIndex: "<<routeIndex<<std::endl;
// std::cout <<"routeIndex: "<<routeIndex<<std::endl;
// OnTarget = 0;
}
// }

12
class_model/src/config.yaml
Unescape Escape View File

@ -2,19 +2,19 @@ DroneParam:
ID: 1
route1:
x: 120.6743161 #(24.1218859, 120.6743161)
y: 24.1220860
y: 24.1221859
z: 0.0
route2:
x: 120.6746461 #(24.1218859, 120.6743161)
y: 24.1218860
x: 120.6745161 #(24.1218859, 120.6743161)
y: 24.1218859
z: 0.0
route3:
x: 120.6743161 #(24.1218859, 120.6743161)
y: 24.1217360
y: 24.1216859
z: 0.0
route4:
x: 120.6743161 #(24.1218859, 120.6743161)
y: 24.1218859
x: 120.6740161 #(24.1218859, 120.6743161)
y: 24.1217359
z: 0.0
formation:
distance: 4.0

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

@ -83,7 +83,7 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
y = slope * follower->lon + c;
// std::cout<< follower.ID << ","<< follower.lat <<","<< y <<std::endl;
std::cout<< "slope: " << slope <<std::endl;
// std::cout<< "slope: " << slope <<std::endl;
if((follower->lat <= y && phi < 180) || (follower->lat > y && phi >= 180)){
// std::cout <<"drone :"<<follower->ID <<", plane: 1" <<std::endl;
@ -298,20 +298,20 @@ int FollowerClass::find_ref_drone(global_location** data, size_t index, int lead
j+=1;
// std::cout << "drone3" <<std::endl;
}
// if(drone4.ID == data[1]->ID){
// self = drone4;
// }else{
// memberDrone[j] = drone4;
// j+=1;
// // std::cout << "drone4" <<std::endl;
// }
// if(drone5.ID == data[1]->ID){
// self = drone5;
// }else{
// memberDrone[j] = drone5;
// j+=1;
// // std::cout << "drone4" <<std::endl;
// }
if(drone4.ID == data[1]->ID){
self = drone4;
}else{
memberDrone[j] = drone4;
j+=1;
// std::cout << "drone4" <<std::endl;
}
if(drone5.ID == data[1]->ID){
self = drone5;
}else{
memberDrone[j] = drone5;
j+=1;
// std::cout << "drone4" <<std::endl;
}
for(int i=0;i<index-2;i++){
@ -343,7 +343,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 << ",mem_index: "<< s_index << std::endl;
// std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << ",mem_index: "<< s_index << std::endl;
// std::cout <<"\n"<< std::endl;
return ref_ID;
@ -379,15 +379,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;
@ -449,7 +449,7 @@ void FollowerClass::face2target(global_location* target, global_location* leader
float x,y,target_heading;
float heading = GPS_object.get_heading();
heading_status = 0;
error_yaw = 0.2;
error_yaw = 0.25;
x = (target->lon/100) - (leader->lon/100);
y = (target->lat/100) - (leader->lat/100);
target_heading = ConvertDeg(x,y);

2
iq_sim/worlds/three_drone.world
Unescape Escape View File

@ -85,7 +85,7 @@
<model name="drone1">
<pose> 0 0 0 0 0 0</pose>
<include>
<uri>model://drone1</uri>
<uri>model://drone_with_camera</uri>
</include>
</model>
<model name="drone2">

Write Preview
Loading…
Cancel
Save