energy change

2019-07-17 23:56:45 +08:00
parent 3258c85888
commit c2a92fc755
9 changed files with 64 additions and 37 deletions
--- a/energy/include/energy/constant.h
+++ b/energy/include/energy/constant.h
@@ -11,7 +11,8 @@ const int SRC_WIDTH = 320;
 const int SRC_HEIGHT = 240;
 const int CLOCKWISE = 1;
 const int ANTICLOCKWISE = -1;
-const float ATTACK_DISTANCE = 718.0;//cm
+//const float ATTACK_DISTANCE = 718.0;//cm
+const float ATTACK_DISTANCE = 750.0;//cm
 const double ARMOR_CENTER_TO_CYCLE_CENTER = 75.0;//cm


--- a/energy/src/energy/clear/energy_init.cpp
+++ b/energy/src/energy/clear/energy_init.cpp
@@ -40,7 +40,7 @@ void Energy::initEnergy() {
    last_target_polar_angle = -1000;
    guess_polar_angle = -1000;
    last_base_angle = -1000;
-    predict_rad = 30;
+    predict_rad = 25;
    attack_distance = ATTACK_DISTANCE;
    center_delta_yaw = 1000;
    center_delta_pitch = 1000;
@@ -67,7 +67,7 @@ void Energy::initEnergy() {
 // 此函数对能量机关参数进行初始化
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initEnergyPartParam() {
-    gimbal_energy_part_param_.GRAY_THRESH = 120;//home
+    gimbal_energy_part_param_.GRAY_THRESH = 140;//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,17 +108,17 @@ void Energy::initEnergyPartParam() {
    gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.7;
    gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;

-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 17000;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 0;
+    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 3000;
+    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 1000;
    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_WIDTH_MIN = 20;
    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 50;
    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3;
    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58;
+    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.65;
    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.58;
+    gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.65;
    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_MIN = 0.08;
@@ -131,7 +131,7 @@ void Energy::initEnergyPartParam() {
    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_MIN = 4;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3;
+    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.7;
    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5;
    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 117;

--- a/energy/src/energy/get/aim_point_get.cpp
+++ b/energy/src/energy/get/aim_point_get.cpp
@@ -12,8 +12,12 @@ using namespace cv;
 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::getAimPoint(cv::Point target_point) {
+//五号车
    double dx = -(target_point.x - 320 - 10);
    double dy = -(target_point.y - 240 - 22);
+//四号车
+//    double dx = -(target_point.x - 320 + 16);
+//    double dy = -(target_point.y - 240 - 82);
    yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
    pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
 }
--- a/energy/src/energy/judge/judge_contour.cpp
+++ b/energy/src/energy/judge/judge_contour.cpp
@@ -143,6 +143,7 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &f
        return false;
        //选区面积大小不合适
    }
+//    cout << "area: " << cur_contour_area << endl;
    RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour);
    Size2f cur_size = cur_rect.size;
    float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长
@@ -151,11 +152,11 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &f
        || width < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN
        || length > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX
        || width > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX) {
-        //cout<<"length width fail."<<endl;
+//        cout<<"length width fail."<<endl;
+//        cout << "length: " << length << '\t' << "width: " << width << endl;
        return false;
        //矩形边长不合适
    }
-
    float length_width_ratio = length / width;//计算矩形长宽比
    if (length_width_ratio > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX ||
        length_width_ratio < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN) {
@@ -166,6 +167,7 @@ bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &f
 //    cout << cur_contour_area / cur_size.area() << endl;
    if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN
        || cur_contour_area / cur_size.area() > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX) {
+//        cout << "rate: " << cur_contour_area / cur_size.area() << endl;
        return false;
    }
    //    轮廓对矩形的面积占有率不合适
@@ -205,6 +207,7 @@ bool Energy::isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour
        return false;
        //矩形边长不合适
    }
