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 
# Conflicts:
#	energy/src/energy/send/send.cpp
#	main.cpp
This commit is contained in:
xinyang
2019-08-04 17:20:41 +08:00
parent d0ab4342fd 6996d3f0dd
commit 8f33549e2c
12 changed files with 334 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -2,7 +2,7 @@
PROJECT(SJTU-RM-CV) PROJECT(SJTU-RM-CV)
SET(CMAKE_CXX_STANDARD 11) SET(CMAKE_CXX_STANDARD 11)
SET(CMAKE_BUILD_TYPE DEBUG) SET(CMAKE_BUILD_TYPE RELEASE)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}")

6
energy/include/energy/energy.h
View File

@@ -66,6 +66,7 @@ private:
int send_cnt;//向主控板发送的数据总次数 int send_cnt;//向主控板发送的数据总次数
int camera_cnt;//摄像头数量 int camera_cnt;//摄像头数量
int fans_cnt;//扇叶个数
int last_fans_cnt;//上一帧的扇叶个数 int last_fans_cnt;//上一帧的扇叶个数
int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区 int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区
int energy_rotation_direction;//风车旋转方向 int energy_rotation_direction;//风车旋转方向
@@ -80,11 +81,13 @@ private:
float guess_polar_angle;//猜测的下一个目标装甲板极坐标角度 float guess_polar_angle;//猜测的下一个目标装甲板极坐标角度
float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度 float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度
float predict_rad;//预测提前角 float predict_rad;//预测提前角
float predict_rad_norm;//预测提前角的绝对值
float attack_distance;//步兵与风车平面距离 float attack_distance;//步兵与风车平面距离
float center_delta_yaw, center_delta_pitch;//对心时相差的角度 float center_delta_yaw, center_delta_pitch;//对心时相差的角度
float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度 float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
float origin_yaw, origin_pitch;//初始的云台角度设定值 float origin_yaw, origin_pitch;//初始的云台角度设定值
float shoot;//若为2，则要求主控板发弹 float shoot;//若为2，则要求主控板发弹
float last_yaw, last_pitch;//PID中微分项
float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项 float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
systime time_start_guess; systime time_start_guess;
@@ -131,6 +134,7 @@ private:
bool findFlowStrip(const cv::Mat src);//寻找图中的流动条 bool findFlowStrip(const cv::Mat src);//寻找图中的流动条
bool findCenterROI(const cv::Mat src);//框取中心R候选区 bool findCenterROI(const cv::Mat src);//框取中心R候选区
bool findFlowStripFan(const cv::Mat src);//寻找图中的流动条所在扇叶 bool findFlowStripFan(const cv::Mat src);//寻找图中的流动条所在扇叶
bool findFlowStripWeak(const cv::Mat src);//弱识别寻找图中的流动条
bool isValidFanContour(cv::Mat &src, const vector<cv::Point> &fan_contour);//扇叶矩形尺寸要求 bool isValidFanContour(cv::Mat &src, const vector<cv::Point> &fan_contour);//扇叶矩形尺寸要求
bool isValidArmorContour(const vector<cv::Point> &armor_contour);//装甲板矩形尺寸要求 bool isValidArmorContour(const vector<cv::Point> &armor_contour);//装甲板矩形尺寸要求
@@ -141,7 +145,9 @@ private:
void showFans(std::string windows_name, const cv::Mat src);//显示扇叶 void showFans(std::string windows_name, const cv::Mat src);//显示扇叶
void showArmors(std::string windows_name, const cv::Mat src);//显示装甲板 void showArmors(std::string windows_name, const cv::Mat src);//显示装甲板
void showBoth(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板 void showBoth(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板
void showTarget(std::string windows_name, const cv::Mat src);//显示目标装甲板
void showCenterR(std::string windows_name, const cv::Mat src);//显示风车中心候选区R void showCenterR(std::string windows_name, const cv::Mat src);//显示风车中心候选区R
void showFlowStrip(std::string windows_name, const cv::Mat src);//显示流动条
void showFlowStripFan(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶 void showFlowStripFan(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶
void showGuessTarget(std::string windows_name, const cv::Mat src);//显示猜测点位 void showGuessTarget(std::string windows_name, const cv::Mat src);//显示猜测点位

3
energy/include/energy/param_struct_define.h
View File

@@ -83,6 +83,9 @@ struct EnergyPartParam {
long TARGET_CHANGE_DISTANCE_MAX;//目标未更改时，目标装甲板中心与原目标装甲板中心的距离变化最大值 long TARGET_CHANGE_DISTANCE_MAX;//目标未更改时，目标装甲板中心与原目标装甲板中心的距离变化最大值
long TWIN_POINT_MAX;//两个点相同时距离最大值 long TWIN_POINT_MAX;//两个点相同时距离最大值
long STRIP_ARMOR_DISTANCE_MIN;//流动条中心和目标装甲板中心距离最小值
long STRIP_ARMOR_DISTANCE_MAX;//流动条中心和目标装甲板中心距离最大值
}; };

79
energy/src/energy/clear/energy_init.cpp
View File

@@ -30,6 +30,7 @@ void Energy::initEnergy() {
send_cnt = 0; send_cnt = 0;
camera_cnt = 1; camera_cnt = 1;
fans_cnt = 0;
last_fans_cnt = 0; last_fans_cnt = 0;
guess_devide = 0; guess_devide = 0;
energy_rotation_direction = ANTICLOCKWISE; energy_rotation_direction = ANTICLOCKWISE;
@@ -46,12 +47,15 @@ void Energy::initEnergy() {
guess_polar_angle = -1000; guess_polar_angle = -1000;
last_base_angle = -1000; last_base_angle = -1000;
predict_rad = 25; predict_rad = 25;
predict_rad_norm = 25;
attack_distance = ATTACK_DISTANCE; attack_distance = ATTACK_DISTANCE;
center_delta_yaw = 1000; center_delta_yaw = 1000;
center_delta_pitch = 1000; center_delta_pitch = 1000;
yaw_rotation = 0; yaw_rotation = 0;
pitch_rotation = 0; pitch_rotation = 0;
shoot = 0; shoot = 0;
last_yaw = 0;
last_pitch = 0;
sum_yaw = 0; sum_yaw = 0;
sum_pitch = 0; sum_pitch = 0;
@@ -69,7 +73,6 @@ void Energy::initEnergy() {
all_target_armor_centers.clear(); all_target_armor_centers.clear();
while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop(); while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop();
} }
@@ -77,29 +80,29 @@ void Energy::initEnergy() {
// 此函数对能量机关参数进行初始化 // 此函数对能量机关参数进行初始化
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergyPartParam() { void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.GRAY_THRESH = 120;//home // gimbal_energy_part_param_.GRAY_THRESH = 120;//home
// gimbal_energy_part_param_.GRAY_THRESH = 200;//official // gimbal_energy_part_param_.GRAY_THRESH = 200;//official
// gimbal_energy_part_param_.GRAY_THRESH = 225; gimbal_energy_part_param_.GRAY_THRESH = 180;//game
gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230; gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180;
gimbal_energy_part_param_.FAN_GRAY_THRESH = 75; gimbal_energy_part_param_.FAN_GRAY_THRESH = 75;
gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80; gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 6600; gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 3000;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0; gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 500;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 80; gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 55;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100; gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 95;
gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 20; gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 20;
gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52; gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52;
gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4; gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5;
gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.65; gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6;
gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8; gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8;
gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48; gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48;
// gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.3; // gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.3;
// gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.16; // gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.16;
gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000; gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500;
gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180;
gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 16; gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 16;
gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 32; gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 32;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 5; gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 5;
@@ -107,42 +110,41 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 200;
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 40;
gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 8; gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 6;
gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 45; gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 20;
gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 8; gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 6;
gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 45; gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 20;
gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 2;
gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.3; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.6;
gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10; gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 3000; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 2000;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 1000; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 500;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 2.8;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1.2;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.65; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34;
gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.65; gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.58;
gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.34; gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.34;
// gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2; // gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2;
// gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08; // gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 100000; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 700;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 100;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 38; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 60; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 8; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 32; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7;
// gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 1.8; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100;
gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10; gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10;
@@ -151,6 +153,9 @@ void Energy::initEnergyPartParam() {
gimbal_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20; gimbal_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
gimbal_energy_part_param_.TWIN_POINT_MAX = 20; gimbal_energy_part_param_.TWIN_POINT_MAX = 20;
gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MIN = 28;
gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52;
chassis_energy_part_param_.GRAY_THRESH = 120;//home chassis_energy_part_param_.GRAY_THRESH = 120;//home

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

@@ -14,7 +14,7 @@ using std::vector;
// 此函数用于寻找图像内所有的大风车扇叶 // 此函数用于寻找图像内所有的大风车扇叶
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
int Energy::findFans(const cv::Mat src) { int Energy::findFans(const cv::Mat src) {
if (src.empty()){ if (src.empty()) {
if (show_info) cout << "empty!" << endl; if (show_info) cout << "empty!" << endl;
return 0; return 0;
} }
@@ -39,14 +39,13 @@ int Energy::findFans(const cv::Mat src) {
// 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;
// double cur_contour_area = contourArea(fan_contour); // double cur_contour_area = contourArea(fan_contour);
// double non_zero_rate = nonZeroRateOfRotateRect(src_bin, cur_rect); // float length_width_ratio = length / width;
// if (length > 60 && width > 20) { // cout << "area: " << cur_contour_area << '\t' << endl;
// fans.emplace_back(cv::minAreaRect(fan_contour)); // cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << cur_rect.center << endl; // cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "fan area: " << length << '\t' << width << endl; // cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout << "non zero: " << nonZeroRateOfRotateRect(src_bin, cur_rect) << endl; // cout<<endl;
// cout << "rate: " << cur_contour_area / cur_size.area() << endl;
// }
} }
// showFans("fan", src_bin); // showFans("fan", src_bin);
if (fans.size() < last_fans_cnt) { if (fans.size() < last_fans_cnt) {
@@ -62,7 +61,7 @@ int Energy::findFans(const cv::Mat src) {
// 此函数用于寻找图像内所有的大风车装甲板模块 // 此函数用于寻找图像内所有的大风车装甲板模块
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
int Energy::findArmors(const cv::Mat src) { int Energy::findArmors(const cv::Mat src) {
if (src.empty()){ if (src.empty()) {
if (show_info) cout << "empty!" << endl; if (show_info) cout << "empty!" << endl;
return 0; return 0;
} }
@@ -77,40 +76,44 @@ int Energy::findArmors(const cv::Mat src) {
ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 ArmorStruct(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);
if (show_process)imshow("armor struct", src_bin); if (show_process)imshow("armor struct", src_bin);
// findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
// for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓 findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
// { for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓
// unsigned long external_contour_size = armor_contours_external[i].size(); {
// for (int j = 0; j < armor_contours.size(); j++) { unsigned long external_contour_size = armor_contours_external[i].size();
// unsigned long all_size = armor_contours[j].size(); for (int j = 0; j < armor_contours.size(); j++) {
// if (external_contour_size == all_size) { unsigned long all_size = armor_contours[j].size();
// swap(armor_contours[j], armor_contours[armor_contours.size() - 1]); if (external_contour_size == all_size) {
// armor_contours.pop_back(); swap(armor_contours[j], armor_contours[armor_contours.size() - 1]);
// break; armor_contours.pop_back();
// } break;
// } }
// } }
}
for (auto &armor_contour : armor_contours) { for (auto &armor_contour : armor_contours) {
if (!isValidArmorContour(armor_contour)) { if (!isValidArmorContour(armor_contour)) {
continue; continue;
} }
armors.emplace_back(cv::minAreaRect(armor_contour)); armors.emplace_back(cv::minAreaRect(armor_contour));
// RotatedRect cur_rect = minAreaRect(armor_contour); // RotatedRect cur_rect = minAreaRect(armor_contour);
// Size2f cur_size = cur_rect.size; // Size2f cur_size = cur_rect.size;
// 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;
// if (length > 5 && width > 5) { // double cur_contour_area = contourArea(armor_contour);
// armors.emplace_back(cv::minAreaRect(armor_contour)); // float length_width_ratio = length / width;
// cout << "armor area: " << length << '\t' << width << '\t' << cur_rect.center << endl; // cout << "area: " << cur_contour_area << '\t' << endl;
// } // cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout<<endl;
} }
// cout<<armors.size()<<endl; if (show_info) {
// showArmors("armor",src); // if (armors.size() < 1)cout << "no armors!" << endl;
}
// if (armors.size() < 1)cout << "no armors!" << endl;
return static_cast<int>(armors.size()); return static_cast<int>(armors.size());
} }
@@ -120,7 +123,7 @@ int Energy::findArmors(const cv::Mat src) {
// 此函数用于寻找图像内大风车中心字母“R” // 此函数用于寻找图像内大风车中心字母“R”
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
bool Energy::findCenterR(const cv::Mat src) { bool Energy::findCenterR(const cv::Mat src) {
if (src.empty()){ if (src.empty()) {
if (show_info) cout << "empty!" << endl; if (show_info) cout << "empty!" << endl;
return false; return false;
} }
@@ -147,22 +150,18 @@ bool Energy::findCenterR(const cv::Mat src) {
// Size2f cur_size = cur_rect.size; // Size2f cur_size = cur_rect.size;
// 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;
// if (length < 10 || width < 5 || length>19) { // double cur_contour_area = contourArea(center_R_contour);
// continue; // float length_width_ratio = length / width;
// } // cout << "area: " << cur_contour_area << '\t' << endl;
// std::vector<cv::Point2f> intersection; // cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// if (rotatedRectangleIntersection(cur_rect, center_ROI, intersection) != 0 && // cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// contourArea(intersection) > energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN) { // cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// centerR = cv::minAreaRect(center_R_contour); // cout<<endl;
// cout << "center R area: " << length << '\t' << width << '\t' << cur_rect.center << endl;
// cout << "R intersection: " << contourArea(intersection) << endl;
// return true;
// }
// cout << cur_rect.center << endl;
return true; return true;
} }
if (show_info)cout << "find center R false!" << endl; if (show_info)cout << "find center R false!" << endl;
// cv::waitKey(0); // cv::waitKey(0);
return false; return false;
} }
@@ -172,7 +171,7 @@ bool Energy::findCenterR(const cv::Mat src) {
// 此函数用于判断找到的矩形候选区是否为含流动条的扇叶 // 此函数用于判断找到的矩形候选区是否为含流动条的扇叶
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
bool Energy::findFlowStripFan(const cv::Mat src) { bool Energy::findFlowStripFan(const cv::Mat src) {
if (src.empty()){ if (src.empty()) {
if (show_info) cout << "empty!" << endl; if (show_info) cout << "empty!" << endl;
return false; return false;
} }
@@ -201,17 +200,16 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
// 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;
// double cur_contour_area = contourArea(flow_strip_fan_contour); // double cur_contour_area = contourArea(flow_strip_fan_contour);
// if (length > 40 && width > 30 && length < 110 && width < 100) { // float length_width_ratio = length / width;
// cout << cur_rect.center<<endl; // cout << "area: " << cur_contour_area << '\t' << endl;
// flow_strip_fan = cv::minAreaRect(flow_strip_fan_contour); // cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "flow strip fan area: " << length << '\t' << width << endl; // cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "non zero: " << non_zero_rate << endl; // cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout<<cur_contour_area / cur_size.area()<<endl; // cout<<endl;
// }
} }
// cout << "flow_strip_fans_cnt: " << flow_strip_fans.size() << endl;
if (flow_strip_fans.empty()) { if (flow_strip_fans.empty()) {
if (show_info)cout << "flow strip fan false!" << endl; if (show_info)cout << "flow strip fan false!" << endl;\
// waitKey(0);
return false; return false;
} }
return true; return true;
@@ -258,28 +256,31 @@ bool Energy::findFlowStrip(const cv::Mat src) {
if (!isValidFlowStripContour(flow_strip_contour)) { if (!isValidFlowStripContour(flow_strip_contour)) {
continue; continue;
} }
std::vector<cv::Point2f> intersection; std::vector<cv::Point2f> intersection;
RotatedRect cur_rect = minAreaRect(flow_strip_contour); RotatedRect cur_rect = minAreaRect(flow_strip_contour);
if (rotatedRectangleIntersection(cur_rect, candidate_flow_strip_fan, intersection) == 0) { if (rotatedRectangleIntersection(cur_rect, candidate_flow_strip_fan, intersection) == 0) {
continue; continue;
} else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { } else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour)); flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour));
// cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl; // cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl;
} else {
continue;
} }
// RotatedRect cur_rect = minAreaRect(flow_strip_contour); // Size2f cur_size = cur_rect.size;
// Size2f cur_size = cur_rect.size; // 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; // double cur_contour_area = contourArea(flow_strip_contour);
// if (length / width > 2.5 && width > 7 && width<40) { // float length_width_ratio = length / width;
// cout << cur_rect.center << endl; // cout << "area: " << cur_contour_area << '\t' << endl;
// flow_strip = cv::minAreaRect(flow_strip_contour); // cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "flow strip area: " << length << '\t' << width << endl; // cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// } // cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout << cur_rect.center << endl; // cout<<endl;
} }
} }
// cout << "flow strip cnt: " << flow_strips.size() << endl;
if (flow_strips.empty()) { if (flow_strips.empty()) {
if (show_info)cout << "flow strip false!" << endl; if (show_info)cout << "flow strip false!" << endl;
// waitKey(0); // waitKey(0);
@@ -314,6 +315,97 @@ bool Energy::findFlowStrip(const cv::Mat src) {
} }
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于弱识别寻找流动条
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findFlowStripWeak(const cv::Mat src) {
if (src.empty()) {
if (show_info) cout << "empty!" << endl;
return false;
}
cv::Mat src_bin;
src_bin = src.clone();
if (src_bin.type() == CV_8UC1) // 黑白图像
{
cvtColor(src_bin, src_bin, COLOR_GRAY2RGB);
}
std::vector<cv::RotatedRect> candidate_armors = armors;
for (auto &candidate_armor: candidate_armors) {
Point2f vertices[4];
candidate_armor.size.height *= 1.3;
candidate_armor.size.width *= 1.3;
candidate_armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) {
line(src_bin, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20);
}
}
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
if (show_process)imshow("weak struct", src_bin);
// waitKey(0);
std::vector<vector<Point> > flow_strip_contours;
findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &flow_strip_contour : flow_strip_contours) {
if (!isValidFlowStripContour(flow_strip_contour)) {
continue;
}
flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour));
// RotatedRect cur_rect = minAreaRect(flow_strip_contour);
// Size2f cur_size = cur_rect.size;
// 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;
// double cur_contour_area = contourArea(flow_strip_contour);
// float length_width_ratio = length / width;
// cout << "area: " << cur_contour_area << '\t' << endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout << endl;
}
// cout << "size: " << flow_strips.size() << endl;
if (flow_strips.empty()) {
if (show_info)cout << "weak flow strip false!" << endl;
// waitKey(0);
return false;
} else {
for (const auto &candidate_flow_strip: flow_strips) {
for (const auto &candidate_armor: armors) {
if (pointDistance(candidate_flow_strip.center, candidate_armor.center) <
energy_part_param_.STRIP_ARMOR_DISTANCE_MIN ||
pointDistance(candidate_flow_strip.center, candidate_armor.center) >
energy_part_param_.STRIP_ARMOR_DISTANCE_MAX) {
continue;
}
float angle_armor = candidate_armor.size.width > candidate_armor.size.height ? candidate_armor.angle :
candidate_armor.angle - 90;
float angle_strip = candidate_flow_strip.size.width > candidate_flow_strip.size.height ?
candidate_flow_strip.angle: candidate_flow_strip.angle - 90;
if (abs(angle_armor - angle_strip) < 60 || abs(angle_armor - angle_strip) > 120) {
continue;
}
target_armor = candidate_armor;
target_point = candidate_armor.center;
flow_strip = candidate_flow_strip;
return true;
}
}
if (show_info)cout << "weak flow strip false!" << endl;
// waitKey(0);
return false;
}
}
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于框取中心R的寻找范围 // 此函数用于框取中心R的寻找范围
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------

