energy changed

energy/include/energy/energy.h

@@ -30,10 +30,13 @@ public:
 
     int runBig(cv::Mat &gimble_src);
 
+    int runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src);
+
     int runSmall(cv::Mat &gimble_src);
 
     Serial &serial;//串口
-    void setEnergyRotationInit();//判断顺逆时针函数
+    void setBigEnergyInit();//设置大符初始化，判断顺逆时针函数
+    void setSmallEnergyInit();//设置小符初始化
     void extract(cv::Mat &src);//框取图像中的一块区域进行处理
     void sendTarget(Serial &serial, float x, float y, float z);
 
@@ -42,41 +45,45 @@ private:
     EnergyPartParam energy_part_param_;//能量机关的参数设置
     EnergyPartParam gimble_energy_part_param_;//云台摄像头能量机关的参数设置
     EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置
+
     bool isMark;//若操作手正在手动标定，则为true
     bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
     bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
     bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
     bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
-    int last_fans_cnt;//上一帧的扇叶个数
+    bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
+    bool manual_mark;//若操作手进行过手动标定，则为true
+    bool shoot;//若为true，则要求主控板发弹
+    bool startguessing;//进入猜测状态的标志
+
+    uint8_t last_mark;//用于记录上一帧操作手是否进行标定
+    uint8_t &ally_color;//我方颜色
+
     double radius;//大风车半径
+
+    int send_cnt;//向主控板发送的数据总次数
+    int last_fans_cnt;//上一帧的扇叶个数
+    int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区
+    int energy_rotation_direction;//风车旋转方向
+    int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数
+    int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数
+
     float target_polar_angle;//待击打装甲板的极坐标角度
     float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
     float guess_polar_angle;//猜测的下一个目标装甲板极坐标角度
     float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度
-    uint8_t &ally_color;//我方颜色
+    float predict_rad;//预测提前角
-    int energy_rotation_direction;//风车旋转方向
     float attack_distance;//步兵与风车平面距离
-    int send_cnt;//向主控板发送的数据总次数
     float yaw_rotation;//云台yaw轴应该转到的角度
     float pitch_rotation;//云台pitch轴应该转到的角度
-    uint8_t last_mark;//用于记录上一帧操作手是否进行标定
-    double predict_rad;//预测提前角
-    bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
-    int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数
-    int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数
-    float red_origin_yaw, red_origin_pitch;//红方的初始云台对心角度设定值
-    float blue_origin_yaw, blue_origin_pitch;//蓝方的初始云台对心角度设定值
     float origin_yaw, origin_pitch;//初始的云台角度设定值
-    float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板
-    bool save_new_mark;//若操作手进行过手动标定，则为true
-    bool shoot;//若为true，则要求主控板发弹
-    int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区
-    bool startguessing;//进入猜测状态的标志
 
     timeval time_start_guess;
 
     std::vector<cv::RotatedRect> fans;//图像中所有扇叶
-    std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别）
+    std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别)
+
+    std::vector<cv::Point> all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
 
     cv::RotatedRect centerR;//风车中心字母R的可能候选区
     cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个）
@@ -89,9 +96,7 @@ private:
     cv::Point last_target_point;//上一帧目标装甲板中心坐标
     cv::Point guess_point;
     cv::Point predict_point;//预测的击打点坐标
-    std::vector<float> fan_polar_angle;//当前帧所有扇叶的极坐标角度
+
-    std::vector<float> armor_polar_angle;//当前帧所有装甲板的极坐标角度
-    std::vector<cv::Point> all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
     cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道
 
     void initEnergy();//能量机关初始化
@@ -102,6 +107,7 @@ private:
     void initImage(cv::Mat &src);//图像预处理
 
     void startChassis();//从云台摄像头对心状态进入底盘摄像头击打状态
+    bool stayGuessing();//保持在猜测模式
 
     int findFans(const cv::Mat src);//寻找图中所有扇叶
     int findArmors(const cv::Mat src);//寻找图中所有装甲板
@@ -128,7 +134,6 @@ private:
 
     void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径
 
-    void findTargetByPolar();//通过极坐标角度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
     void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
     bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板
 

energy/src/energy/calculate/circle_calculate.cpp

@@ -19,7 +19,7 @@ void Energy::circleLeastFit()
     circle_center_point.x = 0;
     circle_center_point.y = 0;
     radius = 0.0f;
