energy changed a lot

.gitignore

@@ -9,6 +9,4 @@ tools/TrainCNN/__pycache__
 others/include/config/config.h
 others/MV-UB31-Group0.config
 .DS_Store
-gimbal_video
+video
-chassis_video
-test_video

energy/include/energy/energy.h

@@ -27,47 +27,31 @@ public:
     Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色
     ~Energy();//默认析构函数
 
-    void runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src);
+    bool is_big;//大符模式为true
-    void runBig(cv::Mat &gimbal_src);
+    bool is_small;//小符模式为true
-    void runSmall(cv::Mat &gimbal_src);
+
+    void run(cv::Mat &src);
 
     Serial &serial;//串口
     void setEnergyInit();//设置能量机关初始化
-    void setBigEnergyInit();//设置大能量机关初始化
-    void setSmallEnergyInit();//设置小能量机关初始化
     void sendEnergy();//发送能量机关数据
-    void sendTarget(Serial& serial, float x, float y, float z);//发送数据
     void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据
 
 
 private:
     EnergyPartParam energy_part_param_;//能量机关的参数设置
-    EnergyPartParam gimbal_energy_part_param_;//云台摄像头能量机关的参数设置
-    EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置
 
-    bool is_mark;//若操作手正在手动标定，则为true
-    bool is_big;//大符模式为true
-    bool is_small;//小符模式为true
-    bool is_gimbal;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
-    bool is_chassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
     bool is_guessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
     bool is_predicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
-    bool is_find_target;//判断当前是否找到了目标
+    bool energy_mode_init;//大小符状态判断
-    bool energy_mode_init;//正在进行大小符判断
     bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
-    bool manual_mark;//若操作手进行过手动标定，则为true
-    bool auto_mark;//云台完成自动对心，则置为true
     bool start_guess;//进入猜测状态的标志
     bool change_target;//目标切换的标志
 
-    uint8_t last_mark;//用于记录上一帧操作手是否进行标定
     uint8_t &ally_color;//我方颜色
 
-    double radius;//大风车半径
-
     int curr_fps;//帧率
     int send_cnt;//向主控板发送的数据总次数
-    int camera_cnt;//摄像头数量
     int fans_cnt;//扇叶个数
     int last_fans_cnt;//上一帧的扇叶个数
     int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区
@@ -79,7 +63,8 @@ private:
     int extra_delta_x, extra_delta_y;//在风车运动到最高点附近的额外补偿量
 
     float target_polar_angle;//待击打装甲板的极坐标角度
-    float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
+    float last_target_polar_angle_judge_change;//上一帧待击打装甲板的极坐标角度（用于判断目标切换）
+    float last_target_polar_angle_judge_rotation;//上一帧待击打装甲板的极坐标角度（用于判断旋向）
     float guess_polar_angle;//猜测的下一个目标装甲板极坐标角度
     float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度
     float predict_rad;//预测提前角
@@ -87,12 +72,11 @@ private:
     float attack_distance;//步兵与风车平面距离
     float center_delta_yaw, center_delta_pitch;//对心时相差的角度
     float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
-    float origin_yaw, origin_pitch;//初始的云台角度设定值
+    float shoot;//给主控板的指令，1表示跟随，2表示发射，3表示目标切换,4表示猜测模式
-    float shoot;//若为2，则要求主控板发弹
     float last_yaw, last_pitch;//PID中微分项
     float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
 
-    systime time_start_guess;
+    systime time_start_guess;//进入猜测模式的时间
 
     cv::RotatedRect centerR;//风车中心字母R的可能候选区
     cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个）
@@ -102,7 +86,6 @@ private:
 
     cv::Point circle_center_point;//风车圆心坐标
     cv::Point target_point;//目标装甲板中心坐标
-    cv::Point last_target_point;//上一帧目标装甲板中心坐标
     cv::Point guess_point;
     cv::Point predict_point;//预测的击打点坐标
 
@@ -125,17 +108,16 @@ private:
     void clearAll();//清空各vector
     void initImage(cv::Mat &src);//图像预处理
 
-    void startChassis();//从云台摄像头对心状态进入底盘摄像头击打状态
     bool stayGuessing();//保持在猜测模式
 
-    int findFans(const cv::Mat src);//寻找图中所有扇叶
+    int findFans(const cv::Mat &src);//寻找图中所有扇叶
-    int findArmors(const cv::Mat src);//寻找图中所有装甲板
+    int findArmors(const cv::Mat &src);//寻找图中所有装甲板
-    bool findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R
+    bool findCenterR(const cv::Mat &src);//寻找图中可能的风车中心字母R
-    bool findFlowStrip(const cv::Mat src);//寻找图中的流动条
+    bool findFlowStrip(const cv::Mat &src);//寻找图中的流动条
-    bool findFlowStripSub(const cv::Mat src);//寻找图中的流动条
+    bool findCenterROI(const cv::Mat &src);//框取中心R候选区
-    bool findCenterROI(const cv::Mat src);//框取中心R候选区
+    bool findFlowStripFan(const cv::Mat &src);//寻找图中的流动条所在扇叶
-    bool findFlowStripFan(const cv::Mat src);//寻找图中的流动条所在扇叶
+    bool findFlowStripWeak(const cv::Mat &src);//弱识别寻找图中的流动条
-    bool findFlowStripWeak(const cv::Mat src);//弱识别寻找图中的流动条
+    bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板
 
     bool isValidFanContour(cv::Mat &src, const vector<cv::Point> &fan_contour);//扇叶矩形尺寸要求
     bool isValidArmorContour(const vector<cv::Point> &armor_contour);//装甲板矩形尺寸要求
