diff --git a/CMakeLists.txt b/CMakeLists.txt index d4ed64a..2f06c31 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/inc/PidController.h b/inc/PidController.h new file mode 100644 index 0000000..570f385 --- /dev/null +++ b/inc/PidController.h @@ -0,0 +1,46 @@ +#ifndef PID_CONTROLLER_H +#define PID_CONTROLLER_H + +#include + +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 \ No newline at end of file diff --git a/src/MindVisionMain.cpp b/src/MindVisionMain.cpp index de20340..6722737 100644 --- a/src/MindVisionMain.cpp +++ b/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(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(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(pid_yaw_output); + int ballistic_offset_pitch = static_cast(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(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(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)); + + initialized_trackbars = true; + } + + // Update trackbar positions in case they were changed externally + 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)); + // 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') { diff --git a/src/PidController.cpp b/src/PidController.cpp new file mode 100644 index 0000000..a8eb133 --- /dev/null +++ b/src/PidController.cpp @@ -0,0 +1,87 @@ +#include "PidController.h" +#include +#include + +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_; +} \ No newline at end of file