/* G3 16 - Capstone robot nhan dien vat the Board: ESP32-S3 DevKit + TB6612FNG + HC-SR04 + ESP32-CAM 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 SEARCH_SPEED = 55; const int DRIVE_SPEED = 65; const float SAFE_STOP_CM = 18.0; const unsigned long VISION_TIMEOUT_MS = 900; const unsigned long LOG_INTERVAL_MS = 300; bool armed = false; String visionSign = "NONE"; long visionCount = 0; float visionConfidence = 0.0; float distanceCm = -1.0; int missionScore = 0; unsigned long lastVisionMs = 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 beep(int count) { for (int i = 0; i < count; i++) { digitalWrite(PIN_BUZZER, HIGH); delay(20); 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(); } float readFieldFloat(const String &line, const String &key, float 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).toFloat(); } void handleVisionLine(const String &line) { if (line.startsWith("SIGN,")) { visionSign = readFieldText(line, "sign=", "UNKNOWN"); visionCount = readFieldLong(line, "count=", 0); visionConfidence = 75.0; lastVisionMs = millis(); } else if (line.startsWith("CHALLENGE,")) { visionSign = readFieldText(line, "sign=", "UNKNOWN"); visionCount = readFieldLong(line, "count=", 0); visionConfidence = readFieldFloat(line, "confidence=", 0.0); lastVisionMs = 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 == "RESET_SCORE") { missionScore = 0; Serial.println("event=SCORE_RESET"); return; } if (upper == "STATUS") { printStatus(); return; } handleVisionLine(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() { distanceCm = readDistanceCm(); if (!armed) { stopRobot("NOT_ARMED"); return; } if (obstacleTooClose()) { stopRobot("OBSTACLE_CLOSE"); return; } if (millis() - lastVisionMs > VISION_TIMEOUT_MS) { drive(-SEARCH_SPEED, SEARCH_SPEED, "SEARCH_OBJECT"); return; } if (visionSign == "STOP") { stopRobot("SIGN_STOP"); missionScore++; } else if (visionSign == "SLOW" || visionSign == "CAUTION") { drive(DRIVE_SPEED, DRIVE_SPEED, "APPROACH_OBJECT"); missionScore++; } else { stopRobot("UNKNOWN_OBJECT"); } } void printStatus() { Serial.print("CAPSTONE,status armed="); Serial.print(armed ? "YES" : "NO"); Serial.print(",sign="); Serial.print(visionSign); Serial.print(",count="); Serial.print(visionCount); Serial.print(",confidence="); Serial.print(visionConfidence, 1); Serial.print(",distance_cm="); Serial.print(distanceCm, 1); Serial.print(",action="); Serial.print(lastAction); Serial.print(",score="); Serial.println(missionScore); } 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 16 capstone controller ready"); Serial.println("Commands: ARM | HOLD | STATUS | RESET_SCORE"); } void loop() { handleSerial(); updateRobot(); printPeriodicLog(); delay(40); }

Capstone Robot nhận diện vật thể

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