ESP32 AI Show Robot

// หุ่นยนต์ไว้โชว์ออกงาน บังคับด้วย esp32 ใช้เซอร์โวในการหมุนหัวหน้าได้
// คุมมอเตอร์ (เดินหน้า ถอยหลัง เลี้ยวซ้าย/ขวา)
// คุมความเร็ว (Triangle เพิ่ม / Cross ลด)
// รีเลย์ 2 ตัว (Square, Circle → กดทีเปิด กดอีกทีปิด)
// Servo3 (หัวหุ่น) มีโหมด หมุนอัตโนมัติไป–กลับ + กด R2/L2 override คุมเอง
// มอเตอร์ (ควบคุมผ่านมอเตอร์ไดร์เวอร์ L298N / TB6612FNG)
// AIN1 → GPIO 25, AIN2 → GPIO 26, PWMA → GPIO 27
// BIN1 → GPIO 12, BIN2 → GPIO 13, PWMB → GPIO 14
// STBY → GPIO 33
// รีเลย์ 1 (Square) → GPIO 4
// รีเลย์ 2 (Circle / เครื่องทำฟองสบู่) → GPIO 16
// Servo3 → GPIO 5 (PWM, ควบคุมมุม 20°–160°)
// === หุ่นยนต์โชว์งาน ESP32 + PS3 Controller + DFPlayer + Servo + Motor + NeoPixel ===
// DFPlayer Mini ต่อขา RX=17, TX=18
// บอร์ดเสี่ยวจื่อ (Xiao Zhi) ต่อขา RX=34, TX=22 (Serial1)

#include <Ps3Controller.h>
#include <ESP32Servo.h>
#include <Adafruit_NeoPixel.h>
#include <DFRobotDFPlayerMini.h>
#include "HardwareSerial.h"

// === DFPlayer Mini (Serial2) ===
HardwareSerial mySerial(2);
DFRobotDFPlayerMini myDFPlayer;
bool musicPlaying = false;
int currentTrack = 1;          // เริ่มต้นที่เพลงที่ 1
const int MAX_TRACKS = 10;     // 🔴 ปรับจำนวนเพลงทั้งหมดที่มีใน SD Card ตรงนี้

// === Xiao Zhi Log Parser Serial (Serial1) ===
HardwareSerial xiaoZhiSerial(1);
const int EXT_RX_PIN = 34; // สาย TX จากเสี่ยวจื่อต่อเข้า GPIO 34
const int EXT_TX_PIN = 22; // สาย RX จากเสี่ยวจื่อต่อเข้า GPIO 22

// === Motor Pins ===
const int AIN1 = 25, AIN2 = 26, PWMA = 27;
const int BIN1 = 12, BIN2 = 13, PWMB = 14;
const int STBY = 33;

// === Relay Pins ===
const int relayPin1 = 4;   // Square
const int relayPin2 = 16;  // Circle (เครื่องทำฟองสบู่)
bool relayState1 = false;
bool relayState2 = false;

// === Servo Head ===
Servo myservo3;
int angle5 = 90;
bool autoMode = true;
bool forceTo90 = false;
unsigned long lastServoMove = 0;
const int servoInterval = 30;
const int servoMin = 20;
const int servoMax = 160;

// === Random Target Servo Behavior ===
int targetAngle = 90;
unsigned long lastTargetChange = 0;
unsigned long nextTargetTime = 0;

// === PWM ===
const int freq = 5000;
const int resolution = 8;
const int pwmA = 2, pwmB = 3;

// === Speed Control ===
int maxSpeed = 120;
const int minSpeed = 80;
const int maxSpeedLimit = 255;
const int speedStep = 10;
String lastDirection = "";
String currentDirection = "Stop";

// === Voice Speed Control (ความเร็วเฉพาะของบอร์ดเสี่ยวจื่อ) ===
int xiaoZhiSpeed = 70; // 🔴 ปรับความเร็วเมื่อสั่งด้วยเสียงตรงนี้ (ค่า 0 - 255)

// === Voice Control Flag ===
bool isVoiceControl = false; // สถานะเช็คว่ากำลังคุมด้วยเสียงอยู่หรือไม่