5
energy/src/energy/get/predict_point_get.cpp
View File

@@ -16,8 +16,9 @@ using std::vector;
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::getPredictPoint(cv::Point target_point) { void Energy::getPredictPoint(cv::Point target_point) {
if (is_big) { if (is_big) {
if (energy_rotation_direction == 1) rotate(target_point); if (energy_rotation_direction == 1) predict_rad = predict_rad_norm;
if (energy_rotation_direction == -1) rotate(target_point); else if (energy_rotation_direction == -1) predict_rad = -predict_rad_norm;
rotate(target_point);
} else if (is_small) predict_point = target_point; } else if (is_small) predict_point = target_point;
} }

10
energy/src/energy/judge/judge_contour.cpp
View File

@@ -100,8 +100,8 @@ bool Energy::isValidArmorContour(const vector<cv::Point> &armor_contour) {
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
bool Energy::isValidCenterRContour(const vector<cv::Point> &center_R_contour) { bool Energy::isValidCenterRContour(const vector<cv::Point> &center_R_contour) {
double cur_contour_area = contourArea(center_R_contour); double cur_contour_area = contourArea(center_R_contour);
if (cur_contour_area > energy_part_param_.ARMOR_CONTOUR_AREA_MAX || if (cur_contour_area > energy_part_param_.CENTER_R_CONTOUR_AREA_MAX ||
cur_contour_area < energy_part_param_.ARMOR_CONTOUR_AREA_MIN) { cur_contour_area < energy_part_param_.CENTER_R_CONTOUR_AREA_MIN) {
//cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl; //cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl;
//cout<<"area fail."<<endl; //cout<<"area fail."<<endl;
return false; return false;
@@ -216,7 +216,7 @@ bool Energy::isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour
length > energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX || length > energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX ||
width > energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX) { width > energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX) {
// cout<<"length width fail."<<endl; // cout<<"length width fail."<<endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; // if(length>30)cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
return false; return false;
//矩形边长不合适 //矩形边长不合适
} }
@@ -225,12 +225,12 @@ bool Energy::isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour
if (length_width_ratio > energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX || if (length_width_ratio > energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX ||
length_width_ratio < energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN) { length_width_ratio < energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN) {
// cout<<"hw fail."<<endl; // cout<<"hw fail."<<endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; // if(length_width_ratio>3.5)cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
return false; return false;
//长宽比不合适 //长宽比不合适
} }
if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN) { if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN) {
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; // if(cur_contour_area / cur_size.area()>0.5)cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
return false;//轮廓对矩形的面积占有率不合适 return false;//轮廓对矩形的面积占有率不合适
} }
return true; return true;

2
energy/src/energy/mark/mark.cpp
View File

@@ -12,7 +12,7 @@ using namespace cv;
// 此函数用于记录操作手的微调dx和dy // 此函数用于记录操作手的微调dx和dy
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::writeDownSlightChange(cv::Mat &src) { void Energy::writeDownSlightChange(cv::Mat &src) {
if (findFans(src) >= 4) { if (fans_cnt >= 4) {
FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w"); FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w");
if (fp_delta) { if (fp_delta) {
fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x, fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x,

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

@@ -78,13 +78,19 @@ void Energy::runBig(cv::Mat &gimbal_src) {
if (show_process)imshow("bin", gimbal_src); if (show_process)imshow("bin", gimbal_src);
if (findArmors(gimbal_src) < 1)return; if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src); if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return; if (!findFlowStripFan(gimbal_src)) {
if (!findTargetInFlowStripFan()) return; if (!findFlowStripWeak(gimbal_src))return;
if (!findFlowStrip(gimbal_src))return; } else {
if (show_energy)showFlowStripFan("strip fan", gimbal_src);
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
}
if (!findCenterROI(gimbal_src))return; if (!findCenterROI(gimbal_src))return;
if (show_energy)showFlowStripFan("strip", gimbal_src); if (show_energy)showFlowStrip("strip", gimbal_src);
if (!findCenterR(gimbal_src))return; if (!findCenterR(gimbal_src))return;
if (show_energy)showCenterR("R", gimbal_src); if (show_energy)showCenterR("R", gimbal_src);
fans_cnt = findFans(gimbal_src);
if (show_energy)showFans("fans", gimbal_src);
// getCenter(); // getCenter();
// sendEnergy(); // sendEnergy();
@@ -92,6 +98,7 @@ void Energy::runBig(cv::Mat &gimbal_src) {
changeTarget(); changeTarget();
getTargetPolarAngle(); getTargetPolarAngle();
if (energy_rotation_init) { if (energy_rotation_init) {
initRotation(); initRotation();
return; return;
@@ -115,9 +122,16 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
if (show_process)imshow("bin", gimbal_src); if (show_process)imshow("bin", gimbal_src);
if (findArmors(gimbal_src) < 1)return; if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src); if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return; if (!findFlowStripFan(gimbal_src)) {
if (!findTargetInFlowStripFan()) return; if (!findFlowStripWeak(gimbal_src))return;
if (!findFlowStrip(gimbal_src))return; } else {
if (show_energy)showFlowStripFan("strip fan", gimbal_src);
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
}
if (show_energy)showTarget("target", gimbal_src);
fans_cnt = findFans(gimbal_src);
if (show_energy)showFans("fans", gimbal_src);
// getCenter(); // getCenter();
// sendEnergy(); // sendEnergy();

30
energy/src/energy/send/send.cpp
View File

@@ -8,6 +8,7 @@
using namespace std; using namespace std;
#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送能量机关数据 // 此函数用于发送能量机关数据
@@ -17,21 +18,34 @@ void Energy::sendEnergy() {
if (camera_cnt == 1) { if (camera_cnt == 1) {
sum_yaw += yaw_rotation; sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation; sum_pitch += pitch_rotation;
yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; MINMAX(sum_yaw, -100, 100);
pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; MINMAX(sum_pitch, -100, 100);\
yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw + BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else if (is_chassis) { } else if (is_chassis) {
sum_yaw += yaw_rotation - mcu_data.curr_yaw; sum_yaw += yaw_rotation - mcuData.curr_yaw;
sum_pitch += pitch_rotation - mcu_data.curr_pitch; sum_pitch += pitch_rotation - mcuData.curr_pitch;
yaw_rotation = AIM_KP * (yaw_rotation - mcu_data.curr_yaw) + AIM_KI * sum_yaw; yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcuData.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw;
pitch_rotation = AIM_KP * (pitch_rotation - mcu_data.curr_pitch) + AIM_KI * sum_pitch; pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcuData.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch;
} }
} else if (is_small){
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_pitch, -100, 100);
yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} }
if (change_target) { if (change_target) {
sendTarget(serial, yaw_rotation, pitch_rotation, 5, 0); sendTarget(serial, yaw_rotation, pitch_rotation, 5, 0);
} else if (is_guessing) { } else if (is_guessing) {
sendTarget(serial, yaw_rotation, pitch_rotation, 6, 0); sendTarget(serial, yaw_rotation, pitch_rotation, 6, 0);
} else { } /*else if (fans_cnt >= 4) {
sendTarget(serial, yaw_rotation, pitch_rotation, 7, 0);
}*/ else {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0); sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);
} }
@@ -51,7 +65,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) {
time_t t = time(nullptr); time_t t = time(nullptr);
if (last_time != t) { if (last_time != t) {
last_time = t; last_time = t;
cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
fps = 0; fps = 0;
} }
fps += 1; fps += 1;

44
energy/src/energy/show/show.cpp
View File

@@ -98,6 +98,32 @@ void Energy::showBoth(std::string windows_name, const cv::Mat src) {
} }
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中目标装甲板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showTarget(std::string windows_name, const cv::Mat src) {
if (src.empty())return;
static Mat image2show;
if (src.type() == CV_8UC1) // 黑白图像
{
cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if (src.type() == CV_8UC3) //RGB 彩色
{
image2show = src.clone();
}
for (const auto &armor : armors) {
if (pointDistance(armor.center, target_point) < energy_part_param_.TWIN_POINT_MAX) {
Point2f vertices[4]; //定义矩形的4个顶点
armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2);
}
}
imshow(windows_name, image2show);
}
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有可能的风车中心候选区R // 此函数用于显示图像中所有可能的风车中心候选区R
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -126,7 +152,7 @@ void Energy::showCenterR(std::string windows_name, const cv::Mat src) {
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有流动条 // 此函数用于显示图像中流动条扇叶
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::showFlowStripFan(std::string windows_name, const cv::Mat src) { void Energy::showFlowStripFan(std::string windows_name, const cv::Mat src) {
if (src.empty())return; if (src.empty())return;
@@ -145,7 +171,23 @@ void Energy::showFlowStripFan(std::string windows_name, const cv::Mat src) {
flow_strip_fan.points(strip_fan_vertices); //计算矩形的4个顶点for (int i = 0; i < 4; i++) flow_strip_fan.points(strip_fan_vertices); //计算矩形的4个顶点for (int i = 0; i < 4; i++)
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
line(image2show, strip_fan_vertices[i], strip_fan_vertices[(i + 1) % 4], Scalar(127, 127, 255), 2); line(image2show, strip_fan_vertices[i], strip_fan_vertices[(i + 1) % 4], Scalar(127, 127, 255), 2);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中流动条扇叶
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showFlowStrip(std::string windows_name, const cv::Mat src) {
if (src.empty())return;
static Mat image2show;
if (src.type() == CV_8UC1) // 黑白图像
{
cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if (src.type() == CV_8UC3) //RGB 彩色
{
image2show = src.clone();
}
Point2f strip_vertices[4]; //定义矩形的4个顶点 Point2f strip_vertices[4]; //定义矩形的4个顶点
flow_strip.points(strip_vertices); //计算矩形的4个顶点 flow_strip.points(strip_vertices); //计算矩形的4个顶点

34
others/include/config/setconfig.h
View File

@@ -29,13 +29,37 @@
#define ARMOR_CAMERA_GAIN (30) #define ARMOR_CAMERA_GAIN (30)
#endif #endif
#ifndef ENERGY_CAMERA_GAIN #ifndef ENERGY_CAMERA_GAIN
#define ENERGY_CAMERA_GAIN (30) #define ENERGY_CAMERA_GAIN (20)
#endif #endif
#ifndef AIM_KP #ifndef SMALL_YAW_AIM_KD
#define AIM_KP (6) #define SMALL_YAW_AIM_KD (1.5)
#endif #endif
#ifndef AIM_KI #ifndef SMALL_YAW_AIM_KP
#define AIM_KI (0.1) #define SMALL_YAW_AIM_KP (2.5)
#endif
#ifndef SMALL_PITCH_AIM_KD
#define SMALL_PITCH_AIM_KD (1.3)
#endif
#ifndef SMALL_PITCH_AIM_KP
#define SMALL_PITCH_AIM_KP (2.4)
#endif
#ifndef BIG_YAW_AIM_KD
#define BIG_YAW_AIM_KD (1)
#endif
#ifndef BIG_YAW_AIM_KP
#define BIG_YAW_AIM_KP (5.5)
#endif
#ifndef BIG_YAW_AIM_KI
#define BIG_YAW_AIM_KI (0.1)
#endif
#ifndef BIG_PITCH_AIM_KD
#define BIG_PITCH_AIM_KD (1)
#endif
#ifndef BIG_PITCH_AIM_KP
#define BIG_PITCH_AIM_KP (5.5)
#endif
#ifndef BIG_PITCH_AIM_KI
#define BIG_PITCH_AIM_KI (0.1)
#endif #endif
#ifndef COMPENSATE_YAW #ifndef COMPENSATE_YAW
#define COMPENSATE_YAW (5) #define COMPENSATE_YAW (5)