Merge pull request 'PID 调整' (#1) from S-3-18 into master

Reviewed-on: #1

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -38,14 +38,6 @@ 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,11 +54,7 @@ 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) {
@@ -79,9 +75,6 @@ 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"));
                 }
             }
@@ -89,8 +82,6 @@ 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;
@@ -102,11 +93,6 @@ 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,7 +7,6 @@
 #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];
 
@@ -25,11 +24,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);
     buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
@@ -43,34 +41,20 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
 //    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,9 +44,7 @@ WrapperHead *video = nullptr;    // 云台摄像头视频源
 Serial serial(115200);                  // 串口对象
 uint8_t last_state = ARMOR_STATE;     // 上次状态，用于初始化
 // 自瞄主程序对象
-uint8_t enemy_color = ENEMY_BLUE;//RED ro BLUE
+ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top);
-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);