反陀螺v1.0完成。

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -4,49 +4,56 @@
 
 #include <armor_finder/armor_finder.h>
 
-#define         B1 1
-#define         B2 2
-#define         B3 3
-#define         B4 4
-#define         B5 5
-#define         B7 6
-#define         B8 7
-#define         R1 8
-#define         R2 9
-#define         R3 10
-#define         R4 11
-#define         R5 12
-#define         R7 13
-#define         R8 14
-
-
-void ArmorFinder::anti_top() {
-//    switch (armor_box.id) {
-//        case R1:
-//        case R7:
-//        case B1:
-//        case B7:
-//            if (armor_box.rect != cv::Rect2d()) {
-//                sendBoxPosition();
-//            }
-//        case R2:
-//        case R3:
-//        case R4:
-//        case R5:
-//        case B2:
-//        case B3:
-//        case B4:
-//        case B5:
-//            if (armor_box.rect != cv::Rect2d()) {
-//                sendBoxPosition();
-//            }
-//        default:
-//            if (armor_box.rect != cv::Rect2d()) {
-//                sendBoxPosition();
-//            }
-//    }
-    if (armor_box.rect != cv::Rect2d()) {
-        sendBoxPosition();
-    }
+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() {
+    if(anti_top_state == ANTI_TOP){
+        cout << "anti top" << endl;
+    }else if(anti_top_state == NORMAL){
+        cout << "Normal" << endl;
+    }
+    ArmorBox::BoxOrientation orientation = armor_box.getOrientation();
+    if(orientation == ArmorBox::UNKNOWN){
+        if(anti_top_state == NORMAL){
+            sendBoxPosition();
+            return;
+        }else{
+            return;
+        }
+    }
+    if(orientation!=last_orient){
+        timeval curr_time;
+        gettimeofday(&curr_time, nullptr);
+        auto interval = getTimeIntervalms(curr_time, last_switch_time);
+        cout << interval << endl;
+        if(50 < interval && interval < 700){
+            anti_top_cnt++;
+        }else{
+            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(anti_top_cnt > 4){
+        anti_top_state = ANTI_TOP;
+    }
+
+    if(anti_top_state == ANTI_TOP){
+        if(orientation == ArmorBox::FRONT){
+            sendBoxPosition();
+        }
+    }else if(anti_top_state == NORMAL){
+        sendBoxPosition();
+    }
+
+
+    last_orient = orientation;
 }
 

armor/src/armor_finder/armor_box/armor_box.cpp

@@ -36,3 +36,44 @@ double ArmorBox::lengthDistanceRatio() const {
         return 100;
     }
 }
+
+double ArmorBox::getDistance() const{
+    if(light_blobs.size() == 2 ){
+        return DISTANCE_HEIGHT / 2 / max(light_blobs[0].length, light_blobs[1].length);
+    } else {
+        return DISTANCE_HEIGHT / rect.height;
+    }
+}
+
+ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
+//    cout << lengthDistanceRatio() << endl;
+    if(light_blobs.size() != 2){
+        return UNKNOWN;
+    }
+    switch (id) {
+        case R1:
+        case R7:
+        case B1:
+        case B7:
+            if(lengthDistanceRatio() < 0.3){
+                return FRONT;
+            }else{
+                return SIDE;
+            }
+        case R2:
+        case R3:
+        case R4:
+        case R5:
+        case B2:
+        case B3:
+        case B4:
+        case B5:
+            if (lengthDistanceRatio() < 0.43) {
+                return FRONT;
+            }else{
+                return SIDE;
+            }
+        default:
+            return UNKNOWN;
+    }
+}

armor/src/armor_finder/armor_finder.cpp

@@ -78,7 +78,7 @@ void ArmorFinder::run(cv::Mat &src) {
             stateStandBy();
     }
 
-    anti_top();
+    antiTop();
 
     if (show_armor_box) {                 // 根据条件显示当前目标装甲板
         showArmorBox("box", src, armor_box);

armor/src/armor_finder/find/find_armor_box.cpp

@@ -72,20 +72,11 @@ static bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &light_
 }
 
 static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) {
-//    cout << (light_blob_i.blob_color==BLOB_RED) << endl;
-//    cout << (light_blob_j.blob_color==BLOB_RED) << endl;
-//    cout << lengthRatioJudge(light_blob_i, light_blob_j) << endl;
-//    cout << lengthJudge(light_blob_i, light_blob_j) << endl;
-////    cout << heightJudge(light_blob_i, light_blob_j) << endl;
-//    cout << angelJudge(light_blob_i, light_blob_j) << endl;
-//    cout << boxAngleJudge(light_blob_i, light_blob_j) << endl;
-//    cout << CuoWeiDuJudge(light_blob_i, light_blob_j) << endl;
-//    cout << "=============" << endl;
     return light_blob_i.blob_color == enemy_color &&
            light_blob_j.blob_color == enemy_color &&
            lengthRatioJudge(light_blob_i, light_blob_j) &&
            lengthJudge(light_blob_i, light_blob_j) &&
-           //           heightJudge(light_blob_i, light_blob_j) &&
+//           heightJudge(light_blob_i, light_blob_j) &&
            angelJudge(light_blob_i, light_blob_j) &&
            boxAngleJudge(light_blob_i, light_blob_j) &&
            CuoWeiDuJudge(light_blob_i, light_blob_j);

armor/src/armor_finder/find/find_light_blobs.cpp

@@ -121,7 +121,7 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
         color_channel = channels[0];        /* 根据目标颜色进行通道提取 */
     else if (enemy_color == ENEMY_RED)      /*                      */
         color_channel = channels[2];        /************************/
-    cv::threshold(color_channel, src_bin, 160, 255, CV_THRESH_BINARY); // 二值化对应通道
+    cv::threshold(color_channel, src_bin, 170, 255, CV_THRESH_BINARY); // 二值化对应通道
     imagePreProcess(src_bin);                                  // 开闭运算
 
     if(src_bin.size() == cv::Size(640, 480))