// === NeoPixel ===
#define NEOPIXEL_PIN 21
#define NUMPIXELS 60
Adafruit_NeoPixel pixels(NUMPIXELS, NEOPIXEL_PIN, NEO_GRB + NEO_KHZ800);

// === Mode ไฟหยุด ===
int stopMode = 0;
unsigned long stopStartTime = 0;
bool stopActive = false;

// === Light Show ===
int lightMode = 0;
bool lightShowActive = false;
unsigned long lightStartTime = 0;
const unsigned long LIGHT_SHOW_DURATION = 15000; // 15 วินาที

// Function Declarations
void updateServoHead();
void updateNeoPixels();
void checkMusicFinished();
void readFromXiaoZhi();
void onConnect();
void notify();
void moveMotor(int aSpeed, int bSpeed);
void moveForward();
void moveBackward();
void turnRight();
void turnLeft();
void stopMotors();
void disableAutoAndReturnTo90();
void printDirection(int speed, int turn);
showLightShowPattern(int mode, unsigned long t);
void playNextTrack();

// === Setup ===
void setup() {
  Serial.begin(115200);

  // PS3
  Ps3.begin();
  String address = Ps3.getAddress();
  Serial.print("The ESP32's Bluetooth MAC address is: ");
  Serial.println(address);
  Ps3.attach(notify);
  Ps3.begin("6c:c8:40:55:4d:fa");
  Ps3.attachOnConnect(onConnect);
  Serial.println("📶 Waiting for PS3 Controller...");

  // Servo
  myservo3.attach(5, 500, 2400);
  myservo3.write(angle5);

  // Motor (สำหรับ ESP32 Core v2.x)
  pinMode(AIN1, OUTPUT); pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT); pinMode(BIN2, OUTPUT);
  pinMode(STBY, OUTPUT); digitalWrite(STBY, HIGH);
  ledcSetup(pwmA, freq, resolution); ledcAttachPin(PWMA, pwmA);
  ledcSetup(pwmB, freq, resolution); ledcAttachPin(PWMB, pwmB);

  // Relay
  pinMode(relayPin1, OUTPUT); digitalWrite(relayPin1, HIGH);
  pinMode(relayPin2, OUTPUT); digitalWrite(relayPin2, HIGH);

  // NeoPixel
  pixels.begin();
  pixels.clear();
  pixels.show();

  // Random
  randomSeed(analogRead(0));
  nextTargetTime = millis() + random(1000, 4001);

  // DFPlayer (ใช้ Serial2: RX=17, TX=18)
  mySerial.begin(9600, SERIAL_8N1, 17, 18);
  if (!myDFPlayer.begin(mySerial)) {
    Serial.println("⚠️ DFPlayer ไม่ตอบกลับ");
  } else {
    Serial.println("✅ DFPlayer พร้อมใช้งาน");
    myDFPlayer.volume(20);
  }

  // Xiao Zhi Log Parser (ใช้ Serial1: RX=34, TX=22)
  xiaoZhiSerial.begin(115200, SERIAL_8N1, EXT_RX_PIN, EXT_TX_PIN);
  Serial.println("✅ Xiao Zhi Serial Log Parser Ready (RX: GPIO 34)");
}

// === Main Loop ===
void loop() {
  updateServoHead();
  updateNeoPixels();
  checkMusicFinished();   // เช็คเมื่อเพลงจบจะหยุดเล่นทันที
  readFromXiaoZhi();      // อ่านคำสั่งเสียงจากเสี่ยวจื่อ
}

// === PS3 Connect ===
void onConnect() { Serial.println("✅ PS3 Controller connected."); }

