11

src/MindVisionMain.cpp

@@ -20,40 +20,52 @@
 #include "TTLCommunicator.h"
 #include "PidController.h"
 
-// Function to safely access PID controllers (avoiding static initialization order issues)
+// 全局PID参数（使用简单类型避免初始化问题）
-PidController& getPitchPid() {
+float g_pitch_kp = 0.1f;
-    static PidController pitch_pid(0.1f, 0.01f, 0.05f);
+float g_pitch_ki = 0.01f;
-    return pitch_pid;
+float g_pitch_kd = 0.05f;
+float g_yaw_kp = 0.1f;
+float g_yaw_ki = 0.01f;
+float g_yaw_kd = 0.05f;
+
+// 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 / 100.0f;
 }
 
 void on_pitch_ki_trackbar(int pos, void*) {
-    getPitchPid().setKi(pos / 1000.0f);
+    g_pitch_ki = pos / 1000.0f;
 }
 
 void on_pitch_kd_trackbar(int pos, void*) {
-    getPitchPid().setKd(pos / 100.0f);
+    g_pitch_kd = pos / 100.0f;
 }
 
 void on_yaw_kp_trackbar(int pos, void*) {
-    getYawPid().setKp(pos / 100.0f);
+    g_yaw_kp = pos / 100.0f;
 }
 
 void on_yaw_ki_trackbar(int pos, void*) {
-    getYawPid().setKi(pos / 1000.0f);
+    g_yaw_ki = pos / 1000.0f;
 }
 
 void on_yaw_kd_trackbar(int pos, void*) {
-    getYawPid().setKd(pos / 100.0f);
+    g_yaw_kd = pos / 100.0f;
 }
 
 // Function to output control data to TTL device (with enable control)
@@ -70,18 +82,21 @@ void output_control_data(const cv::Point2f* ballistic_point,
         float raw_offset_x = -(ballistic_point->x - img_center.x); // yaw 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
+        // Update PID controllers with latest parameters
-        float pid_pitch_output = getPitchPid().update(0.0f, raw_offset_y, dt); // Setpoint is 0, error is raw_offset_y
+        update_pid_controllers();
 
-        // Apply PID control to the yaw (horizontal) component
+        // Apply PID control to the pitch (vertical) component
-        float pid_yaw_output = getYawPid().update(0.0f, raw_offset_x, dt); // Setpoint is 0, error is raw_offset_x
+        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
@@ -93,7 +108,7 @@ void output_control_data(const cv::Point2f* ballistic_point,
             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
+                ballistic_offset_yaw = ballistic_offset_yaw > 0 ? 220 : -220; // or some default value
             }
         }
         if (abs(ballistic_offset_pitch) > 180) {
@@ -101,7 +116,7 @@ void output_control_data(const cv::Point2f* ballistic_point,
             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
+                ballistic_offset_pitch = ballistic_offset_pitch > 0 ? 180 * 1.9 : -180 * 1.9; // or some default value
             }
         }
 
@@ -204,7 +219,7 @@ int main(int /*argc*/, char* /*argv*/[]) {
     // Initialize KCF tracker
     cv::Ptr<cv::Tracker> tracker = cv::TrackerKCF::create();
     bool is_tracking = false;
-    cv::Rect tracking_roi;
+    cv::Rect2d tracking_roi;
     int tracking_frame_count = 0;
     const int MAX_TRACKING_FRAMES = 100; // Maximum frames to track before returning to search
 
@@ -367,32 +382,35 @@ int main(int /*argc*/, char* /*argv*/[]) {
             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);
-
+                
                 // 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);
-
+                
                 // Set initial positions
-                cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(getPitchPid().getKp() * 100));
+                cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(g_pitch_kp * 100));
-                cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(getPitchPid().getKi() * 1000));
+                cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(g_pitch_ki * 1000));
-                cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(getPitchPid().getKd() * 100));
+                cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(g_pitch_kd * 100));
-                cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(getYawPid().getKp() * 100));
+                cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(g_yaw_kp * 100));
-                cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(getYawPid().getKi() * 1000));
+                cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(g_yaw_ki * 1000));
-                cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(getYawPid().getKd() * 100));
+                cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(g_yaw_kd * 100));
-
+                
                 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("Armor Detection", frame);
+
+            // Only call cv::waitKey to keep GUI responsive
+            cv::waitKey(1);
 
             // Exit on 'q' key press
             if (cv::waitKey(1) == 'q') {