energy changed

energy/include/energy/energy.h

@@ -28,7 +28,6 @@ public:
     ~Energy();//默认析构函数
 
     void run(cv::Mat &gimbal_src, cv::Mat &chassis_src);
-
     void run(cv::Mat &gimbal_src);
 
     Serial &serial;//串口
@@ -41,19 +40,20 @@ private:
     EnergyPartParam gimbal_energy_part_param_;//云台摄像头能量机关的参数设置
     EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置
 
-    bool isMark;//若操作手正在手动标定，则为true
+    bool is_mark;//若操作手正在手动标定，则为true
-    bool isBig;//大符模式为true
+    bool is_big;//大符模式为true
-    bool isSmall;//小符模式为true
+    bool is_small;//小符模式为true
-    bool isGimbal;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
+    bool is_gimbal;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
-    bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
+    bool is_chassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
-    bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
+    bool is_guessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
-    bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
+    bool is_predicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
     bool energy_mode_init;//正在进行大小符判断
     bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
     bool manual_mark;//若操作手进行过手动标定，则为true
     bool auto_mark;//云台完成自动对心，则置为true
     bool shoot;//若为true，则要求主控板发弹
-    bool startguessing;//进入猜测状态的标志
+    bool start_guess;//进入猜测状态的标志
+    bool change_target;//目标切换的标志
 
     uint8_t last_mark;//用于记录上一帧操作手是否进行标定
     uint8_t &ally_color;//我方颜色
@@ -80,13 +80,6 @@ private:
 
     timeval time_start_guess;
 
-    std::vector<cv::RotatedRect> fans;//图像中所有扇叶
-    std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别)
-
-    std::vector<cv::Point> all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
-
-    std::queue<float> recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符
-
     cv::RotatedRect centerR;//风车中心字母R的可能候选区
     cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个）
     cv::RotatedRect flow_strip_fan;//图像中所有流动条所在扇叶（理论上只有一个）
@@ -101,6 +94,15 @@ private:
 
     cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道
 
+    std::vector<cv::RotatedRect> fans;//图像中所有扇叶
+    std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别)
+    std::vector<cv::Point> all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
+
+    std::queue<float> recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符
+
+
+
+
     void initEnergy();//能量机关初始化
     void initEnergyPartParam();//能量机关参数初始化
     void initRotation();//顺逆时针初始化
@@ -146,7 +148,7 @@ private:
     void writeDownMark();//记录操作手标定的云台初始角度
 
     bool guessTarget();//获得猜测击打点位
-    bool changeTarget();//判断目标是否改变
+    void changeTarget();//判断目标是否改变
     bool getOrigin();//获得云台对心所需角度
     void getTargetPolarAngle();//获得目标装甲板极坐标角度
     void getPredictPoint(cv::Point target_point);//获取预测点位

energy/src/energy/calibrate/structing.cpp

@@ -34,8 +34,6 @@ void Energy::FanStruct(cv::Mat &src) {
 //    imshow("dilate_2", src);
 //    dilate(src, src, element_dilate_3);
 //    imshow("dilate_3", src);
-//    erode(src,src, element_erode_2);
-//    imshow("erode_2", src);
 }
 
 
@@ -44,19 +42,26 @@ void Energy::FanStruct(cv::Mat &src) {
 // 此函数对图像进行腐蚀与膨胀操作
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::ArmorStruct(cv::Mat &src) {
-    Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(8, 6));
+    Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
-    Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1));
+    Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2));
     Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
-    Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4));
+    Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2));
+    Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3));
+    Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
 
-    erode(src,src, element_erode_1);
-//    imshow("erode_1", src);
     dilate(src, src, element_dilate_1);
 //    imshow("dilate_1", src);
-    dilate(src, src, element_dilate_2);
+    erode(src,src, element_erode_1);
-//    imshow("dilate_2", src);
+//    imshow("erode_1", src);
     erode(src,src, element_erode_2);
 //    imshow("erode_2", src);
