diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index ea5d92c..a26afc6 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -73,7 +73,7 @@ private: int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 int last_mode;//上一帧的能量机关状态 int manual_delta_x, manual_delta_y;//手动微调量 - int extra_delta_y;//在风车运动到最高点附近的额外补偿量 + int extra_delta_x, extra_delta_y;//在风车运动到最高点附近的额外补偿量 float target_polar_angle;//待击打装甲板的极坐标角度 float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度 @@ -163,6 +163,7 @@ private: bool guessTarget();//获得猜测击打点位 bool getOrigin();//获得云台对心所需角度 void changeTarget();//判断目标是否改变 + void getCenter();//对心 void multipleMode(cv::Mat &src);//多模式切换 void getTargetPolarAngle();//获得目标装甲板极坐标角度 void getPredictPoint(cv::Point target_point);//获取预测点位 diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 41123ce..1d11c6c 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -39,6 +39,7 @@ void Energy::initEnergy() { manual_delta_x = 0; manual_delta_y = 0; extra_delta_y = 0; + extra_delta_x = 0; target_polar_angle = -1000; last_target_polar_angle = -1000; @@ -76,7 +77,7 @@ void Energy::initEnergy() { // 此函数对能量机关参数进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergyPartParam() { - gimbal_energy_part_param_.GRAY_THRESH = 90;//home + gimbal_energy_part_param_.GRAY_THRESH = 120;//home // gimbal_energy_part_param_.GRAY_THRESH = 200;//official // gimbal_energy_part_param_.GRAY_THRESH = 225; gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230; @@ -108,9 +109,9 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000; gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0; - gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 5; + gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 8; gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 45; - gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 5; + gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 8; gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 45; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index 8c96e9d..e7e5e78 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -14,7 +14,10 @@ using std::vector; // 此函数用于寻找图像内所有的大风车扇叶 // --------------------------------------------------------------------------------------------------------------------- int Energy::findFans(const cv::Mat src) { - if (src.empty())return 0; + if (src.empty()){ + if (show_info) cout << "empty!" << endl; + return 0; + } static Mat src_bin; src_bin = src.clone(); if (src.type() == CV_8UC3) { @@ -59,7 +62,10 @@ int Energy::findFans(const cv::Mat src) { // 此函数用于寻找图像内所有的大风车装甲板模块 // --------------------------------------------------------------------------------------------------------------------- int Energy::findArmors(const cv::Mat src) { - if (src.empty())return 0; + if (src.empty()){ + if (show_info) cout << "empty!" << endl; + return 0; + } static Mat src_bin; src_bin = src.clone(); if (src.type() == CV_8UC3) { @@ -114,7 +120,10 @@ int Energy::findArmors(const cv::Mat src) { // 此函数用于寻找图像内大风车中心字母“R” // --------------------------------------------------------------------------------------------------------------------- bool Energy::findCenterR(const cv::Mat src) { - if (src.empty())return false; + if (src.empty()){ + if (show_info) cout << "empty!" << endl; + return false; + } static Mat src_bin; src_bin = src.clone(); if (src.type() == CV_8UC3) { @@ -134,7 +143,7 @@ bool Energy::findCenterR(const cv::Mat src) { circle_center_point = centerR.center; circle_center_point.y += target_length / 7.5;//实际最小二乘得到的中心在R的下方 - RotatedRect cur_rect = minAreaRect(center_R_contour); +// RotatedRect cur_rect = minAreaRect(center_R_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; @@ -163,7 +172,10 @@ bool Energy::findCenterR(const cv::Mat src) { // 此函数用于判断找到的矩形候选区是否为含流动条的扇叶 // --------------------------------------------------------------------------------------------------------------------- bool Energy::findFlowStripFan(const cv::Mat src) { - if (src.empty())return false; + if (src.empty()){ + if (show_info) cout << "empty!" << endl; + return false; + } static Mat src_bin; static Mat src_copy; src_bin = src.clone(); @@ -201,9 +213,8 @@ bool Energy::findFlowStripFan(const cv::Mat src) { if (flow_strip_fans.empty()) { if (show_info)cout << "flow strip fan false!" << endl; return false; - } else { - return true; } + return true; } @@ -211,7 +222,10 @@ bool Energy::findFlowStripFan(const cv::Mat src) { // 此函数用于寻找流动条 // --------------------------------------------------------------------------------------------------------------------- bool Energy::findFlowStrip(const cv::Mat src) { - if (src.empty())return false; + if (src.empty()) { + if (show_info) cout << "empty!" << endl; + return false; + } cv::Mat src_bin; src_bin = src.clone(); @@ -296,6 +310,7 @@ bool Energy::findFlowStrip(const cv::Mat src) { } } } + return true; } diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index 8d7c6d2..7199112 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -17,10 +17,19 @@ extern mcu_data mcuData; void Energy::getAimPoint(cv::Point target_point_) { float target_polar_angle_ = static_cast(180 / PI * atan2(-1 * (target_point_.y - circle_center_point.y), (target_point_.x - circle_center_point.x))); - if (target_polar_angle_ > 50 && target_polar_angle_ < 130)extra_delta_y = -16; - else extra_delta_y = 0; - double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x); + if (target_polar_angle_ > 0 && target_polar_angle_ <= 90) { + extra_delta_x = EXTRA_DELTA_X * (90 - target_polar_angle_) / 90; + extra_delta_y = - EXTRA_DELTA_Y * target_polar_angle_ / 90; + } else if (target_polar_angle_ > 90 && target_polar_angle_ < 180) { + extra_delta_x = - EXTRA_DELTA_X * (target_polar_angle_ - 90) / 90; + extra_delta_y = - EXTRA_DELTA_Y * (180 - target_polar_angle_) / 90; + } else { + extra_delta_x = 0; + extra_delta_y = 0; + } + + double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x - extra_delta_x); double dy = -(target_point_.y - 240 - COMPENSATE_PITCH - mcuData.delta_y - manual_delta_y - extra_delta_y); yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; diff --git a/energy/src/energy/get/origin_get.cpp b/energy/src/energy/get/origin_get.cpp index 54dd3b9..8359a1d 100644 --- a/energy/src/energy/get/origin_get.cpp +++ b/energy/src/energy/get/origin_get.cpp @@ -10,12 +10,27 @@ using namespace std; using namespace cv; + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于对心 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::getCenter(){ + double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW); + double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH); + yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; + pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; + if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) { + shoot = 4; + } else + shoot = 2; +} + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于获得云台对心得到的初始yaw和pitch(即以该yaw和pitch发射子弹,可以击中风车中心) // --------------------------------------------------------------------------------------------------------------------- bool Energy::getOrigin() { if (!auto_mark && !manual_mark) { -//五号车 double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW); double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH); center_delta_yaw = static_cast(atan(dx / FOCUS_PIXAL) * 180 / PI); diff --git a/energy/src/energy/mark/mark.cpp b/energy/src/energy/mark/mark.cpp index 2711b92..0168f5f 100644 --- a/energy/src/energy/mark/mark.cpp +++ b/energy/src/energy/mark/mark.cpp @@ -15,7 +15,8 @@ void Energy::writeDownSlightChange(cv::Mat &src) { if (findFans(src) >= 4) { FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w"); if (fp_delta) { - fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcuData.delta_x, mcuData.delta_y); + fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcuData.delta_x + manual_delta_x, + mcuData.delta_y + manual_delta_y); fclose(fp_delta); } } diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index baeed89..549d29b 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -3,6 +3,7 @@ // #include "energy/energy.h" #include "log.h" +#include "config/setconfig.h" #include "options/options.h" #include @@ -85,6 +86,11 @@ void Energy::runBig(cv::Mat &gimbal_src) { if (show_energy)showFlowStripFan("strip", gimbal_src); if (!findCenterR(gimbal_src))return; if (show_energy)showCenterR("R", gimbal_src); + +// getCenter(); +// sendEnergy(); +// return; + changeTarget(); getTargetPolarAngle(); if (energy_rotation_init) { @@ -114,6 +120,10 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (!findTargetInFlowStripFan()) return; if (!findFlowStrip(gimbal_src))return; +// getCenter(); +// sendEnergy(); +// return; + changeTarget(); if (save_mark)writeDownSlightChange(gimbal_src); getPredictPoint(target_point); diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h index 7399504..e23db56 100644 --- a/others/include/config/setconfig.h +++ b/others/include/config/setconfig.h @@ -43,6 +43,12 @@ #ifndef COMPENSATE_PITCH #define COMPENSATE_PITCH (74) #endif +#ifndef EXTRA_DELTA_X + #define EXTRA_DELTA_X (10) +#endif +#ifndef EXTRA_DELTA_Y + #define EXTRA_DELTA_Y (10) +#endif //#define GIMBAL_FLIP_MODE (-1) //#define CHASSIS_FLIP_MODE (-1)