energy changed

energy/include/energy/constant.h

@@ -5,21 +5,14 @@
 #define CONSTANT_H
 
 #include "additions/additions.h"
-
+const int BIG = 1;
-//#define d2r (CV_PI / 180.0)
+const int SMALL = 0;
-
-const int SRC_WIDTH_CAMERA = 640;
-const int SRC_HEIGHT_CAMERA = 480;
 const int SRC_WIDTH = 320;
 const int SRC_HEIGHT = 240;
 const int CLOCKWISE = 1;
 const int ANTICLOCKWISE = -1;
-const float ATTACK_DISTANCE = 718;//cm
+const float ATTACK_DISTANCE = 718.0;//cm
-const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm
+const double ARMOR_CENTER_TO_CYCLE_CENTER = 75.0;//cm
-const int EXTRACT_POINT_X = 120;
-const int EXTRACT_POINT_Y = 0;
-const int EXTRACT_WIDTH = 400;
-const int EXTRACT_HEIGHT = 300;
 
 
 #endif //CONSTANT_H

energy/include/energy/energy.h

@@ -6,6 +6,7 @@
 
 #include <iostream>
 #include <vector>
+#include <queue>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
@@ -26,18 +27,14 @@ public:
     Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色
     ~Energy();//默认析构函数
 
-    int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src);
+    void run(cv::Mat &gimble_src, cv::Mat &chassis_src);
 
-    int runBig(cv::Mat &gimble_src);
+    void run(cv::Mat &gimble_src);
-
-    int runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src);
-
-    int runSmall(cv::Mat &gimble_src);
 
     Serial &serial;//串口
+    void setEnergyInit();//设置能量机关初始化
     void setBigEnergyInit();//设置大符初始化，判断顺逆时针函数
     void setSmallEnergyInit();//设置小符初始化
-    void extract(cv::Mat &src);//框取图像中的一块区域进行处理
     void sendTarget(Serial &serial, float x, float y, float z);
 
 
@@ -47,12 +44,16 @@ private:
     EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置
 
     bool isMark;//若操作手正在手动标定，则为true
+    bool isBig;//大符模式为true
+    bool isSmall;//小符模式为true
     bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
     bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
     bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
     bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
+    bool energy_mode_init;//正在进行大小符判断
     bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
     bool manual_mark;//若操作手进行过手动标定，则为true
+    bool auto_mark;//云台完成自动对心，则置为true
     bool shoot;//若为true，则要求主控板发弹
     bool startguessing;//进入猜测状态的标志
 
@@ -67,6 +68,7 @@ private:
     int energy_rotation_direction;//风车旋转方向
     int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数
     int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数
+    int last_mode;//上一帧的能量机关状态
 
     float target_polar_angle;//待击打装甲板的极坐标角度
     float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
@@ -74,8 +76,7 @@ private:
     float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度
     float predict_rad;//预测提前角
     float attack_distance;//步兵与风车平面距离
-    float yaw_rotation;//云台yaw轴应该转到的角度
+    float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
-    float pitch_rotation;//云台pitch轴应该转到的角度
     float origin_yaw, origin_pitch;//初始的云台角度设定值
 
     timeval time_start_guess;
@@ -85,6 +86,8 @@ private:
 
     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;//图像中所有流动条所在扇叶（理论上只有一个）
@@ -122,41 +125,41 @@ private:
     bool isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour);//流动条扇叶矩形尺寸要求
     bool isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &flow_strip_fan_contour);//流动条扇叶矩形尺寸要求
 
