Merge remote-tracking branch 'origin/master'

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);//获取预测点位

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;

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;
 }
 
 

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<float>(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;

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<float>(atan(dx / FOCUS_PIXAL) * 180 / PI);

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);
         }
     }

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 <sys/time.h>
 
@@ -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);

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)