格式修正

CMakeLists.txt

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

README.md

@@ -79,25 +79,3 @@ make -j$(nproc)
 
 ## MindVision-SDK
 `Linux`: >**wget https://www.mindvision.com.cn/wp-content/uploads/2023/08/linuxSDK_V2.1.0.37.tar.gz**
-
-## PID设置原理及其方式
-
-- **PID初始真实值 = g_pitch(/yaw)_kp（`位于23行`） * 相关系数（`位于410行`）**
-- **PID调节滑块含义** ：`46行`轨迹条回调函数为上下限。例如 pos / 100 ，如果pos范围为0~1000`（位于400行）`则滑块调节范围为 0~10。
-
-###  Kp（比例系数）的作用
- - 响应速度：Kp值越大，系统对误差的响应越快
- - 控制力度：直接根据当前误差大小产生控制输出
-
-### Ki（积分系数）的作用
- - 消除稳态误差：累积历史误差，清除长期存在的小偏差
- - 系统偏移补偿：补偿机械系统的小偏差和静态误差
-
-### Kd（微分系数）的作用
- - 预测变化趋势：根据误差的变化率进行调节
- - 抑制振荡：在系统接近目标时减缓调节速度
-
-**参数调整建议 **
-   - Kp先调：首先调整Kp获得基本响应特性
-   - Kd次调：增加Kd减少振荡
-   - Ki后调：最后调节Ki消除稳态误差

inc/ArmorDetector.h