@@ -143,57 +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 showFans(std::string windows_name, const cv::Mat src);//显示扇叶
+    void showFans(std::string windows_name, const cv::Mat &src);//显示扇叶
-    void showArmors(std::string windows_name, const cv::Mat src);//显示装甲板
+    void showArmors(std::string windows_name, const cv::Mat &src);//显示装甲板
-    void showBoth(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板
+    void showCenterR(std::string windows_name, const cv::Mat &src);//显示风车中心候选区R
-    void showTarget(std::string windows_name, const cv::Mat src);//显示目标装甲板
+    void showFlowStrip(std::string windows_name, const cv::Mat &src);//显示流动条
-    void showCenterR(std::string windows_name, const cv::Mat src);//显示风车中心候选区R
+    void showFlowStripFan(std::string windows_name, const cv::Mat &src);//显示流动条所在扇叶
-    void showFlowStrip(std::string windows_name, const cv::Mat src);//显示流动条
+    void showGuessTarget(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 circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径
-
-    void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
-    bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板
 
     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 writeDownSlightChange(cv::Mat &src);//记录操作手的手动微调
+    void writeDownMark(cv::Mat &src);//记录操作手的手动微调
-    void writeDownMark(cv::Mat &src);//记录操作手标定的云台对心初始角度
 
     bool guessTarget();//获得猜测击打点位
-    bool getOrigin();//获得云台对心所需角度
     void changeTarget();//判断目标是否改变
     void getCenter();//对心
     void multipleMode(cv::Mat &src);//多模式切换
     void getTargetPolarAngle();//获得目标装甲板极坐标角度
     void getPredictPoint(cv::Point target_point);//获取预测点位
     void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位
-    void changeMark();//操作手手动修改标定值
-    void gimbalRotation();//计算云台旋转角度
-    void getAllTargetArmorCenters();//记录所有目标装甲板中心坐标
     void getRecentTargetArmorCenters();//记录近30帧目标装甲板中心坐标
 
     void judgeMode();//判断大符还是小符
-    void judgeShootInGimbal();//在云台坐标系中判断是否可以发弹
+    void judgeShoot();//判断是否可以发弹
-    void judgeShootInWorld();//在世界坐标系中判断是否可以发弹
     bool isGuessingTimeout();//判断猜测模式是否超时（没打中）
 
     void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离
     void imagePreprocess(cv::Mat &src);//图像通道分离
-    void hsvFilter(cv::Mat &src);//此函数通过HSV颜色空间对图像纯度进行限制，进行滤光
 
     void FanStruct(cv::Mat &src);//腐蚀和膨胀
     void ArmorStruct(cv::Mat &src);//腐蚀和膨胀
     void FlowStripFanStruct(cv::Mat &src);//腐蚀和膨胀
     void FlowStripStruct(cv::Mat &src);//腐蚀和膨胀
-    void FlowStripStructSub(cv::Mat &src);//腐蚀和膨胀
     void CenterRStruct(cv::Mat &src);//腐蚀和膨胀
 };
 

energy/include/energy/param_struct_define.h

@@ -16,7 +16,6 @@ using std::vector;
 // 此结构体包括能量机关参数
 // ---------------------------------------------------------------------------------------------------------------------
 struct EnergyPartParam {
-	int GRAY_THRESH;//二值化阈值
 	int RED_GRAY_THRESH;//红方二值化阈值
 	int BLUE_GRAY_THRESH;//蓝方二值化阈值
 	int SPLIT_GRAY_THRESH;//通道分离二值化阈值
@@ -30,8 +29,6 @@ struct EnergyPartParam {
 	float FAN_CONTOUR_HW_RATIO_MAX;//扇叶长宽比最大值
 	float FAN_CONTOUR_HW_RATIO_MIN;//扇叶长宽比最小值
     float FAN_CONTOUR_AREA_RATIO_MIN;//装甲板轮廓占旋转矩形面积比最小值
-    double FAN_NON_ZERO_RATE_MAX;//流动条扇叶中亮点占旋转矩形的比值最大值
-    double FAN_NON_ZERO_RATE_MIN;//流动条扇叶中亮点占旋转矩形的比值最大值
 
 	long ARMOR_CONTOUR_AREA_MAX;//装甲板面积最大值
 	long ARMOR_CONTOUR_AREA_MIN;//装甲板面积最小值
@@ -64,8 +61,6 @@ struct EnergyPartParam {
     float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN;//流动条扇叶（待击打）长宽比最小值
     float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX;//流动条扇叶轮廓占旋转矩形面积比最小值
     float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN;//流动条扇叶占旋转矩形面积比最小值
-    double FLOW_STRIP_FAN_NON_ZERO_RATE_MAX;//流动条扇叶中亮点占旋转矩形的比值最大值
-    double FLOW_STRIP_FAN_NON_ZERO_RATE_MIN;//流动条扇叶中亮点占旋转矩形的比值最大值
 
     long FLOW_STRIP_CONTOUR_AREA_MAX;//流动条（待击打）面积最大值
     long FLOW_STRIP_CONTOUR_AREA_MIN;//流动条（待击打）面积最小值
@@ -81,7 +76,6 @@ struct EnergyPartParam {
 	float TWIN_ANGEL_MAX;//两个理论上相等的角度在计算时具有的可能最大差值
 	long TARGET_INTERSETION_CONTOUR_AREA_MIN;//扇叶与装甲板匹配时的最小重合面积
 
-	long TARGET_CHANGE_DISTANCE_MAX;//目标未更改时，目标装甲板中心与原目标装甲板中心的距离变化最大值
 	long TWIN_POINT_MAX;//两个点相同时距离最大值
 
     long STRIP_ARMOR_DISTANCE_MIN;//流动条中心和目标装甲板中心距离最小值

energy/src/energy/calculate/circle_calculate.cpp

@@ -1,64 +0,0 @@
-//
-// Created by xixiliadorabarry on 1/24/19.
-//
-#include "energy/energy.h"
-
-using namespace cv;
-using std::cout;
-using std::endl;
-using std::vector;
-
-
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数通过最小二乘法计算大风车圆心和半径
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::circleLeastFit()
-{
-    circle_center_point.x = 0;
-    circle_center_point.y = 0;
-    radius = 0.0f;
-    if (all_target_armor_centers.size() < 3)
-    {
-//        cout<<"Cannot calculate a circle"<<endl;
-        return;
-    }
-    double sum_x = 0.0f, sum_y = 0.0f;
-    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_target_armor_centers.size());
-    for (int i = 0; i < N; i++)
-    {
-        double x = all_target_armor_centers.at(i).x;
-        double y = all_target_armor_centers.at(i).y;
-        double x2 = x * x;
-        double y2 = y * y;
-        sum_x += x;
-        sum_y += y;
-        sum_x2 += x2;
-        sum_y2 += y2;
-        sum_x3 += x2 * x;
-        sum_y3 += y2 * y;
-        sum_xy += x * y;
-        sum_x1y2 += x * y2;
-        sum_x2y1 += x2 * y;
-    }
-    double C, D, E, G, H;
-    double a, b, c;
-    C = N * sum_x2 - sum_x * sum_x;
-    D = N * sum_xy - sum_x * sum_y;
-    E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
-    G = N * sum_y2 - sum_y * sum_y;
-    H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
-    a = (H * D - E * G) / (C * G - D * D);
-    b = (H * C - E * D) / (D * D - G * C);
-    c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;
-    circle_center_point.x = static_cast<int>(a / (-2));
-    circle_center_point.y = static_cast<int>(b / (-2));
-    radius = sqrt(a * a + b * b - 4 * c) / 2;
-//    cout << "The cycle center is: " << cycle_center << endl;
-//    cout << "The radius is: " << radius << endl;
-}
-

energy/src/energy/calibrate/hsv_filter.cpp

@@ -1,28 +0,0 @@
-//
-// Created by sun on 19-7-7.
-//
-
-#include "energy/energy.h"
-
-using namespace std;
-using namespace cv;
-
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于利用HSV颜色空间对图像进行预处理
-// ---------------------------------------------------------------------------------------------------------------------
-
-void Energy::hsvFilter(cv::Mat &src){
-    Mat hsv, hsv_in_range;
-    cvtColor(src, hsv, CV_BGR2HSV);
-//    imshow("hsv", hsv);
-    vector<Mat> hsv_channels;
-    split(hsv, hsv_channels);
-    Mat H = hsv_channels.at(0);
-    Mat S = hsv_channels.at(1);
-    Mat V = hsv_channels.at(2);
-//    imshow("S", S);
-    inRange(hsv,Scalar(0,150,0),Scalar(180,255,255),hsv_in_range);
-//    imshow("in range", hsv_in_range);
-}

energy/src/energy/calibrate/structing.cpp

@@ -17,24 +17,15 @@ using std::vector;
 void Energy::FanStruct(cv::Mat &src) {
     Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
     Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2));
-    Mat element_dilate_2 = getStructuringElement(MORPH_RECT,
+    Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
-            Size(3, 3));
     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));
 
     dilate(src, src, element_dilate_1);
-//    imshow("dilate_1", src);
     erode(src,src, element_erode_1);
-//    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);
 }
 
 
@@ -51,19 +42,7 @@ void Energy::ArmorStruct(cv::Mat &src) {
     Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
 
     dilate(src, src, element_dilate_1);
-//    imshow("dilate_1", src);
     erode(src,src, element_erode_1);
-//    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);
-
 }
 
 
@@ -80,17 +59,9 @@ void Energy::FlowStripFanStruct(cv::Mat &src) {
     Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
 
     dilate(src, src, element_dilate_1);
-//    imshow("dilate_1", src);
     erode(src,src, element_erode_1);
-//    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);
 }
 
 
@@ -107,48 +78,9 @@ void Energy::FlowStripStruct(cv::Mat &src) {
     Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
 
     dilate(src, src, element_dilate_1);
-//    imshow("dilate_1", src);
     erode(src,src, element_erode_1);
-//    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);
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数对图像进行腐蚀与膨胀操作
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::FlowStripStructSub(cv::Mat &src) {
-    Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
-    Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(4, 4));
-    Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
-    Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(3 , 3));
-    Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(2, 2));
-    Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(2 , 2));
-    Mat element_dilate_4 = getStructuringElement(MORPH_RECT, Size(2, 2));
-
-    erode(src,src, element_erode_1);
-//    imshow("sub_erode_1", src);
-//    erode(src,src, element_erode_2);
-//    imshow("erode_2", src);
-    erode(src,src, element_erode_3);
-//    imshow("sub_erode_3", src);
-//    erode(src,src, element_erode_4);
-//    imshow("erode_4", src);
-    dilate(src, src, element_dilate_1);
-//    imshow("sub_dilate_1", src);
-    dilate(src, src, element_dilate_2);
-//    imshow("sub_dilate_2", src);
-//    dilate(src, src, element_dilate_3);
-//    imshow("dilate_3", src);
-//    dilate(src, src, element_dilate_4);
-//    imshow("dilate_4", src);
 }
 
 
@@ -162,11 +94,5 @@ void Energy::CenterRStruct(cv::Mat &src) {
     Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4));
 
     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);
-//    imshow("dilate_2", src);
-//    erode(src,src, element_erode_2);
-//    imshow("erode_2", src);
 }

energy/src/energy/change/mode_change.cpp

@@ -8,18 +8,6 @@ using namespace std;
 using namespace cv;
 
 
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于从云台摄像头对心状态切换到底盘摄像头击打状态
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::startChassis(){
-    is_chassis = true;
-    is_gimbal = false;
-    LOGM(STR_CTR(WORD_YELLOW, "Start Chassis!"));
-}
-
-
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于判断是否应当继续保持猜测模式
 // ---------------------------------------------------------------------------------------------------------------------

energy/src/energy/change/multiple_mode.cpp

