面积大小输出

README.md

@@ -79,3 +79,25 @@ make -j$(nproc)
 
 ## MindVision-SDK
 `Linux`: >**wget https://www.mindvision.com.cn/wp-content/uploads/2023/08/linuxSDK_V2.1.0.37.tar.gz**
+
+## PID设置原理及其方式
+
+- **PID初始真实值 = g_pitch(/yaw)_kp（`位于23行`） * 相关系数（`位于410行`）**
+- **PID调节滑块含义** ：`46行`轨迹条回调函数为上下限。例如 pos / 100 ，如果pos范围为0~1000`（位于400行）`则滑块调节范围为 0~10。
+
+###  Kp（比例系数）的作用
+ - 响应速度：Kp值越大，系统对误差的响应越快
+ - 控制力度：直接根据当前误差大小产生控制输出
+
+### Ki（积分系数）的作用
+ - 消除稳态误差：累积历史误差，清除长期存在的小偏差
+ - 系统偏移补偿：补偿机械系统的小偏差和静态误差
+
+### Kd（微分系数）的作用
+ - 预测变化趋势：根据误差的变化率进行调节
+ - 抑制振荡：在系统接近目标时减缓调节速度
+
+**参数调整建议 **
+   - Kp先调：首先调整Kp获得基本响应特性
+   - Kd次调：增加Kd减少振荡
+   - Ki后调：最后调节Ki消除稳态误差

src/MindVisionMain.cpp

@@ -20,40 +20,52 @@
 #include "TTLCommunicator.h"
 #include "PidController.h"
 