@@ -23,23 +23,6 @@ struct ArmorPlate {
     double confidence;
     std::pair<LightBar, LightBar> pair;
     std::vector<cv::Point2f> corners_2d;  // Can be computed later for 3D pose
-
-    // Function to get bounding rectangle of the armor plate
-    cv::Rect2d getBoundingRect() const {
-        if (corners_2d.size() >= 4) {
-            cv::Point2f min_pt = corners_2d[0];
-            cv::Point2f max_pt = corners_2d[0];
-            for (const auto& pt : corners_2d) {
-                min_pt.x = std::min(min_pt.x, pt.x);
-                min_pt.y = std::min(min_pt.y, pt.y);
-                max_pt.x = std::max(max_pt.x, pt.x);
-                max_pt.y = std::max(max_pt.y, pt.y);
-            }
-            return cv::Rect2d(min_pt.x, min_pt.y, max_pt.x - min_pt.x, max_pt.y - min_pt.y);
-        }
-        // Fallback: use center and a fixed size
-        return cv::Rect2d(center.x - 30, center.y - 15, 60, 30);
-    }
 };
 
 class ArmorDetector {

inc/PidController.h

@@ -1,46 +0,0 @@
-#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

inc/config.h

@@ -36,9 +36,6 @@ const float ARMOR_ANGLE_DIFF_THRESHOLD = 15.0f * CV_PI / 180.0f; // Armor light
 const float KF_PROCESS_NOISE = 0.02f;       // Process noise covariance
 const float KF_MEASUREMENT_NOISE = 0.5f;    // Measurement noise covariance
 
-// Focal length in pixels (from camera calibration)
-const double FOCAL_PIXAL = 600.0;  // This should match your actual camera calibration
-
 // TTL communication settings
 const int TTL_BAUDRATE = 115200;
 

src/MindVisionMain.cpp

@@ -1,4 +1,4 @@
-#include <cstdlib>
+#include <cstdlib>  
 #include <iostream>
 #include <string>
 #include <vector>
@@ -6,9 +6,8 @@
 #include <thread>
 #include <memory>
 #include <unistd.h>
-#include <math.h>
+
 #include <opencv2/opencv.hpp>
-#include <opencv2/tracking.hpp>
 
 #include "config.h"
 #include "MindVisionCamera.h"  // 使用MindVision相机
@@ -18,103 +17,40 @@
 #include "Visualizer.h"
 #include "BallisticPredictor.h"
 #include "TTLCommunicator.h"
-#include "PidController.h"
-
-// 全局PID参数（使用简单类型避免初始化问题）
-float g_pitch_kp = 0.5217f;
-float g_pitch_ki = 0.006f;
-float g_pitch_kd = 0.0773f;
-float g_yaw_kp = 0.6419f;
-float g_yaw_ki = 0.2409f;
-float g_yaw_kd = 0.4372f;
-
-// 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);
-}
-
-// 轨迹条回调函数
-void on_pitch_kp_trackbar(int pos, void*) {
-    g_pitch_kp = pos / 10000.0f;
-}
-
-void on_pitch_ki_trackbar(int pos, void*) {
-    g_pitch_ki = pos / 100000.0f;
-}
-
-void on_pitch_kd_trackbar(int pos, void*) {
-    g_pitch_kd = pos / 100000.0f;
-}
-
-void on_yaw_kp_trackbar(int pos, void*) {
-    g_yaw_kp = pos / 10000.0f;
-}
-
-void on_yaw_ki_trackbar(int pos, void*) {
-    g_yaw_ki = pos / 100000.1f;
-}
-
-void on_yaw_kd_trackbar(int pos, void*) {
-    g_yaw_kd = pos / 100000.0f;
-}
 
 // Function to output control data to TTL device (with enable control)
 void output_control_data(const cv::Point2f* ballistic_point,
                         const std::string& target_color,
+                        int frame_counter,
                         TTLCommunicator* ttl_communicator,
                         const cv::Point2f& img_center,
                         bool use_ttl) {
-    // Only send data when TTL is enabled and has valid target
-    if (use_ttl && ballistic_point != nullptr) {
+    // Only send data when TTL is enabled, meets frame interval, and has valid target
+    if (use_ttl && frame_counter % 5 == 0 && ballistic_point != nullptr) {
         std::ostringstream send_str;
 
         // Calculate offset (based on actual image center)
-        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
-        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;
-
-        // Update PID controllers with latest parameters
-        update_pid_controllers();
-
-        // 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 = 1.4*(-static_cast<int>(pid_yaw_output));
-        int ballistic_offset_pitch = 2*(-static_cast<int>(pid_pitch_output));
-
+        int ballistic_offset_x = -static_cast<int>(ballistic_point->x - img_center.x);
+        if ( abs(ballistic_offset_x) > 320){
+            ballistic_offset_x = ( ballistic_offset_x / abs( ballistic_offset_x ) ) * 320 ;
+        }
+        int ballistic_offset_y = -static_cast<int>(img_center.y - ballistic_point->y);
+        if ( abs(ballistic_offset_y) > 180 ) {
+            ballistic_offset_y = ( ballistic_offset_x / abs( ballistic_offset_x ) ) * 180 ;
+        }
 
         // Color simplification mapping
         std::string simplified_color = target_color;
         if (target_color == "red") simplified_color = "r";
         else if (target_color == "blue") simplified_color = "b";
 
-        // Construct send string (original format)
-        send_str << "#s " << simplified_color << " " << std::to_string(ballistic_offset_yaw) << " " << std::to_string(ballistic_offset_pitch) << "*\n";
+        // Construct send string
+        send_str << "#s " << simplified_color << " " << std::to_string(ballistic_offset_x) << " " << std::to_string(ballistic_offset_y) << "*\n";
 
         // Send data
         if (ttl_communicator != nullptr) {
             ttl_communicator->send_data(send_str.str());
+
         }else{
             std::cerr << "Error: TTLCommunicator is a null pointer!" << std::endl;
         }
@@ -136,9 +72,9 @@ int main(int /*argc*/, char* /*argv*/[]) {
     static int Numbe = 0;
     std::string target_color = "red";
     int cam_id = 0;
-    cv::Size default_resolution(1280, 720); // Changed to 640x480 for consistency with SJTU project
-    bool use_ttl = true; // Set to false to disable TTL communication
-
+    cv::Size default_resolution(1280, 720);
+    bool use_ttl = false; // Set to false to disable TTL communication
+    
 
     if (Numbe == 0) {
         // 执行 shell 命令（注意安全风险！）
@@ -146,6 +82,7 @@ int main(int /*argc*/, char* /*argv*/[]) {
         Numbe++;
     }
 
+    return 0;
     // Define optional resolution list (adjust based on camera support)
     std::vector<cv::Size> resolutions = {
         cv::Size(320, 240),   // Low resolution, high frame rate
@@ -200,13 +137,6 @@ int main(int /*argc*/, char* /*argv*/[]) {
     KalmanFilter kalman_tracker;
     Visualizer visualizer;
 
-    // Initialize KCF tracker
-    cv::Ptr<cv::Tracker> tracker = cv::TrackerKCF::create();
-    bool is_tracking = false;
-    cv::Rect tracking_roi;
-    int tracking_frame_count = 0;
-    const int MAX_TRACKING_FRAMES = 100; // Maximum frames to track before returning to search
-
     int frame_counter = 0; // Counter to control output frequency
     int max_consecutive_predicts = 20; // Maximum consecutive prediction times
     int consecutive_predicts = 0; // Current consecutive prediction count
@@ -215,7 +145,7 @@ int main(int /*argc*/, char* /*argv*/[]) {
     try {
         while (true) {
             // 使用新的颜色过滤方法同时获取图像和原始掩码
-
+        
             cv::Mat raw_mask;
             if (!camera.read_frame_with_color_filter(frame, raw_mask, target_color)) {
                 std::cout << "Cannot read from MindVision camera, exiting!，HERERER" << std::endl;
@@ -233,94 +163,30 @@ int main(int /*argc*/, char* /*argv*/[]) {
             cv::Mat color_only_frame;
             frame.copyTo(color_only_frame, raw_mask);
 
-            // Initialize tracking center
-            cv::Point2f* tracking_center = nullptr;
-            std::unique_ptr<cv::Point2f> tracking_point = nullptr;
-
-            if (is_tracking) {
-                // Update tracker
-                bool success = tracker->update(frame, tracking_roi);
-                if (success && tracking_roi.area() > 0) {
-                    // Calculate center of tracked ROI
-                    tracking_point = std::make_unique<cv::Point2f>(
-                        tracking_roi.x + tracking_roi.width / 2.0f,
-                        tracking_roi.y + tracking_roi.height / 2.0f
-                    );
-                    tracking_center = tracking_point.get();
-                    tracking_frame_count++;
-
-                    // If tracking for too long or detection is available, search for armor again
-                    if (tracking_frame_count > MAX_TRACKING_FRAMES) {
-                        is_tracking = false;
-                        tracking_frame_count = 0;
-                    }
-                } else {
-                    // Tracking failed, return to detection mode
-                    is_tracking = false;
-                    tracking_frame_count = 0;
-                }
-            }
-
-            // Armor plate detection - only when not tracking or need to verify tracking
+            // Armor plate detection
             std::vector<LightBar> valid_light_bars;
             std::vector<ArmorPlate> armor_plates;
-
-            if (!is_tracking || tracking_frame_count % 10 == 0) { // Detect every 10 frames when tracking
-                detector.detect(mask, target_color, valid_light_bars, armor_plates);
-            }
+            detector.detect(mask, target_color, valid_light_bars, armor_plates);
 
             // Kalman filter tracking (modified part: get target speed)
-            cv::Point2f* detection_center = nullptr;
+            cv::Point2f* current_center = nullptr;
             if (!armor_plates.empty()) {
-                detection_center = &(armor_plates[0].center);
+                current_center = &(armor_plates[0].center);
             }
 
             // Use smart pointer for safer memory management
             std::unique_ptr<cv::Point2f> predicted_center = nullptr;
 
-            // Priority: detection -> tracking -> Kalman prediction
-            if (detection_center != nullptr) {
+            if (current_center != nullptr) {
                 // Has detection result: update Kalman filter, reset consecutive prediction count
-                kalman_tracker.update(*detection_center);
+                kalman_tracker.update(*current_center);
                 predicted_center = std::make_unique<cv::Point2f>(kalman_tracker.predict());
 
-                // Start tracking if successfully detected
-                if (!is_tracking && !armor_plates.empty() && armor_plates[0].corners_2d.size() >= 4) {
-                    // Only start tracking if we have all 4 corners
-                    cv::Rect2d armor_rect = cv::boundingRect(std::vector<cv::Point2f>{
-                        armor_plates[0].corners_2d[0],
-                        armor_plates[0].corners_2d[1],
-                        armor_plates[0].corners_2d[2],
-                        armor_plates[0].corners_2d[3]
-                    });
-                    // Expand the bounding box slightly for better tracking
-                    cv::Rect2d expanded_rect = cv::Rect2d(
-                        armor_rect.x - armor_rect.width * 0.1,
-                        armor_rect.y - armor_rect.height * 0.1,
-                        armor_rect.width * 1.2,
-                        armor_rect.height * 1.2
-                    );
-                    // Ensure the rectangle is within frame bounds
-                    expanded_rect = expanded_rect & cv::Rect2d(0, 0, frame.cols, frame.rows);
-
-                    // Initialize tracker only if the rectangle has positive area
-                    if (expanded_rect.area() > 0) {
-                        tracker = cv::TrackerKCF::create(); // Create new tracker instance
-                        tracker->init(frame, expanded_rect);
-                        tracking_roi = expanded_rect;
-                        is_tracking = true;
-                        tracking_frame_count = 0;
-                    }
-                }
-
-                consecutive_predicts = 0;
-            } else if (is_tracking && tracking_center != nullptr) {
-                // Use tracking result
-                kalman_tracker.update(*tracking_center);
-                predicted_center = std::make_unique<cv::Point2f>(kalman_tracker.predict());
+                // Get velocity from the Kalman filter
+                target_speed = cv::Point2f(0.0f, 0.0f); // Kalman filter in OpenCV doesn't directly expose velocity
                 consecutive_predicts = 0;
             } else {
-                // No detection or tracking result: only use Kalman prediction, limit consecutive predictions
+                // No detection result: only use Kalman prediction, limit consecutive predictions
                 consecutive_predicts++;
                 if (consecutive_predicts < max_consecutive_predicts) {
                     cv::Point2f temp_pred = kalman_tracker.predict();
@@ -334,80 +200,34 @@ int main(int /*argc*/, char* /*argv*/[]) {
             }
 
             // Determine display center
-            cv::Point2f* display_center = detection_center; // Give priority to detection results
-            if (display_center == nullptr && is_tracking && tracking_center != nullptr) {
-                display_center = tracking_center;
-            }
+            cv::Point2f* display_center = current_center;
             if (display_center == nullptr && predicted_center != nullptr) {
                 display_center = predicted_center.get();
             }
-            bool is_predicted = (display_center != nullptr) && (detection_center == nullptr && (!is_tracking || tracking_center == nullptr));
+            bool is_predicted = (display_center != nullptr) && (current_center == nullptr);
 
             // Calculate ballistic prediction point
-            cv::Point2f* ballistic_point = nullptr;
-            if (predicted_center) {
-                ballistic_point = ballistic_predictor.predict_ballistic_point(
-                    predicted_center.get(), img_center, target_speed
-                );
-            }
+            cv::Point2f* ballistic_point = ballistic_predictor.predict_ballistic_point(
+                predicted_center.get(), img_center, target_speed
+            );
 
             auto current_time = std::chrono::high_resolution_clock::now();
 
             // Visualization
             visualizer.draw_light_bars(frame, valid_light_bars, target_color);
             if (!armor_plates.empty()) {
-                // Only draw armor plate if it has valid data
                 visualizer.draw_armor_plate(frame, armor_plates[0]);
             }
-            // Draw tracking rectangle if tracking
-            if (is_tracking) {
-                cv::rectangle(frame, tracking_roi, cv::Scalar(0, 255, 0), 2);
-            }
             visualizer.draw_offset_text(frame, display_center, target_color, is_predicted);
-            if (ballistic_point != nullptr) {
-                visualizer.draw_ballistic_point(frame, ballistic_point);
-            }
+            visualizer.draw_ballistic_point(frame, ballistic_point);
 
             // Output control data to TTL (passing use_ttl to control whether to send)
-            // Now sending on every frame for smoother control
-            // Only send if ballistic_point is not null and we have a valid display center
-            if (display_center != nullptr && ballistic_point != nullptr) {
-                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, 10000, on_pitch_kp_trackbar);
-                cv::createTrackbar("Pitch Ki", "PID Tuning", nullptr, 10000, on_pitch_ki_trackbar);
-                cv::createTrackbar("Pitch Kd", "PID Tuning", nullptr, 10000, on_pitch_kd_trackbar);
-                
-                // Create trackbars for yaw PID parameters
-                cv::createTrackbar("Yaw Kp", "PID Tuning", nullptr, 10000, on_yaw_kp_trackbar);
-                cv::createTrackbar("Yaw Ki", "PID Tuning", nullptr, 10000, on_yaw_ki_trackbar);
-                cv::createTrackbar("Yaw Kd", "PID Tuning", nullptr, 10000, on_yaw_kd_trackbar);
-                
-                // Set initial positions
-                cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(g_pitch_kp * 10000));
-                cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(g_pitch_ki * 10000));
-                cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(g_pitch_kd * 10000));
-                cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(g_yaw_kp * 10000));
-                cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(g_yaw_ki * 10000));
-                cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(g_yaw_kd * 10000));
-               
-                initialized_trackbars = true;
-            }
+            output_control_data(display_center, target_color, frame_counter, ttl, img_center, use_ttl);
 
             // Display windows
             cv::imshow("Armor Detection", frame);
-            //cv::imshow(target_color + " Mask", mask);
-            //cv::imshow(target_color + " Only", color_only_frame);
-
-            // Only call cv::waitKey to keep GUI responsive
-            cv::waitKey(1);
+            cv::imshow(target_color + " Mask", mask);
+            cv::imshow(target_color + " Only", color_only_frame);
 
             // Exit on 'q' key press
             if (cv::waitKey(1) == 'q') {
@@ -416,17 +236,6 @@ int main(int /*argc*/, char* /*argv*/[]) {
 
             frame_counter++;
 
-            float area = 0.0; 
-            if (!armor_plates.empty()) {
-            // 获取装甲板配对的第一个灯条
-                const LightBar& light_bar = armor_plates[0].pair.first;
-                // 直接使用灯条的area（即rect_area）赋值
-                area = light_bar.area;
-                // 额外校验：确保灯条面积有效（避免负数/0值）
-                if (area <= 0) {
-                    area = 0.0f;
-                }
-            }
             // Control max frame rate (100 FPS)
             auto end_time = std::chrono::high_resolution_clock::now();
             auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - current_time).count();
@@ -469,4 +278,4 @@ int main(int /*argc*/, char* /*argv*/[]) {
     }
 
     return 0;
-}
+}

src/PidController.cpp

@@ -1,87 +0,0 @@
-#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_(-400.0f), output_max_(400.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_;
-}