+//    cout << length << '\t' << width << endl;
    float length_width_ratio = length / width;//计算矩形长宽比
    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) {
--- a/energy/src/energy/judge/judge_shoot.cpp
+++ b/energy/src/energy/judge/judge_shoot.cpp
@@ -10,10 +10,10 @@ using namespace cv;


 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于判断云台坐标系下是否可以发弹
+// 此函数用于判断世界坐标系下是否可以发弹
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::judgeShootInGimbal(){
-    if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) {
+void Energy::judgeShootInWorld(){
+    if (abs(yaw_rotation - mcuData.curr_yaw) < 0.8 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.8) {
        shoot = 4;
        is_predicting = false;
        is_guessing = true;
@@ -26,9 +26,9 @@ void Energy::judgeShootInGimbal(){


 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于判断世界坐标系下是否可以发弹
+// 此函数用于判断云台坐标系下是否可以发弹
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::judgeShootInWorld(){
+void Energy::judgeShootInGimbal(){
    if (abs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) {
        shoot = 4;
        is_predicting = false;
@@ -38,4 +38,11 @@ void Energy::judgeShootInWorld(){
        LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
    } else
        shoot = 2;
+
+    static float sum_yaw = 0;
+    static float sum_pitch = 0;
+    sum_yaw += yaw_rotation;
+    sum_pitch += pitch_rotation;
+    yaw_rotation = 2.5 * yaw_rotation + 0.1 * sum_yaw;
+    pitch_rotation = 2.5 * pitch_rotation + 0.1 * sum_pitch;
 }
--- a/energy/src/energy/judge/judge_time.cpp
+++ b/energy/src/energy/judge/judge_time.cpp
@@ -17,5 +17,5 @@ bool Energy::isGuessingTimeout() {
    timeval cur_time;
    gettimeofday(&cur_time, NULL);
    return (cur_time.tv_sec - time_start_guess.tv_sec) * 1000.0 +
-           (cur_time.tv_usec - time_start_guess.tv_usec) / 1000.0 > 500;
+           (cur_time.tv_usec - time_start_guess.tv_usec) / 1000.0 > 1000;
 };
--- a/energy/src/energy/run.cpp
+++ b/energy/src/energy/run.cpp
@@ -63,7 +63,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
        if (is_predicting) {
            getPredictPoint(target_point);
            gimbalRotation();
-            judgeShootInGimbal();
+            judgeShootInWorld();
            sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
        } else if (is_guessing && stayGuessing()) {
            findFans(chassis_src);
@@ -104,14 +104,17 @@ void Energy::runBig(cv::Mat &gimbal_src) {
 //    judgeMode();
 //    if (energy_mode_init)return;
    if (!getOrigin())return;
-    if (is_big && energy_rotation_init) {
+    if (energy_rotation_init) {
        initRotation();
        return;
    }
+
    if (is_predicting) {
        getPredictPoint(target_point);
-        gimbalRotation();
+        getAimPoint(predict_point);
+        cout << yaw_rotation << '\t' << pitch_rotation << endl;
        judgeShootInGimbal();
+
        sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
    } else if (is_guessing && stayGuessing()) {
        findFans(gimbal_src);
@@ -120,10 +123,26 @@ void Energy::runBig(cv::Mat &gimbal_src) {
        guessTarget();
        if (show_energy)showGuessTarget("guess", gimbal_src);
        getPredictPoint(guess_point);
-        gimbalRotation();
+        getAimPoint(predict_point);
        sendTarget(serial, yaw_rotation, pitch_rotation, 5);
    }

+//    if (is_predicting) {
+//        getPredictPoint(target_point);
+//        gimbalRotation();
+//        judgeShootInWorld();
+//        sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
+//    } else if (is_guessing && stayGuessing()) {
+//        findFans(gimbal_src);
+//        if (show_energy)showFans("fans", gimbal_src);
+//        if (save_mark)writeDownMark();
+//        guessTarget();
+//        if (show_energy)showGuessTarget("guess", gimbal_src);
+//        getPredictPoint(guess_point);
+//        gimbalRotation();
+//        sendTarget(serial, yaw_rotation, pitch_rotation, 5);
+//    }
+
 }


@@ -143,17 +162,11 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
    if (show_energy)showFlowStripFan("strip", gimbal_src);

    changeTarget();
-    getTargetPolarAngle();
-    if (!getOrigin())return;
-
-    getPredictPoint(target_point);
-    gimbalRotation();
-    getAimPoint(target_point);
+    cout << "target point: " << target_point << endl;

    if (is_predicting) {
-        getPredictPoint(target_point);
        getAimPoint(target_point);
-        judgeShootInWorld();
+        judgeShootInGimbal();
        sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
    } else if (is_guessing && stayGuessing()) {
        findFans(gimbal_src);
@@ -161,8 +174,7 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
        if (save_mark)writeDownMark();
        guessTarget();
        if (show_energy)showGuessTarget("guess", gimbal_src);
-        getPredictPoint(guess_point);
-        getAimPoint(target_point);
+        getAimPoint(guess_point);
        sendTarget(serial, yaw_rotation, pitch_rotation, 5);
    }