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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
xinyang
2019-05-07 23:11:58 +08:00
parent 3cee0ea926 6fef2b5193
commit bd52fc33fc
12 changed files with 222 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
energy/include/energy/energy.h
View File

@@ -49,6 +49,7 @@ private:
double radius; double radius;
double target_position; double target_position;
double last_target_position; double last_target_position;
double last_hit_position;
float target_armor; float target_armor;
int ally_color_; int ally_color_;
int energy_part_rotation; int energy_part_rotation;
@@ -59,6 +60,8 @@ private:
double phi;//电机yaw轴应旋转的角度 double phi;//电机yaw轴应旋转的角度
float yaw_rotation; float yaw_rotation;
float pitch_rotation; float pitch_rotation;
int position_mode;
int last_position_mode;
int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound; int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound;
std::vector<EnergyPart> fans; std::vector<EnergyPart> fans;
@@ -122,7 +125,7 @@ private:
void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red); void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);
void imagePreprocess(cv::Mat &src); void imagePreprocess(cv::Mat &src);
void StructingElementClose(cv::Mat &src); void StructingElementClose(cv::Mat &src,int length, int width);
void StructingElementErodeDilate(cv::Mat &src); void StructingElementErodeDilate(cv::Mat &src);
}; };

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

@@ -58,7 +58,7 @@ void Energy::cycleLeastFit()
radius = 0.0f; radius = 0.0f;
if (all_armor_centers.size() < 3) if (all_armor_centers.size() < 3)
{ {
cout<<"Cannot calculate a circle"<<endl; // cout<<"Cannot calculate a circle"<<endl;
return; return;
} }
@@ -101,6 +101,6 @@ void Energy::cycleLeastFit()
cycle_center.y = static_cast<int>(b / (-2)); cycle_center.y = static_cast<int>(b / (-2));
radius = sqrt(a * a + b * b - 4 * c) / 2; radius = sqrt(a * a + b * b - 4 * c) / 2;
cout << "The cycle center is: " << cycle_center << endl; // cout << "The cycle center is: " << cycle_center << endl;
cout << "The radius is: " << radius << endl; // cout << "The radius is: " << radius << endl;
} }

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

@@ -8,19 +8,34 @@ using std::cout;
using std::endl; using std::endl;
using std::vector; using std::vector;
void Energy::StructingElementClose(cv::Mat &src){ void Energy::StructingElementClose(cv::Mat &src,int length, int width){
if (src.empty())return; if (src.empty())return;
//threshold(src, src, energy_part_param_.CAMERA_GRAY_THRESH, 255, THRESH_BINARY); //threshold(src, src, energy_part_param_.CAMERA_GRAY_THRESH, 255, THRESH_BINARY);
Mat element = getStructuringElement(MORPH_RECT, Size(10, 10)); Mat element = getStructuringElement(MORPH_RECT, Size(length, width));
morphologyEx(src, src, MORPH_CLOSE, element); morphologyEx(src, src, MORPH_CLOSE, element);
} }
void Energy::StructingElementErodeDilate(cv::Mat &src) { void Energy::StructingElementErodeDilate(cv::Mat &src) {
cv::Mat src_out, src_out_out; cv::Mat src_out, src_out_out;
Mat element_erode = getStructuringElement(MORPH_RECT, Size(4, 4)); Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_dilate = getStructuringElement(MORPH_RECT, Size(20, 20)); Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1));
erode(src,src_out, element_erode); Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(8, 6));
imshow("erode", src_out); Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4,4));
dilate(src_out, src_out_out, element_dilate); Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3));
imshow("dilate", src_out_out);
// 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_3);
imshow("dilate_3", src);
} }

4
energy/src/energy/energy.cpp
View File

@@ -22,6 +22,7 @@ Energy::Energy(Uart &u):uart(u),
hit_point = Point(0,0); hit_point = Point(0,0);
target_position = -1; target_position = -1;
last_target_position = -1; last_target_position = -1;
last_hit_position = 20000;
target_armor = -1; target_armor = -1;
radius = 0; radius = 0;
@@ -49,6 +50,9 @@ Energy::Energy(Uart &u):uart(u),
top = Point(640, 480); top = Point(640, 480);
bottom = Point(0, 0); bottom = Point(0, 0);
position_mode = 0;
last_position_mode = 0;
initEnergyPartParam(); initEnergyPartParam();
} }

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

