更新反陀螺和分类器参数。

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -3,12 +3,15 @@
 //
 
 #include <armor_finder/armor_finder.h>
+#include <log.h>
 
 static double getTimeIntervalms(const timeval& now, const timeval &last){
     return (now.tv_sec-last.tv_sec)*1000.0 + (now.tv_usec-last.tv_usec)/1000.0;
 }
 
 void ArmorFinder::antiTop() {
+    double top_periodms = 0;
+    timeval curr_time;
     if(anti_top_state == ANTI_TOP){
         cout << "anti top" << endl;
     }else if(anti_top_state == NORMAL){
@@ -23,23 +26,26 @@ void ArmorFinder::antiTop() {
             return;
         }
     }
-    if(orientation!=last_orient){
-        timeval curr_time;
+    if(orientation != last_orient){
         gettimeofday(&curr_time, nullptr);
-        auto interval = getTimeIntervalms(curr_time, last_switch_time);
-        cout << interval << endl;
-        if(50 < interval && interval < 700){
-            anti_top_cnt++;
-        }else{
+        auto interval = getTimeIntervalms(curr_time, last_front_time);
+        if(interval > 700){
             anti_top_cnt = 0;
-        }
-        last_switch_time = curr_time;
-    }else{
-        timeval curr_time;
-        gettimeofday(&curr_time, nullptr);
-        if(getTimeIntervalms(curr_time, last_switch_time) > 700) {
             anti_top_state = NORMAL;
+            if(orientation == ArmorBox::FRONT){
+                last_front_time = curr_time;
+            }
+        }else if(interval > 150){
+            if(orientation == ArmorBox::FRONT){
+                anti_top_cnt++;
+                if(anti_top_state == ANTI_TOP){
+                    top_periodms = interval;
+                    LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms);
+                }
+                last_front_time = curr_time;
+            }
         }
+        last_orient = orientation;
     }
     if(anti_top_cnt > 4){
         anti_top_state = ANTI_TOP;

armor/src/armor_finder/armor_box/armor_box.cpp

@@ -53,9 +53,11 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
     switch (id) {
         case R1:
         case R7:
+        case R8:
         case B1:
         case B7:
-            if(lengthDistanceRatio() < 0.3){
+        case B8:
+            if(lengthDistanceRatio() < 0.30){
                 return FRONT;
             }else{
                 return SIDE;
@@ -68,7 +70,7 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
         case B3:
         case B4:
         case B5:
-            if (lengthDistanceRatio() < 0.43) {
+            if (lengthDistanceRatio() < 0.45) {
                 return FRONT;
             }else{
                 return SIDE;

armor/src/armor_finder/send_target/send_target.cpp

@@ -35,6 +35,7 @@ static bool sendTarget(Serial &serial, double x, double y, double z) {
 }
 
 bool ArmorFinder::sendBoxPosition() {
+    if(armor_box.rect == cv::Rect2d()) return false;
     auto rect = armor_box.rect;
     double dx = rect.x + rect.width / 2 - 320;
     double dy = rect.y + rect.height / 2 - 240 - 20;