智慧生活科技專業社群: [OTTO GO] MQTT測試

broker: mqttgo.io

command：otto_go/000000000000/cmd

payload：

spk:beep
spk:roar
tft:mouth_open
tft:mouth_close
servo:all=90
servo:1=120

status：otto_go/000000000000/status

Arduino:

#include <Arduino.h>
#include <WiFi.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>
#include <ESP32Servo.h>
#include "driver/i2s.h"
#include "freertos/semphr.h"

#include <PubSubClient.h>

// ===================== Pin map (OTTO GO) =====================
// TFT (SPI)
#define TFT_MOSI 19
#define TFT_SCLK 18
#define TFT_CS    5
#define TFT_DC   16
#define TFT_RST  23

// Ultrasonic
#define US_TRIG 27
#define US_ECHO 39

// Servos
#define SERVO1 21
#define SERVO2 22
#define SERVO3 4
#define SERVO4 14

// Speaker (I2S -> AMP -> SPK white socket)
#define SPK_I2S_LRCK 17
#define SPK_I2S_BCLK 2
#define SPK_I2S_DOUT 32

// Microphone (I2S MEMS)
#define MIC_I2S_SCK 13
#define MIC_I2S_SD  12
#define MIC_I2S_WS  15

// ===================== WiFi STA（改成連路由器） =====================
const char* WIFI_SSID = "您的SSID";
const char* WIFI_PASS = "您的密碼";

// ===================== MQTT =====================
const char* MQTT_HOST = "mqttgo.io";  // 你的 broker IP
const uint16_t MQTT_PORT = 1883;

// 若 broker 不用帳密，留空即可
const char* MQTT_USER = "";
const char* MQTT_PASS = "";

// 你可以用 MAC 當 device id（避免多台撞 topic）
String DEVICE_ID;

// Topics
// 訂閱：控制指令
//   otto_go/<id>/cmd
// 發布：狀態回報
//   otto_go/<id>/status
String TOPIC_CMD;
String TOPIC_STATUS;

// ===================== Objects =====================
WiFiClient wifiClient;
PubSubClient mqtt(wifiClient);

Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);
Servo s1, s2, s3, s4;

