低延迟Double-Edge PWM的设计

本文探讨了传统PWM的Trailing-Edge和Leading-Edge实现及其缺点,并详细介绍了Double-Edge PWM如何通过降低延迟来改进。此外,还讨论了基于环形振荡器的PWM设计和低延迟Double-Edge PWM的实现方法。



传统PWM

 

Trailing-Edge PWM实现框图:


Trailing-Edge PWM原理:

    CLK置位锁存器Latch

    比较器输出复位锁存器Latch

    开关周期开始,PWM输出1

    Vramp > Vc时,PWM输出0,直到下一个开关周期开始


Trailing-Edge PWM缺点:


Leading-Edge PWM实现框图:

 

Leading-Edge PWM原理:

    比较器输出置位锁存器Latch

    CLK复位锁存器Latch

    开关周期开始,PWM输出0

 

#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; }
最新发布
10-06
#include <pigpio.h> #include <iostream> #include <unistd.h> #include <opencv2/opencv.hpp> #include <vector> using namespace cv; using namespace std; // ---------- 函数声明 ---------- void GetROI(Mat src, Mat &ROI); int detectAB(Mat &img); // 优化后的字母检测函数 double PID(double error1); int atc(int angle); int dong(); int xunji(Mat frame); void preprocessImg(Mat img, Mat &gray, 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; double kp = 0.3, kd = 0.08; double min_ang = 70, max_ang = 94; // ---------- ROI裁剪 ---------- void GetROI(Mat src, Mat &ROI) { int width = src.cols; int height = src.rows; Rect rect(Point(width/4, height/2), Point(3*width/4, height)); ROI = src(rect).clone(); } // ---------- 图像预处理 ---------- void preprocessImg(Mat img, Mat &gray, Mat &canny) { cvtColor(img, gray, COLOR_BGR2GRAY); GaussianBlur(gray, gray, Size(5, 5), 0.5, 0.5); Canny(gray, canny, 60, 120, 3); } // ---------- PID控制 ---------- double PID(double error) { double angle = kp * error + kd * (error - last_error); last_error = 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初始化失败!" << 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); sleep(2); gpioPWM(servo_pin, atc(82)); cout << "硬件初始化完成(舵机居中82°,电机就绪)" << endl; sleep(2); return 0; } int detectAB(Mat &img) { Mat roi, hsv; GetROI(img, roi); // 保留ROI处理 // 颜色空间转换与掩码处理 cvtColor(roi, hsv, COLOR_BGR2HSV); Mat blueMask, whiteMask, letters; //inRange(hsv, Scalar(90, 70, 70), Scalar(130, 255, 255), blueMask); // 蓝底 inRange(hsv, Scalar(0, 0, 200), Scalar(180, 30, 255), whiteMask); // 白字 Mat redMaskLow, redMaskHigh; inRange(hsv, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), redMaskLow); inRange(hsv, cv::Scalar(160, 100, 100), cv::Scalar(180, 255, 255), redMaskHigh); Mat redMask = redMaskLow | redMaskHigh; // 形态学优化(保留必要操作) Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3)); morphologyEx(whiteMask, whiteMask, MORPH_OPEN, kernel); // 联合掩码:只保留蓝底上的白字 //letters = whiteMask & blueMask; letters= whiteMask & redMask; // 轮廓检测与分析 vector<vector<Point>> contours; findContours(letters, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE); for (auto &contour : contours) { double area = contourArea(contour); // 面积过滤噪声 if (area < 500 || area > 5000) continue; Rect rect = boundingRect(contour); double aspect_ratio = (double)rect.width / rect.height; // 字母分类逻辑简化 if (aspect_ratio < 0.82) { // A通常细高 // 坐标转换到原图并绘制结果 Rect global_rect( rect.x + img.cols/4, rect.y + img.rows/2, rect.width, rect.height ); rectangle(img, global_rect, Scalar(0, 255, 255), 2); putText(img, "A", Point(global_rect.x, global_rect.y-10), FONT_HERSHEY_SIMPLEX, 1.2, Scalar(0, 215, 255), 2); return 1; } else if (aspect_ratio > 0.85 && aspect_ratio < 1.2) { // B接近正方 Rect global_rect( rect.x + img.cols/4, rect.y + img.rows/2, rect.width, rect.height ); rectangle(img, global_rect, Scalar(0, 255, 255), 2); putText(img, "B", Point(global_rect.x, global_rect.y-10), FONT_HERSHEY_SIMPLEX, 1.2, Scalar(0, 215, 255), 2); return 2; } } return 0; // 未检测到 } // ---------- 巡线控制 ---------- int xunji(Mat frame) { Mat img, gray, canny; GetROI(frame, img); preprocessImg(img, gray, canny); int cols = canny.cols, rows = canny.rows; vector<int> left(rows, -1), right(rows, -1); // 改为rows,因为我们要存储每一行的值 vector<int> mid(rows, -1); int mid_sum = 0, valid_cnt = 0; int start_row = min(300, rows - 1); int end_row = max(250, 0); // 从下往上扫描,从start_row到end_row for (int i = start_row; i >= end_row; i--) { bool left_lost = true, right_lost = true; int left_edge = -1, right_edge = -1; // 寻找左边缘:从中间向左搜索 for (int j = cols / 2; j >= 100; j--) { if (canny.at<uchar>(i, j) == 255) { left_edge = j; left_lost = false; break; } } if (left_lost) { left_edge = 0; // 如果没找到,设为0 } // 寻找右边缘:从中间向右搜索 for (int j = cols / 2; j < cols - 100; j++) { if (canny.at<uchar>(i, j) == 255) { right_edge = j; right_lost = false; break; } } if (right_lost) { right_edge = cols - 1; } // 如果左右边缘都找到,则计算中心 if (!left_lost && !right_lost) { int center = (left_edge + right_edge) / 2; mid[valid_cnt] = center; mid_sum += center; valid_cnt++; } } if (valid_cnt > 0) { double center_avg = mid_sum / valid_cnt; double error = center_avg - cols / 2; double angle = PID(error); angle = 82 - angle; // 82度是居中,根据误差调整 angle = max(min_ang, min(max_ang, angle)); gpioPWM(servo_pin, atc((int)angle)); } return 0; } // ---------- 主函数 ---------- int main() { if (dong() < 0) { cout << "硬件初始化失败,程序退出" << endl; return -1; } VideoCapture cap(0); if (!cap.isOpened()) { cout << "摄像头打开失败!" << endl; gpioPWM(pwm_pin, 0); gpioTerminate(); return -1; } Mat frame; bool isStopped = false; while (cap.read(frame) && !isStopped) { char key = waitKey(10); if (key == 27) break; if (!isStopped) { xunji(frame); } // 调用优化后的检测函数 int abType = detectAB(frame); if (abType == 1 && !isStopped) { cout << "检测到字母A → 左转并停车" << endl; gpioPWM(servo_pin, atc(75)); usleep(500000); gpioPWM(pwm_pin, 0); sleep(3); gpioPWM(servo_pin, atc(82)); isStopped = true; } else if (abType == 2 && !isStopped) { cout << "检测到字母B → 右转并停车" << endl; gpioPWM(servo_pin, atc(89)); usleep(500000); gpioPWM(pwm_pin, 0); sleep(3); gpioPWM(servo_pin, atc(82)); isStopped = true; } } gpioPWM(pwm_pin, 0); gpioPWM(servo_pin, atc(82)); gpioTerminate(); cap.release(); cout << "程序正常退出" << endl; return 0; }
09-26
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值