/* G3 09 - Xu ly nhieu anh sang va nen phuc tap Board: ESP32-CAM AI Thinker + OV2640 Compile target: esp32:esp32:esp32cam */ #include "esp_camera.h" #define PWDN_GPIO_NUM 32 #define RESET_GPIO_NUM -1 #define XCLK_GPIO_NUM 0 #define SIOD_GPIO_NUM 26 #define SIOC_GPIO_NUM 27 #define Y9_GPIO_NUM 35 #define Y8_GPIO_NUM 34 #define Y7_GPIO_NUM 39 #define Y6_GPIO_NUM 36 #define Y5_GPIO_NUM 21 #define Y4_GPIO_NUM 19 #define Y3_GPIO_NUM 18 #define Y2_GPIO_NUM 5 #define VSYNC_GPIO_NUM 25 #define HREF_GPIO_NUM 23 #define PCLK_GPIO_NUM 22 const int SAMPLE_STEP = 2; const int MIN_TARGET_PIXELS = 35; unsigned long frameId = 0; struct RobustReport { bool found; int x; int y; int offset; long count; float coverage; float meanBrightness; int minChroma; const char *light; const char *quality; }; bool setupCamera() { camera_config_t config; config.ledc_channel = LEDC_CHANNEL_0; config.ledc_timer = LEDC_TIMER_0; config.pin_d0 = Y2_GPIO_NUM; config.pin_d1 = Y3_GPIO_NUM; config.pin_d2 = Y4_GPIO_NUM; config.pin_d3 = Y5_GPIO_NUM; config.pin_d4 = Y6_GPIO_NUM; config.pin_d5 = Y7_GPIO_NUM; config.pin_d6 = Y8_GPIO_NUM; config.pin_d7 = Y9_GPIO_NUM; config.pin_xclk = XCLK_GPIO_NUM; config.pin_pclk = PCLK_GPIO_NUM; config.pin_vsync = VSYNC_GPIO_NUM; config.pin_href = HREF_GPIO_NUM; config.pin_sccb_sda = SIOD_GPIO_NUM; config.pin_sccb_scl = SIOC_GPIO_NUM; config.pin_pwdn = PWDN_GPIO_NUM; config.pin_reset = RESET_GPIO_NUM; config.xclk_freq_hz = 20000000; config.pixel_format = PIXFORMAT_RGB565; config.frame_size = FRAMESIZE_QQVGA; config.jpeg_quality = 12; config.fb_count = 1; return esp_camera_init(&config) == ESP_OK; } void rgb565ToRgb(uint16_t pixel, int &r, int &g, int &b) { r = ((pixel >> 11) & 0x1F) * 255 / 31; g = ((pixel >> 5) & 0x3F) * 255 / 63; b = (pixel & 0x1F) * 255 / 31; } const char *classifyLight(float meanBrightness) { if (meanBrightness < 45.0) return "TOO_DARK"; if (meanBrightness > 205.0) return "TOO_BRIGHT"; return "OK"; } bool isAdaptiveRed(int r, int g, int b, int minChroma) { int strongestOther = max(g, b); int chroma = r - strongestOther; int value = max(r, max(g, b)); return value > 45 && chroma >= minChroma && r > 70; } RobustReport analyzeFrame(camera_fb_t *frame) { int width = frame->width; int height = frame->height; long brightnessSum = 0; long samples = 0; for (int y = 0; y < height; y += SAMPLE_STEP) { for (int x = 0; x < width; x += SAMPLE_STEP) { int index = (y * width + x) * 2; if (index + 1 >= (int)frame->len) continue; uint16_t pixel = ((uint16_t)frame->buf[index] << 8) | frame->buf[index + 1]; int r, g, b; rgb565ToRgb(pixel, r, g, b); brightnessSum += (r + g + b) / 3; samples++; } } float meanBrightness = samples > 0 ? brightnessSum / (float)samples : 0.0; int minChroma = meanBrightness < 70.0 ? 22 : (meanBrightness > 190.0 ? 48 : 35); long sumX = 0; long sumY = 0; long count = 0; for (int y = 0; y < height; y += SAMPLE_STEP) { for (int x = 0; x < width; x += SAMPLE_STEP) { int index = (y * width + x) * 2; if (index + 1 >= (int)frame->len) continue; uint16_t pixel = ((uint16_t)frame->buf[index] << 8) | frame->buf[index + 1]; int r, g, b; rgb565ToRgb(pixel, r, g, b); if (isAdaptiveRed(r, g, b, minChroma)) { sumX += x; sumY += y; count++; } } } RobustReport report; report.found = count >= MIN_TARGET_PIXELS; report.x = report.found ? sumX / count : -1; report.y = report.found ? sumY / count : -1; report.offset = report.found ? report.x - width / 2 : 0; report.count = count; report.coverage = count * SAMPLE_STEP * SAMPLE_STEP * 100.0 / (width * height); report.meanBrightness = meanBrightness; report.minChroma = minChroma; report.light = classifyLight(meanBrightness); if (!report.found) report.quality = "NO_TARGET"; else if (report.light[0] != 'O') report.quality = "CHECK_LIGHT"; else report.quality = "TRACKABLE"; return report; } void printReport(const RobustReport &report, int width, int height) { Serial.print("ROBUST_TARGET,frame="); Serial.print(frameId); Serial.print(",found="); Serial.print(report.found ? "YES" : "NO"); Serial.print(",x="); Serial.print(report.x); Serial.print(",y="); Serial.print(report.y); Serial.print(",offset="); Serial.print(report.offset); Serial.print(",count="); Serial.print(report.count); Serial.print(",coverage="); Serial.print(report.coverage, 1); Serial.print(",mean="); Serial.print(report.meanBrightness, 1); Serial.print(",min_chroma="); Serial.print(report.minChroma); Serial.print(",light="); Serial.print(report.light); Serial.print(",quality="); Serial.print(report.quality); Serial.print(",width="); Serial.print(width); Serial.print(",height="); Serial.println(height); } void setup() { Serial.begin(115200); delay(1000); Serial.println("G3 09 robust color target tracker"); if (!setupCamera()) { Serial.println("camera=INIT_FAILED"); while (true) delay(1000); } Serial.println("camera=READY format=RGB565 frame=QQVGA target=RED"); } void loop() { camera_fb_t *frame = esp_camera_fb_get(); if (!frame) { Serial.println("ROBUST_TARGET,found=NO,reason=CAPTURE_FAILED"); delay(500); return; } frameId++; RobustReport report = analyzeFrame(frame); printReport(report, frame->width, frame->height); esp_camera_fb_return(frame); delay(220); }

Xử lý nhiễu ánh sáng và nền phức tạp

Đăng nhập để học miễn phí