/* HocLapTrinhRobot G2 14 - Theo doi pin va loi cam bien Board: ESP32-S3 DevKit Hardware: TB6612FNG 2WD, SHT31, LDR, MQ-135, battery divider, LED, buzzer */ #include 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_WARN_V = 7.2f; const float BATTERY_FAULT_V = 6.8f; const unsigned long SAMPLE_INTERVAL_MS = 1000; bool faultActive = false; String faultReason = "NONE"; unsigned long lastSampleMs = 0; int shtErrorCount = 0; int ldrErrorCount = 0; int mqErrorCount = 0; float batteryV = 0.0f; float tempC = 0.0f; float humidity = 0.0f; int ldrRaw = 0; int mqRaw = 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); float pinVoltage = (raw / 4095.0f) * ADC_REF_V; return pinVoltage * 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) { if (faultActive) { leftSpeed = 0; rightSpeed = 0; } digitalWrite(PIN_STBY, faultActive ? LOW : 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 enterFault(const String &reason) { faultActive = true; faultReason = reason; stopMotors(); digitalWrite(PIN_STBY, LOW); Serial.print("fault="); Serial.println(faultReason); } void readHealth() { batteryV = readBatteryVoltage(); bool shtOk = readSht31(tempC, humidity); shtErrorCount = shtOk ? 0 : shtErrorCount + 1; ldrRaw = readAnalogAverage(PIN_LDR, 8); mqRaw = readAnalogAverage(PIN_MQ135, 8); ldrErrorCount = (ldrRaw < 20 || ldrRaw > 4070) ? ldrErrorCount + 1 : 0; mqErrorCount = (mqRaw < 20 || mqRaw > 4070) ? mqErrorCount + 1 : 0; if (batteryV < BATTERY_FAULT_V) enterFault("BATTERY_LOW"); if (shtErrorCount >= 6) enterFault("SHT31_ERROR"); if (ldrErrorCount >= 6) enterFault("LDR_STUCK"); if (mqErrorCount >= 6) enterFault("MQ135_STUCK"); } void updateOutputs() { bool warning = batteryV > 0 && batteryV < BATTERY_WARN_V; if (faultActive) { digitalWrite(PIN_LED, HIGH); digitalWrite(PIN_BUZZER, (millis() / 250) % 2 == 0 ? HIGH : LOW); } else if (warning) { digitalWrite(PIN_LED, (millis() / 600) % 2 == 0 ? HIGH : LOW); digitalWrite(PIN_BUZZER, LOW); } else { digitalWrite(PIN_LED, LOW); digitalWrite(PIN_BUZZER, LOW); } } void printStatus() { Serial.print("status battery_v="); Serial.print(batteryV, 2); Serial.print(" fault="); Serial.print(faultActive ? "YES" : "NO"); Serial.print(" reason="); Serial.print(faultReason); Serial.print(" shtErr="); Serial.print(shtErrorCount); Serial.print(" ldrRaw="); Serial.print(ldrRaw); Serial.print(" mqRaw="); Serial.println(mqRaw); } void handleSerialCommand() { if (!Serial.available()) return; String command = Serial.readStringUntil('\n'); command.trim(); command.toUpperCase(); if (command == "STATUS") { printStatus(); } else if (command == "MOTOR TEST") { setMotors(80, 80); Serial.println("motor_test=RUNNING"); } else if (command == "STOP") { stopMotors(); Serial.println("motor=STOP"); } else if (command == "FAULT TEST") { enterFault("MANUAL_TEST"); } else if (command == "RESET FAULT") { faultActive = false; faultReason = "NONE"; shtErrorCount = 0; ldrErrorCount = 0; mqErrorCount = 0; digitalWrite(PIN_STBY, HIGH); digitalWrite(PIN_BUZZER, LOW); Serial.println("fault_reset=OK"); } else if (command.length() > 0) { Serial.println("commands: STATUS | MOTOR TEST | STOP | FAULT TEST | RESET FAULT"); } } 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_STBY, HIGH); stopMotors(); analogReadResolution(12); Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL); Serial.println("G2 14 battery and sensor health ready"); Serial.println("commands: STATUS | MOTOR TEST | STOP | FAULT TEST | RESET FAULT"); } void loop() { handleSerialCommand(); updateOutputs(); if (millis() - lastSampleMs >= SAMPLE_INTERVAL_MS) { lastSampleMs = millis(); readHealth(); printStatus(); } }

Theo dõi pin và lỗi cảm biến

Ghi danh khóa học để mở bài này