From 1f0a1e3a7cfbf9f8fd1a124f79d29f96bb5072e2 Mon Sep 17 00:00:00 2001 From: LLida <3199335945@qq.com> Date: Fri, 5 Dec 2025 18:03:03 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=8A=A5=E9=94=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/MindVisionMain.cpp | 57 ++++++++++++++++++++++++++---------------- 1 file changed, 36 insertions(+), 21 deletions(-) diff --git a/src/MindVisionMain.cpp b/src/MindVisionMain.cpp index 612fd6c..79d2567 100644 --- a/src/MindVisionMain.cpp +++ b/src/MindVisionMain.cpp @@ -20,33 +20,40 @@ #include "TTLCommunicator.h" #include "PidController.h" -// Global PID controllers for pitch and yaw -PidController pitch_pid(0.1f, 0.01f, 0.05f); // Default PID values for pitch -PidController yaw_pid(0.1f, 0.01f, 0.05f); // Default PID values for yaw +// 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; +} -// Callback functions for trackbars - use direct access to global objects +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*) { - pitch_pid.setKp(pos / 100.0f); + getPitchPid().setKp(pos / 100.0f); } void on_pitch_ki_trackbar(int pos, void*) { - pitch_pid.setKi(pos / 1000.0f); + getPitchPid().setKi(pos / 1000.0f); } void on_pitch_kd_trackbar(int pos, void*) { - pitch_pid.setKd(pos / 100.0f); + getPitchPid().setKd(pos / 100.0f); } void on_yaw_kp_trackbar(int pos, void*) { - yaw_pid.setKp(pos / 100.0f); + getYawPid().setKp(pos / 100.0f); } void on_yaw_ki_trackbar(int pos, void*) { - yaw_pid.setKi(pos / 1000.0f); + getYawPid().setKi(pos / 1000.0f); } void on_yaw_kd_trackbar(int pos, void*) { - yaw_pid.setKd(pos / 100.0f); + getYawPid().setKd(pos / 100.0f); } // Function to output control data to TTL device (with enable control) @@ -71,23 +78,31 @@ void output_control_data(const cv::Point2f* ballistic_point, last_time = current_time; // 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 + float pid_pitch_output = getPitchPid().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 + float pid_yaw_output = getYawPid().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(pid_yaw_output); int ballistic_offset_pitch = static_cast(pid_pitch_output); - // Apply same limits as before + // Apply same limits as before with division by zero check if (abs(ballistic_offset_yaw) > 320) { - ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220; // Keep the scale factor + 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 - ballistic_offset_pitch = (ballistic_offset_pitch / abs(ballistic_offset_pitch)) * 180 * ; + 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 @@ -364,12 +379,12 @@ int main(int /*argc*/, char* /*argv*/[]) { cv::createTrackbar("Yaw Kd", "PID Tuning", nullptr, 1000, on_yaw_kd_trackbar); // Set initial positions - cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast(pitch_pid.getKp() * 100)); - cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast(pitch_pid.getKi() * 1000)); - cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast(pitch_pid.getKd() * 100)); - cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast(yaw_pid.getKp() * 100)); - cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast(yaw_pid.getKi() * 1000)); - cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast(yaw_pid.getKd() * 100)); + 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)); initialized_trackbars = true; }