+    erode(src,src, element_erode_3);
+//    imshow("erode_3", src);
+//    dilate(src, src, element_dilate_2);
+//    imshow("dilate_2", src);
+//    dilate(src, src, element_dilate_3);
+//    imshow("dilate_3", src);
+
 }
 
 
@@ -84,8 +89,6 @@ void Energy::FlowStripFanStruct(cv::Mat &src) {
 //    imshow("dilate_2", src);
 //    dilate(src, src, element_dilate_3);
 //    imshow("dilate_3", src);
-//    erode(src,src, element_erode_2);
-//    imshow("erode_2", src);
 }
 
 
@@ -113,8 +116,6 @@ void Energy::FlowStripStruct(cv::Mat &src) {
 //    imshow("dilate_2", src);
 //    dilate(src, src, element_dilate_3);
 //    imshow("dilate_3", src);
-//    erode(src,src, element_erode_2);
-//    imshow("erode_2", src);
 }
 
 

energy/src/energy/change/mode_change.cpp

@@ -13,8 +13,9 @@ using namespace cv;
 // 此函数用于从云台摄像头对心状态切换到底盘摄像头击打状态
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::startChassis(){
-    isChassis = true;
+    is_chassis = true;
-    isGimbal = false;
+    is_gimbal = false;
+    LOGM(STR_CTR(WORD_YELLOW, "Start Chassis!"));
 }
 
 