@@ -18,8 +18,8 @@ int Energy::findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans
} }
std::vector<vector<Point> > fan_contours; std::vector<vector<Point> > fan_contours;
StructingElementClose(src_bin); StructingElementClose(src_bin,6,6);
// imshow("fan struct",src_bin); imshow("fan struct",src_bin);
findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
@@ -36,7 +36,7 @@ int Energy::findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans
float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
cout<<"fan area: "<<length<<'\t'<<width<<endl; // cout<<"fan area: "<<length<<'\t'<<width<<endl;
// if(length>20&&width>20){ // if(length>20&&width>20){
// cout<<cur_rect.center; // cout<<cur_rect.center;
@@ -70,8 +70,9 @@ int Energy::findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_
std::vector<vector<Point> > armor_contours; std::vector<vector<Point> > armor_contours;
std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。 std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
StructingElementClose(src_bin); StructingElementErodeDilate(src_bin);
// imshow("armor struct",src_bin); // StructingElementClose(src_bin,10,10);
imshow("armor struct",src_bin);
findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
@@ -105,12 +106,12 @@ int Energy::findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_
// if(length>10&&width>5){ // if(length>10&&width>5){
// armors.emplace_back(armor_contour); // armors.emplace_back(armor_contour);
// cout<<"armor area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl; // cout<<"armor area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl;
// armors.emplace_back(armor_contour);
// } // }
armors.emplace_back(armor_contour); armors.emplace_back(armor_contour);
// cout<<"armor area: "<<length<<'\t'<<width<<endl; // cout<<"armor area: "<<length<<'\t'<<width<<endl;
} }
// cout<<armors.size()<<endl;
if(armors.size() < last_armors_cnt){ if(armors.size() < last_armors_cnt){
last_armors_cnt = static_cast<int>(armors.size()); last_armors_cnt = static_cast<int>(armors.size());
return -1; return -1;

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)); // 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)); yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance));
pitch_rotation = 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)); // 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)); // pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(9.1*PI/180)-STRETCH*(hit_point.y-cycle_center.y)), ATTACK_DISTANCE));

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

@@ -12,7 +12,7 @@ using std::vector;
void Energy::getHitPoint(){ void Energy::getHitPoint(){
int hit_position = 1000; int hit_position = 1000;
int limit_angle = 3; int limit_angle = 6;
int angle_interval = 60; int angle_interval = 60;
if(energy_part_rotation==1){ //顺时针 if(energy_part_rotation==1){ //顺时针
@@ -20,33 +20,45 @@ void Energy::getHitPoint(){
hit_point.x = cycle_center.x + static_cast<int>(radius / 2); 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_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -1*angle_interval; hit_position = -1*angle_interval;
position_mode = 1;
// cout<<"666"<<'\t'<<hit_position<<endl;
} }
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.x = cycle_center.x + static_cast<int>(radius);
hit_point.y = cycle_center.y; hit_point.y = cycle_center.y;
hit_position = 0; hit_position = 0;
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.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = angle_interval; hit_position = 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.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = 2*angle_interval; hit_position = 2*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.x = cycle_center.x - static_cast<int>(radius);
hit_point.y = cycle_center.y; hit_point.y = cycle_center.y;
hit_position = 180; hit_position = 180;
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.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -2 * angle_interval; hit_position = -2 * angle_interval;
position_mode = 6;
}
else{
// last_hit_position = 2000;
position_mode = 0;
return;
} }
else return;
} }
if(energy_part_rotation==-1){ //逆时针 if(energy_part_rotation==-1){ //逆时针
@@ -54,38 +66,84 @@ void Energy::getHitPoint(){
hit_point.x = cycle_center.x - static_cast<int>(radius / 2); 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_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = 2*angle_interval; 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.x = cycle_center.x - static_cast<int>(radius);
hit_point.y = cycle_center.y; hit_point.y = cycle_center.y;
hit_position = 180; 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.x = cycle_center.x - static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -2*angle_interval; 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.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
hit_position = -1*angle_interval; 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.x = cycle_center.x + static_cast<int>(radius);
hit_point.y = cycle_center.y; hit_point.y = cycle_center.y;
hit_position = 0; 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.x = cycle_center.x + static_cast<int>(radius / 2);
hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2); hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
hit_position = angle_interval; hit_position = angle_interval;
position_mode = 6;
}
else{
// last_hit_position = 2000;
position_mode = 0;
return;
} }
else return;
} }
cout<<"hit position: "<<hit_position<<endl;
isSendTarget = true; // 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 // double rad = static_cast<double>(energy_part_rotation * energy_part_param_.RPM
// * energy_part_param_.HIT_TIME * 360 / 60); // * energy_part_param_.HIT_TIME * 360 / 60);
@@ -93,6 +151,12 @@ void Energy::getHitPoint(){
} }
bool Energy::changeTarget(){ bool Energy::changeTarget(){
return !(pointDistance(target_center, last_target_center) < 25 || fabs(target_position - last_target_position) < 30 if(fabs(target_position - last_target_position) < 30||fabs(target_position - last_target_position) > 330){
||fabs(target_position - last_target_position) > 330); last_target_position = target_position;
return false;
}
else{
last_target_position = target_position;
return true;
}
} }