@@ -7,11 +7,15 @@
 using namespace std;
 using namespace cv;
 
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于切换预测模式和猜测模式，但最终未使用
+// ---------------------------------------------------------------------------------------------------------------------
 void Energy::multipleMode(cv::Mat &src) {
     if (is_predicting) {
         getPredictPoint(target_point);
         getAimPoint(predict_point);
-        judgeShootInGimbal();
+        judgeShoot();
         sendEnergy();
     } else if (is_guessing && stayGuessing()) {
         findFans(src);

energy/src/energy/change/target_change.cpp

@@ -11,11 +11,11 @@ using namespace cv;
 // 此函数用于判断目标是否切换
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::changeTarget() {
-    if (pointDistance(target_point, last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX) {
+    if (abs(last_target_polar_angle_judge_change - target_polar_angle) < 20 ||
+        abs(last_target_polar_angle_judge_change - target_polar_angle) > 340) {
         change_target = false;
     } else {
         change_target = true;
     }
-    last_target_point = target_point;
+    last_target_polar_angle_judge_change = target_polar_angle;
-//    cout << change_target << endl;
 }

energy/src/energy/clear/clear.cpp

@@ -26,8 +26,6 @@ void Energy::clearAll() {
 // 此函数用于图像预处理
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initImage(cv::Mat &src) {
-//    imagePreprocess(src);
-//    if(show_process)imshow("img_preprocess", src);
     if (src.type() == CV_8UC3){
         cvtColor(src, src, COLOR_BGR2GRAY);
     }
@@ -36,8 +34,7 @@ void Energy::initImage(cv::Mat &src) {
     } else if(mcu_data.enemy_color == ENEMY_RED){
         threshold(src, src, energy_part_param_.BLUE_GRAY_THRESH, 255, THRESH_BINARY);
     }
-    if (show_process){
+    if (show_process) imshow("bin", src);
-        imshow("bin", src);
-    }
     if (show_energy || show_process)waitKey(1);
 }
+

energy/src/energy/clear/energy_init.cpp

@@ -14,24 +14,15 @@ using std::vector;
 // 此函数对能量机关成员变量进行初始化
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initEnergy() {
-    is_mark = false;
     is_guessing = false;
     is_predicting = true;
-    is_find_target = false;
     energy_mode_init = true;
     energy_rotation_init = true;
-    manual_mark = false;
-    auto_mark = false;
     start_guess = false;
     change_target = false;
 
-    last_mark = 0;
-
-    radius = 0;
-
     curr_fps = 0;
     send_cnt = 0;
-    camera_cnt = 1;
     fans_cnt = 0;
     last_fans_cnt = 0;
     guess_devide = 0;
@@ -45,10 +36,11 @@ void Energy::initEnergy() {
     extra_delta_x = 0;
 
     target_polar_angle = -1000;
-    last_target_polar_angle = -1000;
+    last_target_polar_angle_judge_change = -1000;
+    last_target_polar_angle_judge_rotation = -1000;
     guess_polar_angle = -1000;
     last_base_angle = -1000;
-    predict_rad = 25;
+    predict_rad = 0;
     predict_rad_norm = 25;
     attack_distance = ATTACK_DISTANCE;
     center_delta_yaw = 1000;
@@ -63,7 +55,6 @@ void Energy::initEnergy() {
 
     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);
 
@@ -82,154 +73,69 @@ void Energy::initEnergy() {
 // 此函数对能量机关参数进行初始化
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initEnergyPartParam() {
-//    gimbal_energy_part_param_.GRAY_THRESH = 120;//home
+    energy_part_param_.RED_GRAY_THRESH = 180;//game
-//    gimbal_energy_part_param_.GRAY_THRESH = 200;//official
+    energy_part_param_.BLUE_GRAY_THRESH = 100;//game
-    gimbal_energy_part_param_.RED_GRAY_THRESH = 180;//game
+    energy_part_param_.SPLIT_GRAY_THRESH = 180;
-    gimbal_energy_part_param_.BLUE_GRAY_THRESH = 100;//game
-    gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180;
 
-    gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000;
+    energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000;
-    gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500;
+    energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500;
-    gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 45;
+    energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 45;
-    gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100;
+    energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100;
-    gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 10;
+    energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 10;
-    gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52;
+    energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52;
-    gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5;
+    energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5;
-    gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2;
+    energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2;
-    gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6;
+    energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6;
-    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;
-//    gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.16;
 
-    gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500;
+    energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180;
+    energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 10;
+    energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 10;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
+    energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 0;
+    energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 0;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 30;
+    energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 30;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
+    energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
-    gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
+    energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
 
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 200;
+    energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 200;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 40;
+    energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 40;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 6;
+    energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 6;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 20;
+    energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 20;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 6;
+    energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 6;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 20;
+    energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 20;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 2;
+    energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 2;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
+    energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.6;
+    energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.6;
-    gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;
+    energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;
 
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 2000;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 2000;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 500;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 500;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 2.8;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 2.8;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1.2;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1.2;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34;
+    energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.58;
-    gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.34;
-//    gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2;
-//    gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08;
 
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 700;
+    energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 700;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 100;
+    energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 100;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32;
+    energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55;
+    energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4;
+    energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
+    energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7;
+    energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3;
+    energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6;
+    energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6;
-    gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100;
+    energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100;
 
-    gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10;
+    energy_part_param_.TWIN_ANGEL_MAX = 10;
-    gimbal_energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40;
+    energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40;
 
-    gimbal_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
+    energy_part_param_.TWIN_POINT_MAX = 20;
-    gimbal_energy_part_param_.TWIN_POINT_MAX = 20;
 
-    gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MIN = 28;
+    energy_part_param_.STRIP_ARMOR_DISTANCE_MIN = 28;
-    gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52;
+    energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52;
-
-
-    chassis_energy_part_param_.GRAY_THRESH = 120;//home
-//    chassis_energy_part_param_.GRAY_THRESH = 200;//official
-//    chassis_energy_part_param_.GRAY_THRESH = 225;
-    chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230;
-
-    chassis_energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000;
-    chassis_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0;
-    chassis_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 90;
-    chassis_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 140;
-    chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 35;
-    chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 65;
-    chassis_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4;
-    chassis_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1;
-    chassis_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6;
-    chassis_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8;
-    chassis_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48;
-//    chassis_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.3;
-//    chassis_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.16;
-
-    chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000;
-    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 = 13;
-    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;
-
-    chassis_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 10;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 30;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 10;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 30;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.7;
-    chassis_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;
-
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 17000;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 0;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 90;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 140;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 35;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 65;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.55;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.25;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.48;
-    chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.25;
-//    chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2;
-//    chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08;
-
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 100000;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 55;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 95;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 8;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 25;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12;
-//    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.7;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5;
-    chassis_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 300;
-
-    chassis_energy_part_param_.TWIN_ANGEL_MAX = 10;
-    chassis_energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 60;
-
-    chassis_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
-    chassis_energy_part_param_.TWIN_POINT_MAX = 20;
 }
 
 
@@ -237,12 +143,12 @@ void Energy::initEnergyPartParam() {
 // 此函数对能量机关旋转方向进行初始化
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::initRotation() {
-    if (target_polar_angle >= -180 && last_target_polar_angle >= -180
+    if (target_polar_angle >= -180 && last_target_polar_angle_judge_rotation >= -180
-        && fabs(target_polar_angle - last_target_polar_angle) < 30) {
+        && fabs(target_polar_angle - last_target_polar_angle_judge_rotation) < 30) {
-        //target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要
+        //target_polar_angle和last_target_polar_angle_judge_rotation的初值均为1000，大于-180表示刚开始几帧不要
         //若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断
-        if (target_polar_angle < last_target_polar_angle) clockwise_rotation_init_cnt++;
+        if (target_polar_angle < last_target_polar_angle_judge_rotation) clockwise_rotation_init_cnt++;
-        else if (target_polar_angle > last_target_polar_angle) anticlockwise_rotation_init_cnt++;
+        else if (target_polar_angle > last_target_polar_angle_judge_rotation) anticlockwise_rotation_init_cnt++;
     }
     //由于刚开始圆心判断不准，角度变化可能计算有误，因此需要在角度正向或逆向变化足够大时才可确定是否为顺逆时针
     if (clockwise_rotation_init_cnt == 15) {
@@ -254,5 +160,5 @@ void Energy::initRotation() {
         cout << "rotation: " << energy_rotation_direction << endl;
         energy_rotation_init = false;
     }
-    last_target_polar_angle = target_polar_angle;
+    last_target_polar_angle_judge_rotation = target_polar_angle;
 }

energy/src/energy/energy.cpp

@@ -21,8 +21,6 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color),
 
     is_big = false;
     is_small = false;
-    is_gimbal = true;
-    is_chassis = false;
 }
 
 
@@ -39,45 +37,6 @@ void Energy::setEnergyInit() {
     initEnergy();
     initEnergyPartParam();
 
-    is_big = false;
-    is_small = false;
-    is_gimbal = true;
-    is_chassis = false;
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数为大能量机关再初始化函数
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::setBigEnergyInit() {
-    initEnergy();
-    initEnergyPartParam();
-
-    is_big = true;
-    is_small = false;
-    is_gimbal = true;
-    is_chassis = false;
-
-    FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r");
-    if (fp) {
-        fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y);
-        fclose(fp);
-    }
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数为大能量机关再初始化函数
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::setSmallEnergyInit() {
-    initEnergy();
-    initEnergyPartParam();
-
-    is_big = false;
-    is_small = true;
-    is_gimbal = true;
-    is_chassis = false;
-
     FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r");
     if (fp) {
         fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y);

energy/src/energy/find/energy_finder.cpp

@@ -13,7 +13,7 @@ using std::vector;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于寻找图像内所有的大风车扇叶
 // ---------------------------------------------------------------------------------------------------------------------
-int Energy::findFans(const cv::Mat src) {
+int Energy::findFans(const cv::Mat &src) {
     if (src.empty()) {
         if (show_info) cout << "empty!" << endl;
         return 0;
@@ -33,21 +33,7 @@ int Energy::findFans(const cv::Mat src) {
             continue;
         }
         fans.emplace_back(cv::minAreaRect(fan_contour));
-
-//        RotatedRect cur_rect = minAreaRect(fan_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;
-//        double cur_contour_area = contourArea(fan_contour);
-//        float length_width_ratio = length / width;
-//        cout << "area: " << cur_contour_area << '\t' << endl;
-//        cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
-//        cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
-//        cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
-//        cout<<endl;
-
     }
-//    showFans("fan", src_bin);
     if (fans.size() < last_fans_cnt) {
         last_fans_cnt = static_cast<int>(fans.size());
         return -1;//寻找到的扇叶比上一帧少，说明该帧有误，返回-1
@@ -60,7 +46,7 @@ int Energy::findFans(const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于寻找图像内所有的大风车装甲板模块
 // ---------------------------------------------------------------------------------------------------------------------
-int Energy::findArmors(const cv::Mat src) {
+int Energy::findArmors(const cv::Mat &src) {
     if (src.empty()) {
         if (show_info) cout << "empty!" << endl;
         return 0;
@@ -96,19 +82,6 @@ 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;
-//        double cur_contour_area = contourArea(armor_contour);
-//        float length_width_ratio = length / width;
-//        cout << "area: " << cur_contour_area << '\t' << endl;
-//        cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
-//        cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
-//        cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
-//        cout<<endl;
-
     }
 
     if (show_info) {
@@ -122,7 +95,7 @@ int Energy::findArmors(const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于寻找图像内大风车中心字母“R”
 // ---------------------------------------------------------------------------------------------------------------------
-bool Energy::findCenterR(const cv::Mat src) {
+bool Energy::findCenterR(const cv::Mat &src) {
     if (src.empty()) {
         if (show_info) cout << "empty!" << endl;
         return false;
@@ -145,23 +118,9 @@ bool Energy::findCenterR(const cv::Mat src) {
                 target_armor.size.height > target_armor.size.width ? target_armor.size.height : target_armor.size.width;
         circle_center_point = centerR.center;
         circle_center_point.y += target_length / 7.5;//实际最小二乘得到的中心在R的下方
-
-//        RotatedRect cur_rect = minAreaRect(center_R_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;
-//        double cur_contour_area = contourArea(center_R_contour);
-//        float length_width_ratio = length / width;
-//        cout << "area: " << cur_contour_area << '\t' << endl;
-//        cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
-//        cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
-//        cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
-//        cout<<endl;
-
         return true;
     }
     if (show_info)cout << "find center R false!" << endl;
-//    cv::waitKey(0);
     return false;
 
 }
@@ -170,7 +129,7 @@ bool Energy::findCenterR(const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于判断找到的矩形候选区是否为含流动条的扇叶
 // ---------------------------------------------------------------------------------------------------------------------
-bool Energy::findFlowStripFan(const cv::Mat src) {
+bool Energy::findFlowStripFan(const cv::Mat &src) {
     if (src.empty()) {
         if (show_info) cout << "empty!" << endl;
         return false;
@@ -194,22 +153,9 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
             continue;
         }
         flow_strip_fans.emplace_back(cv::minAreaRect(flow_strip_fan_contour));
-
-//        RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour);
-//        Size2f cur_size = cur_rect.size;
-//        float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
-//        float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
-//        double cur_contour_area = contourArea(flow_strip_fan_contour);
-//        float length_width_ratio = length / width;
-//        cout << "area: " << cur_contour_area << '\t' << endl;
-//        cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
-//        cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
-//        cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
-//        cout<<endl;
     }
     if (flow_strip_fans.empty()) {
         if (show_info)cout << "flow strip fan false!" << endl;\
-//        waitKey(0);
         return false;
     }
     return true;
@@ -219,7 +165,7 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于寻找流动条
 // ---------------------------------------------------------------------------------------------------------------------
-bool Energy::findFlowStrip(const cv::Mat src) {
+bool Energy::findFlowStrip(const cv::Mat &src) {
     if (src.empty()) {
         if (show_info) cout << "empty!" << endl;
         return false;
@@ -268,125 +214,13 @@ bool Energy::findFlowStrip(const cv::Mat src) {
             } else {
                 continue;
             }
-
-//            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;
-//            double cur_contour_area = contourArea(flow_strip_contour);
-//            float length_width_ratio = length / width;
-//            cout << "area: " << cur_contour_area << '\t' << endl;
-//            cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
-//            cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
-//            cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
-//            cout<<endl;
         }
     }
     if (flow_strips.empty()) {
         if (show_info)cout << "flow strip false!" << endl;
-//        waitKey(0);
         return false;
     } else if (flow_strips.size() > 1) {
         if (show_info)cout << "Too many flow strips!" << endl;
-//        waitKey(0);
-        return false;
-    } else {
-        flow_strip = flow_strips.at(0);
-        for (auto &candidate_flow_strip_fan: flow_strip_fans) {
-            std::vector<cv::Point2f> intersection;
-            if (rotatedRectangleIntersection(flow_strip, candidate_flow_strip_fan, intersection) == 0) {
-                continue;
-            } else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
-                flow_strip_fan = candidate_flow_strip_fan;
-            }
-        }
-        int i = 0;
-        for (i = 0; i < target_armors.size(); ++i) {
-            std::vector<cv::Point2f> intersection;
-            if (rotatedRectangleIntersection(target_armors.at(i), flow_strip_fan, intersection) == 0)
-                continue;//返回0表示没有重合面积
-            double cur_contour_area = contourArea(intersection);
-            if (cur_contour_area > energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN) {
-                target_armor = target_armors.at(i);
-                target_point = target_armor.center;
-            }
-        }
-    }
-    return true;
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于寻找流动条
-// ---------------------------------------------------------------------------------------------------------------------
-bool Energy::findFlowStripSub(const cv::Mat src) {
-    if (src.empty()) {
-        if (show_info) cout << "empty!" << endl;
-        return false;
-    }
-    cv::Mat src_bin;
-    src_bin = src.clone();
-
-    if (src_bin.type() == CV_8UC1) // 黑白图像
-    {
-        cvtColor(src_bin, src_bin, COLOR_GRAY2RGB);
-
-    }
-    std::vector<cv::RotatedRect> candidate_target_armors = target_armors;
-    for (auto &candidate_target_armor: candidate_target_armors) {
-        Point2f vertices[4];
-        candidate_target_armor.size.height *= 1.3;
-        candidate_target_armor.size.width *= 1.3;
-        candidate_target_armor.points(vertices);   //计算矩形的4个顶点
-        for (int i = 0; i < 4; i++) {
-            line(src_bin, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20);
-        }
-    }
-
-    cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
-
-    FlowStripStructSub(src_bin);//图像膨胀，防止图像断开并更方便寻找
-    if (show_process)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);
-
-    for (auto candidate_flow_strip_fan: flow_strip_fans) {
-        for (auto &flow_strip_contour : flow_strip_contours) {
-            if (!isValidFlowStripContour(flow_strip_contour)) {
-                continue;
-            }
-
-            std::vector<cv::Point2f> intersection;
-            RotatedRect cur_rect = minAreaRect(flow_strip_contour);
-
-            if (rotatedRectangleIntersection(cur_rect, candidate_flow_strip_fan, intersection) == 0) {
-                continue;
-            } else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
-                flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour));
-//                cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl;
-            } else {
-                continue;
-            }
-
-//            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;
-//            double cur_contour_area = contourArea(flow_strip_contour);
-//            float length_width_ratio = length / width;
-//            cout << "area: " << cur_contour_area << '\t' << endl;
-//            cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
-//            cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
-//            cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
-//            cout<<endl;
-        }
-    }
-    if (flow_strips.empty()) {
-        if (show_info)cout << "flow strip false!" << endl;
-//        waitKey(0);
-        return false;
-    } else if (flow_strips.size() > 1) {
-        if (show_info)cout << "Too many flow strips!" << endl;
-//        waitKey(0);
         return false;
     } else {
         flow_strip = flow_strips.at(0);
@@ -417,7 +251,7 @@ bool Energy::findFlowStripSub(const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于弱识别寻找流动条
 // ---------------------------------------------------------------------------------------------------------------------
-bool Energy::findFlowStripWeak(const cv::Mat src) {
+bool Energy::findFlowStripWeak(const cv::Mat &src) {
     if (src.empty()) {
         if (show_info) cout << "empty!" << endl;
         return false;
@@ -445,7 +279,6 @@ bool Energy::findFlowStripWeak(const cv::Mat src) {
 
     FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("weak struct", src_bin);
-//    waitKey(0);
 
     std::vector<vector<Point> > flow_strip_contours;
     findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
@@ -454,24 +287,8 @@ bool Energy::findFlowStripWeak(const cv::Mat src) {
         if (!isValidFlowStripContour(flow_strip_contour)) {
             continue;
         }
-
         flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour));
-
-//        RotatedRect cur_rect = minAreaRect(flow_strip_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;
-//        double cur_contour_area = contourArea(flow_strip_contour);
-//        float length_width_ratio = length / width;
-//        cout << "area: " << cur_contour_area << '\t' << endl;
-//        cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
-//        cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
-//        cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
-//        cout << endl;
-
     }
-//    cout << "size: " << flow_strips.size() << endl;
-
     if (flow_strips.empty()) {
         if (show_info)cout << "weak flow strip false!" << endl;
 //        waitKey(0);
@@ -500,7 +317,6 @@ bool Energy::findFlowStripWeak(const cv::Mat src) {
             }
         }
         if (show_info)cout << "weak flow strip false!" << endl;
-//        waitKey(0);
         return false;
     }
 }
@@ -508,7 +324,7 @@ bool Energy::findFlowStripWeak(const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于框取中心R的寻找范围
 // ---------------------------------------------------------------------------------------------------------------------
-bool Energy::findCenterROI(const cv::Mat src) {
+bool Energy::findCenterROI(const cv::Mat &src) {
     float length = target_armor.size.height > target_armor.size.width ?
                    target_armor.size.height : target_armor.size.width;
 

energy/src/energy/find/target_finder.cpp

@@ -11,43 +11,6 @@ using std::endl;
 using std::vector;
 
 
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数根据矩形重合面积匹配扇叶与装甲板
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::findTargetByIntersection() {
-    if (fans.size() >= armors.size()) return;//扇叶多于装甲板，识别错误
-    for (const auto &fan : fans) {
-        for (const auto &armor : armors) {
-
-        }
-    }
-    /* if (fans.empty()) {
-         target_point = armors.at(0).rect.center;
-         return;
-     }
-     int i = 0, j = 0;
-     while (i < armors.size()) {
-         for (j = 0; j < fans.size(); ++j) {
-             std::vector<cv::Point2f> intersection;
-             if (rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积
-                 continue;
-             else
-                 rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection);
-             double cur_contour_area = contourArea(intersection);
-             if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) {
- //                    cout << endl;
- //                    cout << "NO. " << i << " armor and No. " << j << "fans are matched, the intersection area is"
- //                         << cur_contour_area << endl;
-                 break;
-             }
-         }
-         if (j == fans.size()) {
-             target_point = armors.at(i).rect.center;
-             break;
-         }
-         i++;
-     }*/
-}
 
 
 //----------------------------------------------------------------------------------------------------------------------

energy/src/energy/get/get_center.cpp

@@ -0,0 +1,34 @@
+//
+// Created by sun on 19-7-10.
+//
+#include "energy/energy.h"
+#include "log.h"
+#include "config/setconfig.h"
+
+
+using namespace std;
+using namespace cv;
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于对心
+// ---------------------------------------------------------------------------------------------------------------------
+void Energy::getCenter(){
+    int compensate_yaw = 0, compensate_pitch = 0;
+    if (mcu_data.enemy_color == ENEMY_BLUE) {
+        compensate_yaw = RED_COMPENSATE_YAW;
+        compensate_pitch = RED_COMPENSATE_PITCH;
+    } else if (mcu_data.enemy_color == ENEMY_RED) {
+        compensate_yaw = BLUE_COMPENSATE_YAW;
+        compensate_pitch = BLUE_COMPENSATE_PITCH;
+    }
+    double dx = -(circle_center_point.x - 320 - compensate_yaw);
+    double dy = -(circle_center_point.y - 240 - compensate_pitch);
+    yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
+    pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
+    if (abs(yaw_rotation) < 0.7 && abs(pitch_rotation) < 0.7) {
+        shoot = 2;
+    } else
+        shoot = 1;
+}

energy/src/energy/get/get_guess_point.cpp

@@ -52,7 +52,7 @@ bool Energy::guessTarget() {
         guess_polar_angle = base_angle + guess_devide * 72;
     }
     if (guess_polar_angle > 180)guess_polar_angle -= 360;
-    radius = pointDistance(target_point, circle_center_point);
+    double radius = pointDistance(target_point, circle_center_point);
     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;

energy/src/energy/get/get_polar_angle.cpp

@@ -16,13 +16,6 @@ void Energy::getTargetPolarAngle() {
 }
 
 
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::getAllTargetArmorCenters() {
-    all_target_armor_centers.emplace_back(target_point);
-}
-
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于存储近30帧图像中所有装甲板的中心坐标，用于判断小符和大符

energy/src/energy/get/gimble_rotation_get.cpp

@@ -1,50 +0,0 @@
-//
-// Created by xixiliadorabarry on 19-3-23.
-//
-#include "energy/energy.h"
-#include <cmath>
-#include "log.h"
-
-using namespace cv;
-using std::cout;
-using std::endl;
-using std::vector;
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于操作手手动标定
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::changeMark() {
-    if (mcu_data.mark == 0 && last_mark == 1) {//完成标定
-        last_mark = mcu_data.mark;
-        origin_yaw = mcu_data.curr_yaw;
-        origin_pitch = mcu_data.curr_pitch;
-        is_mark = false;
-        manual_mark = true;
-//        LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark"));
-    } else if (mcu_data.mark == 1) {//正在标定
-        last_mark = mcu_data.mark;
-        is_mark = true;
-//		LOGM(STR_CTR(WORD_BLUE,"Marking..."));
-
-    } else {//未在标定
-        last_mark = mcu_data.mark;
-        is_mark = false;
-    }
-    //cout<<"mark: "<<int(mcu_data.mark)<<endl;
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于计算云台应当转到的角度
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::gimbalRotation() {
-    cv::Point2f real_predict_point;//计算在真实世界中的预测点位，进而计算云台的旋转角度
-    stretch(predict_point, real_predict_point);
-    yaw_rotation = static_cast<float>(180 / PI *
-                                      atan2((attack_distance * tan(origin_yaw * PI / 180) - real_predict_point.x),
-                                            attack_distance));
-    pitch_rotation = static_cast<float>(180 / PI *
-                                        atan2((attack_distance * tan(origin_pitch * PI / 180) - real_predict_point.y),
-                                              attack_distance));
-}

energy/src/energy/get/origin_get.cpp

@@ -1,51 +0,0 @@
-//
-// Created by sun on 19-7-10.
-//
-#include "energy/energy.h"
-#include "log.h"
-#include "config/setconfig.h"
-
-
-using namespace std;
-using namespace cv;
-
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于对心
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::getCenter(){
-    double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW);
-    double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH);
-    yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
-    pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
-    if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) {
-        shoot = 4;
-    } else
-        shoot = 2;
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于获得云台对心得到的初始yaw和pitch（即以该yaw和pitch发射子弹，可以击中风车中心）
-// ---------------------------------------------------------------------------------------------------------------------
-bool Energy::getOrigin() {
-    if (!auto_mark && !manual_mark) {
-        double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW);
-        double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH);
-        center_delta_yaw = static_cast<float>(atan(dx / FOCUS_PIXAL) * 180 / PI);
-        center_delta_pitch = static_cast<float>(atan(dy / FOCUS_PIXAL) * 180 / PI);
-        shoot = 1;
-        sendEnergy();
-        if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) {
-            return false;
-        } else {
-            origin_yaw = mcu_data.curr_yaw;
-            origin_pitch = mcu_data.curr_pitch;
-            auto_mark = true;
-            LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
-            return true;
-        }
-    }
-    return true;
-}

energy/src/energy/judge/judge_shoot.cpp

@@ -10,34 +10,18 @@ using namespace std;
 using namespace cv;
 
 
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于判断世界坐标系下是否可以发弹
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::judgeShootInWorld() {
-    if (abs(yaw_rotation - mcu_data.curr_yaw) < 0.5 && abs(pitch_rotation - mcu_data.curr_pitch) < 0.5) {
-        shoot = 4;
-//        is_predicting = false;
-//        is_guessing = true;
-//        start_guess = true;
-//        getsystime(time_start_guess);
-        LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
-    } else
-        shoot = 2;
-}
-
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于判断云台坐标系下是否可以发弹
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::judgeShootInGimbal() {
+void Energy::judgeShoot() {
     if (abs(yaw_rotation) < 0.7 && abs(pitch_rotation) < 0.7) {
-        shoot = 4;
+        shoot = 2;
 //        is_predicting = false;
 //        is_guessing = true;
 //        start_guess = true;
 //        getsystime(time_start_guess);
 //        LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
     } else
-        shoot = 2;
+        shoot = 1;
-//    cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
 }

energy/src/energy/mark/mark.cpp

@@ -13,7 +13,7 @@ extern McuData mcu_data;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于记录操作手的微调dx和dy
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::writeDownSlightChange(cv::Mat &src) {
+void Energy::writeDownMark(cv::Mat &src) {
     if (fans_cnt >= 2) {
         FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w");
         if (fp_delta) {
@@ -43,29 +43,3 @@ void Energy::writeDownSlightChange(cv::Mat &src) {
         fclose(fp_data);
     }
 }
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于记录操作手手动标定的初始对心角度
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::writeDownMark(cv::Mat &src) {
-    if (findFans(src) >= 3) {
-        FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w");
-        if (fp) {
-            fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
-            fclose(fp);
-        }
-        FILE *fp_all = fopen(PROJECT_DIR"/Mark/mark_all.txt", "a");
-        if (fp_all) {
-            fprintf(fp_all, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
-            fclose(fp_all);
-        }
-    }
-    if (findFans(src) == 4) {
-        FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a");
-        if (fp_best) {
-            fprintf(fp_best, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
-            fclose(fp_best);
-        }
-    }
-}

energy/src/energy/run.cpp

@@ -9,150 +9,43 @@
 using namespace std;
 using namespace cv;
 
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数为能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
-    if (chassis_src.empty())
-        runBig(gimbal_src);//仅拥有云台摄像头则调用单摄像头的run函数
-    else if (is_gimbal) {
-        energy_part_param_ = gimbal_energy_part_param_;
-        clearAll();
-        initImage(gimbal_src);
-        camera_cnt = 2;
-
-        if (findArmors(gimbal_src) < 1)return;
-        if (show_energy)showArmors("armor", gimbal_src);
-        if (!findFlowStripFan(gimbal_src))return;
-        if (!findTargetInFlowStripFan()) return;
-        if (!findFlowStrip(gimbal_src))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);
-
-        if (!getOrigin())return;
-        startChassis();
-        initEnergy();
-        destroyAllWindows();
-    } else if (is_chassis) {
-        energy_part_param_ = chassis_energy_part_param_;
-        clearAll();
-        initImage(chassis_src);
-
-        changeMark();
-        if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch
-
-        if (findArmors(chassis_src) < 1)return;
-        if (show_energy)showArmors("armor", chassis_src);
-        if (!findFlowStripFan(chassis_src))return;
-        if (!findTargetInFlowStripFan()) return;
-        if (!findFlowStrip(gimbal_src))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();
-        if (is_big && energy_rotation_init) {
-            initRotation();
-            return;
-        }
-        if (save_mark)writeDownMark(chassis_src);
-        getPredictPoint(target_point);
-        gimbalRotation();
-        judgeShootInWorld();
-        sendEnergy();
-    }
-}
-
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数为能量机关模式主控制流函数，且步兵仅拥有云台摄像头
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::runBig(cv::Mat &gimbal_src) {
+void Energy::run(cv::Mat &src) {
-    energy_part_param_ = gimbal_energy_part_param_;
     clearAll();
-    initImage(gimbal_src);
+    initImage(src);
 
-    if (show_process)imshow("bin", gimbal_src);
+    if (show_process)imshow("bin", src);
-    if (findArmors(gimbal_src) < 1)return;
+    if (findArmors(src) < 1)return;
-    if (show_energy)showArmors("armor", gimbal_src);
+    if (show_energy)showArmors("armor", src);
-    if (!findFlowStripFan(gimbal_src)) {
+    if (!findFlowStripFan(src)) {
-        if (!findFlowStripWeak(gimbal_src)) return;
+        if (!findFlowStripWeak(src)) return;
     } else {
-        if (show_energy)showFlowStripFan("strip fan", gimbal_src);
+        if (show_energy)showFlowStripFan("strip fan", src);
         if (!findTargetInFlowStripFan()) return;
-            if(!findFlowStrip(gimbal_src)){
+        if (!findFlowStrip(src)) return;
-                if(!findFlowStripSub(gimbal_src)) {
-                    return;
     }
-            }
+    findCenterROI(src);
-    }
+    if (show_energy)showFlowStrip("strip", src);
-    if (!findCenterROI(gimbal_src))return;
+    if (!findCenterR(src)) return;
-    if (show_energy)showFlowStrip("strip", gimbal_src);
+    if (show_energy)showCenterR("R", src);
-    if (!findCenterR(gimbal_src))return;
+    fans_cnt = findFans(src);
-    if (show_energy)showCenterR("R", gimbal_src);
+    if (show_energy)showFans("fans", src);
-    fans_cnt = findFans(gimbal_src);
-    if (show_energy)showFans("fans", gimbal_src);
-
-//    getCenter();
-//    sendEnergy();
-//    return;
 
     changeTarget();
     getTargetPolarAngle();
 
-    if (energy_rotation_init) {
+    if (is_big && energy_rotation_init) {
         initRotation();
         return;
     }
     getPredictPoint(target_point);
     getAimPoint(predict_point);
-//    cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl;
+    judgeShoot();
-//    waitKey(0);
-    judgeShootInGimbal();
     sendEnergy();
-    if (save_mark)writeDownSlightChange(gimbal_src);
+    if (save_mark)writeDownMark(src);
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::runSmall(cv::Mat &gimbal_src) {
-    energy_part_param_ = gimbal_energy_part_param_;
-    clearAll();
-    initImage(gimbal_src);
-
-    if (show_process)imshow("bin", gimbal_src);
-    if (findArmors(gimbal_src) < 1)return;
-    if (show_energy)showArmors("armor", gimbal_src);
-    if (!findFlowStripFan(gimbal_src)) {
-        if (!findFlowStripWeak(gimbal_src)) return;
-    } else {
-        if (show_energy)showFlowStripFan("strip fan", gimbal_src);
-        if (!findTargetInFlowStripFan()) return;
-        if (!findFlowStrip(gimbal_src)) {
-            if(!findFlowStripSub(gimbal_src)) {
-                return;
-            }
-        }
-    }
-    if (show_energy)showTarget("target", gimbal_src);
-    fans_cnt = findFans(gimbal_src);
-    if (show_energy)showFans("fans", gimbal_src);
-
-//    getCenter();
-//    sendEnergy();
-//    return;
-
-    changeTarget();
-    getPredictPoint(target_point);
-    getAimPoint(predict_point);
-    judgeShootInGimbal();
-    sendEnergy();
-    if (save_mark)writeDownSlightChange(gimbal_src);
 }
 
 

energy/src/energy/send/send.cpp

@@ -14,127 +14,34 @@ using namespace std;
 // 此函数用于发送能量机关数据
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::sendEnergy() {
-    if (is_big) {
     sum_yaw += yaw_rotation;
     sum_pitch += pitch_rotation;
-        if (ROBOT_ID == 4) {
+    float yaw_I_component = YAW_AIM_KI * sum_yaw;
-            MINMAX(sum_yaw, -100, 100);
+    float pitch_I_component = PITCH_AIM_KI * sum_pitch;
-            MINMAX(sum_yaw, -100, 100);
-        } else if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
-            float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
-            float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
     MINMAX(yaw_I_component, -2, 2);
     MINMAX(pitch_I_component, -2, 2);
-        }
 
     double tmp_yaw = yaw_rotation;
     double tmp_pitch = pitch_rotation;
-        if (mcu_data.mark == 1) {
+    yaw_rotation = YAW_AIM_KP * yaw_rotation + YAW_AIM_KI * sum_yaw +
-            yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw +
+                   YAW_AIM_KD * (yaw_rotation - last_yaw);
-                           TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
+    pitch_rotation = PITCH_AIM_KP * pitch_rotation + PITCH_AIM_KI * sum_pitch +
-            pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch +
+                     PITCH_AIM_KD * (pitch_rotation - last_pitch);
-                             TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
-        } else {
-            yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw +
-                           BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
-            pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
-                             BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
-        }
 
-//        cout << "yaw: "<<BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
-//             << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
-//        cout << "pitch: "<<BIG_PITCH_AIM_KP * pitch_rotation << '\t' << BIG_PITCH_AIM_KI * sum_pitch << '\t'
-//             << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
-        if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
-            MINMAX(yaw_rotation, -6, 6);
-            MINMAX(pitch_rotation, -6, 6);
-        }
     last_yaw = tmp_yaw;
     last_pitch = tmp_pitch;
-    } else if (is_small) {
-        sum_yaw += yaw_rotation;
-        sum_pitch += pitch_rotation;
-        if (ROBOT_ID == 4) {
-            MINMAX(sum_yaw, -100, 100);
-            MINMAX(sum_yaw, -100, 100);
-        } else if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
-            float yaw_I_component = SMALL_YAW_AIM_KI * sum_yaw;
-            float pitch_I_component = SMALL_PITCH_AIM_KI * sum_pitch;
-            MINMAX(yaw_I_component, -2, 2);
-            MINMAX(pitch_I_component, -2, 2);
-        }
-
-        double tmp_yaw = yaw_rotation;
-        double tmp_pitch = pitch_rotation;
-        if (mcu_data.mark == 1) {
-            yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KI * sum_yaw +
-                           TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
-            pitch_rotation =
-                    TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KI * sum_pitch +
-                    TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
-        } else {
-            yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KI * sum_yaw +
-                           SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
-            pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KI * sum_pitch +
-                             SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
-        }
-        if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
-            MINMAX(yaw_rotation, -6, 6);
-            MINMAX(pitch_rotation, -6, 6);
-        }
-        last_yaw = tmp_yaw;
-        last_pitch = tmp_pitch;
-    }
-
 
     if (change_target) {
-        sendTarget(serial, yaw_rotation, pitch_rotation, 5, 0);
+        sendTarget(serial, yaw_rotation, pitch_rotation, 3, 0);//表示目标切换
     } else if (is_guessing) {
-        sendTarget(serial, yaw_rotation, pitch_rotation, 6, 0);
+        sendTarget(serial, yaw_rotation, pitch_rotation, 4, 0);//表示猜测模式
     } else {
-        sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);
+        sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);//跟随或发弹
     }
 
 }
 
 
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于发送数据给主控板
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::sendTarget(Serial &serial, float x, float y, float z) {
-    short x_tmp, y_tmp, z_tmp;
-    uint8_t buff[8];
-
-#ifdef WITH_COUNT_FPS
-    static auto last_time = time(nullptr);
-    static int fps = 0;
-    time_t t = time(nullptr);
-    if (last_time != t) {
-        last_time = t;
-        cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
-        curr_fps = fps;
-        fps = 0;
-    }
-    fps += 1;
-#endif
-
-    x_tmp = static_cast<short>(x * (32768 - 1) / 100);
-    y_tmp = static_cast<short>(y * (32768 - 1) / 100);
-    z_tmp = static_cast<short>(z * (32768 - 1) / 100);
-    buff[0] = 's';
-    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
-    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
-    buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
-    buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
-    buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
-    buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
-    buff[7] = 'e';
-    serial.WriteData(buff, sizeof(buff));
-    send_cnt += 1;
-//    LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
-}
-
-
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于发送数据给主控板
 // ---------------------------------------------------------------------------------------------------------------------

energy/src/energy/show/show.cpp

@@ -13,7 +13,7 @@ using std::vector;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有扇叶
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showFans(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::showFans(std::string windows_name, const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有装甲板
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showArmors(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;
 
@@ -60,74 +60,11 @@ void Energy::showArmors(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) // 黑白图像
-    {
-        cvtColor(src, image2show, COLOR_GRAY2RGB);
-
-    } else if (src.type() == CV_8UC3) //RGB 彩色
-    {
-        image2show = src.clone();
-    }
-    for (const auto &fan : fans) {
-        Point2f vertices[4];      //定义矩形的4个顶点
-        fan.points(vertices);   //计算矩形的4个顶点
-        for (int i = 0; i < 4; i++)
-            line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4);
-    }
-    for (const auto &armor : armors) {
-        Point2f vertices[4];      //定义矩形的4个顶点
-        armor.points(vertices);   //计算矩形的4个顶点
-        for (int i = 0; i < 4; i++) {
-            if (pointDistance(static_cast<cv::Point2f>(armor.center), target_point) < 5)
-                line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4);
-            else
-                line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4);
-        }
-        cv::Point2f point = armor.center;
-        cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径
-
-    }
-
-    imshow(windows_name, image2show);
-}
-
-
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数用于显示图像中目标装甲板
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::showTarget(std::string windows_name, const cv::Mat src) {
-    if (src.empty())return;
-    static Mat image2show;
-
-    if (src.type() == CV_8UC1) // 黑白图像
-    {
-        cvtColor(src, image2show, COLOR_GRAY2RGB);
-
-    } else if (src.type() == CV_8UC3) //RGB 彩色
-    {
-        image2show = src.clone();
-    }
-    for (const auto &armor : armors) {
-        if (pointDistance(armor.center, target_point) < energy_part_param_.TWIN_POINT_MAX) {
-            Point2f vertices[4];      //定义矩形的4个顶点
-            armor.points(vertices);   //计算矩形的4个顶点
-            for (int i = 0; i < 4; i++)
-                line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2);
-        }
-    }
-    imshow(windows_name, image2show);
-}
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有可能的风车中心候选区R
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showCenterR(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;
 
@@ -154,7 +91,7 @@ void Energy::showCenterR(std::string windows_name, const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中流动条扇叶
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showFlowStripFan(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;
 
@@ -176,7 +113,7 @@ void Energy::showFlowStripFan(std::string windows_name, const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中流动条扇叶
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showFlowStrip(std::string windows_name, const cv::Mat src) {
+void Energy::showFlowStrip(std::string windows_name, const cv::Mat &src) {
     if (src.empty())return;
     static Mat image2show;
 
@@ -214,7 +151,7 @@ void Energy::showFlowStrip(std::string windows_name, const cv::Mat src) {
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示猜测的下一个目标点
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::showGuessTarget(std::string windows_name, const cv::Mat src) {
+void Energy::showGuessTarget(std::string windows_name, const cv::Mat &src) {
     if (src.empty())return;
     static Mat image2show;
 

energy/src/energy/tool/tool.cpp

@@ -32,22 +32,6 @@ void Energy::rotate(cv::Point target_point) {
 }
 
 
-//----------------------------------------------------------------------------------------------------------------------
-// 此函数将像素差转换到实际距离差
-// ---------------------------------------------------------------------------------------------------------------------
-void Energy::stretch(cv::Point point_1, cv::Point2f &point_2) {
-    if (point_1 == circle_center_point) {
-//        cout<<"stretch wrong!"<<endl;
-        return;
-    }
-    double x_0 = point_1.x - circle_center_point.x;
-    double y_0 = point_1.y - circle_center_point.y;
-    double r_0 = sqrt(pow(x_0, 2) + pow(y_0, 2));
-    point_2.x = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0);
-    point_2.y = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0);
-}
-
-
 
 
 //----------------------------------------------------------------------------------------------------------------------

main.cpp

@@ -30,7 +30,7 @@ using namespace std;
 McuData mcu_data = {    // 单片机端回传结构体
         0,              // 当前云台yaw角
         0,              // 当前云台pitch角
-        BIG_ENERGY_STATE,    // 当前状态，自瞄-大符-小符
+        ARMOR_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
         0,              // 是否为反陀螺模式
         ENEMY_RED,      // 敌方颜色
@@ -38,11 +38,10 @@ McuData mcu_data = {    // 单片机端回传结构体
         0,              // 能量机关y轴补偿量
 };
 
-WrapperHead *video_gimbal = nullptr;    // 云台摄像头视频源
+WrapperHead *video = nullptr;    // 云台摄像头视频源
-WrapperHead *video_chassis = nullptr;   // 底盘摄像头视频源
 
 Serial serial(115200);                  // 串口对象
-uint8_t last_state = INIT_STATE;     // 上次状态，用于初始化
+uint8_t last_state = ARMOR_STATE;     // 上次状态，用于初始化
 // 自瞄主程序对象
 ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top);
 // 能量机关主程序对象
@@ -63,31 +62,21 @@ int main(int argc, char *argv[]) {
     while (true) {
         // 打开视频源
         if (from_camera) {
-            video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
+            video = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2);
-            video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
         } else {
-            video_gimbal = new VideoWrapper(PROJECT_DIR"/test_video/blue_small.avi");
+            video = new VideoWrapper(PROJECT_DIR"/video/blue_big.avi");
-            video_chassis = new VideoWrapper(PROJECT_DIR"/test_video/red_small.avi");
         }
-        if (video_gimbal->init()) {
+        if (video->init()) {
-            LOGM("video_gimbal source initialization successfully.");
+            LOGM("video_source initialization successfully.");
         } else {
-            LOGW("video_gimbal source unavailable!");
+            LOGW("video_source unavailable!");
-        }
-        if (video_chassis->init()) {
-            LOGM("video_chassis source initialization successfully.");
-        } else {
-            LOGW("video_chassis source unavailable!");
         }
 
         // 跳过前10帧噪声图像。
-        Mat gimbal_src, chassis_src;
+        Mat src;
         for (int i = 0; i < 10; i++) {
-            if (video_gimbal) {
+            if (video) {
-                video_gimbal->read(gimbal_src);
+                video->read(src);
-            }
-            if (video_chassis) {
-                video_chassis->read(chassis_src);
             }
         }
         bool ok = true;
@@ -95,71 +84,46 @@ int main(int argc, char *argv[]) {
         do {
             char curr_state = mcu_data.state;
             CNT_TIME("Total", {
-            if (curr_state == BIG_ENERGY_STATE) {//大能量机关模式
+            if (curr_state != ARMOR_STATE) {//大能量机关模式
-                if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化
+                if (last_state == ARMOR_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化
+                    destroyAllWindows();
+                    if (from_camera) {
+                        delete video;
+                        video = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2);
+                        if (video->init()) {
+                            LOGM("video_gimbal source initialization successfully.");
+                        } else {
+                            LOGW("video_gimbal source unavailable!");
+                        }
+                    }
+                    if(curr_state == BIG_ENERGY_STATE){
+                        energy.is_small = false;
+                        energy.is_big = true;
                         LOGM(STR_CTR(WORD_BLUE, "Start Big Energy!"));
-                    destroyAllWindows();
-                    if (from_camera) {
-                        delete video_gimbal;
-                        video_gimbal = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "armor"*/);
-                        if (video_gimbal->init()) {
-                            LOGM("video_gimbal source initialization successfully.");
-                        } else {
-                            LOGW("video_gimbal source unavailable!");
-                        }
-                    }
-//                    checkReconnect(video_chassis->read(chassis_src));
-                    energy.setBigEnergyInit();
-                }
-                last_state = curr_state;//更新上一帧状态
-                ok = checkReconnect(video_gimbal->read(gimbal_src));
-//                video_chassis->read(chassis_src);
-#ifdef GIMBAL_FLIP_MODE
-                flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
-#endif
-//#ifdef CHASSIS_FLIP_MODE
-//                flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE);
-//#endif
-//                if (!from_camera) extract(gimbal_src, chassis_src);
-//                if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频
-//                if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像
-                if (!from_camera) extract(gimbal_src);
-                if (save_video) saveVideos(gimbal_src);//保存视频
-                if (show_origin) showOrigin(gimbal_src);//显示原始图像
-//                energy.runBig(gimbal_src, chassis_src);
-                energy.runBig(gimbal_src);
                     } else if (curr_state == SMALL_ENERGY_STATE){
-                if (last_state != SMALL_ENERGY_STATE) {
+                        energy.is_small = true;
+                        energy.is_big = false;
                         LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!"));
-                    destroyAllWindows();
-                    if (from_camera) {
-                        delete video_gimbal;
-                        video_gimbal = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "armor"*/);
-                        if (video_gimbal->init()) {
-                            LOGM("video_gimbal source initialization successfully.");
-                        } else {
-                            LOGW("video_gimbal source unavailable!");
                     }
-                    }
+                    energy.setEnergyInit();
-                    energy.setSmallEnergyInit();
                 }
                 last_state = curr_state;//更新上一帧状态
-                ok = checkReconnect(video_gimbal->read(gimbal_src));
+                ok = checkReconnect(video->read(src));
 #ifdef GIMBAL_FLIP_MODE
-                flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
+                flip(src, src, GIMBAL_FLIP_MODE);
 #endif
-                if (!from_camera) extract(gimbal_src);
+                if (!from_camera) extract(src);
-                if (save_video) saveVideos(gimbal_src);//保存视频
+                if (save_video) saveVideos(src);//保存视频
-                if (show_origin) showOrigin(gimbal_src);//显示原始图像
+                if (show_origin) showOrigin(src);//显示原始图像
-                energy.runSmall(gimbal_src);
+                energy.run(src);
             }  else {                                         // 自瞄模式
                 if (last_state != ARMOR_STATE) {
                     LOGM(STR_CTR(WORD_RED, "Start Armor!"));
                     destroyAllWindows();
                     if (from_camera) {
-                        delete video_gimbal;
+                        delete video;
-                        video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
+                        video = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
-                        if (video_gimbal->init()) {
+                        if (video->init()) {
                             LOGM("video_gimbal source initialization successfully.");
                         } else {
                             LOGW("video_gimbal source unavailable!");
@@ -168,27 +132,25 @@ int main(int argc, char *argv[]) {
                 }
                 last_state = curr_state;
                 CNT_TIME(STR_CTR(WORD_GREEN, "read img"), {
-                    if(!checkReconnect(video_gimbal->read(gimbal_src))) continue;
+                    if(!checkReconnect(video->read(src))) continue;
                 });
 #ifdef GIMBAL_FLIP_MODE
-                flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE);
+                flip(src, src, GIMBAL_FLIP_MODE);
 #endif
 //                CNT_TIME("something whatever", {
-                    if (!from_camera) extract(gimbal_src);
+                    if (!from_camera) extract(src);
-                    if (save_video) saveVideos(gimbal_src);
+                    if (save_video) saveVideos(src);
-                    if (show_origin) showOrigin(gimbal_src);
+                    if (show_origin) showOrigin(src);
 //                });
                 CNT_TIME(STR_CTR(WORD_CYAN, "Armor Time"), {
-                    armor_finder.run(gimbal_src);
+                    armor_finder.run(src);
                 });
             }
 //            cv::waitKey(0);
             });
         } while (ok);
-        delete video_gimbal;
+        delete video;
-        video_gimbal = nullptr;
+        video = nullptr;
-        delete video_chassis;
-        video_chassis = nullptr;
         cout << "Program fails. Restarting" << endl;
     }
     return 0;

others/include/additions.h

@@ -29,17 +29,11 @@ bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect);
 
 bool checkReconnect(bool is_camera_connect);
 
-void saveVideos(const cv::Mat &gimbal_src, const cv::Mat &chassis_src);
+void saveVideos(const cv::Mat &src);
 
-void saveVideos(const cv::Mat &gimbal_src);
+void showOrigin(const cv::Mat &src);
 
-void showOrigin(const cv::Mat &gimbal_src, const cv::Mat &chassis_src);
+void extract(cv::Mat &src);
-
-void showOrigin(const cv::Mat &gimbal_src);
-
-void extract(cv::Mat &gimbal_src, cv::Mat &chassis_src);
-
-void extract(cv::Mat &gimbal_src);
 
 double getTimeIntervalms(const systime &now, const systime &last);
 

others/include/config/setconfig.h

@@ -30,52 +30,45 @@
 
 
 #ifndef ARMOR_CAMERA_GAIN
-    #define ARMOR_CAMERA_GAIN   (30)
+    #define ARMOR_CAMERA_GAIN   (64)
 #endif
 #ifndef ENERGY_CAMERA_GAIN
-    #define ENERGY_CAMERA_GAIN  (20)
+    #define ENERGY_CAMERA_GAIN  (30)
 #endif
 
-#ifndef ROBOT_ID
+#ifndef YAW_AIM_KD
-    #define ROBOT_ID (0)
+    #define YAW_AIM_KD (0.4)
+#endif
+#ifndef YAW_AIM_KP
+    #define YAW_AIM_KP (0.75)
+#endif
+#ifndef YAW_AIM_KI
+    #define YAW_AIM_KI (0.01)
+#endif
+#ifndef PITCH_AIM_KD
+    #define PITCH_AIM_KD (0.4)
+#endif
+#ifndef PITCH_AIM_KP
+    #define PITCH_AIM_KP (0.75)
+#endif
+#ifndef PITCH_AIM_KI
+    #define PITCH_AIM_KI (0.01)
 #endif
 
-#ifndef SMALL_YAW_AIM_KD
+#ifndef RED_COMPENSATE_YAW
-    #define SMALL_YAW_AIM_KD (0.35)
+    #define RED_COMPENSATE_YAW (5)
 #endif
-#ifndef SMALL_YAW_AIM_KP
+#ifndef RED_COMPENSATE_PITCH
-    #define SMALL_YAW_AIM_KP (0.85)
+    #define RED_COMPENSATE_PITCH (74)
 #endif
-#ifndef SMALL_PITCH_AIM_KD
+
-    #define SMALL_PITCH_AIM_KD (0.35)
+#ifndef BLUE_COMPENSATE_YAW
+    #define BLUE_COMPENSATE_YAW (5)
 #endif
-#ifndef SMALL_PITCH_AIM_KP
+#ifndef BLUE_COMPENSATE_PITCH
-    #define SMALL_PITCH_AIM_KP (0.85)
+    #define BLUE_COMPENSATE_PITCH (74)
-#endif
-#ifndef BIG_YAW_AIM_KD
-    #define BIG_YAW_AIM_KD (0.35)
-#endif
-#ifndef BIG_YAW_AIM_KP
-    #define BIG_YAW_AIM_KP (0.85)
-#endif
-#ifndef BIG_YAW_AIM_KI
-    #define BIG_YAW_AIM_KI (0)
-#endif
-#ifndef BIG_PITCH_AIM_KD
-    #define BIG_PITCH_AIM_KD (0.35)
-#endif
-#ifndef BIG_PITCH_AIM_KP
-    #define BIG_PITCH_AIM_KP (0.85)
-#endif
-#ifndef BIG_PITCH_AIM_KI
-    #define BIG_PITCH_AIM_KI (0)
-#endif
-#ifndef COMPENSATE_YAW
-    #define COMPENSATE_YAW (5)
-#endif
-#ifndef COMPENSATE_PITCH
-    #define COMPENSATE_PITCH (74)
 #endif
+
 #ifndef EXTRA_DELTA_X
     #define EXTRA_DELTA_X (0)
 #endif

others/include/constants.h

@@ -16,7 +16,6 @@
 #define BIG_ENERGY_STATE   'b'
 #define SMALL_ENERGY_STATE 's'
 #define ARMOR_STATE        'a'
-#define INIT_STATE          0
 
 #define FOCUS_PIXAL_8MM  (1488)
 #define FOCUS_PIXAL_5MM  (917)

others/src/additions.cpp

@@ -20,8 +20,7 @@
 using namespace std;
 using namespace cv;
 
-extern WrapperHead *video_gimbal;
+extern WrapperHead *video;
-extern WrapperHead *video_chassis;
 
 extern Serial serial;
 extern uint8_t last_state;
@@ -74,102 +73,44 @@ cv::VideoWriter initVideoWriter(const std::string &filename_prefix) {
     return video;
 }
 
-bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect) {
-    if (!is_camera_0_connect) {
-        int curr_gain = ((CameraWrapper* )video_gimbal)->gain;
-        int curr_exposure = ((CameraWrapper* )video_gimbal)->exposure;
-        delete video_gimbal;
-        video_gimbal = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
-        is_camera_0_connect = video_gimbal->init();
-    }
-    if (!is_camera_1_connect) {
-        int curr_gain = ((CameraWrapper* )video_gimbal)->gain;
-        int curr_exposure = ((CameraWrapper* )video_gimbal)->exposure;
-        delete video_chassis;
-        video_chassis = new CameraWrapper(curr_exposure, curr_gain, 0/*, "energy"*/);
-        is_camera_1_connect = video_chassis->init();
-    }
-    return is_camera_0_connect && is_camera_1_connect;
-}
-
 bool checkReconnect(bool is_camera_connect) {
     if (!is_camera_connect) {
-        int curr_gain = ((CameraWrapper* )video_gimbal)->gain;
+        int curr_gain = ((CameraWrapper* )video)->gain;
-        int curr_exposure = ((CameraWrapper* )video_gimbal)->exposure;
+        int curr_exposure = ((CameraWrapper* )video)->exposure;
-        delete video_gimbal;
+        delete video;
-        video_gimbal = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
+        video = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
-        is_camera_connect = video_gimbal->init();
+        is_camera_connect = video->init();
     }
     return is_camera_connect;
 }
 
-auto gimbal_video_writer = initVideoWriter(PROJECT_DIR"/gimbal_video/");
+auto video_writer = initVideoWriter(PROJECT_DIR"/video/");
-auto chassis_video_writer = initVideoWriter(PROJECT_DIR"/chassis_video/");
-
-void saveVideos(const cv::Mat &gimbal_src, const cv::Mat &chassis_src) {
-    if (!gimbal_src.empty() && !chassis_src.empty()) {
-        gimbal_video_writer.write(gimbal_src);
-        Mat chassis_save = chassis_src.clone();
-        cvtColor(chassis_save, chassis_save, COLOR_GRAY2BGR);
-        chassis_video_writer.write(chassis_save);
-    } else if (!gimbal_src.empty() && chassis_src.empty()) {
-        gimbal_video_writer.write(gimbal_src);
-    } else if (gimbal_src.empty() && !chassis_src.empty()) {
-        if (show_origin)imshow("src", gimbal_src);
-        Mat chassis_save = chassis_src.clone();
-        cvtColor(chassis_save, chassis_save, COLOR_GRAY2BGR);
-        chassis_video_writer.write(chassis_save);
-    } else return;
-}
 
 void saveVideos(const cv::Mat &gimbal_src) {
     if (!gimbal_src.empty()) {
-        gimbal_video_writer.write(gimbal_src);
+        video_writer.write(gimbal_src);
     } else return;
 }
 
-void showOrigin(const cv::Mat &gimbal_src, const cv::Mat &chassis_src) {
+void showOrigin(const cv::Mat &src) {
-    if (!gimbal_src.empty() && !chassis_src.empty()) {
+    if (!src.empty()) {
-        imshow("gimbal", gimbal_src);
+        imshow("origin", src);
-        imshow("chassis", chassis_src);
-    } else if (!gimbal_src.empty() && chassis_src.empty()) {
-        imshow("gimbal", gimbal_src);
-    } else if (gimbal_src.empty() && !chassis_src.empty()) {
-        imshow("chassis", chassis_src);
     } else return;
     cv::waitKey(1);
 }
 
-void showOrigin(const cv::Mat &gimbal_src) {
+void extract(cv::Mat &src) {//图像预处理，将视频切成640×480的大小
-    if (!gimbal_src.empty()) {
+    if (src.empty()) return;
-        imshow("gimbal", gimbal_src);
+    float length = static_cast<float>(src.cols);
-    } else return;
+    float width = static_cast<float>(src.rows);
-    cv::waitKey(1);
-}
-
-void extract(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
-    if (!gimbal_src.empty() && !chassis_src.empty()) {
-        extract(gimbal_src);
-        extract(chassis_src);
-    } else if (!gimbal_src.empty() && chassis_src.empty()) {
-        extract(gimbal_src);
-    } else if (gimbal_src.empty() && !chassis_src.empty()) {
-        extract(chassis_src);
-    } else return;
-}
-
-void extract(cv::Mat &gimbal_src) {//图像预处理，将视频切成640×480的大小
-    if (gimbal_src.empty()) return;
-    float length = static_cast<float>(gimbal_src.cols);
-    float width = static_cast<float>(gimbal_src.rows);
     if (length / width > 640.0 / 480.0) {
         length *= 480.0 / width;
-        resize(gimbal_src, gimbal_src, cv::Size(length, 480));
+        resize(src, src, cv::Size(length, 480));
-        gimbal_src = gimbal_src(Rect((length - 640) / 2, 0, 640, 480));
+        src = src(Rect((length - 640) / 2, 0, 640, 480));
     } else {
         width *= 640.0 / length;
-        resize(gimbal_src, gimbal_src, cv::Size(640, width));
+        resize(src, src, cv::Size(640, width));
-        gimbal_src = gimbal_src(Rect(0, (width - 480) / 2, 640, 480));
+        src = src(Rect(0, (width - 480) / 2, 640, 480));
     }
 }