optimize anti-top

.gitignore

@@ -7,6 +7,7 @@ tools/TrainCNN/.idea
 tools/TrainCNN/model
 tools/TrainCNN/__pycache__
 others/include/config/config.h
+others/MV-UB31-Group0.config
 .DS_Store
-gimble_video
+gimbal_video
 chassis_video

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -6,9 +6,17 @@
 #include <additions/additions.h>
 #include <log.h>
 
+static double boxDistance(const cv::Rect2d &a, const cv::Rect2d &b){
+    cv::Point2d centerA(a.x+a.width/2, a.y+a.height/2);
+    cv::Point2d centerB(b.x+b.width/2, b.y+b.height/2);
+    auto dist = centerA-centerB;
+    return sqrt(dist.x*dist.x + dist.y*dist.y);
+}
+
 void ArmorFinder::antiTop() {
     static double top_periodms = 0;
     static double last_top_periodms = 0;
+    static cv::Rect2d last_pos;
     uint16_t shoot_delay = 0;
     timeval curr_time;
 //    if(anti_top_state == ANTI_TOP){
@@ -31,8 +39,8 @@ void ArmorFinder::antiTop() {
         return;
     }
 
-
-    if(orientation!=last_orient && orientation==ArmorBox::FRONT){
+    auto dist = boxDistance(last_pos, armor_box.rect);
+    if(orientation!=last_orient && orientation==ArmorBox::FRONT && dist>=6){
         last_front_time = curr_time;
         if(150<interval && interval<700){
             if(anti_top_state == ANTI_TOP){

main.cpp

@@ -20,7 +20,7 @@
 #include <additions/additions.h>
 #include <config/setconfig.h>
 
-//#define DO_NOT_CNT_TIME
+#define DO_NOT_CNT_TIME
 
 #include <log.h>