// === ฟังก์ชันอ่าน Log จากบอร์ดเสี่ยวจื่อ (รองรับประโยคยาวและสกัดตัวเลขเวลา) ===
void readFromXiaoZhi() {
  if (xiaoZhiSerial.available() > 0) {
    String input = xiaoZhiSerial.readStringUntil('\n');
    input.trim();

    if (input.length() == 0) return;

    int tagIndex = input.indexOf("<<");

    if (tagIndex != -1) {
      String responseText = input.substring(tagIndex + 2);
      responseText.trim();

      if (responseText.length() > 0) {
        // 1. แยกตัวอักษรคำสั่งแรก (F, B, L, R, S, O, C, X)
        char commandCode = toupper(responseText.charAt(0));

        // 2. ดึงเฉพาะตัวเลขแรกที่พบในข้อความ (รองรับทั้งจำนวนเต็มและทศนิยม)
        float durationSec = 0;
        String numStr = "";
        bool foundDigit = false;

        for (size_t i = 0; i < responseText.length(); i++) {
          char c = responseText.charAt(i);
          if (isDigit(c) || (c == '.' && foundDigit && numStr.indexOf('.') == -1)) {
            numStr += c;
            foundDigit = true;
          } else if (foundDigit && !isDigit(c) && c != '.') {
            // เมื่อสกัดกลุ่มตัวเลขชุดแรกจบแล้ว ให้หยุดค้นหา
            break;
          }
        }

        if (numStr.length() > 0) {
          durationSec = numStr.toFloat(); // แปลงชุดตัวเลขเป็นทศนิยม (วินาที)
        }

        // 3. ทำงานตามคำสั่งทิศทาง
        switch (commandCode) {
          case 'F':
            Serial.printf("🗣️ เสี่ยวจื่อสั่ง: เดินหน้า (ความเร็ว %d)", xiaoZhiSpeed);
            if (durationSec > 0) Serial.printf(" เป็นเวลา %.1f วินาที", durationSec);
            Serial.println();

            moveMotor(xiaoZhiSpeed, xiaoZhiSpeed);
            printDirection(xiaoZhiSpeed, 0);
            disableAutoAndReturnTo90();
            isVoiceControl = true;
            break;

          case 'B':
            Serial.printf("🗣️ เสี่ยวจื่อสั่ง: ถอยหลัง (ความเร็ว %d)", xiaoZhiSpeed);
            if (durationSec > 0) Serial.printf(" เป็นเวลา %.1f วินาที", durationSec);
            Serial.println();

            moveMotor(-xiaoZhiSpeed, -xiaoZhiSpeed);
            printDirection(-xiaoZhiSpeed, 0);
            disableAutoAndReturnTo90();
            isVoiceControl = true;
            break;

          case 'L':
            Serial.printf("🗣️ เสี่ยวจื่อสั่ง: เลี้ยวซ้าย (ความเร็ว %d)", xiaoZhiSpeed);
            if (durationSec > 0) Serial.printf(" เป็นเวลา %.1f วินาที", durationSec);
            Serial.println();

            moveMotor(xiaoZhiSpeed, -xiaoZhiSpeed);
            printDirection(0, 100);
            disableAutoAndReturnTo90();
            isVoiceControl = true;
            break;

          case 'R':
            Serial.printf("🗣️ เสี่ยวจื่อสั่ง: เลี้ยวขวา (ความเร็ว %d)", xiaoZhiSpeed);
            if (durationSec > 0) Serial.printf(" เป็นเวลา %.1f วินาที", durationSec);
            Serial.println();

            moveMotor(-xiaoZhiSpeed, xiaoZhiSpeed);
            printDirection(0, -100);
            disableAutoAndReturnTo90();
            isVoiceControl = true;
            break;

          case 'S':
            Serial.println("🗣️ เสี่ยวจื่อสั่ง: หยุด");
            stopMotors();
            isVoiceControl = false;
            break;

          // เปิดเครื่องทำฟองสบู่ (Relay 2)
          case 'O':
            Serial.println("🗣️ เสี่ยวจื่อสั่ง: เปิดเครื่องทำฟองสบู่ (Relay 2 ON)");
            relayState2 = true;
            digitalWrite(relayPin2, LOW);
            break;

          // ปิดเครื่องทำฟองสบู่ (Relay 2)
          case 'C':
          case 'X':
            Serial.println("🗣️ เสี่ยวจื่อสั่ง: ปิดเครื่องทำฟองสบู่ (Relay 2 OFF)");
            relayState2 = false;
            digitalWrite(relayPin2, HIGH);
            break;

          default:
            break;
        }

        // 4. หากมีระยะเวลาส่งมาด้วย (durationSec > 0) ให้หน่วงเวลาตามสั่งแล้วหยุดมอเตอร์
        if (durationSec > 0 && (commandCode == 'F' || commandCode == 'B' || commandCode == 'L' || commandCode == 'R')) {
          delay((unsigned long)(durationSec * 1000)); // แปลงวินาทีเป็นมิลลิวินาที
          stopMotors();
          isVoiceControl = false;
          Serial.println("⏱️ ครบกำหนดเวลา สั่งหยุดมอเตอร์อัตโนมัติ");
        }
      }
    }
  }
}

