From 3f7cc22f95c75868f42e3cd934cf0402c2a95de6 Mon Sep 17 00:00:00 2001 From: LLida <3199335945@qq.com> Date: Fri, 5 Dec 2025 18:22:50 +0800 Subject: [PATCH] 11 --- src/MindVisionMain.cpp | 88 +++++++++++++++++++++++++----------------- 1 file changed, 53 insertions(+), 35 deletions(-) diff --git a/src/MindVisionMain.cpp b/src/MindVisionMain.cpp index 0ff184f..8492b99 100644 --- a/src/MindVisionMain.cpp +++ b/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.1f; +float g_pitch_ki = 0.01f; +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(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 @@ -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 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(getPitchPid().getKp() * 100)); - cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast(getPitchPid().getKi() * 1000)); - cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast(getPitchPid().getKd() * 100)); - cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast(getYawPid().getKp() * 100)); - cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast(getYawPid().getKi() * 1000)); - cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast(getYawPid().getKd() * 100)); - + cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast(g_pitch_kp * 100)); + cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast(g_pitch_ki * 1000)); + cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast(g_pitch_kd * 100)); + cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast(g_yaw_kp * 100)); + cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast(g_yaw_ki * 1000)); + cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast(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') {