#include <pigpio.h>
#include <iostream>
#include <unistd.h>
#include <opencv2/opencv.hpp>
#include <vector>
using namespace cv;
using namespace std;
// ---------- 配置参数 ----------
const int BACKGROUND_COLOR = 1;  // 1: 红色背景, 2: 蓝色背景
// ---------- 函数声明 ----------
void GetLineROI(Mat src, Mat &ROI);
void GetSignROI(Mat src, Mat &ROI);
void GetROI(Mat src, Mat &ROI);
bool detectBackgroundPresence(Mat &img);
int detectABLetter(Mat &img);
double PID(double error1);
int atc(int angle);
int dong();
int xunji(Mat frame);
void preprocessImg(Mat img, Mat &gray, Mat &canny);
void saveDebugImage(Mat img, string name, int frame_count);
bool detect_A_shape(Mat &roi);
bool detect_B_shape(Mat &roi);
void performAction(int letter);
Mat preprocessForLetter(Mat &roi);
void improveLineDetection(Mat &canny);
// ---------- 全局变量 ----------
const int pwm_pin = 13;
const int servo_pin = 12;
const int pwm_freq1 = 200;
const int pwm_freq2 = 50;
const int pwm_range1 = 40000;
const int pwm_range2 = 100;
const int init_duty = 10000;
double last_error = 0;
const double kp = 0.25, kd = 0.06;  // 进一步降低PID参数
const double min_ang = 75, max_ang = 89;  // 缩小角度范围
bool debug_save = true;
bool ab_detected = false;
int detected_letter = 0;
int action_cooldown = 0;
// ---------- 保存调试图像 ----------
void saveDebugImage(Mat img, string name, int frame_count) {
    if (debug_save && frame_count % 100 == 0) {  // 进一步减少保存频率
        string filename = "/tmp/" + name + "_" + to_string(frame_count) + ".jpg";
        imwrite(filename, img);
        cout << "Saved debug image: " << filename << endl;
    }
}
// ---------- 循迹ROI ----------
void GetROI(Mat src, Mat &ROI) {
    int width = src.cols;
    int height = src.rows;
    // 调整ROI,使用更靠近车辆的区域
    Rect rect(Point(width/4, height*3/4), Point(3*width/4, height-20));
    ROI = src(rect).clone();
}
// ---------- 标志检测ROI ----------
void GetSignROI(Mat src, Mat &ROI) {
    int width = src.cols;
    int height = src.rows;
    // 扩大检测区域
    Rect rect(Point(width/4, height/3), Point(3*width/4, 2*height/3));
    ROI = src(rect).clone();
}
// ---------- 图像预处理(优化版) ----------
void preprocessImg(Mat img, Mat &gray, Mat &canny) {
    cvtColor(img, gray, COLOR_BGR2GRAY);
    GaussianBlur(gray, gray, Size(7, 7), 1.0, 1.0);  // 增加模糊程度
    
    // 使用自适应Canny阈值
    double median_val = cv::median(gray);
    int lower_thresh = max(0, (int)(0.5 * median_val));
    int upper_thresh = min(255, (int)(1.5 * median_val));
    
    Canny(gray, canny, lower_thresh, upper_thresh, 3);
    
    // 改善线条检测
    improveLineDetection(canny);
}
// ---------- 改善线条检测 ----------
void improveLineDetection(Mat &canny) {
    // 使用形态学操作连接断开的边缘
    Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
    morphologyEx(canny, canny, MORPH_CLOSE, kernel);
    
    // 去除小噪点
    vector<vector<Point>> contours;
    findContours(canny, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
    
    for (size_t i = 0; i < contours.size(); i++) {
        if (contourArea(contours[i]) < 10) {
            drawContours(canny, contours, (int)i, Scalar(0), -1);
        }
    }
}
// ---------- 字母预处理 ----------
Mat preprocessForLetter(Mat &roi) {
    Mat gray, binary;
    
    if (roi.channels() == 3) {
        cvtColor(roi, gray, COLOR_BGR2GRAY);
    } else {
        gray = roi.clone();
    }
    
    GaussianBlur(gray, gray, Size(5, 5), 1.0);
    
    // 尝试多种二值化方法
    Mat binary1, binary2;
    adaptiveThreshold(gray, binary1, 255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY, 11, 2);
    threshold(gray, binary2, 0, 255, THRESH_BINARY | THRESH_OTSU);
    
    // 选择效果更好的二值化结果
    int nonZero1 = countNonZero(binary1);
    int nonZero2 = countNonZero(binary2);
    
    binary = (nonZero1 > nonZero2) ? binary1 : binary2;
    
    // 对于深色背景浅色字母的情况,可能需要反转
    if (nonZero1 < roi.total() * 0.3 || nonZero2 < roi.total() * 0.3) {
        bitwise_not(binary, binary);
    }
    
    Mat kernel_open = getStructuringElement(MORPH_RECT, Size(2, 2));
    Mat kernel_close = getStructuringElement(MORPH_RECT, Size(6, 6));
    
    morphologyEx(binary, binary, MORPH_OPEN, kernel_open);
    morphologyEx(binary, binary, MORPH_CLOSE, kernel_close);
    
    return binary;
}
// ---------- 背景存在检测(优化版) ----------
bool detectBackgroundPresence(Mat &img) {
    Mat roi;
    GetSignROI(img, roi);
    
    if (roi.empty()) {
        return false;
    }
    
    Mat hsv;
    cvtColor(roi, hsv, COLOR_BGR2HSV);
    
    Mat color_mask;
    if (BACKGROUND_COLOR == 1) {
        // 扩大红色检测范围
        Mat red_mask1, red_mask2, red_mask3;
        inRange(hsv, Scalar(0, 30, 30), Scalar(20, 255, 255), red_mask1);
        inRange(hsv, Scalar(150, 30, 30), Scalar(180, 255, 255), red_mask2);
        // 增加亮度较低的红色检测
        inRange(hsv, Scalar(0, 50, 20), Scalar(10, 255, 100), red_mask3);
        color_mask = red_mask1 | red_mask2 | red_mask3;
    } else {
        // 扩大蓝色检测范围
        Mat blue_mask1, blue_mask2;
        inRange(hsv, Scalar(85, 30, 30), Scalar(145, 255, 255), blue_mask1);
        inRange(hsv, Scalar(100, 40, 20), Scalar(140, 255, 100), blue_mask2);
        color_mask = blue_mask1 | blue_mask2;
    }
    
    // 使用形态学操作填充区域
    Mat kernel_dilate = getStructuringElement(MORPH_ELLIPSE, Size(7, 7));
    dilate(color_mask, color_mask, kernel_dilate);
    
    Mat kernel_close = getStructuringElement(MORPH_ELLIPSE, Size(15, 15));
    morphologyEx(color_mask, color_mask, MORPH_CLOSE, kernel_close);
    
    int color_pixels = countNonZero(color_mask);
    int total_pixels = roi.rows * roi.cols;
    double color_ratio = (double)color_pixels / total_pixels;
    
    cout << "Background presence - Color pixels: " << color_pixels 
         << " / " << total_pixels << " (" << color_ratio*100 << "%)" << endl;
    
    saveDebugImage(color_mask, "bg_color_mask", 0);
    
    // 大幅降低阈值
    return color_ratio > 0.005;  // 从0.02降到0.005
}
// ---------- A/B字母检测 ----------
int detectABLetter(Mat &img) {
    Mat roi;
    GetSignROI(img, roi);
    
    if (roi.empty()) {
        cout << "Sign ROI is empty!" << endl;
        return 0;
    }
    
    saveDebugImage(roi, "sign_roi", 0);
    
    // 直接使用颜色分割尝试找到字母
    Mat hsv;
    cvtColor(roi, hsv, COLOR_BGR2HSV);
    
    Mat letter_mask;
    if (BACKGROUND_COLOR == 1) {
        // 红色背景上的字母可能是黑色或白色
        // 寻找非红色区域
        Mat red_mask;
        inRange(hsv, Scalar(0, 30, 30), Scalar(20, 255, 255), red_mask);
        inRange(hsv, Scalar(150, 30, 30), Scalar(180, 255, 255), letter_mask);
        letter_mask = ~red_mask;  // 反转,得到非红色区域
    } else {
        // 蓝色背景上的字母
        Mat blue_mask;
        inRange(hsv, Scalar(85, 30, 30), Scalar(145, 255, 255), blue_mask);
        letter_mask = ~blue_mask;
    }
    
    // 形态学操作
    Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
    morphologyEx(letter_mask, letter_mask, MORPH_OPEN, kernel);
    
    saveDebugImage(letter_mask, "letter_mask", 0);
    
    vector<vector<Point>> contours;
    findContours(letter_mask, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
    
    cout << "Found " << contours.size() << " contours for letter detection" << endl;
    
    sort(contours.begin(), contours.end(), 
         [](const vector<Point>& a, const vector<Point>& b) {
             return contourArea(a) > contourArea(b);
         });
    
    if (!contours.empty()) {
        double area = contourArea(contours[0]);
        Rect rect = boundingRect(contours[0]);
        
        double aspect_ratio = (double)rect.width / rect.height;
        
        cout << "Main contour - Area: " << area 
             << ", Bounding rect: " << rect.width << "x" << rect.height
             << ", Aspect ratio: " << aspect_ratio << endl;
        
        // 放宽条件
        if (area > 200 && area < 50000 && aspect_ratio > 0.3 && aspect_ratio < 3.0) {
            Mat letter_roi = letter_mask(rect);
            saveDebugImage(letter_roi, "letter_candidate", 0);
            
            cout << "  -> Testing for A/B shape" << endl;
            
            if (detect_A_shape(letter_roi)) {
                cout << "*** Detected A ***" << endl;
                Rect global_rect(
                    rect.x + img.cols/4,
                    rect.y + img.rows/3,
                    rect.width,
                    rect.height
                );
                rectangle(img, global_rect, Scalar(0, 255, 0), 3);
                putText(img, "A", Point(global_rect.x, global_rect.y-10),
                        FONT_HERSHEY_SIMPLEX, 1.5, Scalar(0, 255, 0), 3);
                return 1;
            }
            else if (detect_B_shape(letter_roi)) {
                cout << "*** Detected B ***" << endl;
                Rect global_rect(
                    rect.x + img.cols/4,
                    rect.y + img.rows/3,
                    rect.width,
                    rect.height
                );
                rectangle(img, global_rect, Scalar(0, 255, 0), 3);
                putText(img, "B", Point(global_rect.x, global_rect.y-10),
                        FONT_HERSHEY_SIMPLEX, 1.5, Scalar(0, 255, 0), 3);
                return 2;
            } else {
                cout << "  -> Not A or B" << endl;
            }
        } else {
            cout << "  -> Skipped due to area/aspect ratio" << endl;
        }
    }
    
    return 0;
}
// ---------- A形状检测 ----------
bool detect_A_shape(Mat &roi) {
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(roi.clone(), contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);
    
    if (hierarchy.empty() || contours.empty()) return false;
    
    int valid_hole_count = 0;
    
    // 找到最大的轮廓
    int main_contour_idx = -1;
    double max_area = 0;
    for (int i = 0; i < contours.size(); i++) {
        double area = contourArea(contours[i]);
        if (area > max_area) {
            max_area = area;
            main_contour_idx = i;
        }
    }
    
    if (main_contour_idx == -1) return false;
    
    // 统计孔洞
    for (int i = 0; i < hierarchy.size(); i++) {
        if (hierarchy[i][3] == main_contour_idx) {
            double hole_area = contourArea(contours[i]);
            if (hole_area > 10) {  // 最小孔洞面积
                valid_hole_count++;
            }
        }
    }
    
    cout << "  A shape - Valid holes: " << valid_hole_count << endl;
    
    return (valid_hole_count >= 1);  // A至少有一个孔洞
}
// ---------- B形状检测 ----------
bool detect_B_shape(Mat &roi) {
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(roi.clone(), contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);
    
    if (hierarchy.empty() || contours.empty()) return false;
    
    int valid_hole_count = 0;
    
    // 找到最大的轮廓
    int main_contour_idx = -1;
    double max_area = 0;
    for (int i = 0; i < contours.size(); i++) {
        double area = contourArea(contours[i]);
        if (area > max_area) {
            max_area = area;
            main_contour_idx = i;
        }
    }
    
    if (main_contour_idx == -1) return false;
    
    // 统计孔洞
    for (int i = 0; i < hierarchy.size(); i++) {
        if (hierarchy[i][3] == main_contour_idx) {
            double hole_area = contourArea(contours[i]);
            if (hole_area > 10) {  // 最小孔洞面积
                valid_hole_count++;
            }
        }
    }
    
    cout << "  B shape - Valid holes: " << valid_hole_count << endl;
    
    return (valid_hole_count >= 2);  // B至少有两个孔洞
}
// ---------- PID控制(稳定版) ----------
double PID(double error) {
    // 限制误差范围
    double limited_error = max(min(error, 50.0), -50.0);
    double angle = kp * limited_error + kd * (limited_error - last_error);
    last_error = limited_error;
    return angle;
}
// ---------- 舵机角度转占空比 ----------
int atc(int angle) {
    int pulse_width = 500 + (angle / 180.0) * 2000;
    int duty = pulse_width / 20000.0 * 100;
    return duty;
}
// ---------- 硬件初始化 ----------
int dong() {
    if (gpioInitialise() < 0) {
        cout << "pigpio init error" << endl;
        return -1;
    }
    gpioSetMode(pwm_pin, PI_OUTPUT);
    gpioSetPWMfrequency(pwm_pin, pwm_freq1);
    gpioSetPWMrange(pwm_pin, pwm_range1);
    gpioSetMode(servo_pin, PI_OUTPUT);
    gpioSetPWMfrequency(servo_pin, pwm_freq2);
    gpioSetPWMrange(servo_pin, pwm_range2);
    gpioPWM(pwm_pin, init_duty);
    gpioPWM(servo_pin, atc(82));
    cout << "pigpio ok!" << endl;
    sleep(2);
    return 0;
}
// ---------- 巡线控制(稳定版) ----------
int xunji(Mat frame) {
    Mat img, gray, canny;
    GetROI(frame, img);
    
    if (img.empty()) {
        cout << "Line ROI is empty!" << endl;
        return 0;
    }
    
    preprocessImg(img, gray, canny);
    
    int cols = canny.cols;
    int rows = canny.rows;
    
    saveDebugImage(canny, "line_canny", 0);
    
    // 改进的巡线算法 - 使用加权平均
    vector<double> weights;
    vector<int> centers;
    double total_weight = 0;
    double weighted_sum = 0;
    
    // 从底部向上扫描,越靠近底部权重越高
    for (int i = rows-1; i >= max(0, rows-80); i--) {
        int left_edge = -1;
        int right_edge = -1;
        
        // 寻找左边缘
        for (int j = cols/3; j >= 5; j--) {
            if (canny.at<uchar>(i, j) == 255) {
                left_edge = j;
                break;
            }
        }
        
        // 寻找右边缘
        for (int j = 2*cols/3; j < cols-5; j++) {
            if (canny.at<uchar>(i, j) == 255) {
                right_edge = j;
                break;
            }
        }
        
        // 如果找到左右边缘,计算中心点
        if (left_edge != -1 && right_edge != -1) {
            int center = (left_edge + right_edge) / 2;
            double weight = (double)(i + 1) / rows;  // 底部权重更高
            
            centers.push_back(center);
            weights.push_back(weight);
            weighted_sum += center * weight;
            total_weight += weight;
        }
    }
    
    if (total_weight > 0 && !centers.empty()) {
        double center_avg = weighted_sum / total_weight;
        double error = center_avg - cols / 2.0;
        
        cout << "Line following - Valid rows: " << centers.size() 
             << ", Center avg: " << center_avg << ", Target: " << cols/2 
             << ", Error: " << error << endl;
        
        double angle = PID(error);
        angle = 82 - angle;
        angle = max(min_ang, min(max_ang, angle));
        
        cout << "Calculated angle: " << angle << endl;
        gpioPWM(servo_pin, atc((int)angle));
        return 1;
    } else {
        cout << "Line lost! Only " << centers.size() << " valid rows found." << endl;
        // 丢失线路时保持直行,但稍微减速
        gpioPWM(servo_pin, atc(82));
        gpioPWM(pwm_pin, 8500);  // 减速
        return 0;
    }
}
// ---------- 执行动作函数 ----------
void performAction(int letter) {
    if (letter == 1) {
        cout << "=== Performing A Action: Turn Left ===" << endl;
        
        // 减速
        gpioPWM(pwm_pin, 8000);
        usleep(300000);
        
        // 左转
        gpioPWM(servo_pin, atc(75));
        usleep(800000);
        
        // 转回直行并恢复正常速度
        gpioPWM(servo_pin, atc(82));
        gpioPWM(pwm_pin, 9300);
        usleep(400000);
        
    } else if (letter == 2) {
        cout << "=== Performing B Action: Turn Right ===" << endl;
        
        // 减速
        gpioPWM(pwm_pin, 8000);
        usleep(300000);
        
        // 右转
        gpioPWM(servo_pin, atc(89));
        usleep(800000);
        
        // 转回直行并恢复正常速度
        gpioPWM(servo_pin, atc(82));
        gpioPWM(pwm_pin, 9300);
        usleep(400000);
    }
}
// ---------- 主函数 ----------
int main() {
    cout << "=== STABLE LINE FOLLOWING WITH A/B DETECTION ===" << endl;
    cout << "Background Color: " << (BACKGROUND_COLOR == 1 ? "RED" : "BLUE") << endl;
    cout << "A: Turn Left, B: Turn Right" << endl;
    cout << "=====================================" << endl;
    
    if (dong() < 0) {
        cout << "pigpio error !" << endl;
        return -1;
    }
    
    VideoCapture cap(0);
    if (!cap.isOpened()) {
        cout << "cap error" << endl;
        gpioPWM(pwm_pin, 0);
        gpioTerminate();
        return -1;
    }
    
    // 设置摄像头参数
    cap.set(CAP_PROP_FRAME_WIDTH, 640);
    cap.set(CAP_PROP_FRAME_HEIGHT, 480);
    cap.set(CAP_PROP_FPS, 30);
    cap.set(CAP_PROP_BRIGHTNESS, 50);
    cap.set(CAP_PROP_CONTRAST, 50);
    cap.set(CAP_PROP_SATURATION, 60);  // 提高饱和度有助于颜色检测
    
    Mat frame;
    int frame_count = 0;
    int detection_interval = 15;  // 降低检测频率
    int line_lost_count = 0;
    const int max_line_lost = 30;  // 降低丢失阈值
    int sign_detection_count = 0;
    const int sign_confirm_threshold = 2;
    
    cout << "Starting stable line following..." << endl;
    
    // 初始前进
    gpioPWM(pwm_pin, 9300);
    
    try {
        while (cap.read(frame)) {
            frame_count++;
            
            // 正常巡线
            int line_status = xunji(frame);
            if (line_status == 0) {
                line_lost_count++;
                if (line_lost_count > max_line_lost) {
                    cout << "Line lost for too long, stopping!" << endl;
                    gpioPWM(pwm_pin, 6000);
                    break;
                }
            } else {
                line_lost_count = 0;
                // 恢复速度
                gpioPWM(pwm_pin, 9300);
            }
            
            // 检测标志和字母(降低频率)
            if (frame_count % detection_interval == 0 && action_cooldown <= 0) {
                bool background_present = detectBackgroundPresence(frame);
                
                if (background_present) {
                    sign_detection_count++;
                    cout << "Background detected: " << sign_detection_count << "/" << sign_confirm_threshold << endl;
                    
                    if (sign_detection_count >= sign_confirm_threshold && !ab_detected) {
                        int detected = detectABLetter(frame);
                        
                        if (detected != 0) {
                            ab_detected = true;
                            detected_letter = detected;
                            cout << "=== LETTER DETECTED: " << (detected == 1 ? "A" : "B") << " ===" << endl;
                            
                            performAction(detected);
                            action_cooldown = 120;  // 冷却时间
                            sign_detection_count = 0;
                        }
                    }
                } else {
                    sign_detection_count = 0;
                }
            }
            
            // 更新冷却时间
            if (action_cooldown > 0) {
                action_cooldown--;
            }
            
            // 重置检测状态
            if (action_cooldown == 0 && ab_detected) {
                ab_detected = false;
                detected_letter = 0;
                cout << "Reset detection state, ready for next sign" << endl;
            }
            
            usleep(20000);  // 增加延迟,提高稳定性
        }
    } catch (const exception& e) {
        cout << "Exception occurred: " << e.what() << endl;
    }
    
    // 清理
    gpioPWM(pwm_pin, 0);
    gpioPWM(servo_pin, atc(82));
    gpioTerminate();
    cap.release();
    
    cout << "Program exited!" << endl;
    return 0;
}
					最新发布