// === PS3 Events (ตัดการคุมด้วยเสียงทันทีเมื่อจอยทำงาน) ===
void notify() {
  static unsigned long lastSpeedChangeTime = 0;
  unsigned long now = millis();

  // --- Relay ---
  if (Ps3.event.button_down.square) {
    relayState1 = !relayState1;
    digitalWrite(relayPin1, relayState1);
  }
  if (Ps3.event.button_down.circle) {
    relayState2 = !relayState2;
    digitalWrite(relayPin2, relayState2);
  }

  // --- Speed ---
  if (now - lastSpeedChangeTime >= 200) {
    if (Ps3.data.button.triangle) maxSpeed = min(maxSpeedLimit, maxSpeed + speedStep);
    if (Ps3.data.button.cross) maxSpeed = max(minSpeed, maxSpeed - speedStep);
    lastSpeedChangeTime = now;
  }

  // --- Light Show ---
  if (Ps3.event.button_down.l1 || Ps3.event.button_down.r1) {
    if (!lightShowActive) {
      lightShowActive = true;
      lightStartTime = millis();
      Serial.printf("🌈 Start Light Show Mode %d\n", lightMode);
    } else {
      lightMode = (lightMode + 1) % 3;
      lightStartTime = millis();
      Serial.printf("🔄 Change Light Mode to %d\n", lightMode);
    }
  }

  // --- Auto Servo Toggle ---
  if (Ps3.event.button_down.start) {
    autoMode = !autoMode;
    forceTo90 = false;
    Serial.printf("🌀 Auto Servo Mode: %s\n", autoMode ? "ON" : "OFF");
    if (autoMode) nextTargetTime = millis() + random(1000, 4001);
  }

  // --- Servo Manual ---
  if (Ps3.data.button.r2) {
    if (autoMode) autoMode = false, forceTo90 = false;
    angle5 = max(servoMin, angle5 - 1);
    myservo3.write(angle5);
    delay(20);
  }
  if (Ps3.data.button.l2) {
    if (autoMode) autoMode = false, forceTo90 = false;
    angle5 = min(servoMax, angle5 + 1);
    myservo3.write(angle5);
    delay(20);
  }

  // --- กดปุ่ม SELECT เพื่อเล่น / ข้ามไปเพลงถัดไป ---
  if (Ps3.event.button_down.select) {
    playNextTrack();
  }

  // --- Movement (บังคับจากจอย PS3) ---
  if (Ps3.data.button.up) {
    isVoiceControl = false; // ยกเลิกคุมด้วยเสียง
    moveForward();
  }
  else if (Ps3.data.button.down) {
    isVoiceControl = false; // ยกเลิกคุมด้วยเสียง
    moveBackward();
  }
  else if (Ps3.data.button.left) {
    isVoiceControl = false; // ยกเลิกคุมด้วยเสียง
    turnLeft();
  }
  else if (Ps3.data.button.right) {
    isVoiceControl = false; // ยกเลิกคุมด้วยเสียง
    turnRight();
  }
  else {
    int ly = Ps3.data.analog.stick.ly;
    int rx = -Ps3.data.analog.stick.rx;
    if (abs(ly) < 15) ly = 0;
    if (abs(rx) < 15) rx = 0;

    if (ly == 0 && rx != 0) {
      isVoiceControl = false; // ยกเลิกคุมด้วยเสียงเมื่อโยกหมุน
      int spin = map(rx, -128, 127, -maxSpeed, maxSpeed);
      moveMotor(spin, -spin);
      printDirection(0, spin);
    } 
    else if (ly != 0) {
      isVoiceControl = false; // ยกเลิกคุมด้วยเสียงเมื่อโยกเดินหน้า/ถอยหลัง
      int speedVal = map(ly, -128, 127, maxSpeed, -maxSpeed);
      int turnVal  = map(rx, -128, 127, -maxSpeed / 1.2, maxSpeed / 1.2);
      int motorA = constrain(speedVal + turnVal, -255, 255);
      int motorB = constrain(speedVal - turnVal, -255, 255);
      moveMotor(motorA, motorB);
      printDirection(speedVal, turnVal);
    } 
    else {
      if (!isVoiceControl) {
        stopMotors();
      }
    }
  }
}

