有vofa验证

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -38,6 +38,14 @@ void ArmorFinder::antiTop() {
     // 通过两次装甲角度为零的时间差计算陀螺旋转周期
     // 根据旋转周期计算下一次装甲出现在角度为零的时间点
     if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
+        /* PID相关控制 */
+        sum_yaw = sum_pitch = 0;
+        double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+        double dy = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
+        double yaw = dx;
+        double pitch = dy;
+        /* PID相关控制 */
+        
         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) {

armor/src/armor_finder/armor_finder.cpp

@@ -54,7 +54,11 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
         anti_switch_cnt(0),
         classifier(paras_folder),
         contour_area(0),
-        tracking_cnt(0) {
+        tracking_cnt(0),
+        last_yaw(0), 
+        last_pitch(0), 
+        sum_yaw(0), 
+        sum_pitch(0) {
 }
 
 void ArmorFinder::run(cv::Mat &src) {
@@ -75,6 +79,9 @@ void ArmorFinder::run(cv::Mat &src) {
                     tracker->init(src, target_box.rect);
                     state = TRACKING_STATE;
                     tracking_cnt = 0;
+                    last_yaw = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+                    last_pitch = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
+                    sum_yaw = sum_pitch = 0;
                     LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
                 }
             }
@@ -82,6 +89,8 @@ void ArmorFinder::run(cv::Mat &src) {
         case TRACKING_STATE:
             if (!stateTrackingTarget(src) || ++tracking_cnt > 100) {    // 最多追踪100帧图像
                 state = SEARCHING_STATE;
+                last_yaw = last_pitch = 0;
+                sum_yaw = sum_pitch = 0;
                 LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
             }
             break;
@@ -93,6 +102,11 @@ end:
     if(is_anti_top) { // 判断当前是否为反陀螺模式
         antiTop();
     }else if(target_box.rect != cv::Rect2d()) {
+        if(last_box.rect == cv::Rect2d()) { // 如果上一帧没有目标装甲板
+            sum_yaw = sum_pitch = 0;
+            last_yaw = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+            last_pitch = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
+        }
         anti_top_cnt = 0;
         time_seq.clear();
         angle_seq.clear();

armor/src/armor_finder/send_target/send_target.cpp

@@ -7,6 +7,7 @@
 #include <log.h>
 
 static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
+    
     static short x_tmp, y_tmp, z_tmp;
     uint8_t buff[10];
 
@@ -24,9 +25,10 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
 
 #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
-    x_tmp = static_cast<short>(x * (32768 - 1) / 100);
+    x_tmp = -static_cast<short>(x * (32768 - 1) / 100);
     y_tmp = static_cast<short>(y * (32768 - 1) / 100);
     z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
+    
 
     buff[0] = 's';
     buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
@@ -38,23 +40,37 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
     buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
     buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
     buff[9] = 'e';
-//    if(buff[7]<<8 | buff[8])
-//        cout << (buff[7]<<8 | buff[8]) << endl;
+    //if(buff[7]<<8 | buff[8])
+    //   cout << (buff[7]<<8 | buff[8]) << endl;
     return serial.WriteData(buff, sizeof(buff));
+  
+    
+    // Vofa串口验证
+    
+    //char buff[128];
+    //int len = (buff, "s %lf %lf %lf %d e", x, y, z, shoot_delay);
+    //return serial.WriteData((unsigned char *)buff, len);
+    
+    
+    
 }
 
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     if (target_box.rect == cv::Rect2d()) return false;
-    if (shoot_delay) {
+    /*if (shoot_delay) {
         LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay);
-    }
+    }*/
     auto rect = target_box.rect;
     double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
     double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
 
     // PID
+    #define MAX_YAW 100
+    #define MAX_PITCH 80
     sum_yaw += dx;
     sum_pitch += dy;
+    sum_yaw =(sum_yaw > MAX_YAW) ? MAX_YAW : (sum_yaw < -MAX_YAW ? -MAX_YAW : sum_yaw);
+    sum_pitch =(sum_pitch > MAX_PITCH) ? MAX_PITCH : (sum_pitch < -MAX_PITCH ? -MAX_PITCH : sum_pitch);
     float yaw_I_component = YAW_AIM_KI * sum_yaw;
     float pitch_I_component = PITCH_AIM_KI * sum_pitch;
 

main.cpp

@@ -44,7 +44,9 @@ WrapperHead *video = nullptr;    // 云台摄像头视频源
 Serial serial(115200);                  // 串口对象
 uint8_t last_state = ARMOR_STATE;     // 上次状态，用于初始化
 // 自瞄主程序对象
-ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top);
+uint8_t enemy_color = ENEMY_BLUE;//RED ro BLUE
+uint8_t forced_anti_top = 1;
+ArmorFinder armor_finder(enemy_color, serial, PROJECT_DIR"/tools/para/", forced_anti_top);
 // 能量机关主程序对象
 Energy energy(serial, mcu_data.enemy_color);
 

others/include/camera/camera_define.h

@@ -662,4 +662,4 @@ typedef void (*CAMERA_PAGE_MSG_PROC)(CameraHandle hCamera,UINT MSG,UINT uParam,P
 #define CAMERA_MEDIA_TYPE_RGB16_PLANAR       (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x0024)
 
 
-#endif
+#endif