添加了pid以及gui

CMakeLists.txt

@@ -23,6 +23,7 @@ add_executable(armor_detector_mdv
     src/Visualizer.cpp
     src/BallisticPredictor.cpp
     src/TTLCommunicator.cpp
+    src/PidController.cpp
 )
 
 # Link OpenCV libraries

inc/PidController.h

@@ -0,0 +1,46 @@
+#ifndef PID_CONTROLLER_H
+#define PID_CONTROLLER_H
+
+#include <opencv2/opencv.hpp>
+
+class PidController {
+public:
+    // 构造函数
+    PidController(float kp = 0.0f, float ki = 0.0f, float kd = 0.0f);
+    
+    // 更新PID计算
+    float update(float setpoint, float measured_value, float dt);
+    
+    // 重置PID控制器
+    void reset();
+    
+    // 设置PID参数
+    void setKp(float kp);
+    void setKi(float ki);
+    void setKd(float kd);
+    void setParameters(float kp, float ki, float kd);
+    
+    // 获取PID参数
+    float getKp() const;
+    float getKi() const;
+    float getKd() const;
+    
+    // 设置输出限制
+    void setOutputLimits(float min, float max);
+    
+    // 获取输出
+    float getOutput() const;
+    
+    // 获取误差
+    float getError() const;
+
+private:
+    float kp_, ki_, kd_;           // PID参数
+    float last_error_;             // 上一次误差
+    float integral_;               // 积分项
+    float output_;                 // 输出值
+    float output_min_, output_max_; // 输出限制
+    bool first_iteration_;         // 是否为第一次迭代
+};
+
+#endif // PID_CONTROLLER_H

src/MindVisionMain.cpp

@@ -18,6 +18,40 @@
 #include "Visualizer.h"
 #include "BallisticPredictor.h"
 #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
+
+// Global pointers to PID controllers to be accessed by trackbar callbacks
+PidController* g_pitch_pid = &pitch_pid;
+PidController* g_yaw_pid = &yaw_pid;
+
+// Callback functions for trackbars
+void on_pitch_kp_trackbar(int pos, void*) {
+    g_pitch_pid->setKp(pos / 100.0f);
+}
+
+void on_pitch_ki_trackbar(int pos, void*) {
+    g_pitch_pid->setKi(pos / 1000.0f);
+}
+
+void on_pitch_kd_trackbar(int pos, void*) {
+    g_pitch_pid->setKd(pos / 100.0f);
+}
+
+void on_yaw_kp_trackbar(int pos, void*) {
+    g_yaw_pid->setKp(pos / 100.0f);
+}
+
+void on_yaw_ki_trackbar(int pos, void*) {
+    g_yaw_pid->setKi(pos / 1000.0f);
+}
+
+void on_yaw_kd_trackbar(int pos, void*) {
+    g_yaw_pid->setKd(pos / 100.0f);
+}
 
 // Function to output control data to TTL device (with enable control)
 void output_control_data(const cv::Point2f* ballistic_point,
@@ -30,13 +64,34 @@ void output_control_data(const cv::Point2f* ballistic_point,
         std::ostringstream send_str;
 
         // Calculate offset (based on actual image center)
-        int ballistic_offset_yaw = 1.9*-static_cast<int>(ballistic_point->x - img_center.x);
-        if ( abs(ballistic_offset_yaw) > 320){
-            ballistic_offset_yaw = ( ballistic_offset_yaw / abs( ballistic_offset_yaw ) ) * 220 ;
+        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)
+        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.001f) 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 = 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);
+
+        // Apply same limits as before
+        if (abs(ballistic_offset_yaw) > 320) {
+            ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220; // Keep the scale factor
         }
-        int ballistic_offset_pitch = 1.9*-static_cast<int>(img_center.y - ballistic_point->y);
-        if ( abs(ballistic_offset_pitch) > 180 ) {
-            ballistic_offset_pitch = ( ballistic_offset_pitch / abs( ballistic_offset_pitch ) ) * 180*1.9 ;
+        if (abs(ballistic_offset_pitch) > 180) {
+            // Use the same scale factor as before
+            ballistic_offset_pitch = (ballistic_offset_pitch / abs(ballistic_offset_pitch)) * 180 * 1.9;
         }
 
         // Color simplification mapping
@@ -297,10 +352,44 @@ int main(int /*argc*/, char* /*argv*/[]) {
             // Now sending on every frame for smoother control
             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);
+
+                // 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>(pitch_pid.getKp() * 100));
+                cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(pitch_pid.getKi() * 1000));
+                cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(pitch_pid.getKd() * 100));
+                cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(yaw_pid.getKp() * 100));
+                cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(yaw_pid.getKi() * 1000));
+                cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(yaw_pid.getKd() * 100));
+
+                initialized_trackbars = true;
+            }
+
+            // Update trackbar positions in case they were changed externally
+            cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(pitch_pid.getKp() * 100));
+            cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(pitch_pid.getKi() * 1000));
+            cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(pitch_pid.getKd() * 100));
+            cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(yaw_pid.getKp() * 100));
+            cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(yaw_pid.getKi() * 1000));
+            cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(yaw_pid.getKd() * 100));
+
             // 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);
 
             // Exit on 'q' key press
             if (cv::waitKey(1) == 'q') {

src/PidController.cpp

@@ -0,0 +1,87 @@
+#include "PidController.h"
+#include <algorithm>
+#include <cmath>
+
+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) {}
+
+float PidController::update(float setpoint, float measured_value, float dt) {
+    float error = setpoint - measured_value;
+    
+    // 处理积分饱和
+    integral_ += error * dt;
+    
+    // 计算微分（使用前向差分）
+    float derivative = 0.0f;
+    if (!first_iteration_) {
+        derivative = (error - last_error_) / dt;
+    } else {
+        first_iteration_ = false;
+    }
+    
+    // PID计算
+    float proportional = kp_ * error;
+    float integral_contribution = ki_ * integral_;
+    float derivative_contribution = kd_ * derivative;
+    
+    output_ = proportional + integral_contribution + derivative_contribution;
+    
+    // 限制输出
+    output_ = std::max(output_min_, std::min(output_max_, output_));
+    
+    // 保存当前误差用于下次计算微分
+    last_error_ = error;
+    
+    return output_;
+}
+
+void PidController::reset() {
+    last_error_ = 0.0f;
+    integral_ = 0.0f;
+    output_ = 0.0f;
+    first_iteration_ = true;
+}
+
+void PidController::setKp(float kp) {
+    kp_ = kp;
+}
+
+void PidController::setKi(float ki) {
+    ki_ = ki;
+}
+
+void PidController::setKd(float kd) {
+    kd_ = kd;
+}
+
+void PidController::setParameters(float kp, float ki, float kd) {
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+}
+
+float PidController::getKp() const {
+    return kp_;
+}
+
+float PidController::getKi() const {
+    return ki_;
+}
+
+float PidController::getKd() const {
+    return kd_;
+}
+
+void PidController::setOutputLimits(float min, float max) {
+    output_min_ = min;
+    output_max_ = max;
+}
+
+float PidController::getOutput() const {
+    return output_;
+}
+
+float PidController::getError() const {
+    return last_error_;
+}