// ===================== Helpers =====================
static inline uint16_t RGB565(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

// ===================== Ultrasonic =====================
long readDistanceCM() {
  digitalWrite(US_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(US_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(US_TRIG, LOW);

  unsigned long duration = pulseIn(US_ECHO, HIGH, 30000UL);
  if (duration == 0) return -1;
  return (long)(duration / 58UL);
}
long median3(long a, long b, long c) {
  if (a > b) { long t=a; a=b; b=t; }
  if (b > c) { long t=b; b=c; c=t; }
  if (a > b) { long t=a; a=b; b=t; }
  return b;
}

// ===================== TFT =====================
void tftHello() {
  tft.fillScreen(ST77XX_BLACK);
  tft.setTextColor(ST77XX_WHITE);
  tft.setTextSize(3);
  tft.setCursor(20, 40);
  tft.print("Hello");
}

void drawTigerMouth(bool open) {
  int cx = tft.width()/2;
  int cy = tft.height()/2;

  uint16_t furLight = RGB565(235, 228, 210);
  uint16_t shadow   = RGB565(160, 150, 135);
  uint16_t lineDark = RGB565(40, 40, 40);
  uint16_t dotDark  = RGB565(70, 70, 70);

  tft.fillScreen(ST77XX_BLACK);

  tft.fillCircle(cx - 55, cy + 10, 55, furLight);
  tft.fillCircle(cx + 55, cy + 10, 55, furLight);
  tft.fillRoundRect(cx - 90, cy - 25, 180, 110, 28, furLight);

  tft.fillRoundRect(cx - 85, cy + 60, 170, 22, 12, shadow);
  tft.fillRoundRect(cx - 40, cy + 55, 80, 10, 5, RGB565(120,110,100));

  int dotY1 = cy + 6, dotY2 = cy + 26;
  for (int k=0;k<3;k++){
    tft.fillCircle(cx - 55 - k*12, dotY1 + k*2, 2, dotDark);
    tft.fillCircle(cx - 55 - k*12, dotY2 + k*2, 2, dotDark);
    tft.fillCircle(cx + 55 + k*12, dotY1 + k*2, 2, dotDark);
    tft.fillCircle(cx + 55 + k*12, dotY2 + k*2, 2, dotDark);
  }

  tft.drawLine(cx, cy - 5, cx, open ? (cy + 10) : (cy + 20), lineDark);
  tft.drawLine(cx-1, cy - 5, cx-1, open ? (cy + 10) : (cy + 20), RGB565(80,80,80));

  int mouthY = open ? (cy + 16) : (cy + 18);
  int N = open ? 36 : 34;
  int div = open ? 85 : 95;
  for (int i=0;i<N;i++){
    int y = mouthY + (i*i)/div;
    tft.drawPixel(cx-i, y, lineDark);
    tft.drawPixel(cx-i, y+1, lineDark);
    tft.drawPixel(cx+i, y, lineDark);
    tft.drawPixel(cx+i, y+1, lineDark);
  }

  tft.fillCircle(cx - (open?38:36), mouthY + (open?14:12), 3, lineDark);
  tft.fillCircle(cx + (open?38:36), mouthY + (open?14:12), 3, lineDark);

  if (!open) {
    tft.fillRoundRect(cx - 10, mouthY + 20, 20, 4, 2, RGB565(90,80,70));
  } else {
    uint16_t mouthIn = RGB565(15,15,15);
    uint16_t tongue  = RGB565(210,120,120);
    int w=90, h=45;
    int x=cx-w/2, y0=cy+28;
    tft.fillRoundRect(x, y0, w, h, 18, mouthIn);
    tft.fillCircle(cx, y0+h, 22, mouthIn);
    tft.fillRoundRect(cx-22, y0+18, 44, 20, 10, tongue);
    tft.fillCircle(cx, y0+36, 14, tongue);
  }

  tft.setTextSize(2);
  tft.setTextColor(ST77XX_YELLOW, ST77XX_BLACK);
  tft.setCursor(10, 10);
  tft.print(open ? "MOUTH: OPEN" : "MOUTH: CLOSE");
}

// ===================== I2S0 mode switching (穩定版) =====================
const i2s_port_t I2S_PORT = I2S_NUM_0;

enum I2SMode { MODE_NONE=0, MODE_SPK=1, MODE_MIC=2 };
static volatile I2SMode curMode = MODE_NONE;

static SemaphoreHandle_t i2sMutex = nullptr;

// 狀態顯示
bool spk_ok = false;
bool mic_ok = false;

static void lockI2S()   { if (i2sMutex) xSemaphoreTake(i2sMutex, portMAX_DELAY); }
static void unlockI2S() { if (i2sMutex) xSemaphoreGive(i2sMutex); }

static void i2sStopAndUninstallSafe() {
  if (curMode == MODE_NONE) return;
  i2s_stop(I2S_PORT);
  i2s_driver_uninstall(I2S_PORT);
  curMode = MODE_NONE;
}

bool i2sInitSpeaker() {
  i2sStopAndUninstallSafe();

  i2s_config_t cfg = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX),
    .sample_rate = 22050,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT,
    .channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,
    .communication_format = I2S_COMM_FORMAT_I2S_MSB,
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 4,
    .dma_buf_len   = 128,
    .use_apll = false,
    .tx_desc_auto_clear = true,
    .fixed_mclk = 0
  };

  i2s_pin_config_t pin_cfg = {
    .bck_io_num = SPK_I2S_BCLK,
    .ws_io_num  = SPK_I2S_LRCK,
    .data_out_num = SPK_I2S_DOUT,
    .data_in_num  = I2S_PIN_NO_CHANGE
  };

  esp_err_t e1 = i2s_driver_install(I2S_PORT, &cfg, 0, NULL);
  if (e1 != ESP_OK) return false;

  esp_err_t e2 = i2s_set_pin(I2S_PORT, &pin_cfg);
  if (e2 != ESP_OK) return false;

  i2s_zero_dma_buffer(I2S_PORT);
  i2s_start(I2S_PORT);
  curMode = MODE_SPK;
  return true;
}

bool i2sInitMic() {
  i2sStopAndUninstallSafe();

  i2s_config_t cfg = {
    .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX),
    .sample_rate = 16000,
    .bits_per_sample = I2S_BITS_PER_SAMPLE_32BIT,
    .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,
    .communication_format = I2S_COMM_FORMAT_I2S_MSB,
    .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1,
    .dma_buf_count = 4,
    .dma_buf_len   = 128,
    .use_apll = false,
    .tx_desc_auto_clear = false,
    .fixed_mclk = 0
  };

  i2s_pin_config_t pin_cfg = {
    .bck_io_num = MIC_I2S_SCK,
    .ws_io_num  = MIC_I2S_WS,
    .data_out_num = I2S_PIN_NO_CHANGE,
    .data_in_num  = MIC_I2S_SD
  };

  esp_err_t e1 = i2s_driver_install(I2S_PORT, &cfg, 0, NULL);
  if (e1 != ESP_OK) return false;

  esp_err_t e2 = i2s_set_pin(I2S_PORT, &pin_cfg);
  if (e2 != ESP_OK) return false;

  i2s_zero_dma_buffer(I2S_PORT);
  i2s_start(I2S_PORT);
  curMode = MODE_MIC;
  return true;
}