// === Movement ===
void moveMotor(int aSpeed, int bSpeed) {
  aSpeed = constrain(aSpeed, -255, 255);
  bSpeed = constrain(bSpeed, -255, 255);
  digitalWrite(AIN1, aSpeed > 0 ? HIGH : LOW);
  digitalWrite(AIN2, aSpeed < 0 ? HIGH : LOW);
  ledcWrite(pwmA, abs(aSpeed));
  digitalWrite(BIN1, bSpeed > 0 ? HIGH : LOW);
  digitalWrite(BIN2, bSpeed < 0 ? HIGH : LOW);
  ledcWrite(pwmB, abs(bSpeed));
}

void moveForward()  { moveMotor(maxSpeed, maxSpeed); printDirection(maxSpeed, 0); disableAutoAndReturnTo90(); }
void moveBackward() { moveMotor(-maxSpeed, -maxSpeed); printDirection(-maxSpeed, 0); disableAutoAndReturnTo90(); }
void turnRight()    { moveMotor(-maxSpeed, maxSpeed); printDirection(0, -100); disableAutoAndReturnTo90(); }
void turnLeft()     { moveMotor(maxSpeed, -maxSpeed); printDirection(0, 100); disableAutoAndReturnTo90(); }
void stopMotors()   { moveMotor(0, 0); printDirection(0, 0); }

void disableAutoAndReturnTo90() {
  if (autoMode) {
    autoMode = false;
    forceTo90 = true;
    Serial.println("🚫 AutoMode OFF: Return head to 90°");
  }
}

// === Servo ===
void updateServoHead() {
  unsigned long now = millis();
  if (autoMode) {
    if (currentDirection == "Stop") {
      if (now >= nextTargetTime) {
        targetAngle = random(servoMin, servoMax + 1);
        nextTargetTime = now + random(1000, 4001);
      }
      if (now - lastServoMove >= servoInterval) {
        if (angle5 < targetAngle) angle5++;
        else if (angle5 > targetAngle) angle5--;
        myservo3.write(angle5);
        lastServoMove = now;
      }
    } else if (angle5 != 90) {
      angle5 = 90;
      myservo3.write(angle5);
    }
  } else if (forceTo90) {
    if (now - lastServoMove >= 30) {
      if (angle5 < 90) angle5++;
      else if (angle5 > 90) angle5--;
      myservo3.write(angle5);
      lastServoMove = now;
      if (angle5 == 90) forceTo90 = false;
    }
  }
}

// === Direction Tracking ===
void printDirection(int speed, int turn) {
  String dir;
  if (speed > 0) dir = "Forward";
  else if (speed < 0) dir = "Backward";
  else if (turn > 0) dir = "Left";
  else if (turn < 0) dir = "Right";
  else dir = "Stop";

  if (dir != currentDirection) currentDirection = dir;
  lastDirection = dir;
}