-    if (all_armor_centers.size() < 3)
+    if (all_target_armor_centers.size() < 3)
     {
 //        cout<<"Cannot calculate a circle"<<endl;
         return;
@@ -28,11 +28,11 @@ void Energy::circleLeastFit()
     double sum_x2 = 0.0f, sum_y2 = 0.0f;
     double sum_x3 = 0.0f, sum_y3 = 0.0f;
     double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;
-    int N = static_cast<int>(all_armor_centers.size());
+    int N = static_cast<int>(all_target_armor_centers.size());
     for (int i = 0; i < N; i++)
     {
-        double x = all_armor_centers.at(i).x;
+        double x = all_target_armor_centers.at(i).x;
-        double y = all_armor_centers.at(i).y;
+        double y = all_target_armor_centers.at(i).y;
         double x2 = x * x;
         double y2 = y * y;
         sum_x += x;

energy/src/energy/change/mode_change.cpp

@@ -2,6 +2,7 @@
 // Created by sun on 19-7-10.
 //
 #include "energy/energy.h"
+#include "log.h"
 
 using namespace std;
 using namespace cv;
@@ -14,4 +15,19 @@ using namespace cv;
 void Energy::startChassis(){
     isChassis = true;
     isGimble = false;
+}
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于判断是否应当继续保持猜测模式
+// ---------------------------------------------------------------------------------------------------------------------
+bool Energy::stayGuessing(){
+    if (changeTarget() || is_guessing_timeout()) {
+        isPredicting = true;
+        isGuessing = false;
+        LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!"));
+        return false;
+    }
+    return true;
 }

energy/src/energy/energy.cpp

@@ -13,56 +13,40 @@ using std::vector;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为能量机关构造函数，只要程序不重启就不会重新构造
 // ---------------------------------------------------------------------------------------------------------------------
-Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color),
+Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color),
-        src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
+                                            src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
-        src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1)
+                                            src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1) {
-{
+    initEnergy();
-	initEnergy();
+    initEnergyPartParam();
-	initEnergyPartParam();
 
     energy_rotation_init = false;
     isGimble = true;
     isChassis = false;
-    save_new_mark = false;
-
-    if(ally_color == ALLY_RED){
-        origin_yaw = red_origin_yaw;
-        origin_pitch = red_origin_pitch;
-    }
-    else if(ally_color == ALLY_BLUE){
-        origin_yaw = blue_origin_yaw;
-        origin_pitch = blue_origin_pitch;
-    }
-    else {
-        LOGE_INFO("ally color_run");
-    }
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为能量机关析构函数，设置为默认
 // ---------------------------------------------------------------------------------------------------------------------
 Energy::~Energy() = default;
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数为能量机关再初始化函数，如果未重启程序但重新进入能量机关，则会进行初始化，但不会将save_new_mark置为false
+// 此函数为大能量机关再初始化函数
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::setEnergyRotationInit() {
+void Energy::setBigEnergyInit() {
-	initEnergy();
+    initEnergy();
-	initEnergyPartParam();
+    initEnergyPartParam();
 
-	energy_rotation_init = true;
+    energy_rotation_init = true;
     isGimble = true;
     isChassis = false;
-
+}
-	if(save_new_mark){
+
-        FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r");
+//----------------------------------------------------------------------------------------------------------------------
-        if(fp){
+// 此函数为小能量机关再初始化函数
-            fscanf(fp,"%f %f",&origin_yaw,&origin_pitch);
+// ---------------------------------------------------------------------------------------------------------------------
-            fclose(fp);
+void Energy::setSmallEnergyInit() {
-        }
+    initEnergy();
-	}
+    initEnergyPartParam();
 }

energy/src/energy/find/target_finder.cpp

@@ -9,49 +9,6 @@ using std::endl;
 using std::vector;
 
 
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数通过极坐标角度匹配扇叶和装甲板，找到目标装甲板,计算其极坐标角度和中心坐标
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::findTargetByPolar() {
-    if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误
-    if (armor_polar_angle.empty())return;//找不到扇叶，识别错误
-    if (fan_polar_angle.empty()) {
-        target_polar_angle = armor_polar_angle.at(0);//视野中没有扇叶，说明在击打第一个装甲板
-        for (const auto &armor : armors) {
-            target_point = armor.center;
-        }
-        return;
-    }
-    sort(fan_polar_angle.begin(), fan_polar_angle.end());//对扇叶的极坐标角度进行排序
-    sort(armor_polar_angle.begin(), armor_polar_angle.end());//对装甲板的极坐标角度进行排序
-    int i = 0, j = 0;
-    for (i = 0; i < fan_polar_angle.size(); ++i) {
-        if (armor_polar_angle.at(i) - fan_polar_angle.at(j) < energy_part_param_.TWIN_ANGEL_MAX
-            && armor_polar_angle.at(i) - fan_polar_angle.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) {
-            j++;
-            continue;//若第i个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功，i与j都加1
-        } else {
-            target_polar_angle = armor_polar_angle.at(j);//无法被匹配到的装甲板为待击打装甲板
-            for (const auto &armor : armors) {
-                float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.center.y - circle_center_point.y),
-                                                                  (armor.center.x - circle_center_point.x)));
-                if (target_polar_angle == angle) {
-                    target_point = armor.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
-                }
-            }
-            return;
-        }
-    }
-    target_polar_angle = armor_polar_angle.at(armor_polar_angle.size() - 1);//前几个扇叶都匹配到装甲板，则最后剩下的装甲板为目标
-    for (const auto &armor : armors) {
-        float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.center.y - circle_center_point.y),
-                                                          (armor.center.x - circle_center_point.x)));
-        if (target_polar_angle == angle) {
-            target_point = armor.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
-        }
-    }
-}
-
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数根据矩形重合面积匹配扇叶与装甲板
@@ -63,7 +20,7 @@ void Energy::findTargetByIntersection() {
 
         }
     }
