From 2f39c61bf1b920a07c8b9d178490108e5e0d5c33 Mon Sep 17 00:00:00 2001
From: JiatongSun <SJT_sjtu1998@163.com>
Date: Fri, 9 Aug 2019 02:14:33 +0800
Subject: [PATCH] anti top v2.1

---
 armor/include/armor_finder/armor_finder.h    |  1 +
 armor/src/armor_finder/anti_top/anti_top.cpp | 11 +++++++++--
 armor/src/armor_finder/armor_finder.cpp      |  1 +
 main.cpp                                     |  6 +++---
 4 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h
index 600d7cc..39c00e1 100644
--- a/armor/include/armor_finder/armor_finder.h
+++ b/armor/include/armor_finder/armor_finder.h
@@ -115,6 +115,7 @@ private:
     int tracking_cnt;                                   // 记录追踪帧数，用于定时退出追踪
     Serial &serial;                                     // 串口对象，引用外部变量，用于和能量机关共享同一个变量
     systime last_front_time;                            // 上次陀螺正对时间
+    int anti_top_cnt;
     RoundQueue<double, 4> top_periodms;                 // 陀螺周期循环队列
     vector<systime> time_seq;                           // 一个周期内的时间采样点
     vector<float> angle_seq;                            // 一个周期内的角度采样点
diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp
index f51a2ce..60e506b 100644
--- a/armor/src/armor_finder/anti_top/anti_top.cpp
+++ b/armor/src/armor_finder/anti_top/anti_top.cpp
@@ -32,7 +32,6 @@ static systime getFrontTime(const vector<systime> time_seq, const vector<float>
 
 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);
@@ -40,13 +39,20 @@ void ArmorFinder::antiTop() {
 //            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);
+        if (anti_top_cnt < 4) {
+            sendBoxPosition(0);
+        } else if (abs(once_periodms - top_periodms[-1]) > 50) {
+            sendBoxPosition(0);
+        } else {
+            sendBoxPosition(shoot_delay);
+        }
         time_seq.clear();
         angle_seq.clear();
         last_front_time = front_time;
@@ -57,5 +63,6 @@ void ArmorFinder::antiTop() {
         angle_seq.emplace_back(yaw);
         sendBoxPosition(0);
     }
+    anti_top_cnt++;
 }
 
diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp
index 43c48c7..2d801e9 100644
--- a/armor/src/armor_finder/armor_finder.cpp
+++ b/armor/src/armor_finder/armor_finder.cpp
@@ -93,6 +93,7 @@ end:
     if(is_anti_top) {
         antiTop();
     }else if(target_box.rect != cv::Rect2d()) {
+        anti_top_cnt = 0;
         time_seq.clear();
         angle_seq.clear();
         sendBoxPosition(0);
diff --git a/main.cpp b/main.cpp
index 488a78c..b1a75f0 100644
--- a/main.cpp
+++ b/main.cpp
@@ -32,7 +32,7 @@ McuData mcu_data = {    // 单片机端回传结构体
         0,              // 当前云台pitch角
         ARMOR_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
-        1,              // 是否启用数字识别
+        0,              // 是否为反陀螺模式
         ENEMY_RED,      // 敌方颜色
         0,              // 能量机关x轴补偿量
         0,              // 能量机关y轴补偿量
@@ -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"/test_video/blue_big.avi");
-            video_chassis = new VideoWrapper(PROJECT_DIR"/test_video/blue_big.avi");
+            video_gimbal = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi");
+            video_chassis = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");