-// Function to safely access PID controllers (avoiding static initialization order issues)
-PidController& getPitchPid() {
-    static PidController pitch_pid(0.1f, 0.01f, 0.05f);
-    return pitch_pid;
+// 全局PID参数（使用简单类型避免初始化问题）
+float g_pitch_kp = 0.5217f;
+float g_pitch_ki = 0.006f;
+float g_pitch_kd = 0.0773f;
+float g_yaw_kp = 0.6419f;
+float g_yaw_ki = 0.2409f;
+float g_yaw_kd = 0.4372f;
+
+// PID控制器实例
+PidController pitch_pid(g_pitch_kp, g_pitch_ki, g_pitch_kd);
+PidController yaw_pid(g_yaw_kp, g_yaw_ki, g_yaw_kd);
+
+// 更新PID控制器参数的函数
+void update_pid_controllers() {
+    static bool first_call = true;
+    if (first_call) {
+        first_call = false;
+        return;
+    }
+    pitch_pid.setParameters(g_pitch_kp, g_pitch_ki, g_pitch_kd);
+    yaw_pid.setParameters(g_yaw_kp, g_yaw_ki, g_yaw_kd);
 }
 
-PidController& getYawPid() {
-    static PidController yaw_pid(0.1f, 0.01f, 0.05f);
-    return yaw_pid;
-}
-
-// Callback functions for trackbars - use safe access to PID controllers
+// 轨迹条回调函数
 void on_pitch_kp_trackbar(int pos, void*) {
-    getPitchPid().setKp(pos / 100.0f);
+    g_pitch_kp = pos / 10000.0f;
 }
 
 void on_pitch_ki_trackbar(int pos, void*) {
-    getPitchPid().setKi(pos / 1000.0f);
+    g_pitch_ki = pos / 100000.0f;
 }
 
 void on_pitch_kd_trackbar(int pos, void*) {
-    getPitchPid().setKd(pos / 100.0f);
+    g_pitch_kd = pos / 100000.0f;
 }
 
 void on_yaw_kp_trackbar(int pos, void*) {
-    getYawPid().setKp(pos / 100.0f);
+    g_yaw_kp = pos / 10000.0f;
 }
 
 void on_yaw_ki_trackbar(int pos, void*) {
-    getYawPid().setKi(pos / 1000.0f);
+    g_yaw_ki = pos / 100000.1f;
 }
 
 void on_yaw_kd_trackbar(int pos, void*) {
-    getYawPid().setKd(pos / 100.0f);
+    g_yaw_kd = pos / 100000.0f;
 }
 
 // Function to output control data to TTL device (with enable control)
@@ -68,42 +80,29 @@ void output_control_data(const cv::Point2f* ballistic_point,
 
         // Calculate offset (based on actual image center)
         float raw_offset_x = -(ballistic_point->x - img_center.x); // yaw error
-        float raw_offset_y = -(img_center.y - ballistic_point->y); // pitch error
+        float raw_offset_y = -(img_center.y - ballistic_point->y ); // pitch error
 
-        // Calculate time delta (assuming we have access to time)
+        // Calculate time delta
         static auto last_time = std::chrono::high_resolution_clock::now();
         auto current_time = std::chrono::high_resolution_clock::now();
         float dt = std::chrono::duration<float>(current_time - last_time).count();
         if (dt <= 0) dt = 0.01f; // Minimum dt to avoid division by zero
         last_time = current_time;
 
-        // Apply PID control to the pitch (vertical) component
-        float pid_pitch_output = getPitchPid().update(0.0f, raw_offset_y, dt); // Setpoint is 0, error is raw_offset_y
+        // Update PID controllers with latest parameters
+        update_pid_controllers();
 
-        // Apply PID control to the yaw (horizontal) component
-        float pid_yaw_output = getYawPid().update(0.0f, raw_offset_x, dt); // Setpoint is 0, error is raw_offset_x
+        // Apply PID control to the pitch (vertical) component
+        float pid_pitch_output = pitch_pid.update(0.0f, raw_offset_y, dt); // Setpoint is 0, error is raw_offset_y
+
+        // Apply PID control to the yaw (horizontal) component  
+        float pid_yaw_output = yaw_pid.update(0.0f, raw_offset_x, dt); // Setpoint is 0, error is raw_offset_x
 
         // Convert PID outputs to the expected format
         // The PID output might be large, so we might need to scale it
-        int ballistic_offset_yaw = static_cast<int>(pid_yaw_output);
-        int ballistic_offset_pitch = static_cast<int>(pid_pitch_output);
+        int ballistic_offset_yaw = 1.4*(-static_cast<int>(pid_yaw_output));
+        int ballistic_offset_pitch = 2*(-static_cast<int>(pid_pitch_output));
 
-        // Apply same limits as before with division by zero check
-        if (abs(ballistic_offset_yaw) > 320) {
-            if (ballistic_offset_yaw != 0) {
-                ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220; // Keep the scale factor
-            } else {
-                ballistic_offset_yaw = 220; // or some default value
-            }
-        }
-        if (abs(ballistic_offset_pitch) > 180) {
-            // Use the same scale factor as before
-            if (ballistic_offset_pitch != 0) {
-                ballistic_offset_pitch = (ballistic_offset_pitch / abs(ballistic_offset_pitch)) * 180 * 1.9;
-            } else {
-                ballistic_offset_pitch = 180 * 1.9; // or some default value
-            }
-        }
 
         // Color simplification mapping
         std::string simplified_color = target_color;
@@ -137,8 +136,8 @@ int main(int /*argc*/, char* /*argv*/[]) {
     static int Numbe = 0;
     std::string target_color = "red";
     int cam_id = 0;
-    cv::Size default_resolution(640, 480); // Changed to 640x480 for consistency with SJTU project
-    bool use_ttl = false; // Set to false to disable TTL communication
+    cv::Size default_resolution(1280, 720); // Changed to 640x480 for consistency with SJTU project
+    bool use_ttl = true; // Set to false to disable TTL communication
 
 
     if (Numbe == 0) {
@@ -204,7 +203,7 @@ int main(int /*argc*/, char* /*argv*/[]) {
     // Initialize KCF tracker
     cv::Ptr<cv::Tracker> tracker = cv::TrackerKCF::create();
     bool is_tracking = false;
-    cv::Rect2d tracking_roi;
+    cv::Rect tracking_roi;
     int tracking_frame_count = 0;
     const int MAX_TRACKING_FRAMES = 100; // Maximum frames to track before returning to search
 
@@ -286,7 +285,8 @@ int main(int /*argc*/, char* /*argv*/[]) {
                 predicted_center = std::make_unique<cv::Point2f>(kalman_tracker.predict());
 
                 // Start tracking if successfully detected
-                if (!is_tracking && !armor_plates.empty()) {
+                if (!is_tracking && !armor_plates.empty() && armor_plates[0].corners_2d.size() >= 4) {
+                    // Only start tracking if we have all 4 corners
                     cv::Rect2d armor_rect = cv::boundingRect(std::vector<cv::Point2f>{
                         armor_plates[0].corners_2d[0],
                         armor_plates[0].corners_2d[1],
@@ -303,11 +303,14 @@ int main(int /*argc*/, char* /*argv*/[]) {
                     // Ensure the rectangle is within frame bounds
                     expanded_rect = expanded_rect & cv::Rect2d(0, 0, frame.cols, frame.rows);
 
-                    tracker = cv::TrackerKCF::create(); // Create new tracker instance
-                    tracker->init(frame, expanded_rect);
-                    tracking_roi = expanded_rect;
-                    is_tracking = true;
-                    tracking_frame_count = 0;
+                    // Initialize tracker only if the rectangle has positive area
+                    if (expanded_rect.area() > 0) {
+                        tracker = cv::TrackerKCF::create(); // Create new tracker instance
+                        tracker->init(frame, expanded_rect);
+                        tracking_roi = expanded_rect;
+                        is_tracking = true;
+                        tracking_frame_count = 0;
+                    }
                 }
 
                 consecutive_predicts = 0;
@@ -341,15 +344,19 @@ int main(int /*argc*/, char* /*argv*/[]) {
             bool is_predicted = (display_center != nullptr) && (detection_center == nullptr && (!is_tracking || tracking_center == nullptr));
 
             // Calculate ballistic prediction point
-            cv::Point2f* ballistic_point = ballistic_predictor.predict_ballistic_point(
-                predicted_center.get(), img_center, target_speed
-            );
+            cv::Point2f* ballistic_point = nullptr;
+            if (predicted_center) {
+                ballistic_point = ballistic_predictor.predict_ballistic_point(
+                    predicted_center.get(), img_center, target_speed
+                );
+            }
 
             auto current_time = std::chrono::high_resolution_clock::now();
 
             // Visualization
             visualizer.draw_light_bars(frame, valid_light_bars, target_color);
             if (!armor_plates.empty()) {
+                // Only draw armor plate if it has valid data
                 visualizer.draw_armor_plate(frame, armor_plates[0]);
             }
             // Draw tracking rectangle if tracking
@@ -357,42 +364,50 @@ int main(int /*argc*/, char* /*argv*/[]) {
                 cv::rectangle(frame, tracking_roi, cv::Scalar(0, 255, 0), 2);
             }
             visualizer.draw_offset_text(frame, display_center, target_color, is_predicted);
-            visualizer.draw_ballistic_point(frame, ballistic_point);
+            if (ballistic_point != nullptr) {
+                visualizer.draw_ballistic_point(frame, ballistic_point);
+            }
 
             // Output control data to TTL (passing use_ttl to control whether to send)
             // Now sending on every frame for smoother control
-            output_control_data(display_center, target_color, ttl, img_center, use_ttl);
+            // Only send if ballistic_point is not null and we have a valid display center
+            if (display_center != nullptr && ballistic_point != nullptr) {
+                output_control_data(display_center, target_color, ttl, img_center, use_ttl);
+            }
 
             // Create trackbars for PID parameter tuning
             static bool initialized_trackbars = false;
             if (!initialized_trackbars) {
                 cv::namedWindow("PID Tuning", cv::WINDOW_AUTOSIZE);
-
+                
                 // Create trackbars for pitch PID parameters
-                cv::createTrackbar("Pitch Kp", "PID Tuning", nullptr, 1000, on_pitch_kp_trackbar);
-                cv::createTrackbar("Pitch Ki", "PID Tuning", nullptr, 1000, on_pitch_ki_trackbar);
-                cv::createTrackbar("Pitch Kd", "PID Tuning", nullptr, 1000, on_pitch_kd_trackbar);
-
+                cv::createTrackbar("Pitch Kp", "PID Tuning", nullptr, 10000, on_pitch_kp_trackbar);
+                cv::createTrackbar("Pitch Ki", "PID Tuning", nullptr, 10000, on_pitch_ki_trackbar);
+                cv::createTrackbar("Pitch Kd", "PID Tuning", nullptr, 10000, on_pitch_kd_trackbar);
+                
                 // Create trackbars for yaw PID parameters
-                cv::createTrackbar("Yaw Kp", "PID Tuning", nullptr, 1000, on_yaw_kp_trackbar);
-                cv::createTrackbar("Yaw Ki", "PID Tuning", nullptr, 1000, on_yaw_ki_trackbar);
-                cv::createTrackbar("Yaw Kd", "PID Tuning", nullptr, 1000, on_yaw_kd_trackbar);
-
+                cv::createTrackbar("Yaw Kp", "PID Tuning", nullptr, 10000, on_yaw_kp_trackbar);
+                cv::createTrackbar("Yaw Ki", "PID Tuning", nullptr, 10000, on_yaw_ki_trackbar);
+                cv::createTrackbar("Yaw Kd", "PID Tuning", nullptr, 10000, on_yaw_kd_trackbar);
+                
                 // Set initial positions
-                cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(getPitchPid().getKp() * 100));
-                cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(getPitchPid().getKi() * 1000));
-                cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(getPitchPid().getKd() * 100));
-                cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(getYawPid().getKp() * 100));
-                cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(getYawPid().getKi() * 1000));
-                cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(getYawPid().getKd() * 100));
-
+                cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(g_pitch_kp * 10000));
+                cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(g_pitch_ki * 10000));
+                cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(g_pitch_kd * 10000));
+                cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(g_yaw_kp * 10000));
+                cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(g_yaw_ki * 10000));
+                cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(g_yaw_kd * 10000));
+               
                 initialized_trackbars = true;
             }
 
             // Display windows
             cv::imshow("Armor Detection", frame);
