反陀螺v2.0

armor/include/armor_finder/armor_finder.h

@@ -93,7 +93,7 @@ typedef std::vector<ArmorBox> ArmorBoxes;
 /********************* 自瞄类定义 **********************/
 class ArmorFinder{
 public:
-    ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder);
+    ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder, const uint8_t &anti_top);
     ~ArmorFinder() = default;
 
 private:
@@ -103,24 +103,21 @@ private:
         SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
     } State;                                            // 自瞄状态枚举定义
 
-    typedef enum{
+    systime frame_time;                                 // 当前帧对应时间
-        NORMAL, ANTI_TOP
-    } AntiTopState;
-
-    systime frame_time;                                 // 当前帧对应时间;
     const uint8_t &enemy_color;                         // 敌方颜色，引用外部变量，自动变化
+    const uint8_t &is_anti_top;                         // 进入反陀螺，引用外部变量，自动变化
     State state;                                        // 自瞄状态对象实例
     ArmorBox target_box, last_box;                      // 目标装甲板
     int anti_switch_cnt;                                // 防止乱切目标计数器
     cv::Ptr<cv::Tracker> tracker;                       // tracker对象实例
     Classifier classifier;                              // CNN分类器对象实例，用于数字识别
-    int contour_area;                                  // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
+    int contour_area;                                   // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
     int tracking_cnt;                                   // 记录追踪帧数，用于定时退出追踪
     Serial &serial;                                     // 串口对象，引用外部变量，用于和能量机关共享同一个变量
+    systime last_front_time;                            // 上次陀螺正对时间
     RoundQueue<double, 4> top_periodms;                 // 陀螺周期循环队列
-    systime last_front_time;                            // 上一次发生装甲板方向切换的时间
+    vector<systime> time_seq;                           // 一个周期内的时间采样点
-    int anti_top_cnt;                                   // 满足条件的装甲板方向切换持续次数，用于反陀螺
+    vector<float> angle_seq;                            // 一个周期内的角度采样点
-    AntiTopState anti_top_state;                        // 当前是否识别到陀螺
 
     bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
     bool findArmorBox(const cv::Mat &src, ArmorBox &box);

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -15,38 +15,47 @@ static double mean(RoundQueue<double, length> &vec) {
     return sum / length;
 }
 
-void ArmorFinder::antiTop() {
+static systime getFrontTime(const vector<systime> time_seq, const vector<float> angle_seq) {
-    if (target_box.rect == cv::Rect2d()) return;
+    double A = 0, B = 0, C = 0, D = 0;
-    uint16_t shoot_delay = 0;
+    int len = time_seq.size();
-    auto interval = getTimeIntervalms(frame_time, last_front_time);
+    for (int i = 0; i < len; i++) {
-    if (anti_top_state == ANTI_TOP && interval > 700) {
+        A += angle_seq[i] * angle_seq[i];
-        anti_top_state = NORMAL;
+        B += angle_seq[i];
-        LOGM(STR_CTR(WORD_YELLOW, "switch to normal"));
+        C += angle_seq[i] * time_seq[i];
+        D += time_seq[i];
+        cout << "(" << angle_seq[i] << ", " << time_seq[i] << ") ";
+    }
+    double b = (A * D - B * C) / (len * A - B * B);
+    cout << b << endl;
+    return b;
+}
+
+void ArmorFinder::antiTop() {
+    if (target_box.rect == cv::Rect2d()) return;
+
+    if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
+        auto front_time = getFrontTime(time_seq, angle_seq);
+        auto once_periodms = getTimeIntervalms(front_time, last_front_time);
+//        if (abs(once_periodms - top_periodms[-1]) > 50) {
+//            sendBoxPosition(0);
+//            return;
+//        }
+        LOGM(STR_CTR(WORD_GREEN, "Top period: %.1lf"), once_periodms);
+        top_periodms.push(once_periodms);
+        auto periodms = mean(top_periodms);
+        systime curr_time;
+        getsystime(curr_time);
+        uint16_t shoot_delay = front_time + periodms * 2 - curr_time;
+        sendBoxPosition(shoot_delay);
+        time_seq.clear();
+        angle_seq.clear();
+        last_front_time = front_time;
+    } else {
+        time_seq.emplace_back(frame_time);
+        double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+        double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+        angle_seq.emplace_back(yaw);
+        sendBoxPosition(0);
     }
-    if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
-        if (150 < interval && interval < 700) {
-            if (anti_top_state == ANTI_TOP) {
-                top_periodms.push(interval);
-                LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), interval);
-                systime curr_time;
-                getsystime(curr_time);
-                auto calculate_time = getTimeIntervalms(curr_time, frame_time);
-                shoot_delay = mean(top_periodms) - calculate_time;
-                sendBoxPosition(shoot_delay);
-            } else {
-                if (++anti_top_cnt > 4) {
-                    anti_top_state = ANTI_TOP;
-                    LOGM(STR_CTR(WORD_CYAN, "switch to anti-top"));
-                }
-            }
-        }
-        last_front_time = frame_time;
-    }
-    if (anti_top_state == NORMAL) {
-        sendBoxPosition(0);
-    } else if (interval < top_periodms[-1] * 0.1){
-        sendBoxPosition(shoot_delay);
-    }
-    last_box = target_box;
 }
 