@@ -23,9 +24,9 @@ void Energy::startChassis(){
 // 此函数用于判断是否应当继续保持猜测模式
 // ---------------------------------------------------------------------------------------------------------------------
 bool Energy::stayGuessing(){
-    if (changeTarget() || isGuessingTimeout()) {
+    if (change_target || isGuessingTimeout()) {
-        isPredicting = true;
+        is_predicting = true;
-        isGuessing = false;
+        is_guessing = false;
         LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!"));
         return false;
     }

energy/src/energy/change/target_change.cpp

@@ -12,13 +12,11 @@ using namespace cv;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于判断目标是否切换
 // ---------------------------------------------------------------------------------------------------------------------
-bool Energy::changeTarget(){
+void Energy::changeTarget(){
     if (pointDistance(target_point, last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX) {
+        change_target = false;
+    } else {
+        change_target = true;
+    }
     last_target_point = target_point;
-        return false;
-    }
-    else{
-        last_target_point= target_point;
-        return true;
-    }
 }

energy/src/energy/clear/energy_init.cpp

@@ -14,43 +14,54 @@ using std::vector;
 // 此函数对能量机关成员变量进行初始化
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initEnergy() {
-    isMark = false;
+    is_mark = false;
-    isPredicting = true;
+    is_guessing = false;
-    isGuessing = false;
+    is_predicting = true;
+    energy_mode_init = true;
+    energy_rotation_init = true;
     manual_mark = false;
     auto_mark = false;
-    energy_mode_init = true;
+    shoot = false;
+    start_guess = false;
+    change_target = false;
+
+    last_mark = 0;
+
+    radius = 0;
+
+    send_cnt = 0;
+    last_fans_cnt = 0;
+    guess_devide = 0;
+    energy_rotation_direction = ANTICLOCKWISE;
+    clockwise_rotation_init_cnt = 0;
+    anticlockwise_rotation_init_cnt = 0;
+    last_mode = -1;//既不是大符也不是小符
+
+    target_polar_angle = -1000;
+    last_target_polar_angle = -1000;
+    guess_polar_angle = -1000;
+    last_base_angle = -1000;
+    predict_rad = 20;
+    attack_distance = ATTACK_DISTANCE;
+    center_delta_yaw = 1000;
+    center_delta_pitch = 1000;
+    yaw_rotation = 0;
+    pitch_rotation = 0;
+    origin_yaw = 0;
+    origin_pitch = 0;
+
     circle_center_point = Point(0, 0);
     target_point = Point(0, 0);
     last_target_point = Point(0, 0);
     guess_point = Point(0, 0);
     predict_point = Point(0, 0);
-    target_polar_angle = -1000;
-    guess_polar_angle = -1000;
-    last_target_polar_angle = -1000;
-    radius = 0;
-    energy_rotation_direction = ANTICLOCKWISE;
-    attack_distance = ATTACK_DISTANCE;
-    last_mode = -1;//既不是大符也不是小符
-    last_fans_cnt = 0;
-    send_cnt = 0;
-    yaw_rotation = 0;
-    pitch_rotation = 0;
-    last_mark = 0;
-
-    shoot = false;
-    guess_devide = 0;
-    startguessing = false;
-    predict_rad = 20;
 
     fans.clear();
     armors.clear();
-
     all_target_armor_centers.clear();
     while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop();
 
-    clockwise_rotation_init_cnt = 0;
+
-    anticlockwise_rotation_init_cnt = 0;
 }
 
 
@@ -58,19 +69,21 @@ void Energy::initEnergy() {
 // 此函数对能量机关参数进行初始化
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initEnergyPartParam() {
-    gimbal_energy_part_param_.GRAY_THRESH = 80;
+//    gimbal_energy_part_param_.GRAY_THRESH = 80;
+    gimbal_energy_part_param_.GRAY_THRESH = 225;
     gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230;
     gimbal_energy_part_param_.FAN_GRAY_THRESH = 75;
     gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80;
 
     gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 6600;
     gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0;
-    gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 56;
+    gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 80;
-    gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 88;
+    gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100;
-    gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 21;
+    gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 20;
-    gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 40;
+    gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 50;
     gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4;
     gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1;
+    gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.65;
     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_MAX = 0.3;
@@ -80,8 +93,8 @@ void Energy::initEnergyPartParam() {
     gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
     gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 16;
     gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 32;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 7;
+    gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 5;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 25;
+    gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 20;
     gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
     gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
 
@@ -128,7 +141,8 @@ void Energy::initEnergyPartParam() {
 
 
 
-    chassis_energy_part_param_.GRAY_THRESH = 80;
+//    chassis_energy_part_param_.GRAY_THRESH = 80;
+    chassis_energy_part_param_.GRAY_THRESH = 225;
     chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230;
     chassis_energy_part_param_.FAN_GRAY_THRESH = 75;
     chassis_energy_part_param_.ARMOR_GRAY_THRESH = 80;
@@ -151,8 +165,8 @@ void Energy::initEnergyPartParam() {
     chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
     chassis_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30;
     chassis_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
-    chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 15;
+    chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 13;
-    chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 45;
+    chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 33;
     chassis_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
     chassis_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
     chassis_energy_part_param_.ARMOR_CONTOUR_AREA_RATIO_MIN = 0.7;

energy/src/energy/energy.cpp

@@ -19,11 +19,10 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color),
     initEnergy();
     initEnergyPartParam();
 
-    energy_rotation_init = false;
+    is_big = false;
-    isBig = false;
+    is_small = false;
-    isSmall = false;
+    is_gimbal = true;
-    isGimbal = true;
+    is_chassis = false;
-    isChassis = false;
 }
 
 
@@ -41,9 +40,8 @@ void Energy::setEnergyInit() {
     initEnergy();
     initEnergyPartParam();
 
-    energy_rotation_init = true;
+    is_big = false;
-    isBig = false;
+    is_small = false;
-    isSmall = false;
+    is_gimbal = true;
-    isGimbal = true;
+    is_chassis = false;
-    isChassis = false;
 }

energy/src/energy/find/energy_finder.cpp

@@ -34,12 +34,17 @@ int Energy::findFans(const cv::Mat src) {
 //        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;
-//        if(length>20&&width>20){
+//        double cur_contour_area = contourArea(fan_contour);
+//        double non_zero_rate = nonZeroRateOfRotateRect(src_bin, cur_rect);
+//        if (length > 60 && width > 20) {
 //            fans.emplace_back(cv::minAreaRect(fan_contour));
+//            cout << cur_rect.center << endl;
 //            cout << "fan area: " << length << '\t' << width << endl;
-//			cout<<nonZeroRateOfRotateRect(src_bin,cur_rect)<<endl;
+//            cout << "non zero: " << nonZeroRateOfRotateRect(src_bin, cur_rect) << endl;
+//            cout << "rate: " << cur_contour_area / cur_size.area() << endl;
 //        }
     }
+//    showFans("fan", src_bin);
     if (fans.size() < last_fans_cnt) {
         last_fans_cnt = static_cast<int>(fans.size());
         return -1;//寻找到的扇叶比上一帧少，说明该帧有误，返回-1
@@ -63,10 +68,10 @@ int Energy::findArmors(const cv::Mat src) {
     std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
 
     ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
-//    imshow("armor struct", src_bin);
+    imshow("armor struct", src_bin);
 
     findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
-    findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+//    findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
 
 //    for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓
 //    {
@@ -85,12 +90,11 @@ int Energy::findArmors(const cv::Mat src) {
             continue;
         }
         armors.emplace_back(cv::minAreaRect(armor_contour));
-
 //        RotatedRect cur_rect = minAreaRect(armor_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;
-//        if (length > 10 && width > 7) {
+//        if (length > 5 && width > 5) {
 //            armors.emplace_back(cv::minAreaRect(armor_contour));
 //            cout << "armor area: " << length << '\t' << width << '\t' << cur_rect.center << endl;
 //        }
@@ -98,7 +102,9 @@ int Energy::findArmors(const cv::Mat src) {
     }
 //    cout<<armors.size()<<endl;
 //    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());
 }
 
@@ -165,7 +171,7 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
     }
     std::vector<vector<Point> > flow_strip_fan_contours;
     FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
-//    imshow("flow strip fan struct", src_bin);
+    imshow("flow strip fan struct", src_bin);
 
     findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
 
@@ -208,7 +214,7 @@ bool Energy::findFlowStrip(const cv::Mat src) {
         cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
     }
     FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
-    imshow("flow strip struct", src_bin);
+//    imshow("flow strip struct", src_bin);
 
     std::vector<vector<Point> > flow_strip_contours;
     findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);

energy/src/energy/get/gimble_rotation_get.cpp

@@ -19,17 +19,17 @@ void Energy::changeMark() {
         last_mark = mcuData.mark;
         origin_yaw = mcuData.curr_yaw;
         origin_pitch = mcuData.curr_pitch;
-        isMark = false;
+        is_mark = false;
         manual_mark = true;
 //        LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark"));
     } else if (mcuData.mark == 1) {//正在标定
         last_mark = mcuData.mark;
-        isMark = true;
+        is_mark = true;
 //		LOGM(STR_CTR(WORD_BLUE,"Marking..."));
 
     } else {//未在标定
         last_mark = mcuData.mark;
-        isMark = false;
+        is_mark = false;
     }
     //cout<<"mark: "<<int(mcuData.mark)<<endl;
 }

energy/src/energy/get/guess_point_get.cpp

@@ -12,7 +12,7 @@ using namespace cv;
 // 此函数获得猜测的击打点
 // ---------------------------------------------------------------------------------------------------------------------
 bool Energy::guessTarget() {
-    vector<int> all_fan_angles;
+    vector<float> all_fan_angles;
     float angle;
     for (const auto &fan : fans) {
         angle = static_cast<float>(180 / PI * atan2(-1 * (fan.center.y - circle_center_point.y),
@@ -33,7 +33,7 @@ bool Energy::guessTarget() {
     float max_angle = all_fan_angles.at(all_fan_angles.size() - 1);
     float base_angle = min_angle;
     while (base_angle > 72)base_angle -= 72;
-    if (startguessing) {
+    if (start_guess) {
         int i = 0;
         for (i = 1; i < all_fan_angles.size(); ++i) {
             if (abs(min_angle + 72 * i - all_fan_angles.at(i)) > energy_part_param_.TWIN_ANGEL_MAX) {
@@ -41,8 +41,8 @@ bool Energy::guessTarget() {
                 break;
             }
         }
-        if (i == all_fan_angles.size()) guess_polar_angle = max_angle + 72;
+        if (i == all_fan_angles.size()) guess_polar_angle = (max_angle + 72);
-        startguessing = false;
+        start_guess = false;
         guess_devide = devide(guess_polar_angle);
     } else if (abs(base_angle - last_base_angle) > 20) {
         if (energy_rotation_direction == CLOCKWISE)guess_devide = (guess_devide + 4) % 5;

energy/src/energy/get/polar_angle_get.cpp

@@ -28,7 +28,7 @@ void Energy::getAllTargetArmorCenters() {
 // 此函数用于存储近30帧图像中所有装甲板的中心坐标，用于判断小符和大符
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::getRecentTargetArmorCenters() {
-    if (changeTarget()) {
+    if (change_target) {
         while (!recent_target_armor_centers.empty())recent_target_armor_centers.pop();//目标切换时清空
         return;
     }

energy/src/energy/get/predict_point_get.cpp

@@ -15,10 +15,9 @@ using std::vector;
 // 此函数获取预测点坐标
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::getPredictPoint(cv::Point target_point) {
-    if (isBig) {
+    if (is_big) {
         if (energy_rotation_direction == 1) rotate(target_point);
         if (energy_rotation_direction == -1) rotate(target_point);
-    } else if (isSmall) predict_point = target_point;
+    } else if (is_small) predict_point = target_point;
-
 }
 

energy/src/energy/judge/judge_mode.cpp

@@ -19,16 +19,16 @@ void Energy::JudgeMode() {
     } else {
         if (abs(recent_target_armor_centers.back() - recent_target_armor_centers.front()) > 10) {
             if(last_mode!=BIG){
-                isBig = true;
+                is_big = true;
-                isSmall = false;
+                is_small = false;
                 energy_mode_init = false;
                 LOGM(STR_CTR(WORD_CYAN,"start big!"));
             }
             last_mode = BIG;
         } else {
             if(last_mode!=SMALL){
-                isBig = false;
+                is_big = false;
-                isSmall = true;
+                is_small = true;
                 energy_mode_init = false;
                 LOGM(STR_CTR(WORD_GREEN,"start small!"));
             }

energy/src/energy/judge/judge_shoot.cpp

@@ -16,9 +16,9 @@ using namespace cv;
 void Energy::judgeShoot(){
     if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) {
         shoot = true;
-        isPredicting = false;
+        is_predicting = false;
-        isGuessing = true;
+        is_guessing = true;
-        startguessing = true;
+        start_guess = true;
         gettimeofday(&time_start_guess, NULL);
         LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
     } else

energy/src/energy/run.cpp

@@ -15,12 +15,13 @@ using namespace cv;
 void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
     if (chassis_src.empty())
         run(gimbal_src);//仅拥有云台摄像头则调用单摄像头的run函数
-    else if (isGimbal) {
+    else if (is_gimbal) {
-        energy_part_param_ = gimbal_energy_part_param_;
+        energy_part_param_ = chassis_energy_part_param_;
         clearAll();
         initImage(gimbal_src);
 
         if (findArmors(gimbal_src) < 1)return;
+        if (show_energy)showArmors("armor", gimbal_src);
         if (!findFlowStripFan(gimbal_src))return;
         if (!findTargetInFlowStripFan()) return;
         if (!findCenterROI(gimbal_src))return;
@@ -32,36 +33,39 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
         startChassis();
         initEnergy();
         destroyAllWindows();
-    } else if (isChassis) {
+    } else if (is_chassis) {
-        energy_part_param_ = gimbal_energy_part_param_;
+        energy_part_param_ = chassis_energy_part_param_;
         clearAll();
         initImage(chassis_src);
 
         changeMark();
-        if (isMark)return;//操作手强制手动标定origin_yaw和origin_pitch
+        if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch
 
         if (findArmors(chassis_src) < 1)return;
-        if (!findFlowStripFan(chassis_src))return;showFlowStripFan("flow strip fan", chassis_src);
+        if (show_energy)showArmors("armor", chassis_src);
+        if (!findFlowStripFan(chassis_src))return;
+        showFlowStripFan("flow strip fan", chassis_src);
         if (!findTargetInFlowStripFan()) return;
         if (!findCenterROI(chassis_src))return;
         if (show_energy)showFlowStripFan("strip", chassis_src);
         if (!findCenterR(chassis_src))return;
         if (show_energy)showCenterR("R", chassis_src);
-
         getTargetPolarAngle();
+        changeTarget();
         JudgeMode();
         if (energy_mode_init)return;
-        if (isBig && energy_rotation_init) {
+        if (is_big && energy_rotation_init) {
             initRotation();
             return;
         }
-        if (isPredicting) {
+        if (is_predicting) {
             getPredictPoint(target_point);
             gimbalRotation();
             judgeShoot();
             sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
-        } else if (isGuessing && stayGuessing()) {
+        } else if (is_guessing && stayGuessing()) {
             findFans(chassis_src);
+            if (show_energy)showFans("fans", chassis_src);
             if (save_mark)writeDownMark();
             if (!guessTarget()) return;
             if (show_energy)showGuessTarget("guess", chassis_src);
@@ -82,31 +86,34 @@ void Energy::run(cv::Mat &gimbal_src) {
     initImage(gimbal_src);
 
     changeMark();
-    if (isMark)return;//操作手强制手动标定origin_yaw和origin_pitch
+    if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch
 
     if (show_process)imshow("bin", gimbal_src);
     if (findArmors(gimbal_src) < 1)return;
+    if (show_energy)showArmors("armor", gimbal_src);
     if (!findFlowStripFan(gimbal_src))return;
     if (!findTargetInFlowStripFan()) return;
     if (!findCenterROI(gimbal_src))return;
     if (show_energy)showFlowStripFan("strip", gimbal_src);
     if (!findCenterR(gimbal_src))return;
     if (show_energy)showCenterR("R", gimbal_src);
+    changeTarget();
     getTargetPolarAngle();
     JudgeMode();
     if (energy_mode_init)return;
     if (!getOrigin())return;
-    if (isBig & energy_rotation_init) {
+    if (is_big && energy_rotation_init) {
         initRotation();
         return;
     }
-    if (isPredicting) {
+    if (is_predicting) {
         getPredictPoint(target_point);
         gimbalRotation();
         judgeShoot();
         sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
-    } else if (isGuessing && stayGuessing()) {
+    } 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);

energy/src/energy/tool/tool.cpp

@@ -68,9 +68,7 @@ double Energy::pointDistance(cv::Point point_1, cv::Point point_2) {
 // ---------------------------------------------------------------------------------------------------------------------
 int Energy::devide(float angle) {
     if (angle < 0)angle += 360;//若angle小于0，说明当前角度范围为-180°~180°
-    int i = 0;
+    return angle / 72.0;
-    while (angle - 72 * i > 72)i++;
-    return i;
 }
 
 

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/147.avi");
+            video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/大能量机关（蓝+开灯）.mov");
-            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/147.avi");
+            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/大能量机关（蓝+开灯）.mov");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");
@@ -128,7 +128,7 @@ int main(int argc, char *argv[]) {
                             armorFinder.run(gimbal_src);
                     });
                 }
-                cv::waitKey(0);
+//                cv::waitKey(0);
             });
         } while (ok);
         delete video_gimbal;