-            cv::imshow(target_color + " Mask", mask);
-            cv::imshow(target_color + " Only", color_only_frame);
+            //cv::imshow(target_color + " Mask", mask);
+            //cv::imshow(target_color + " Only", color_only_frame);
+
+            // Only call cv::waitKey to keep GUI responsive
+            cv::waitKey(1);
 
             // Exit on 'q' key press
             if (cv::waitKey(1) == 'q') {
@@ -401,6 +416,17 @@ int main(int /*argc*/, char* /*argv*/[]) {
 
             frame_counter++;
 
+            float area = 0.0; 
+            if (!armor_plates.empty()) {
+            // 获取装甲板配对的第一个灯条
+                const LightBar& light_bar = armor_plates[0].pair.first;
+                // 直接使用灯条的area（即rect_area）赋值
+                area = light_bar.area;
+                // 额外校验：确保灯条面积有效（避免负数/0值）
+                if (area <= 0) {
+                    area = 0.0f;
+                }
+            }
             // Control max frame rate (100 FPS)
             auto end_time = std::chrono::high_resolution_clock::now();
             auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - current_time).count();
@@ -443,4 +469,4 @@ int main(int /*argc*/, char* /*argv*/[]) {
     }
 
     return 0;
-}
+}

src/PidController.cpp

@@ -4,7 +4,7 @@
 
 PidController::PidController(float kp, float ki, float kd) 
     : kp_(kp), ki_(ki), kd_(kd), last_error_(0.0f), integral_(0.0f), output_(0.0f), 
-      output_min_(-100.0f), output_max_(100.0f), first_iteration_(true) {}
+      output_min_(-400.0f), output_max_(400.0f), first_iteration_(true) {}
 
 float PidController::update(float setpoint, float measured_value, float dt) {
     float error = setpoint - measured_value;
@@ -84,4 +84,4 @@ float PidController::getOutput() const {
 
 float PidController::getError() const {
     return last_error_;
-}
+}