energy change

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
 
 

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_MAX = 3000;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 0;
+    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;
 

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

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) {

energy/src/energy/judge/judge_shoot.cpp

@@ -10,10 +10,10 @@ using namespace cv;
 
 
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于判断云台坐标系下是否可以发弹
+// 此函数用于判断世界坐标系下是否可以发弹
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::judgeShootInGimbal(){
+void Energy::judgeShootInWorld(){
-    if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) {
+    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;
 }

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

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();
+    cout << "target point: " << target_point << endl;
-    if (!getOrigin())return;
-
-    getPredictPoint(target_point);
-    gimbalRotation();
-    getAimPoint(target_point);
 
     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(guess_point);
-        getAimPoint(target_point);
         sendTarget(serial, yaw_rotation, pitch_rotation, 5);
     }
 

main.cpp

@@ -64,8 +64,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(0/*, "armor"*/);
             video_chassis = new CameraWrapper(1/*, "energy"*/);
         } else {
-            video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/148.avi");
+            video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi");
-            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/148.avi");
+            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/gimble3.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");
@@ -91,7 +91,7 @@ int main(int argc, char *argv[]) {
         bool ok = true;
         cout << "start running" << endl;
         do {
-            CNT_TIME("Total", {
+//            CNT_TIME("Total", {
                 if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式
                     if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化
                         destroyAllWindows();
@@ -145,7 +145,7 @@ int main(int argc, char *argv[]) {
                     });
                 }
 //                cv::waitKey(1);
-            });
+//            });
         } while (ok);
         delete video_gimbal;
         video_gimbal = nullptr;

others/src/additions/additions.cpp

@@ -76,12 +76,12 @@ cv::VideoWriter initVideoWriter(const std::string &filename_prefix) {
 bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) {
     if (!is_camera_0_connect) {
         delete video_gimbal;
-        video_gimbal = new CameraWrapper(0, "armor");
+        video_gimbal = new CameraWrapper(0/*, "armor"*/);
         is_camera_0_connect = video_gimbal->init();
     }
     if (!is_camera_1_connect) {
         delete video_chassis;
-        video_chassis = new CameraWrapper(1, "energy");
+        video_chassis = new CameraWrapper(1/*, "energy"*/);
         is_camera_1_connect = video_chassis->init();
     }
     return is_camera_0_connect && is_camera_1_connect;
@@ -90,7 +90,7 @@ bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) {
 bool checkReconnect(bool is_camera_connect) {
     if (!is_camera_connect) {
         delete video_gimbal;
-        video_gimbal = new CameraWrapper(0, "armor");
+        video_gimbal = new CameraWrapper(0/*, "armor"*/);
         is_camera_connect = video_gimbal->init();
     }
     return is_camera_connect;