bool ensureSpeakerModeLocked() {
  if (curMode == MODE_SPK) return true;
  spk_ok = i2sInitSpeaker();
  return spk_ok;
}

bool ensureMicModeLocked() {
  if (curMode == MODE_MIC) return true;
  mic_ok = i2sInitMic();
  return mic_ok;
}

// ===================== Speaker sound =====================
static uint32_t rng = 1;
static inline int16_t noise16() {
  rng = rng * 1664525UL + 1013904223UL;
  return (int16_t)(rng >> 16);
}

void playBeep1kHz_200ms_locked() {
  if (!ensureSpeakerModeLocked()) return;

  const int sampleRate = 22050;
  const float freq = 1000.0f;
  const float durSec = 0.2f;
  const int total = (int)(sampleRate * durSec);

  const int N = 256;
  int16_t stereo[N * 2];
  float phase = 0.0f;

  for (int i=0;i<total;){
    int n = (total-i > N) ? N : (total-i);
    for (int k=0;k<n;k++){
      phase += 2.0f * 3.1415926f * freq / (float)sampleRate;
      if (phase > 2.0f*3.1415926f) phase -= 2.0f*3.1415926f;

      int16_t v = (int16_t)(sinf(phase) * 12000);
      stereo[2*k+0] = v;
      stereo[2*k+1] = v;
    }
    size_t written=0;
    i2s_write(I2S_PORT, (const char*)stereo, n*2*sizeof(int16_t), &written, portMAX_DELAY);
    i += n;
  }
}

void playRoarOnce_locked() {
  if (!ensureSpeakerModeLocked()) return;

  const int sampleRate = 22050;
  const float durSec = 0.6f;
  const int total = (int)(sampleRate * durSec);

  const int N = 256;
  int16_t stereo[N * 2];
  float phase = 0.0f;

  for (int i=0;i<total;){
    int n = (total-i > N) ? N : (total-i);

    for (int k=0;k<n;k++){
      float t = (float)(i+k)/(float)total;
      float f = 120.0f - 50.0f*t;
      phase += 2.0f * 3.1415926f * f / (float)sampleRate;
      if (phase > 2.0f*3.1415926f) phase -= 2.0f*3.1415926f;

      float env = (t < 0.08f) ? (t/0.08f) : (1.0f - (t-0.08f)/0.92f);
      if (env < 0) env = 0;

      float s  = sinf(phase)*0.65f + sinf(phase*0.5f)*0.35f;
      float nz = (noise16()/32768.0f)*0.35f;
      float out = (s + nz) * env;

      float gain = 0.85f;
      int32_t v = (int32_t)(out * gain * 30000.0f);
      if (v > 32767) v = 32767;
      if (v < -32768) v = -32768;

      int16_t vv = (int16_t)v;
      stereo[2*k+0] = vv;
      stereo[2*k+1] = vv;
    }

    size_t written=0;
    i2s_write(I2S_PORT, (const char*)stereo, n*2*sizeof(int16_t), &written, portMAX_DELAY);
    i += n;
  }
}

