/* G3 12 - Ket hop camera va cam bien khoang cach Board: ESP32-S3 DevKit + TB6612FNG + HC-SR04 + ESP32-CAM sign packet Compile target: esp32:esp32:esp32s3 */ const int PIN_TRIG = 5; const int PIN_ECHO = 18; const int PIN_LED = 2; const int PIN_BUZZER = 15; const int PIN_STBY = 33; const int VISION_RX = 16; const int VISION_TX = 17; const int L_PWM = 25; const int L_IN1 = 26; const int L_IN2 = 27; const int R_PWM = 14; const int R_IN1 = 12; const int R_IN2 = 13; const int SLOW_SPEED = 55; const int CAUTION_SPEED = 40; const float SAFE_STOP_CM = 18.0; const unsigned long SIGN_TIMEOUT_MS = 900; const unsigned long SENSOR_INTERVAL_MS = 90; const unsigned long LOG_INTERVAL_MS = 300; bool armed = false; String signName = "NONE"; String signColor = "NONE"; String signShape = "NONE"; long signCount = 0; float distanceCm = -1.0; unsigned long lastSignMs = 0; unsigned long lastSensorMs = 0; unsigned long lastLogMs = 0; String lastAction = "BOOT"; void setMotor(int pwmPin, int in1, int in2, int speed) { speed = constrain(speed, -255, 255); digitalWrite(in1, speed >= 0 ? HIGH : LOW); digitalWrite(in2, speed >= 0 ? LOW : HIGH); analogWrite(pwmPin, abs(speed)); } void drive(int leftSpeed, int rightSpeed, const String &action) { setMotor(L_PWM, L_IN1, L_IN2, leftSpeed); setMotor(R_PWM, R_IN1, R_IN2, rightSpeed); lastAction = action; } void stopRobot(const String &reason) { drive(0, 0, "STOP_" + reason); } float readDistanceCm() { digitalWrite(PIN_TRIG, LOW); delayMicroseconds(2); digitalWrite(PIN_TRIG, HIGH); delayMicroseconds(10); digitalWrite(PIN_TRIG, LOW); unsigned long duration = pulseIn(PIN_ECHO, HIGH, 30000UL); if (duration == 0) return -1.0; float cm = duration * 0.0343 / 2.0; if (cm < 3.0 || cm > 180.0) return -1.0; return cm; } void updateDistance() { if (millis() - lastSensorMs < SENSOR_INTERVAL_MS) return; lastSensorMs = millis(); distanceCm = readDistanceCm(); } void beep(int count) { for (int i = 0; i < count; i++) { digitalWrite(PIN_BUZZER, HIGH); delay(25); digitalWrite(PIN_BUZZER, LOW); delay(35); } } String readFieldText(const String &line, const String &key, const String &fallback) { int start = line.indexOf(key); if (start < 0) return fallback; start += key.length(); int end = line.indexOf(',', start); if (end < 0) end = line.length(); return line.substring(start, end); } long readFieldLong(const String &line, const String &key, long fallback) { int start = line.indexOf(key); if (start < 0) return fallback; start += key.length(); int end = line.indexOf(',', start); if (end < 0) end = line.length(); return line.substring(start, end).toInt(); } void handleSignLine(const String &line) { if (line.indexOf("found=YES") < 0) { signName = "NONE"; signColor = "NONE"; signShape = "NONE"; signCount = 0; return; } signName = readFieldText(line, "sign=", "UNKNOWN"); signColor = readFieldText(line, "color=", "NONE"); signShape = readFieldText(line, "shape=", "NONE"); signCount = readFieldLong(line, "count=", 0); lastSignMs = millis(); } void handleLine(String line) { line.trim(); if (line.length() == 0) return; String upper = line; upper.toUpperCase(); if (upper == "ARM") { armed = true; digitalWrite(PIN_LED, HIGH); stopRobot("ARMED"); beep(1); Serial.println("event=ARMED"); return; } if (upper == "HOLD" || upper == "STOP") { armed = false; digitalWrite(PIN_LED, LOW); stopRobot("HOLD"); beep(2); Serial.println("event=HOLD"); return; } if (upper == "STATUS") { printStatus(); return; } if (line.startsWith("SIGN,")) { handleSignLine(line); return; } Serial.print("event=IGNORED line="); Serial.println(line); } void handleSerial() { if (Serial.available()) handleLine(Serial.readStringUntil('\n')); if (Serial1.available()) handleLine(Serial1.readStringUntil('\n')); } bool obstacleTooClose() { return distanceCm > 0.0 && distanceCm <= SAFE_STOP_CM; } void updateRobot() { if (!armed) { stopRobot("NOT_ARMED"); return; } if (obstacleTooClose()) { stopRobot("OBSTACLE_CLOSE"); return; } bool signStale = millis() - lastSignMs > SIGN_TIMEOUT_MS; if (signStale || signName == "NONE") { stopRobot(signStale ? "SIGN_TIMEOUT" : "NO_SIGN"); return; } if (signName == "STOP") { stopRobot("SIGN_STOP"); } else if (signName == "SLOW") { drive(SLOW_SPEED, SLOW_SPEED, "SIGN_SLOW_FORWARD"); } else if (signName == "CAUTION") { drive(CAUTION_SPEED, CAUTION_SPEED, "SIGN_CAUTION_CRAWL"); } else { stopRobot("SIGN_UNKNOWN"); } } void printStatus() { Serial.print("status armed="); Serial.print(armed ? "YES" : "NO"); Serial.print(",sign="); Serial.print(signName); Serial.print(",color="); Serial.print(signColor); Serial.print(",shape="); Serial.print(signShape); Serial.print(",count="); Serial.print(signCount); Serial.print(",distance_cm="); Serial.print(distanceCm, 1); Serial.print(",action="); Serial.println(lastAction); } void printPeriodicLog() { if (millis() - lastLogMs < LOG_INTERVAL_MS) return; lastLogMs = millis(); printStatus(); } void setup() { Serial.begin(115200); Serial1.begin(115200, SERIAL_8N1, VISION_RX, VISION_TX); pinMode(PIN_TRIG, OUTPUT); pinMode(PIN_ECHO, INPUT); pinMode(PIN_LED, OUTPUT); pinMode(PIN_BUZZER, OUTPUT); pinMode(PIN_STBY, OUTPUT); pinMode(L_PWM, OUTPUT); pinMode(L_IN1, OUTPUT); pinMode(L_IN2, OUTPUT); pinMode(R_PWM, OUTPUT); pinMode(R_IN1, OUTPUT); pinMode(R_IN2, OUTPUT); digitalWrite(PIN_STBY, HIGH); stopRobot("BOOT"); Serial.println("G3 12 camera distance fusion ready"); Serial.println("Commands: ARM | HOLD | STATUS | SIGN,... from G3 11"); Serial.print("vision_uart_rx_gpio="); Serial.println(VISION_RX); } void loop() { updateDistance(); handleSerial(); updateRobot(); printPeriodicLog(); delay(25); }

Kết hợp camera và cảm biến khoảng cách

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