/* HocLapTrinhRobot G2 15 - Kiem thu robot moi truong 5 trong 1 Board: ESP32-S3 DevKit Hardware: TB6612FNG 2WD, SHT31, LDR, MQ-135, battery divider, LED, buzzer, WiFi */ #include #include const char *WIFI_SSID = "TenMangWiFi"; const char *WIFI_PASSWORD = "MatKhauWiFi"; const int PIN_I2C_SDA = 8; const int PIN_I2C_SCL = 9; const int PIN_LDR = 4; const int PIN_MQ135 = 6; const int PIN_BATTERY = 7; const int PIN_LED = 2; const int PIN_BUZZER = 15; const int PIN_STBY = 33; const int PIN_LEFT_PWM = 25; const int PIN_LEFT_IN1 = 26; const int PIN_LEFT_IN2 = 27; const int PIN_RIGHT_PWM = 14; const int PIN_RIGHT_IN1 = 12; const int PIN_RIGHT_IN2 = 13; const uint8_t SHT31_ADDR = 0x44; const float ADC_REF_V = 3.3f; const float BATTERY_SCALE = 2.0f; const float BATTERY_FAULT_V = 6.8f; int passCount = 0; int failCount = 0; uint8_t crc8(const uint8_t *data, int len) { uint8_t crc = 0xFF; for (int i = 0; i < len; i++) { crc ^= data[i]; for (int bit = 0; bit < 8; bit++) { crc = (crc & 0x80) ? (crc << 1) ^ 0x31 : (crc << 1); } } return crc; } bool readSht31(float &temperatureC, float &humidityPercent) { Wire.beginTransmission(SHT31_ADDR); Wire.write(0x24); Wire.write(0x00); if (Wire.endTransmission() != 0) return false; delay(20); if (Wire.requestFrom((int)SHT31_ADDR, 6) != 6) return false; uint8_t data[6]; for (int i = 0; i < 6; i++) data[i] = Wire.read(); if (crc8(data, 2) != data[2] || crc8(data + 3, 2) != data[5]) return false; uint16_t rawTemp = ((uint16_t)data[0] << 8) | data[1]; uint16_t rawHum = ((uint16_t)data[3] << 8) | data[4]; temperatureC = -45.0f + 175.0f * ((float)rawTemp / 65535.0f); humidityPercent = 100.0f * ((float)rawHum / 65535.0f); return true; } int readAnalogAverage(int pin, int sampleCount) { long total = 0; for (int i = 0; i < sampleCount; i++) { total += analogRead(pin); delay(2); } return total / sampleCount; } float readBatteryVoltage() { int raw = readAnalogAverage(PIN_BATTERY, 16); return (raw / 4095.0f) * ADC_REF_V * BATTERY_SCALE; } void setOneMotor(int pwmPin, int in1Pin, int in2Pin, int speed) { int pwm = constrain(abs(speed), 0, 255); if (speed > 0) { digitalWrite(in1Pin, HIGH); digitalWrite(in2Pin, LOW); } else if (speed < 0) { digitalWrite(in1Pin, LOW); digitalWrite(in2Pin, HIGH); } else { digitalWrite(in1Pin, LOW); digitalWrite(in2Pin, LOW); } analogWrite(pwmPin, pwm); } void setMotors(int leftSpeed, int rightSpeed) { digitalWrite(PIN_STBY, HIGH); setOneMotor(PIN_LEFT_PWM, PIN_LEFT_IN1, PIN_LEFT_IN2, leftSpeed); setOneMotor(PIN_RIGHT_PWM, PIN_RIGHT_IN1, PIN_RIGHT_IN2, rightSpeed); } void stopMotors() { setOneMotor(PIN_LEFT_PWM, PIN_LEFT_IN1, PIN_LEFT_IN2, 0); setOneMotor(PIN_RIGHT_PWM, PIN_RIGHT_IN1, PIN_RIGHT_IN2, 0); } void recordResult(const char *testName, bool passed, const String &detail) { Serial.print("test="); Serial.print(testName); Serial.print(" result="); Serial.print(passed ? "PASS" : "FAIL"); Serial.print(" "); Serial.println(detail); if (passed) passCount++; else failCount++; } void testBattery() { float voltage = readBatteryVoltage(); recordResult("BATTERY", voltage > BATTERY_FAULT_V, "voltage=" + String(voltage, 2)); } void testSensors() { float tempC = 0.0f; float humidity = 0.0f; bool shtOk = readSht31(tempC, humidity); int ldrRaw = readAnalogAverage(PIN_LDR, 8); int mqRaw = readAnalogAverage(PIN_MQ135, 8); bool analogOk = ldrRaw > 20 && ldrRaw < 4070 && mqRaw > 20 && mqRaw < 4070; String detail = "sht=" + String(shtOk ? "OK" : "ERR") + " temp=" + String(tempC, 1); detail += " hum=" + String(humidity, 1) + " ldr=" + String(ldrRaw) + " mq=" + String(mqRaw); recordResult("SENSORS", shtOk && analogOk, detail); } void testMotor() { Serial.println("motor_test note=robot_must_be_on_stand"); setMotors(80, 80); delay(700); setMotors(-80, -80); delay(700); stopMotors(); recordResult("MOTOR", true, "visual_check_required=YES"); } void testAlert() { digitalWrite(PIN_LED, HIGH); digitalWrite(PIN_BUZZER, HIGH); delay(180); digitalWrite(PIN_BUZZER, LOW); delay(180); digitalWrite(PIN_LED, LOW); recordResult("ALERT", true, "led_buzzer_visual_check=YES"); } void testWifi() { WiFi.mode(WIFI_STA); WiFi.begin(WIFI_SSID, WIFI_PASSWORD); unsigned long started = millis(); while (WiFi.status() != WL_CONNECTED && millis() - started < 8000) { delay(250); } bool ok = WiFi.status() == WL_CONNECTED; String detail = ok ? "ip=" + WiFi.localIP().toString() + " rssi=" + String(WiFi.RSSI()) : "ip=none"; recordResult("WIFI", ok, detail); } void runSelfTest() { passCount = 0; failCount = 0; Serial.println("self_test=START"); testBattery(); testSensors(); testMotor(); testAlert(); testWifi(); Serial.print("summary pass="); Serial.print(passCount); Serial.print(" fail="); Serial.println(failCount); Serial.println("self_test=DONE"); } void handleSerialCommand() { if (!Serial.available()) return; String command = Serial.readStringUntil('\n'); command.trim(); command.toUpperCase(); if (command == "RUN TEST") { runSelfTest(); } else if (command == "STOP") { stopMotors(); Serial.println("motor=STOP"); } else if (command == "STATUS") { Serial.print("status pass="); Serial.print(passCount); Serial.print(" fail="); Serial.println(failCount); } else if (command.length() > 0) { Serial.println("commands: RUN TEST | STOP | STATUS"); } } void setup() { Serial.begin(115200); delay(400); pinMode(PIN_LED, OUTPUT); pinMode(PIN_BUZZER, OUTPUT); pinMode(PIN_STBY, OUTPUT); pinMode(PIN_LEFT_PWM, OUTPUT); pinMode(PIN_LEFT_IN1, OUTPUT); pinMode(PIN_LEFT_IN2, OUTPUT); pinMode(PIN_RIGHT_PWM, OUTPUT); pinMode(PIN_RIGHT_IN1, OUTPUT); pinMode(PIN_RIGHT_IN2, OUTPUT); digitalWrite(PIN_LED, LOW); digitalWrite(PIN_BUZZER, LOW); digitalWrite(PIN_STBY, HIGH); stopMotors(); analogReadResolution(12); Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL); WiFi.persistent(false); WiFi.setSleep(false); Serial.println("G2 15 5-in-1 self test ready"); Serial.println("commands: RUN TEST | STOP | STATUS"); } void loop() { handleSerialCommand(); }

Kiểm thử robot môi trường 5 trong 1

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