energy changed

energy/include/energy/constant.h

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

energy/include/energy/energy.h

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