// === NeoPixel Update ===
void updateNeoPixels() {
  unsigned long now = millis();
  if (lightShowActive) {
    if (now - lightStartTime < LIGHT_SHOW_DURATION)
      showLightShowPattern(lightMode, now - lightStartTime);
    else {
      lightShowActive = false;
      pixels.clear();
      pixels.show();
    }
    return;
  }

  if (currentDirection == "Stop") {
    if (!stopActive) stopStartTime = now, stopActive = true;
    unsigned long elapsed = now - stopStartTime;
    if (elapsed <= 5000) {
      pixels.clear();
      if (stopMode == 0) {
        if ((now / 400) % 2 == 0)
          for (int i = 0; i < NUMPIXELS; i++) pixels.setPixelColor(i, pixels.Color(255, 0, 0));
      } else if (stopMode == 1) {
        int val = ((now / 10) % 512);
        int brightness = abs((int)val - 255);
        for (int i = 0; i < NUMPIXELS; i++) pixels.setPixelColor(i, pixels.Color(0, 0, brightness));
      } else if (stopMode == 2) {
        for (int i = 0; i < NUMPIXELS; i++) pixels.setPixelColor(i, pixels.Color(0, 255, 0));
      }
      pixels.show();
    } else {
      pixels.clear();
      pixels.show();
    }
  } else {
    stopActive = false;
    pixels.clear();
    if (currentDirection == "Forward")
      for (int i = 0; i < NUMPIXELS; i++) pixels.setPixelColor(i, pixels.Color(0, 255, 0));
    else if (currentDirection == "Backward")
      for (int i = 0; i < NUMPIXELS; i++) pixels.setPixelColor(i, pixels.Color(255, 0, 0));
    else if (currentDirection == "Left" || currentDirection == "Right")
      for (int i = 0; i < NUMPIXELS; i++) pixels.setPixelColor(i, pixels.Color(255, 255, 0));
    pixels.show();
  }
}

// === Light Show ===
void showLightShowPattern(int mode, unsigned long t) {
  if (mode == 0) {
    for (int i = 0; i < NUMPIXELS; i++) {
      int hue = (i * 50 + (t / 8)) % 360;
      uint32_t color = pixels.ColorHSV(hue * 182, 255, 255);
      pixels.setPixelColor(i, color);
    }
  }
  else if (mode == 1) {
    float wave = sin(t / 100.0) * 0.5 + 0.5;
    int brightness = (int)(wave * 255);
    int blue = brightness;
    int white = brightness / 2;
    for (int i = 0; i < NUMPIXELS; i++) {
      pixels.setPixelColor(i, pixels.Color(white / 3, white, blue));
    }
  }
  else if (mode == 2) {
    int runner = (t / 50) % NUMPIXELS;
    for (int i = 0; i < NUMPIXELS; i++) {
      int flicker = random(150, 255);
      int r = flicker;
      int g = random(40, 100);
      int b = random(0, 20);
      if (random(0, 10) > 7) r = g = b = 0;
      int diff = abs(i - runner);
      if (diff < 2) {
        r = 255;
        g = 180 - diff * 80;
        b = 0;
      }
      pixels.setPixelColor(i, pixels.Color(r, g, b));
    }
  }
  pixels.show();
}

// === ฟังก์ชันสั่งเล่น / เปลี่ยนเพลงถัดไป ===
void playNextTrack() {
  if (!musicPlaying) {
    myDFPlayer.play(currentTrack);
    musicPlaying = true;
    Serial.printf("🎵 เริ่มเล่นเพลงที่ %d\n", currentTrack);
  } else {
    currentTrack++;
    if (currentTrack > MAX_TRACKS) {
      currentTrack = 1;
    }
    myDFPlayer.play(currentTrack);
    Serial.printf("⏭️ ข้ามไปเล่นเพลงที่ %d\n", currentTrack);
  }
}

// === ฟังก์ชันตรวจจับเมื่อเพลงจบลงเอง ===
void checkMusicFinished() {
  if (myDFPlayer.available()) {
    uint8_t type = myDFPlayer.readType();
    int value = myDFPlayer.read();

    if (type == DFPlayerPlayFinished) {
      musicPlaying = false;
      Serial.printf("⏹️ เพลงที่ %d เล่นจบแล้ว (หยุดเล่นเรียบร้อย)\n", value);
      
      currentTrack++;
      if (currentTrack > MAX_TRACKS) {
        currentTrack = 1;
      }
    }
  }
}