-    /*if (fans.empty()) {
+   /* if (fans.empty()) {
         target_point = armors.at(0).rect.center;
         return;
     }

energy/src/energy/get/gimble_rotation_get.cpp

@@ -20,6 +20,7 @@ void Energy::changeMark() {
         origin_yaw = mcuData.curr_yaw;
         origin_pitch = mcuData.curr_pitch;
         isMark = false;
+        manual_mark = true;
 //        LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark"));
     } else if (mcuData.mark == 1) {//正在标定
         last_mark = mcuData.mark;

energy/src/energy/get/polar_angle_get.cpp

@@ -7,21 +7,17 @@ using namespace cv;
 using namespace std;
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数获取目标装甲板极坐标角度
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::getTargetPolarAngle() {
     target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
-                                                            (target_point.x - circle_center_point.x)));
+                                                             (target_point.x - circle_center_point.x)));
 }
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::getAllArmorCenters()
+void Energy::getAllArmorCenters() {
-{
+    all_target_armor_centers.emplace_back(target_point);
-    for (const auto &armor : armors) {
-        all_armor_centers.push_back(armor.center);
-    }
 }

energy/src/energy/init/energy_init.cpp

@@ -17,6 +17,7 @@ void Energy::initEnergy() {
     isMark = false;
     isPredicting = true;
     isGuessing = false;
+    manual_mark = false;
     circle_center_point = Point(0, 0);
     target_point = Point(0, 0);
     last_target_point = Point(0, 0);
@@ -34,7 +35,6 @@ void Energy::initEnergy() {
     pitch_rotation = 0;
     last_mark = 0;
 
-    target_cnt = 0;
     shoot = false;
     guess_devide = 0;
     startguessing = false;
@@ -43,10 +43,7 @@ void Energy::initEnergy() {
     fans.clear();
     armors.clear();
 
-    fan_polar_angle.clear();
+    all_target_armor_centers.clear();
-    armor_polar_angle.clear();
-
-    all_armor_centers.clear();
 
     clockwise_rotation_init_cnt = 0;
     anticlockwise_rotation_init_cnt = 0;

energy/src/energy/mark/mark.cpp

@@ -13,7 +13,6 @@ void Energy::writeDownMark() {
         if (fp) {
             fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
             fclose(fp);
-            save_new_mark = true;
         }
         FILE *fp_all = fopen(PROJECT_DIR"/Mark/mark_all.txt", "a");
         if (fp_all) {

energy/src/energy/run.cpp

@@ -10,8 +10,6 @@ using namespace std;
 using namespace cv;
 
 
-
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
 // ---------------------------------------------------------------------------------------------------------------------
@@ -54,7 +52,6 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
         if (show_energy)showCenterRContours("R", chassis_src);
 
         getTargetPolarAngle();
-        if (save_mark)writeDownMark();
         if (energy_rotation_init) {
             initRotation();
             return 0;
@@ -64,27 +61,20 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
             gimbleRotation();
             judgeBigShoot();
             sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
-        } else if (isGuessing) {
+        } else if (isGuessing && stayGuessing()) {
-            if (changeTarget() || is_guessing_timeout()) {
+            findFans(chassis_src);
-                isPredicting = true;
+            if (save_mark)writeDownMark();
-                isGuessing = false;
+            guessTarget();
-                LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!"));
+            if (show_energy)showGuessTarget("guess", chassis_src);
-            } else {
+            getPredictPoint(guess_point);
-                findFans(chassis_src);
+            gimbleRotation();
-                guessTarget();
+            sendTarget(serial, yaw_rotation, pitch_rotation, false);
-                if (show_energy)showGuessTarget("guess", chassis_src);
-                getPredictPoint(guess_point);
-                gimbleRotation();
-                sendTarget(serial, yaw_rotation, pitch_rotation, false);
-            }
         }
     }
     return 0;
 }
 
 
-
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
 // ---------------------------------------------------------------------------------------------------------------------
@@ -115,13 +105,78 @@ int Energy::runBig(cv::Mat &gimble_src) {
         gimbleRotation();
         judgeBigShoot();
         sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
-    } else if (isGuessing) {
+    } else if (isGuessing && stayGuessing()) {
-        if (changeTarget() || is_guessing_timeout()){
+        findFans(gimble_src);
-            isPredicting = true;
+        if (save_mark)writeDownMark();
-            isGuessing = false;
+        guessTarget();
-            LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!"));
+        if (show_energy)showGuessTarget("guess", gimble_src);
-        } else {
+        getPredictPoint(guess_point);
+        gimbleRotation();
+        sendTarget(serial, yaw_rotation, pitch_rotation, false);
+    }
+    return 0;
+}
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
+// ---------------------------------------------------------------------------------------------------------------------
+int Energy::runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src) {
+    if (!manual_mark) {
+        runSmall(gimble_src);
+    } else if (!chassis_src.empty()) {
+        energy_part_param_ = chassis_energy_part_param_;
+        clearAll();
+        initImage(chassis_src);
+
+        changeMark();
+        if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
+
+        if (findArmors(chassis_src) < 1)return 0;
+        if (!findFlowStripFan(chassis_src))return 0;
+        if (!findTargetInFlowStripFan()) return 0;
+        if (!findCenterROI(chassis_src))return 0;
+        if (!findCenterR(chassis_src))return 0;
+
+        if (show_energy)showFlowStripFanContours("strip", chassis_src);
+        if (show_energy)showCenterRContours("R", chassis_src);
+
+        getTargetPolarAngle();
+        if (isPredicting) {
+            getPredictPoint(target_point);
+            gimbleRotation();
+            judgeBigShoot();
+            sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
+        } else if (isGuessing && stayGuessing()) {
+            findFans(chassis_src);
+            if (save_mark)writeDownMark();
+            guessTarget();
+            if (show_energy)showGuessTarget("guess", chassis_src);
+            getPredictPoint(guess_point);
+            gimbleRotation();
+            sendTarget(serial, yaw_rotation, pitch_rotation, false);
+        }
+    } else {
+        energy_part_param_ = gimble_energy_part_param_;
+        clearAll();
+        initImage(gimble_src);
+
+        changeMark();
+        if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
+
+        if (show_process)imshow("bin", gimble_src);
+        if (findArmors(gimble_src) < 1)return 0;
+        if (!findFlowStripFan(gimble_src))return 0;
+        if (!findTargetInFlowStripFan()) return 0;
+
+        if (isPredicting) {
+            getPredictPoint(target_point);
+            gimbleRotation();
+            judgeBigShoot();
+            sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
+        } else if (isGuessing && stayGuessing()) {
             findFans(gimble_src);
+            if (save_mark)writeDownMark();
             guessTarget();
             if (show_energy)showGuessTarget("guess", gimble_src);
             getPredictPoint(guess_point);
@@ -129,16 +184,14 @@ int Energy::runBig(cv::Mat &gimble_src) {
             sendTarget(serial, yaw_rotation, pitch_rotation, false);
         }
     }
-    return 0;
 }
 
 
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
 // ---------------------------------------------------------------------------------------------------------------------
 int Energy::runSmall(cv::Mat &gimble_src) {
-    energy_part_param_ = chassis_energy_part_param_;
+    energy_part_param_ = gimble_energy_part_param_;
     clearAll();
     initImage(gimble_src);
 
@@ -154,26 +207,19 @@ int Energy::runSmall(cv::Mat &gimble_src) {
         getAimPoint(target_point);
         judgeSmallShoot();
         sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
-    } else if (isGuessing) {
+    } else if (isGuessing && stayGuessing()) {
-        if (changeTarget() || is_guessing_timeout()){
+        if (!findCenterROI(gimble_src))return 0;
-            isPredicting = true;
+        if (show_energy)showFlowStripFanContours("strip", gimble_src);
-            isGuessing = false;
+        if (!findCenterR(gimble_src))return 0;
-            LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!"));
+        if (show_energy)showCenterRContours("R", gimble_src);
-        } else {
+        getTargetPolarAngle();
-            if (!findCenterROI(gimble_src))return 0;
+        findFans(gimble_src);
-            if (show_energy)showFlowStripFanContours("strip", gimble_src);
+        guessTarget();
-            if (!findCenterR(gimble_src))return 0;
+        if (show_energy)showGuessTarget("guess", gimble_src);
-            if (show_energy)showCenterRContours("R", gimble_src);
+        getAimPoint(guess_point);
-            getTargetPolarAngle();
+        sendTarget(serial, yaw_rotation, pitch_rotation, false);
-            findFans(gimble_src);
-            guessTarget();
-            if (show_energy)showGuessTarget("guess", gimble_src);
-            getAimPoint(guess_point);
-            sendTarget(serial, yaw_rotation, pitch_rotation, false);
-        }
     }
     return 0;
-
 }
 
 

main.cpp

@@ -18,7 +18,9 @@
 #include <armor_finder/armor_finder.h>
 #include <options/options.h>
 #include <additions/additions.h>
+
 #define DO_NOT_CNT_TIME
+
 #include <log.h>
 
 using namespace cv;
@@ -93,40 +95,35 @@ int main(int argc, char *argv[]) {
         cout << "start running" << endl;
         do {
             CNT_TIME("Total", {
-                if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
+                if (mcuData.state != ARMOR_STATE) {//能量机关模式
-                    if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
+                    if (last_state == ARMOR_STATE) {//若上一帧是自瞄模式，即刚往完成切换，则需要初始化
-                        energy.setEnergyRotationInit();
+                        ((CameraWrapper *) video_gimble)->changeBrightness(20);
-                        ((CameraWrapper*)video_gimble)->changeBrightness(20);
-                        cout << "set" << endl;
                     }
                     ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头
+                    if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
+                    if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
+                    if (mcuData.state == BIG_ENERGY_STATE) {
+                        if (last_state != BIG_ENERGY_STATE) {
+                            energy.setBigEnergyInit();
+                            cout << "set" << endl;
+                        }
+                        energy.runBig(gimble_src, chassis_src);
+                    } else if (mcuData.state == SMALL_ENERGY_STATE) {
+                        if (last_state != SMALL_ENERGY_STATE) {
+                            energy.setSmallEnergyInit();
+                            cout << "set" << endl;
+                        }
+                        energy.runSmall(gimble_src, chassis_src);
+                    }
+                    last_state = mcuData.state;//更新上一帧状态
 
 //                    resize(gimble_src,gimble_src,cv::Size(853,480));
 //                    resize(chassis_src,chassis_src,cv::Size(853,480));
 //                    gimble_src = gimble_src(Rect(106, 0, 640, 480));
 //                    chassis_src = chassis_src(Rect(106, 0, 640, 480));
-
-                    if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
-                    if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
-                    energy.runBig(gimble_src, chassis_src);//击打大符
-//                    energy.runBig(gimble_src);
-                    last_state = mcuData.state;//更新上一帧状态
-                } else if (mcuData.state == SMALL_ENERGY_STATE) {// 小符模式
-                    if(last_state == ARMOR_STATE){
-                        ((CameraWrapper*)video_gimble)->changeBrightness(20);
-                    }
-                    last_state = mcuData.state;
-                    ok = checkReconnect(video_gimble->read(gimble_src));
-
-//                    resize(gimble_src,gimble_src,cv::Size(853,480));
-//                    gimble_src = gimble_src(Rect(106, 0, 640, 480));
-
-                    if (save_video) saveVideos(gimble_src);
-                    if (show_origin) showOrigin(gimble_src);
-                    energy.runSmall(gimble_src);
                 } else {                                         // 自瞄模式
-                    if(last_state != ARMOR_STATE){
+                    if (last_state != ARMOR_STATE) {
-                        ((CameraWrapper*)video_gimble)->changeBrightness(40);
+                        ((CameraWrapper *) video_gimble)->changeBrightness(40);
                     }
                     last_state = mcuData.state;
                     ok = checkReconnect(video_gimble->read(gimble_src));