-    void showFanContours(std::string windows_name, const cv::Mat src);//显示扇叶
+    void showFans(std::string windows_name, const cv::Mat src);//显示扇叶
-    void showArmorContours(std::string windows_name, const cv::Mat src);//显示装甲板
+    void showArmors(std::string windows_name, const cv::Mat src);//显示装甲板
-    void showBothContours(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板
+    void showBoth(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板
-    void showCenterRContours(std::string windows_name, const cv::Mat src);//显示风车中心候选区R
+    void showCenterR(std::string windows_name, const cv::Mat src);//显示风车中心候选区R
-    void showFlowStripFanContours(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶
+    void showFlowStripFan(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶
     void showGuessTarget(std::string windows_name, const cv::Mat src);//显示猜测点位
 
-    void getOrigin();//获得云台对心所需角度
-    void guessTarget();//获得猜测击打点位
-
     void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径
 
     void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
     bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板
 
-    void rotate(cv::Point target_point);//获取预测点位
-    void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差
     int devide(float angle);//将极坐标分为五个区域，判断一个角度处于哪个区域
     int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y);//计算直线上一点横坐标
+    void rotate(cv::Point target_point);//获取预测点位
+    void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差
     double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离
     double nonZeroRateOfRotateRect(cv::Mat &bin, const cv::RotatedRect &rotatedRect);//计算旋转矩形内亮点占比
 
     void writeDownMark();//记录操作手标定的云台初始角度
 
-    void getTargetPolarAngle();
+    bool guessTarget();//获得猜测击打点位
+    bool changeTarget();//判断目标是否改变
+    void getOrigin();//获得云台对心所需角度
+    void getTargetPolarAngle();//获得目标装甲板极坐标角度
     void getPredictPoint(cv::Point target_point);//获取预测点位
     void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位
-    bool changeTarget();//判断目标是否改变
     void changeMark();//操作手手动修改标定值
     void gimbleRotation();//计算云台旋转角度
-    void getAllArmorCenters();//记录所有装甲板中心坐标
+    void getAllTargetArmorCenters();//记录所有目标装甲板中心坐标
+    void getRecentTargetArmorCenters();//记录近30帧目标装甲板中心坐标
 
-    void judgeBigShoot();//判断大符模式是否可以发弹
+    void judgeShoot();//判断是否可以发弹
-    void judgeSmallShoot();//判断小符模式是否可以发弹
+    void JudgeMode();//判断大符还是小符
-    bool is_guessing_timeout();//判断猜测模式是否超时（没打中）
+    bool isGuessingTimeout();//判断猜测模式是否超时（没打中）
 
     void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离
     void imagePreprocess(cv::Mat &src);//图像通道分离

energy/src/energy/change/mode_change.cpp

@@ -23,7 +23,7 @@ void Energy::startChassis(){
 // 此函数用于判断是否应当继续保持猜测模式
 // ---------------------------------------------------------------------------------------------------------------------
 bool Energy::stayGuessing(){
-    if (changeTarget() || is_guessing_timeout()) {
+    if (changeTarget() || isGuessingTimeout()) {
         isPredicting = true;
         isGuessing = false;
         LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!"));

energy/src/energy/energy.cpp

@@ -20,6 +20,8 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color),
     initEnergyPartParam();
 
     energy_rotation_init = false;
+    isBig = false;
+    isSmall = false;
     isGimble = true;
     isChassis = false;
 }
@@ -31,6 +33,23 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color),
 Energy::~Energy() = default;
 
 
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为大能量机关再初始化函数
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::setEnergyInit() {
+    initEnergy();
+    initEnergyPartParam();
+
+    energy_rotation_init = true;
+    isBig = false;
+    isSmall = false;
+    isGimble = true;
+    isChassis = false;
+}
+
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为大能量机关再初始化函数
 // ---------------------------------------------------------------------------------------------------------------------
@@ -39,6 +58,8 @@ void Energy::setBigEnergyInit() {
     initEnergyPartParam();
 
     energy_rotation_init = true;
+    isBig = true;
+    isSmall = false;
     isGimble = true;
     isChassis = false;
 }
@@ -49,4 +70,9 @@ void Energy::setBigEnergyInit() {
 void Energy::setSmallEnergyInit() {
     initEnergy();
     initEnergyPartParam();
+
+    isBig = false;
+    isSmall = true;
+    isGimble = true;
+    isChassis = false;
 }

energy/src/energy/get/gimble_rotation_get.cpp

@@ -47,9 +47,4 @@ void Energy::gimbleRotation() {
     pitch_rotation = static_cast<float>(180 / PI *
                                         atan2((attack_distance * tan(origin_pitch * PI / 180) - real_predict_point.y),
                                               attack_distance));
-    if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.5){
-        shoot = true;
-        isPredicting = false;
-        isGuessing = true;
-    }
 }

energy/src/energy/get/guess_point_get.cpp

@@ -2,6 +2,7 @@
 // Created by sun on 19-7-11.
 //
 #include "energy/energy.h"
+#include "log.h"
 
 using namespace std;
 using namespace cv;
@@ -10,7 +11,7 @@ using namespace cv;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数获得猜测的击打点
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::guessTarget() {
+bool Energy::guessTarget() {
     vector<int> all_fan_angles;
     float angle;
     for (const auto &fan : fans) {
@@ -22,13 +23,17 @@ void Energy::guessTarget() {
     angle = target_polar_angle;
     if (angle < 0)angle += 360;
     all_fan_angles.emplace_back(angle);
+    if(all_fan_angles.size()==5){
+        LOGM(STR_CTR(WORD_PURPLE,"all lighted!"));
+        return false;
+    }
 
     sort(all_fan_angles.begin(), all_fan_angles.end());
     float min_angle = all_fan_angles.at(0);
     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) {cout<<"666"<<endl;
+    if (startguessing) {
         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) {
@@ -51,4 +56,5 @@ void Energy::guessTarget() {
     guess_point.x = circle_center_point.x + radius * cos(PI / 180.0 * guess_polar_angle);
     guess_point.y = circle_center_point.y - radius * sin(PI / 180.0 * guess_polar_angle);
     last_base_angle = base_angle;
+    return true;
 }

energy/src/energy/get/origin_get.cpp

@@ -2,19 +2,23 @@
 // Created by sun on 19-7-10.
 //
 #include "energy/energy.h"
+#include "log.h"
 
 using namespace std;
 using namespace cv;
 
 
-
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于获得云台对心得到的初始yaw和pitch（即以该yaw和pitch发射子弹，可以击中风车中心）
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::getOrigin(){
+void Energy::getOrigin() {
-    double dx = circle_center_point.x - 320;
+    if (!auto_mark && !manual_mark) {
-    double dy = circle_center_point.y - 240;
+        double dx = circle_center_point.x - 320;
-    origin_yaw   = atan(dx / FOCUS_PIXAL) * 180 / PI;
+        double dy = circle_center_point.y - 240;
-    origin_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
+        origin_yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+        origin_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
+
+        auto_mark = true;
+        LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
+    }
 }

energy/src/energy/get/polar_angle_get.cpp

@@ -15,9 +15,26 @@ void Energy::getTargetPolarAngle() {
                                                              (target_point.x - circle_center_point.x)));
 }
 
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::getAllArmorCenters() {
+void Energy::getAllTargetArmorCenters() {
     all_target_armor_centers.emplace_back(target_point);
+}
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于存储近30帧图像中所有装甲板的中心坐标，用于判断小符和大符
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::getRecentTargetArmorCenters() {
+    if (changeTarget()) {
+        while (!recent_target_armor_centers.empty())recent_target_armor_centers.pop();//目标切换时清空
+        return;
+    }
+    if (recent_target_armor_centers.size() < 30)recent_target_armor_centers.push(target_polar_angle);
+    else {
+        recent_target_armor_centers.pop();
+        recent_target_armor_centers.push(target_polar_angle);
+    }
 }

energy/src/energy/get/predict_point_get.cpp

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

energy/src/energy/init/energy_init.cpp

@@ -18,6 +18,8 @@ void Energy::initEnergy() {
     isPredicting = true;
     isGuessing = false;
     manual_mark = false;
+    auto_mark = false;
+    energy_mode_init = true;
     circle_center_point = Point(0, 0);
     target_point = Point(0, 0);
     last_target_point = Point(0, 0);
@@ -29,6 +31,7 @@ void Energy::initEnergy() {
     radius = 0;
     energy_rotation_direction = ANTICLOCKWISE;
     attack_distance = ATTACK_DISTANCE;
+    last_mode = -1;//既不是大符也不是小符
     last_fans_cnt = 0;
     send_cnt = 0;
     yaw_rotation = 0;
@@ -44,6 +47,7 @@ void Energy::initEnergy() {
     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;

energy/src/energy/judge/judge_mode.cpp

@@ -0,0 +1,38 @@
+//
+// Created by sun on 19-7-12.
+//
+
+#include "energy/energy.h"
+#include "log.h"
+
+using namespace std;
+using namespace cv;
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于判断大小符
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::JudgeMode() {
+    getRecentTargetArmorCenters();
+    if (recent_target_armor_centers.size() < 30) {
+        return;
+    } else {
+        if (abs(recent_target_armor_centers.back() - recent_target_armor_centers.front()) > 10) {
+            if(last_mode!=BIG){
+                isBig = true;
+                isSmall = false;
+                energy_mode_init = false;
+                LOGM(STR_CTR(WORD_CYAN,"start big!"));
+            }
+            last_mode = BIG;
+        } else {
+            if(last_mode!=SMALL){
+                isBig = false;
+                isSmall = true;
+                energy_mode_init = false;
+                LOGM(STR_CTR(WORD_GREEN,"start small!"));
+            }
+            last_mode = SMALL;
+        }
+    }
+}

energy/src/energy/judge/judge_shoot.cpp

@@ -9,10 +9,11 @@ using namespace std;
 using namespace cv;
 
 
+
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于判断大符模式是否可以发弹
+// 此函数用于判断是否可以发弹
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::judgeBigShoot() {
+void Energy::judgeShoot(){
     if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) {
         shoot = true;
         isPredicting = false;
@@ -23,16 +24,3 @@ void Energy::judgeBigShoot() {
     } else
         shoot = false;
 }
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于判断小符模式是否可以发弹
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::judgeSmallShoot() {
-    if (fabs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) {
-        shoot = true;
-        isPredicting = false;
-        isGuessing = true;
-    } else
-        shoot = false;
-}

energy/src/energy/judge/judge_time.cpp

@@ -13,7 +13,7 @@ using namespace cv;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于判断guess模式是否超时
 // ---------------------------------------------------------------------------------------------------------------------
-bool Energy::is_guessing_timeout() {
+bool Energy::isGuessingTimeout() {
     timeval cur_time;
     gettimeofday(&cur_time, NULL);
     return (cur_time.tv_sec - time_start_guess.tv_sec) * 1000.0 +

energy/src/energy/run.cpp

@@ -9,26 +9,25 @@
 using namespace std;
 using namespace cv;
 
-
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
+// 此函数为能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
 // ---------------------------------------------------------------------------------------------------------------------
-int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
+void Energy::run(cv::Mat &gimble_src, cv::Mat &chassis_src) {
     if (chassis_src.empty())
-        runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
+        run(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
     else if (isGimble) {
-        energy_part_param_ = gimble_energy_part_param_;
+        energy_part_param_ = chassis_energy_part_param_;
         clearAll();
         initImage(gimble_src);
 
-        if (findArmors(gimble_src) < 1)return 0;
+        if (findArmors(gimble_src) < 1)return;
-        if (!findFlowStripFan(gimble_src))return 0;
+        if (!findFlowStripFan(gimble_src))return;
-        if (!findTargetInFlowStripFan()) return 0;
+        if (!findTargetInFlowStripFan()) return;
-        if (!findCenterROI(gimble_src))return 0;
+        if (!findCenterROI(gimble_src))return;
-        if (!findCenterR(gimble_src))return 0;
+        if (!findCenterR(gimble_src))return;
 
-        if (show_energy)showFlowStripFanContours("strip", gimble_src);
+        if (show_energy)showFlowStripFan("strip", gimble_src);
-        if (show_energy)showCenterRContours("R", gimble_src);
+        if (show_energy)showCenterR("R", gimble_src);
 
         startChassis();
         getOrigin();
@@ -40,70 +39,73 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
         initImage(chassis_src);
 
         changeMark();
-        if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
+        if (isMark)return;//操作手强制手动标定origin_yaw和origin_pitch
 
-        if (findArmors(chassis_src) < 1)return 0;
+        if (findArmors(chassis_src) < 1)return;
-        if (!findFlowStripFan(chassis_src))return 0;
+        if (!findFlowStripFan(chassis_src))return;
-        if (!findTargetInFlowStripFan()) return 0;
+        if (!findTargetInFlowStripFan()) return;
-        if (!findCenterROI(chassis_src))return 0;
+        if (!findCenterROI(chassis_src))return;
-        if (!findCenterR(chassis_src))return 0;
+        if (!findCenterR(chassis_src))return;
 
-        if (show_energy)showFlowStripFanContours("strip", chassis_src);
+        if (show_energy)showFlowStripFan("strip", chassis_src);
-        if (show_energy)showCenterRContours("R", chassis_src);
+        if (show_energy)showCenterR("R", chassis_src);
 
         getTargetPolarAngle();
-        if (energy_rotation_init) {
+        JudgeMode();
+        if (energy_mode_init)return;
+        if (isBig && energy_rotation_init) {
             initRotation();
-            return 0;
+            return;
         }
         if (isPredicting) {
             getPredictPoint(target_point);
             gimbleRotation();
-            judgeBigShoot();
+            judgeShoot();
             sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
         } else if (isGuessing && stayGuessing()) {
             findFans(chassis_src);
             if (save_mark)writeDownMark();
-            guessTarget();
+            if (!guessTarget()) return;
             if (show_energy)showGuessTarget("guess", chassis_src);
             getPredictPoint(guess_point);
             gimbleRotation();
             sendTarget(serial, yaw_rotation, pitch_rotation, false);
         }
     }
-    return 0;
 }
 
 
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
+// 此函数为能量机关模式主控制流函数，且步兵仅拥有云台摄像头
 // ---------------------------------------------------------------------------------------------------------------------
-int Energy::runBig(cv::Mat &gimble_src) {
+void Energy::run(cv::Mat &gimble_src) {
     energy_part_param_ = gimble_energy_part_param_;
     clearAll();
     initImage(gimble_src);
 
     changeMark();
-    if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
+    if (isMark)return;//操作手强制手动标定origin_yaw和origin_pitch
 
     if (show_process)imshow("bin", gimble_src);
-    if (findArmors(gimble_src) < 1)return 0;
+    if (findArmors(gimble_src) < 1)return;
-    if (!findFlowStripFan(gimble_src))return 0;
+    if (!findFlowStripFan(gimble_src))return;
-    if (!findTargetInFlowStripFan()) return 0;
+    if (!findTargetInFlowStripFan()) return;
-    if (!findCenterROI(gimble_src))return 0;
+    if (!findCenterROI(gimble_src))return;
-    if (show_energy)showFlowStripFanContours("strip", gimble_src);
+    if (show_energy)showFlowStripFan("strip", gimble_src);
-    if (!findCenterR(gimble_src))return 0;
+    if (!findCenterR(gimble_src))return;
-    if (show_energy)showCenterRContours("R", gimble_src);
+    if (show_energy)showCenterR("R", gimble_src);
     getTargetPolarAngle();
-    if (energy_rotation_init) {
+    JudgeMode();
+    if (energy_mode_init)return;
+    getOrigin();
+    if (isBig & energy_rotation_init) {
         initRotation();
-        getOrigin();//一旦确定风车旋向后，就开始移动云台，此时不再更新origin_yaw和origin_pitch
+        return;
-        return 0;
     }
     if (isPredicting) {
         getPredictPoint(target_point);
         gimbleRotation();
-        judgeBigShoot();
+        judgeShoot();
         sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
     } else if (isGuessing && stayGuessing()) {
         findFans(gimble_src);
@@ -114,113 +116,4 @@ int Energy::runBig(cv::Mat &gimble_src) {
         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);
-            gimbleRotation();
-            sendTarget(serial, yaw_rotation, pitch_rotation, false);
-        }
-    }
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
-// ---------------------------------------------------------------------------------------------------------------------
-int Energy::runSmall(cv::Mat &gimble_src) {
-    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) {
-        getAimPoint(target_point);
-        judgeSmallShoot();
-        sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
-    } else if (isGuessing && stayGuessing()) {
-        if (!findCenterROI(gimble_src))return 0;
-        if (show_energy)showFlowStripFanContours("strip", gimble_src);
-        if (!findCenterR(gimble_src))return 0;
-        if (show_energy)showCenterRContours("R", gimble_src);
-        getTargetPolarAngle();
-        findFans(gimble_src);
-        guessTarget();
-        if (show_energy)showGuessTarget("guess", gimble_src);
-        getAimPoint(guess_point);
-        sendTarget(serial, yaw_rotation, pitch_rotation, false);
-    }
-    return 0;
-}
-
-
-

energy/src/energy/show/show.cpp

@@ -13,7 +13,7 @@ using std::vector;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有扇叶
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
+void Energy::showFans(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
 
@@ -38,7 +38,7 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有装甲板
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showArmorContours(std::string windows_name, const cv::Mat src) {
+void Energy::showArmors(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
 
@@ -63,7 +63,7 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有扇叶和装甲板,并框出待击打装甲板
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
+void Energy::showBoth(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
     if (src.type() == CV_8UC1) // 黑白图像
@@ -101,7 +101,7 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有可能的风车中心候选区R
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) {
+void Energy::showCenterR(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
 
@@ -117,7 +117,7 @@ void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) {
     Point2f vertices[4];      //定义矩形的4个顶点
     centerR.points(vertices);   //计算矩形的4个顶点
     for (int i = 0; i < 4; i++)
-        line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 255), 2);
+        line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2);
 
     cv::circle(image2show, circle_center_point, 4, cv::Scalar(0, 0, 255), 2);//在图像中画出特征点，2是圆的半径
 
@@ -128,7 +128,7 @@ void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有流动条
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showFlowStripFanContours(std::string windows_name, const cv::Mat src) {
+void Energy::showFlowStripFan(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
 

energy/src/energy/tool/tool.cpp

@@ -13,16 +13,6 @@ using namespace std;
 using namespace cv;
 
 
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数选取图像中的一部分进行处理
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::extract(cv::Mat &src) {
-    cv::Rect rect(EXTRACT_POINT_X, EXTRACT_POINT_Y, EXTRACT_WIDTH, EXTRACT_HEIGHT);
-    src = src(rect).clone();
-    cv::resize(src, src, cv::Size(640, 480), 2);
-    imshow("extract", src);
-}
-
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于计算预测的击打点坐标
@@ -59,6 +49,8 @@ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2) {
 }
 
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于计算两点距离
 // ---------------------------------------------------------------------------------------------------------------------

main.cpp

@@ -29,7 +29,7 @@ using namespace std;
 mcu_data mcuData = {    // 单片机端回传结构体
         0,              // 当前云台yaw角
         0,              // 当前云台pitch角
-        SMALL_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
+        BIG_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
         1,              // 是否启用数字识别
         ENEMY_RED,      // 敌方颜色
@@ -98,35 +98,35 @@ int main(int argc, char *argv[]) {
                 if (mcuData.state != ARMOR_STATE) {//能量机关模式
                     if (last_state == ARMOR_STATE) {//若上一帧是自瞄模式，即刚往完成切换，则需要初始化
                         ((CameraWrapper *) video_gimble)->changeBrightness(20);
+                        energy.setEnergyInit();
                     }
                     ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头
+                    if (!from_camera) extract(gimble_src, chassis_src);
                     if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
                     if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
-                    if (mcuData.state == BIG_ENERGY_STATE) {
+                    energy.run(gimble_src, chassis_src);
-                        if (last_state != BIG_ENERGY_STATE) {
+//                    if (mcuData.state == BIG_ENERGY_STATE) {
-                            energy.setBigEnergyInit();
+//                        if (last_state != BIG_ENERGY_STATE) {
-                            cout << "set" << endl;
+//                            energy.setBigEnergyInit();
-                        }
+//                            cout << "start big" << endl;
-                        energy.runBig(gimble_src, chassis_src);
+//                        }
-                    } else if (mcuData.state == SMALL_ENERGY_STATE) {
+//                        energy.run(gimble_src, chassis_src);
-                        if (last_state != SMALL_ENERGY_STATE) {
+//                    } else if (mcuData.state == SMALL_ENERGY_STATE) {
-                            energy.setSmallEnergyInit();
+//                        if (last_state != SMALL_ENERGY_STATE) {
-                            cout << "set" << endl;
+//                            energy.setSmallEnergyInit();
-                        }
+//                            cout << "start small" << endl;
-                        energy.runSmall(gimble_src, chassis_src);
+//                        }
-                    }
+//                        energy.run(gimble_src, chassis_src);
-                    last_state = mcuData.state;//更新上一帧状态
+//                    }
 
-//                    resize(gimble_src,gimble_src,cv::Size(853,480));
+                    last_state = mcuData.state;//更新上一帧状态
-//                    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));
                 } else {                                         // 自瞄模式
                     if (last_state != ARMOR_STATE) {
                         ((CameraWrapper *) video_gimble)->changeBrightness(40);
                     }
                     last_state = mcuData.state;
                     ok = checkReconnect(video_gimble->read(gimble_src));
+                    if (!from_camera) extract(gimble_src);
                     if (save_video) saveVideos(gimble_src);
                     if (show_origin) showOrigin(gimble_src);
                     CNT_TIME("Armor Time", {

others/include/additions/additions.h

@@ -26,5 +26,7 @@ void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src);
 void saveVideos(const cv::Mat &gimble_src);
 void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src);
 void showOrigin(const cv::Mat &gimble_src);
+void extract(cv::Mat &gimble_src, cv::Mat &chassis_src);
+void extract(cv::Mat &gimble_src);
 
 #endif /* _ADDITIONS_H_ */

others/src/additions/additions.cpp

@@ -29,37 +29,36 @@ extern uint8_t last_state;
 extern ArmorFinder armorFinder;
 extern Energy energy;
 
-void uartReceive(Serial* pSerial) {
+void uartReceive(Serial *pSerial) {
     char buffer[20];
     int cnt = 0;
-    LOGM(STR_CTR(WORD_LIGHT_WHITE,"data receive start!"));
+    LOGM(STR_CTR(WORD_LIGHT_WHITE, "data receive start!"));
     while (true) {
         char byte = 0;
         memset(buffer, 0, sizeof(buffer));
-        while (pSerial->ReadData((uint8_t*)&byte, 1) && byte!='\n') {
+        while (pSerial->ReadData((uint8_t *) &byte, 1) && byte != '\n') {
             buffer[cnt++] = byte;
             if (cnt >= sizeof(buffer)) {
 //                LOGE("data receive over flow!");
                 cnt = 0;
             }
         }
-        if (cnt==0 && byte=='\n'){
+        if (cnt == 0 && byte == '\n') {
             LOGM("%d", cnt);
         }
         if (cnt == sizeof(mcuData)) {
             memcpy(&mcuData, buffer, sizeof(mcuData));
-            LOGM("Get, state:%c, mark:%d!", mcuData.state, (int)mcuData.mark);
+            LOGM("Get, state:%c, mark:%d!", mcuData.state, (int) mcuData.mark);
         }
         cnt = 0;
     }
 }
 
-cv::VideoWriter initVideoWriter(const std::string &filename_prefix){
+cv::VideoWriter initVideoWriter(const std::string &filename_prefix) {
     cv::VideoWriter video;
     std::ifstream in(filename_prefix + "cnt.txt");
     int cnt = 0;
-    if (in.is_open())
+    if (in.is_open()) {
-    {
         in >> cnt;
         in.close();
     }
@@ -74,17 +73,17 @@ cv::VideoWriter initVideoWriter(const std::string &filename_prefix){
     return video;
 }
 
-bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect){
+bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect) {
-    if(!is_gimble_connect){
+    if (!is_gimble_connect) {
         video_gimble = new CameraWrapper(0, "armor");
-        if(!(is_gimble_connect = video_gimble->init())){
+        if (!(is_gimble_connect = video_gimble->init())) {
             delete video_gimble;
             video_gimble = nullptr;
         }
     }
-    if(!is_chassis_connect){
+    if (!is_chassis_connect) {
         video_chassis = new CameraWrapper(1, "energy");
-        if(!(is_chassis_connect = video_chassis->init())){
+        if (!(is_chassis_connect = video_chassis->init())) {
             delete video_chassis;
             video_chassis = nullptr;
         }
@@ -92,10 +91,10 @@ bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect){
     return is_gimble_connect && is_chassis_connect;
 }
 
-bool checkReconnect(bool is_gimble_connect){
+bool checkReconnect(bool is_gimble_connect) {
-    if(!is_gimble_connect){
+    if (!is_gimble_connect) {
         video_gimble = new CameraWrapper(0, "armor");
-        if(!(is_gimble_connect = video_gimble->init())){
+        if (!(is_gimble_connect = video_gimble->init())) {
             delete video_gimble;
             video_gimble = nullptr;
         }
@@ -106,50 +105,69 @@ bool checkReconnect(bool is_gimble_connect){
 auto gimble_video_writer = initVideoWriter(PROJECT_DIR"/gimble_video/");
 auto chassis_video_writer = initVideoWriter(PROJECT_DIR"/chassis_video/");
 
-void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src){
+void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src) {
-    if(!gimble_src.empty() && !chassis_src.empty()){
+    if (!gimble_src.empty() && !chassis_src.empty()) {
         gimble_video_writer.write(gimble_src);
         Mat chassis_save = chassis_src.clone();
-        cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR);
+        cvtColor(chassis_save, chassis_save, COLOR_GRAY2BGR);
         chassis_video_writer.write(chassis_save);
-    }
+    } else if (!gimble_src.empty() && chassis_src.empty()) {
-    else if(!gimble_src.empty() && chassis_src.empty()){
         gimble_video_writer.write(gimble_src);
-    }
+    } else if (gimble_src.empty() && !chassis_src.empty()) {
-    else if(gimble_src.empty() && !chassis_src.empty()){    if (show_origin)imshow("src", gimble_src);
+        if (show_origin)imshow("src", gimble_src);
         Mat chassis_save = chassis_src.clone();
-        cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR);
+        cvtColor(chassis_save, chassis_save, COLOR_GRAY2BGR);
         chassis_video_writer.write(chassis_save);
-    }
+    } else return;
-    else return;
 }
 
-void saveVideos(const cv::Mat &gimble_src){
+void saveVideos(const cv::Mat &gimble_src) {
-    if(!gimble_src.empty()){
+    if (!gimble_src.empty()) {
         gimble_video_writer.write(gimble_src);
-    }
+    } else return;
-    else return;
 }
 
-void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src){
+void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src) {
-    if(!gimble_src.empty() && !chassis_src.empty()){
+    if (!gimble_src.empty() && !chassis_src.empty()) {
         imshow("gimble", gimble_src);
         imshow("chassis", chassis_src);
-    }
+    } else if (!gimble_src.empty() && chassis_src.empty()) {
-    else if(!gimble_src.empty() && chassis_src.empty()){
         imshow("gimble", gimble_src);
-    }
+    } else if (gimble_src.empty() && !chassis_src.empty()) {
-    else if(gimble_src.empty() && !chassis_src.empty()){
         imshow("chassis", chassis_src);
-    }
+    } else return;
-    else return;
     cv::waitKey(1);
 }
 
-void showOrigin(const cv::Mat &gimble_src){
+void showOrigin(const cv::Mat &gimble_src) {
-    if(!gimble_src.empty()){
+    if (!gimble_src.empty()) {
         imshow("gimble", gimble_src);
-    }
+    } else return;
-    else return;
     cv::waitKey(1);
+}
+
+void extract(cv::Mat &gimble_src, cv::Mat &chassis_src) {
+    if (!gimble_src.empty() && !chassis_src.empty()) {
+        extract(gimble_src);
+        extract(chassis_src);
+    } else if (!gimble_src.empty() && chassis_src.empty()) {
+        extract(gimble_src);
+    } else if (gimble_src.empty() && !chassis_src.empty()) {
+        extract(chassis_src);
+    } else return;
+}
+
+void extract(cv::Mat &gimble_src) {//图像预处理，将视频切成640×480的大小
+    if (gimble_src.empty()) return;
+    float length = static_cast<float>(gimble_src.cols);
+    float width = static_cast<float>(gimble_src.rows);
+    if (length / width > 640.0 / 480.0) {
+        length *= 480.0 / width;
+        resize(gimble_src, gimble_src, cv::Size(length, 480));
+        gimble_src = gimble_src(Rect((length - 640) / 2, 0, 640, 480));
+    } else {
+        width *= 640.0 / length;
+        resize(gimble_src, gimble_src, cv::Size(640, width));
+        gimble_src = gimble_src(Rect(0, (width - 480) / 2, 640, 480));
+    }
 }

others/src/camera/video_wrapper.cpp

@@ -18,5 +18,4 @@ bool VideoWrapper::init() {
 
 bool VideoWrapper::read(cv::Mat &src) {
     return video.read(src);
-
 }