/* HocLapTrinhRobot G2 13 - Robot tuan tra do du lieu Board: ESP32-S3 DevKit Hardware: TB6612FNG 2WD, SHT31 I2C, LDR, MQ-135, LED, active buzzer */ #include enum MissionState { IDLE, DRIVE, MEASURE, TURN, DONE, EMERGENCY_STOP }; 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_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 int PATROL_SPEED = 90; const int TURN_SPEED = 80; const unsigned long DRIVE_MS = 2200; const unsigned long MEASURE_HOLD_MS = 2500; const unsigned long TURN_MS = 750; MissionState missionState = IDLE; unsigned long stateStartedMs = 0; int pointIndex = 0; int gasBaselineRaw = 1800; bool shtOk = false; float tempC = 0.0f; float humidity = 0.0f; float lightPercent = 0.0f; float airScore = 0.0f; 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 constrainFloat(float value, float low, float high) { if (value < low) return low; if (value > high) return high; return value; } void readSensors() { shtOk = readSht31(tempC, humidity); int ldrRaw = readAnalogAverage(PIN_LDR, 12); int mqRaw = readAnalogAverage(PIN_MQ135, 12); lightPercent = constrainFloat((ldrRaw / 4095.0f) * 100.0f, 0.0f, 100.0f); airScore = constrainFloat((mqRaw - gasBaselineRaw) / 12.0f, 0.0f, 100.0f); } 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); } const char *missionStateName(MissionState state) { if (state == DRIVE) return "DRIVE"; if (state == MEASURE) return "MEASURE"; if (state == TURN) return "TURN"; if (state == DONE) return "DONE"; if (state == EMERGENCY_STOP) return "EMERGENCY_STOP"; return "IDLE"; } char currentPointName() { return (char)('A' + pointIndex); } void changeState(MissionState nextState) { missionState = nextState; stateStartedMs = millis(); Serial.print("mission state="); Serial.print(missionStateName(missionState)); Serial.print(" point="); Serial.println(currentPointName()); } void printMeasurement() { Serial.print("measure point="); Serial.print(currentPointName()); Serial.print(" sht="); Serial.print(shtOk ? "OK" : "ERR"); Serial.print(" temp="); Serial.print(tempC, 1); Serial.print(" hum="); Serial.print(humidity, 1); Serial.print(" light="); Serial.print(lightPercent, 1); Serial.print(" air="); Serial.println(airScore, 1); } void startMission() { pointIndex = 0; changeState(DRIVE); } void emergencyStop() { stopMotors(); digitalWrite(PIN_LED, HIGH); digitalWrite(PIN_BUZZER, HIGH); changeState(EMERGENCY_STOP); } void updateMission() { unsigned long elapsed = millis() - stateStartedMs; if (missionState == DRIVE) { setMotors(PATROL_SPEED, PATROL_SPEED); digitalWrite(PIN_LED, (millis() / 300) % 2 == 0 ? HIGH : LOW); if (elapsed >= DRIVE_MS) { stopMotors(); changeState(MEASURE); } } else if (missionState == MEASURE) { stopMotors(); digitalWrite(PIN_LED, HIGH); if (elapsed >= MEASURE_HOLD_MS) { readSensors(); printMeasurement(); digitalWrite(PIN_BUZZER, HIGH); delay(80); digitalWrite(PIN_BUZZER, LOW); if (pointIndex >= 2) { changeState(DONE); } else { changeState(TURN); } } } else if (missionState == TURN) { setMotors(TURN_SPEED, -TURN_SPEED); if (elapsed >= TURN_MS) { pointIndex++; stopMotors(); changeState(DRIVE); } } else if (missionState == DONE || missionState == IDLE) { stopMotors(); digitalWrite(PIN_LED, missionState == DONE ? HIGH : LOW); } } void calibrateGasBaseline() { gasBaselineRaw = readAnalogAverage(PIN_MQ135, 40); Serial.print("baseline_gas_raw="); Serial.println(gasBaselineRaw); } void printStatus() { Serial.print("status state="); Serial.print(missionStateName(missionState)); Serial.print(" point="); Serial.print(currentPointName()); Serial.print(" baseline="); Serial.println(gasBaselineRaw); } void handleSerialCommand() { if (!Serial.available()) return; String command = Serial.readStringUntil('\n'); command.trim(); command.toUpperCase(); if (command == "START") { startMission(); } else if (command == "STOP") { emergencyStop(); } else if (command == "RESET") { stopMotors(); digitalWrite(PIN_BUZZER, LOW); changeState(IDLE); } else if (command == "BASELINE GAS") { calibrateGasBaseline(); } else if (command == "STATUS") { printStatus(); } else if (command.length() > 0) { Serial.println("commands: START | STOP | RESET | BASELINE GAS | 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); Serial.println("G2 13 patrol measurement ready"); Serial.println("commands: START | STOP | RESET | BASELINE GAS | STATUS"); } void loop() { handleSerialCommand(); updateMission(); }

Robot tuần tra đo dữ liệu

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