4
energy/src/energy/get/position_get.cpp
View File

@@ -15,7 +15,7 @@ void Energy::getFanPosition(std::vector<float> &fanPosition, const std::vector<E
float angle = static_cast<float>(180 / PI * atan2(-1 * (fan.rect.center.y - cycle_center.y), (fan.rect.center.x - cycle_center.x))); float angle = static_cast<float>(180 / PI * atan2(-1 * (fan.rect.center.y - cycle_center.y), (fan.rect.center.x - cycle_center.x)));
fanPosition.push_back(angle); fanPosition.push_back(angle);
} }
cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl; // cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl;
} }
void Energy::getArmorPosition(std::vector<float> &armorPosition, const std::vector<EnergyPart> &armors, cv::Point cycle_center, double radius) { void Energy::getArmorPosition(std::vector<float> &armorPosition, const std::vector<EnergyPart> &armors, cv::Point cycle_center, double radius) {
@@ -26,7 +26,7 @@ void Energy::getArmorPosition(std::vector<float> &armorPosition, const std::vect
armorPosition.push_back(angle); armorPosition.push_back(angle);
} }
cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl; // cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl;
} }
void Energy::getFirstArmorCenters(vector<EnergyPart> &armors, std::vector<cv::Point> &first_armor_centers) void Energy::getFirstArmorCenters(vector<EnergyPart> &armors, std::vector<cv::Point> &first_armor_centers)

8
energy/src/energy/param_init.cpp
View File

