VSAG/amadeus_26_fb
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto Aim

Browse Source
This commit is contained in:
JiatongSun
2019-05-07 09:33:28 +08:00
parent 7fcd8e6c84
commit 508004c548
9 changed files with 191 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
energy/include/energy/energy.h
View File

@@ -60,6 +60,8 @@ private:
double phi;//电机yaw轴应旋转的角度
float yaw_rotation;
float pitch_rotation;
int position_mode;
int last_position_mode;
int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound;
std::vector<EnergyPart> fans;

2
energy/src/energy/calculate/cycle_calculate.cpp
View File

@@ -58,7 +58,7 @@ void Energy::cycleLeastFit()
radius = 0.0f;
if (all_armor_centers.size() < 3)
{
cout<<"Cannot calculate a circle"<<endl;
// cout<<"Cannot calculate a circle"<<endl;
return;
}

27
energy/src/energy/calibrate/structing.cpp
View File

@@ -17,20 +17,25 @@ void Energy::StructingElementClose(cv::Mat &src,int length, int width){
void Energy::StructingElementErodeDilate(cv::Mat &src) {
cv::Mat src_out, src_out_out;
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(6, 6));
Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(10, 10));
Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2, 2));
Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1));
Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(8, 6));
Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4,4));
Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3));
dilate(src, src, element_dilate_1);
imshow("dilate_1", src);
// dilate(src, src, element_dilate_1);
// imshow("dilate_1", src);
erode(src,src, element_erode_1);
imshow("erode_1", src);
// dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
//
// erode(src,src, element_erode_2);
// imshow("erode_2", src);
dilate(src, src, element_dilate_2);
imshow("dilate_2", src);
erode(src,src, element_erode_2);
imshow("erode_2", src);
dilate(src, src, element_dilate_3);
imshow("dilate_3", src);
}

3
energy/src/energy/energy.cpp
View File

@@ -50,6 +50,9 @@ Energy::Energy(Uart &u):uart(u),
top = Point(640, 480);
bottom = Point(0, 0);
position_mode = 0;
last_position_mode = 0;
initEnergyPartParam();
}

1
energy/src/energy/find/energy_finder.cpp
View File

@@ -71,6 +71,7 @@ int Energy::findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_
std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
StructingElementErodeDilate(src_bin);
// StructingElementClose(src_bin,10,10);
imshow("armor struct",src_bin);
findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

23
energy/src/energy/get/gimble_rotation_get.cpp
View File

@@ -24,7 +24,28 @@ void Energy::gimbleRotation(){
// pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-real_hit_point.y), ATTACK_DISTANCE));
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance));
pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
if(position_mode == 1){
pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
}
if(position_mode == 2){
pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
}
if(position_mode == 3){
pitch_rotation = 4.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
}
if(position_mode == 4){
pitch_rotation = 4.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
}
if(position_mode == 5){
pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
}
if(position_mode == 6){
pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
}
// else{
// pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(3.5*PI/180)-STRETCH*(hit_point.x-cycle_center.x)), ATTACK_DISTANCE));
// pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(9.1*PI/180)-STRETCH*(hit_point.y-cycle_center.y)), ATTACK_DISTANCE));

127
energy/src/energy/get/hit_point_get.cpp
View File