// ===================== Microphone energy =====================
static const int MIC_FRAME = 128;
static int32_t micBuf[MIC_FRAME];

float readMicEnergyOnce_locked() {
  if (!ensureMicModeLocked()) return 0;

  size_t bytesRead = 0;
  i2s_read(I2S_PORT, (void*)micBuf, sizeof(micBuf), &bytesRead, 20 / portTICK_PERIOD_MS);
  int n = bytesRead / 4;
  if (n <= 0) return 0;

  uint64_t sumAbs = 0;
  for (int i=0;i<n;i++){
    int16_t s16 = (int16_t)(micBuf[i] >> 16);
    sumAbs += (uint16_t)abs(s16);
  }
  return (float)sumAbs / (float)n;
}

// ===================== MQTT：指令解析 =====================
// 指令格式（payload 文字）
// 1) spk:beep
// 2) spk:roar
// 3) tft:hello
// 4) tft:mouth_open
// 5) tft:mouth_close
// 6) tft:clear
// 7) servo:all=90
// 8) servo:1=120   (ch=1..4, angle 0..180)

static void applyCommand(const String& cmd) {
  // Serial
  Serial.print("[CMD] "); Serial.println(cmd);

  if (cmd.startsWith("spk:")) {
    String m = cmd.substring(4);
    lockI2S();
    if (m == "beep") playBeep1kHz_200ms_locked();
    else if (m == "roar") playRoarOnce_locked();
    unlockI2S();
    return;
  }

  if (cmd.startsWith("tft:")) {
    String m = cmd.substring(4);
    if (m == "hello") tftHello();
    else if (m == "mouth_open") drawTigerMouth(true);
    else if (m == "mouth_close") drawTigerMouth(false);
    else if (m == "clear") tft.fillScreen(ST77XX_BLACK);
    return;
  }

  if (cmd.startsWith("servo:")) {
    String m = cmd.substring(6);
    if (m.startsWith("all=")) {
      int a = constrain(m.substring(4).toInt(), 0, 180);
      s1.write(a); s2.write(a); s3.write(a); s4.write(a);
      return;
    }
    int eq = m.indexOf('=');
    if (eq > 0) {
      int ch = m.substring(0, eq).toInt();
      int ang = constrain(m.substring(eq+1).toInt(), 0, 180);
      switch(ch){
        case 1: s1.write(ang); break;
        case 2: s2.write(ang); break;
        case 3: s3.write(ang); break;
        case 4: s4.write(ang); break;
        default: break;
      }
      return;
    }
  }
}

// MQTT callback
void mqttCallback(char* topic, byte* payload, unsigned int length) {
  String msg;
  msg.reserve(length+1);
  for (unsigned int i=0;i<length;i++) msg += (char)payload[i];
  applyCommand(msg);
}

// ===================== WiFi/MQTT connect =====================
void connectWiFi() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(WIFI_SSID, WIFI_PASS);
  Serial.print("WiFi connecting");
  uint32_t t0 = millis();
  while (WiFi.status() != WL_CONNECTED) {
    delay(300);
    Serial.print(".");
    if (millis() - t0 > 20000) break;
  }
  Serial.println();
  if (WiFi.status() == WL_CONNECTED) {
    Serial.print("WiFi OK IP=");
    Serial.println(WiFi.localIP());
  } else {
    Serial.println("WiFi FAIL");
  }
}