@@ -13,7 +13,7 @@ void Energy::initEnergyPartParam() {
energy_part_param_.RPM = 10; energy_part_param_.RPM = 10;
energy_part_param_.HIT_TIME = 1.14; energy_part_param_.HIT_TIME = 1.14;
energy_part_param_.GRAY_THRESH = 220; energy_part_param_.GRAY_THRESH = 240;
energy_part_param_.SPLIT_GRAY_THRESH = 60; energy_part_param_.SPLIT_GRAY_THRESH = 60;
energy_part_param_.FAN_GRAY_THRESH = 75; energy_part_param_.FAN_GRAY_THRESH = 75;
energy_part_param_.ARMOR_GRAY_THRESH = 80; energy_part_param_.ARMOR_GRAY_THRESH = 80;
@@ -30,9 +30,9 @@ void Energy::initEnergyPartParam() {
energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000; energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000;
energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30; energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 20; energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 15;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 60; energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 50; energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 45;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;

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

@@ -12,12 +12,13 @@ extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
extern int mark; extern int mark;
int Energy::run(cv::Mat &src){ int Energy::run(cv::Mat &src){
// imshow("src",src); imshow("src",src);
fans.clear(); fans.clear();
armors.clear(); armors.clear();
fanPosition.clear(); fanPosition.clear();
armorPosition.clear(); armorPosition.clear();
gimble_zero_points.clear(); gimble_zero_points.clear();
isSendTarget = false;
if(mark==0)return 0; if(mark==0)return 0;
@@ -48,26 +49,27 @@ int Energy::run(cv::Mat &src){
if(armors_cnt != fans_cnt+1) return 0; if(armors_cnt != fans_cnt+1) return 0;
getAllArmorCenters(); getAllArmorCenters();
cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl; // cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
cycleLeastFit(); cycleLeastFit();
cycle_center = cv::Point(214,305); // cycle_center = cv::Point(335, 246);
radius = 114.695; // radius = 116.936;
attack_distance = ATTACK_DISTANCE * 123.323 / radius; attack_distance = ATTACK_DISTANCE * 120/ radius;
getFanPosition(fanPosition, fans, cycle_center, radius); getFanPosition(fanPosition, fans, cycle_center, radius);
getArmorPosition(armorPosition, armors, cycle_center, radius); getArmorPosition(armorPosition, armors, cycle_center, radius);
findTarget(fanPosition, armorPosition, target_armor); 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; // cout<<"The target armor center is: "<<target_center<<endl;
getHitPoint(); getHitPoint();
hit_point = target_center; // hit_point = target_center;
cout << "The hit point position is " << hit_point << endl; // cout << "The hit point position is " << hit_point << endl;
// hit_point = cycle_center;
// if(!isSendTarget)return 0;
if(!isSendTarget)return 0;
// cout<<"send"<<endl;
cout<<"position mode: "<<position_mode<<endl;
gimbleRotation(); gimbleRotation();

113
main.cpp
View File

@@ -26,116 +26,115 @@ using namespace cv;
using namespace std; using namespace std;
int state = ARMOR_STATE; int state = ENERGY_STATE;
float curr_yaw = 0, curr_pitch = 0; float curr_yaw=0, curr_pitch=0;
float mark_yaw = 0, mark_pitch = 0; float mark_yaw=0, mark_pitch=0;
int mark = 0; int mark = 0;
void uartReceive(Uart *uart); void uartReceive(Uart* uart);
int main(int argc, char *argv[]) { int main(int argc, char *argv[]){
process_options(argc, argv); process_options(argc, argv);
Uart uart; Uart uart;
thread receive(uartReceive, &uart); thread receive(uartReceive, &uart);
bool flag = true; bool flag = true;
while (flag) { while (flag){
int ally_color = ALLY_RED; int ally_color = ALLY_RED;
int energy_part_rotation = CLOCKWISE; int energy_part_rotation = CLOCKWISE;
int from_camera = 1; int from_camera = 1;
if (!run_with_camera) { if(!run_with_camera) {
cout << "Input 1 for camera, 0 for video files" << endl; cout << "Input 1 for camera, 0 for video files" << endl;
cin >> from_camera; cin >> from_camera;
} }
WrapperHead *video_armor; WrapperHead *video_armor;
WrapperHead *video_energy; WrapperHead *video_energy;
if (from_camera) { if(from_camera) {
video_armor = new CameraWrapper(0); // video_armor = new CameraWrapper(0);
video_energy = new CameraWrapper(1); video_energy = new CameraWrapper(1);
} else { }else {
video_armor = new VideoWrapper("/home/xinyang/Desktop/Video.mp4"); video_armor = new VideoWrapper("r_l_640.avi");
video_energy = new VideoWrapper("/home/xinyang/Desktop/Video.mp4"); video_energy = new VideoWrapper("r_l_640.avi");
}
if (video_armor->init()) {
cout << "Video source initialization successfully." << endl;
} }
if (video_energy->init()) {
cout << "Video source initialization successfully." << endl;
}
Mat energy_src, armor_src; Mat energy_src, armor_src;
ArmorFinder armorFinder(ENEMY_BLUE, uart, PROJECT_DIR"/tools/para/");
ArmorFinder armorFinder(ENEMY_BLUE, uart, PROJECT_DIR"/tools/para/");
Energy energy(uart); Energy energy(uart);
energy.setAllyColor(ally_color); energy.setAllyColor(ally_color);
energy.setRotation(energy_part_rotation); energy.setRotation(energy_part_rotation);
bool ok = true; bool ok=true;
while (ok) { while (ok){
CNT_TIME(WORD_LIGHT_CYAN, "Total", { // CNT_TIME(WORD_LIGHT_CYAN, "Total", {
// ok = video_energy->read(energy_src) && video_energy->read(armor_src);
ok = video_energy->read(energy_src);
if (show_origin) {
imshow("enery src", energy_src);
imshow("armor src", armor_src);
}
if (state == ENERGY_STATE) { if (state == ENERGY_STATE) {
ok = video_energy->read(energy_src);
if (show_origin) {
imshow("energy src", energy_src);
}
if (from_camera == 0) { if (from_camera == 0) {
energy.extract(energy_src); energy.extract(energy_src);
} }
energy.run(energy_src); energy.run(energy_src);
} else { } else {
ok = video_armor->read(armor_src); // CNT_TIME(WORD_LIGHT_BLUE, "Armor Time", {
if (show_origin) { // armorFinder.run(armor_src);
imshow("armor src", armor_src); // });
}
CNT_TIME(WORD_LIGHT_BLUE, "Armor Time", {
armorFinder.run(armor_src);
});
} }
if (waitKey(1) == 'q') { if (waitKey(1) == 'q') {
flag = false; flag = false;
break; break;
} }
}); // });
} }
delete video_armor; delete video_energy;
cout << "Program fails. Restarting" << endl; cout << "Program fails. Restarting" << endl;
} }
return 0; return 0;
} }
#define RECEIVE_LOG_LEVEL LOG_NOTHING #define RECEIVE_LOG_LEVEL LOG_MSG
void uartReceive(Uart *uart) { void uartReceive(Uart* uart){
char buffer[100]; char buffer[100];
int cnt = 0; int cnt=0;
while (true) { while(true){
char data; char data;
while ((data = uart->receive()) != '\n') { while((data=uart->receive()) != '\n'){
buffer[cnt++] = data; buffer[cnt++] = data;
if (cnt >= 100) { if(cnt >= 100){
LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); LOG(RECEIVE_LOG_LEVEL, "data receive over flow!");
cnt = 0; cnt = 0;
} }
} }
if (cnt == 10) { if(cnt == 10){
if (buffer[8] == 'e') { if(buffer[8] == 'e'){
state = ENERGY_STATE; state = ENERGY_STATE;
LOG(RECEIVE_LOG_LEVEL, "Energy state"); // LOG(RECEIVE_LOG_LEVEL, "Energy state");
} else if (buffer[8] == 'a') { }else if(buffer[8] == 'a'){
state = ARMOR_STATE; state = ARMOR_STATE;
LOG(RECEIVE_LOG_LEVEL, "Armor state"); // LOG(RECEIVE_LOG_LEVEL, "Armor state");
} }
memcpy(&curr_yaw, buffer, 4); memcpy(&curr_yaw, buffer, 4);
memcpy(&curr_pitch, buffer + 4, 4); memcpy(&curr_pitch, buffer+4, 4);
LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch); // LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch);
if (buffer[9] == 1) { if(buffer[9] == 1){
if (mark == 0) { if(mark == 0){
mark = 1; mark = 1;
mark_yaw = curr_yaw; mark_yaw = curr_yaw;
mark_pitch = curr_pitch; mark_pitch = curr_pitch;
} }
LOG(RECEIVE_LOG_LEVEL, "Marked"); // LOG(RECEIVE_LOG_LEVEL, "Marked");
} }
} }
cnt = 0; cnt = 0;

6
others/src/camera/camera_wrapper.cpp
View File

@@ -79,10 +79,10 @@ stop:
// CameraSetAntiFlick(h_camera, true); // CameraSetAntiFlick(h_camera, true);
} }
else if(mode == 1){ else if(mode == 1){
// 使用自动曝光 // 不使用自动曝光
CameraSetAeState(h_camera, true); CameraSetAeState(h_camera, false);
// 抗频闪 // 抗频闪
CameraSetAntiFlick(h_camera, true); // CameraSetAntiFlick(h_camera, true);
} }
/*让SDK进入工作模式，开始接收来自相机发送的图像 /*让SDK进入工作模式，开始接收来自相机发送的图像