@@ -16,38 +16,47 @@ void Energy::getHitPoint(){
int angle_interval = 60;
if(energy_part_rotation==1){ //顺时针
if(target_armor>=15 && target_armor<=15+limit_angle){
if(target_armor>=0 && target_armor<=limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -1*angle_interval;
position_mode = 1;
// cout<<"666"<<'\t'<<hit_position<<endl;
}
else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius);
hit_point.y = cycle_center.y;
hit_position = 0;
position_mode = 2;
}
else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = angle_interval;
position_mode = 3;
}
else if(target_armor>=-180 && target_armor<-180+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = 2*angle_interval;
position_mode = 4;
}
else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius);
hit_point.y = cycle_center.y;
hit_position = 180;
position_mode = 5;
}
else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) {
hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -2 * angle_interval;
position_mode = 6;
}
// if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 0;
// }
// if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = angle_interval;
// }
// if(target_armor>=-180 && target_armor<-180+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = 2*angle_interval;
// }
// if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 180;
// }
// if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) {
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -2 * angle_interval;
// }
else{
last_hit_position = 2000;
// last_hit_position = 2000;
position_mode = 0;
return;
}
}
@@ -57,54 +66,84 @@ void Energy::getHitPoint(){
hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = 2*angle_interval;
position_mode = 1;
}
if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius);
hit_point.y = cycle_center.y;
hit_position = 180;
position_mode = 2;
}
if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -2*angle_interval;
position_mode = 3;
}
if(target_armor>=-180 && target_armor<-180+limit_angle){
else if(target_armor>=-180 && target_armor<-180+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -1*angle_interval;
position_mode = 4;
}
if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius);
hit_point.y = cycle_center.y;
hit_position = 0;
position_mode = 5;
}
if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle){
else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = angle_interval;
position_mode = 6;
}
else{
last_hit_position = 2000;
// last_hit_position = 2000;
position_mode = 0;
return;
}
}
if(hit_position>360&&last_hit_position>360){
isSendTarget = false;
return;
}
if(fabs(hit_position - last_hit_position) < 30||fabs(hit_position - last_hit_position) > 330 && fabs(hit_position - last_hit_position) <= 360){
last_hit_position = hit_position;
isSendTarget = false;
return;
}
else{
last_hit_position = hit_position;
// cout<<"last: "<<last_position_mode<<'\t'<<"now: "<<position_mode<<endl;
if(position_mode!=0 && position_mode!=last_position_mode){
// last_hit_position = hit_position;
last_position_mode = position_mode;
isSendTarget = true;
cout<<"hit position: "<<hit_position<<endl;
return;
}
else if(position_mode == 0){
// last_hit_position = hit_position;
// last_position_mode = position_mode;
isSendTarget = false;
return;
}
else{
// last_hit_position = hit_position;
last_position_mode = position_mode;
isSendTarget = false;
return;
}
// if(hit_position>360&&last_hit_position>360){
// isSendTarget = false;
// return;
// }
// if(fabs(hit_position - last_hit_position) < 15||fabs(hit_position - last_hit_position) > 345 && fabs(hit_position - last_hit_position) <= 360){
// last_hit_position = hit_position;
// isSendTarget = false;
// return;
// }
// else{
// last_hit_position = hit_position;
// isSendTarget = true;
// cout<<"hit position: "<<hit_position<<endl;
// return;
// }
// double rad = static_cast<double>(energy_part_rotation * energy_part_param_.RPM
// * energy_part_param_.HIT_TIME * 360 / 60);

13
energy/src/energy/run.cpp
View File

@@ -12,7 +12,7 @@ extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
extern int mark;
int Energy::run(cv::Mat &src){
// imshow("src",src);
imshow("src",src);
fans.clear();
armors.clear();
fanPosition.clear();
@@ -52,14 +52,14 @@ int Energy::run(cv::Mat &src){
// cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
cycleLeastFit();
// cycle_center = cv::Point(338,296);
// radius = 119.22;
attack_distance = ATTACK_DISTANCE * 130 / radius;
// cycle_center = cv::Point(335, 246);
// radius = 116.936;
attack_distance = ATTACK_DISTANCE * 120/ radius;
getFanPosition(fanPosition, fans, cycle_center, radius);
getArmorPosition(armorPosition, armors, cycle_center, radius);
findTarget(fanPosition, armorPosition, target_armor);
cout << "The target armor's position is " << target_armor << endl;
// cout << "The target armor's position is " << target_armor << endl;
// cout<<"The target armor center is: "<<target_center<<endl;
getHitPoint();
@@ -68,7 +68,8 @@ int Energy::run(cv::Mat &src){
if(!isSendTarget)return 0;
cout<<"send"<<endl;
// cout<<"send"<<endl;
cout<<"position mode: "<<position_mode<<endl;
gimbleRotation();