bool connectMQTT() {
  if (WiFi.status() != WL_CONNECTED) return false;

  mqtt.setServer(MQTT_HOST, MQTT_PORT);
  mqtt.setCallback(mqttCallback);

  String clientId = "OTTOGO-" + DEVICE_ID;

  Serial.print("MQTT connecting... ");
  bool ok;
  if (strlen(MQTT_USER) == 0) ok = mqtt.connect(clientId.c_str());
  else ok = mqtt.connect(clientId.c_str(), MQTT_USER, MQTT_PASS);

  if (!ok) {
    Serial.print("FAIL rc=");
    Serial.println(mqtt.state());
    return false;
  }

  Serial.println("OK");
  mqtt.subscribe(TOPIC_CMD.c_str());
  Serial.println(TOPIC_CMD.c_str());

  // 上線宣告
  String online = String("{\"id\":\"") + DEVICE_ID + "\",\"online\":1}";
  mqtt.publish(TOPIC_STATUS.c_str(), online.c_str(), true);
  Serial.println(TOPIC_STATUS.c_str());
  Serial.println(online.c_str());
  return true;
}

// ===================== Status publish =====================
uint32_t lastPubMs = 0;

void publishStatus() {
  // 超音波
  long d1 = readDistanceCM();
  long d2 = readDistanceCM();
  long d3 = readDistanceCM();
  long cm = median3(d1,d2,d3);
  if (cm < 0) cm = -1;

  // 麥克風能量（一次就好，避免一直切 I2S）
  float e = 0;
  lockI2S();
  e = readMicEnergyOnce_locked();
  // 讀完立刻切回 SPK（確保隨時可播）
  spk_ok = ensureSpeakerModeLocked();
  unlockI2S();

  // JSON（用 String 拼，避免 ArduinoJson 佔 RAM）
  String json;
  json.reserve(200);
  json += "{\"id\":\""; json += DEVICE_ID; json += "\"";
  json += ",\"ip\":\""; json += WiFi.localIP().toString(); json += "\"";
  json += ",\"energy\":"; json += String(e, 1);
  json += ",\"mic_ok\":"; json += (mic_ok ? "1":"0");
  json += ",\"spk_ok\":"; json += (spk_ok ? "1":"0");
  json += ",\"ultra_cm\":"; json += String(cm);
  json += "}";

  mqtt.publish(TOPIC_STATUS.c_str(), json.c_str(), false);
}

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

  DEVICE_ID = WiFi.macAddress();
  DEVICE_ID.replace(":", "");

  TOPIC_CMD    = "otto_go/" + DEVICE_ID + "/cmd";
  TOPIC_STATUS = "otto_go/" + DEVICE_ID + "/status";

  i2sMutex = xSemaphoreCreateMutex();

  pinMode(US_TRIG, OUTPUT);
  pinMode(US_ECHO, INPUT);

  // TFT init
  SPI.begin(TFT_SCLK, -1, TFT_MOSI, TFT_CS);
  tft.init(240, 320);
  tft.setRotation(1);
  tftHello();

  // Servo init
  s1.attach(SERVO1, 600, 2400);
  s2.attach(SERVO2, 600, 2400);
  s3.attach(SERVO3, 600, 2400);
  s4.attach(SERVO4, 600, 2400);
  s1.write(90); s2.write(90); s3.write(90); s4.write(90);

  // 開機先進 Speaker 模式並 beep（驗證喇叭）
  lockI2S();
  spk_ok = i2sInitSpeaker();
  if (spk_ok) playBeep1kHz_200ms_locked();
  unlockI2S();

  connectWiFi();

  // MQTT 連線
  connectMQTT();

  Serial.print("TOPIC_CMD: "); Serial.println(TOPIC_CMD);
  Serial.print("TOPIC_STATUS: "); Serial.println(TOPIC_STATUS);
}

void loop() {
  // WiFi 斷線重連
  if (WiFi.status() != WL_CONNECTED) {
    connectWiFi();
    delay(200);
  }

  // MQTT 斷線重連
  if (!mqtt.connected()) {
    connectMQTT();
    delay(200);
  }

  mqtt.loop();

  // 每 2 秒發布一次狀態
  uint32_t now = millis();
  if (now - lastPubMs >= 2000) {
    lastPubMs = now;
    if (mqtt.connected()) publishStatus();
  }
}

智慧生活科技專業社群

2026年2月14日星期六

[OTTO GO] MQTT測試

沒有留言:

張貼留言

2026年2月14日 星期六

[OTTO GO] MQTT測試

沒有留言:

張貼留言

2026年2月14日星期六