armor/src/armor_finder/armor_finder.cpp

@@ -46,13 +46,12 @@ std::map<string, int> prior_red = {
         {"NO", 10},
 };
 
-ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder) :
+ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder, const uint8_t &anti_top) :
         serial(u),
         enemy_color(color),
+        is_anti_top(anti_top),
         state(STANDBY_STATE),
-        anti_top_cnt(0),
         anti_switch_cnt(0),
-        anti_top_state(NORMAL),
         classifier(paras_folder),
         contour_area(0),
         tracking_cnt(0) {
@@ -91,9 +90,18 @@ void ArmorFinder::run(cv::Mat &src) {
             stateStandBy();
     }
 end:
-//    antiTop();
+    if(is_anti_top) {
-    if(target_box.rect != cv::Rect2d())
+        antiTop();
+    }else if(target_box.rect != cv::Rect2d()) {
+        time_seq.clear();
+        angle_seq.clear();
         sendBoxPosition(0);
+    }
+
+    if(target_box.rect != cv::Rect2d()){
+        last_box = target_box;
+    }
+
     if (show_armor_box) {                 // 根据条件显示当前目标装甲板
         showArmorBox("box", src, target_box);
         cv::waitKey(1);

main.cpp

@@ -32,7 +32,7 @@ McuData mcu_data = {    // 单片机端回传结构体
         0,              // 当前云台pitch角
         ARMOR_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
-        1,              // 是否启用数字识别
+        0,              // 是否为反陀螺模式
         ENEMY_RED,      // 敌方颜色
         0,              // 能量机关x轴补偿量
         0,              // 能量机关y轴补偿量
@@ -44,7 +44,7 @@ WrapperHead *video_chassis = nullptr;   // 底盘摄像头视频源
 Serial serial(115200);                  // 串口对象
 uint8_t last_state = INIT_STATE;     // 上次状态，用于初始化
 // 自瞄主程序对象
-ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/");
+ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top);
 // 能量机关主程序对象
 Energy energy(serial, mcu_data.enemy_color);
 
@@ -66,8 +66,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
             video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
         } else {
-            video_gimbal = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi");
+            video_gimbal = new VideoWrapper(PROJECT_DIR"/gimbal_video/1.avi");
-            video_chassis = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi");
+            video_chassis = new VideoWrapper(PROJECT_DIR"/gimbal_video/1.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");

others/include/additions.h

@@ -15,7 +15,7 @@ struct McuData {
     float curr_pitch;
     uint8_t state;
     uint8_t mark;
-    uint8_t use_classifier;
+    uint8_t anti_top;
     uint8_t enemy_color;
     int delta_x;
     int delta_y;
@@ -41,7 +41,7 @@ void extract(cv::Mat &gimbal_src, cv::Mat &chassis_src);
 
 void extract(cv::Mat &gimbal_src);
 
-float getTimeIntervalms(const systime &now, const systime &last);
+double getTimeIntervalms(const systime &now, const systime &last);
 
 double getPointLength(const cv::Point2f &p);
 

others/include/systime.h

@@ -5,12 +5,15 @@
 #ifndef _PLATFORM_H_
 #define _PLATFORM_H_
 
-typedef struct{
+//typedef struct{
-    int second;
+//    float second;
-    int millisecond;
+//    float millisecond;
-} systime;
+//} systime;
+
+typedef double systime;
 
 void getsystime(systime &t);
+double getTimeIntervalms(const systime &now, const systime &last);
 
 #if defined(Linux) || defined(Darwin)
     #include <sys/time.h>

others/src/additions.cpp

@@ -173,10 +173,6 @@ void extract(cv::Mat &gimbal_src) {//图像预处理，将视频切成640×480
     }
 }
 
-float getTimeIntervalms(const systime &now, const systime &last){
-    return (now.second-last.second)*1000.0 + (now.millisecond-last.millisecond);
-}
-
 double getPointLength(const cv::Point2f &p) {
     return sqrt(p.x * p.x + p.y * p.y);
 }

others/src/systime.cpp

@@ -5,11 +5,17 @@
 
 #if defined(Linux) || defined(Darwin)
 
-void getsystime(systime &t){
+static systime getsystime(){
     timeval tv;
     gettimeofday(&tv, nullptr);
-    t.second = tv.tv_sec;
+    return tv.tv_usec / 1000.0 + tv.tv_sec * 1000.0;
-    t.millisecond = tv.tv_usec/1000;
+}
+
+void getsystime(systime &t) {
+    static systime time_base = getsystime();
+    timeval tv;
+    gettimeofday(&tv, nullptr);
+    t = tv.tv_usec / 1000.0 + tv.tv_sec * 1000.0 - time_base;
 }
 
 #elif defined(Windows)
@@ -17,10 +23,14 @@ void getsystime(systime &t){
 void getsystime(systime &t){
     SYSTEMTIME tv;
     GetLocalTime(&tv);
-    t.second = tv.wSecond;
+    t = tv.wMilliseconds + tv.wSecond * 1000.0;
-    t.millisecond = tv.wMilliseconds;
 }
 
 #else
-    #error "nonsupport platform."
+#error "nonsupport platform."
 #endif
+
+
+double getTimeIntervalms(const systime &now, const systime &last) {
+    return now - last;
+}