智慧生活科技專業社群: [OTTO GO] MQTT整合表情和動作

MQTT Topics

CMD（下指令）：otto_go/OTTO_GO_A1/4A65B7A0/cmd
ACK（回覆）：otto_go/OTTO_GO_A1/4A65B7A0/ack
STATUS（狀態回報，約每 2 秒）：otto_go/OTTO_GO_A1/4A65B7A0/status

指令格式

單一指令：key=value
多指令同時下：用 ; 或 & 分隔
例：servo_speed=fast;action=happy
例：servo_speed=slow&expr=sleepy

注意：此版程式一次只會優先執行 action（如果有 action 就做完直接 return），沒有 action 才會處理 expr / spk。

支援的命令一覽

1) `servo_speed`

控制動作快慢（影響 action 內的伺服移動）

servo_speed=slow
servo_speed=normal
servo_speed=fast

2) `action`

一個「機器人行為包」（表情 + 伺服動作 + 音效）

action=happy
顯示 HAPPY → 擺動 → 張嘴（虎嘴）→ roar → 回到 HAPPY
action=angry
顯示 ANGRY → 擺動 → beep
action=sleepy
顯示 SLEEPY → 慢速回中位
action=neutral（或其它不認得的 action 會當 neutral）
顯示 NEUTRAL → 回中位

建議用法：

servo_speed=fast;action=happy
servo_speed=slow;action=sleepy

3) `expr`

只切換 TFT 表情（不一定有動作/音效）

expr=neutral
expr=happy
expr=angry
expr=sleepy
expr=mouth_open（虎嘴張開，並把 mouthOpen=true）
expr=mouth_close（虎嘴閉合，並把 mouthOpen=false）

4) `spk`

只播放音效

spk=beep
spk=roar

ACK / STATUS 會回什麼

ACK（執行完會回）例如：
- OK action=happy
- OK expr=happy
- OK spk=roar
STATUS（每 2 秒）目前內容：
- {"expr":"mouth_open" 或 "neutral","ip":"192.168.x.x"}
  （此版只回嘴巴是否開，還沒把 happy/angry/sleepy 狀態放進去）

arduino:

#include <Arduino.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>
#include <ESP32Servo.h>
#include "driver/i2s.h"
#include "freertos/semphr.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) - 目前此版不使用麥克風（避免 I2S 切換干擾）
#define MIC_I2S_SCK 13
#define MIC_I2S_SD  12
#define MIC_I2S_WS  15

// ===================== WiFi / MQTT =====================
const char* WIFI_SSID = "您的SSID";
const char* WIFI_PASS = "您的密碼";

// 你已連上 mqttgo，通常 host 用 broker.mqttgo.io 或 broker.emqx.io 之類
// 你現在既然已成功，就沿用你現場可用的那個
const char* MQTT_HOST = "mqttgo.io";
const uint16_t MQTT_PORT = 1883;

const char* MQTT_TOPIC_CMD    = "otto_go/OTTO_GO_A1/4A65B7A0/cmd";
const char* MQTT_TOPIC_ACK    = "otto_go/OTTO_GO_A1/4A65B7A0/ack";
const char* MQTT_TOPIC_STATUS = "otto_go/OTTO_GO_A1/4A65B7A0/status";

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

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

// ===================== Servo speed =====================
enum ServoSpeed { SPD_SLOW=0, SPD_NORMAL=1, SPD_FAST=2 };
ServoSpeed servoSpeed = SPD_NORMAL;

static inline int speedToStepDelay(ServoSpeed s){
  if (s == SPD_FAST) return 6;
  if (s == SPD_SLOW) return 16;
  return 10;
}

int curA1=90, curA2=90, curA3=90, curA4=90;

void servoWriteAll(int a){
  a = constrain(a,0,180);
  curA1=curA2=curA3=curA4=a;
  s1.write(a); s2.write(a); s3.write(a); s4.write(a);
}

void servoMoveAllTo(int target, int ms){
  target = constrain(target,0,180);
  int stepDelay = speedToStepDelay(servoSpeed);
  int steps = max(1, ms / stepDelay);

  float a1=curA1, a2=curA2, a3=curA3, a4=curA4;
  float d1=(target-a1)/(float)steps;
  float d2=(target-a2)/(float)steps;
  float d3=(target-a3)/(float)steps;
  float d4=(target-a4)/(float)steps;

  for(int i=0;i<steps;i++){
    a1+=d1; a2+=d2; a3+=d3; a4+=d4;
    s1.write((int)a1); s2.write((int)a2); s3.write((int)a3); s4.write((int)a4);
    delay(stepDelay);
  }
  curA1=curA2=curA3=curA4=target;
}

// ===================== TFT helpers =====================
void tftBeginOnce(){
  // 只做一次，完全照你「可用」的那份
  SPI.begin(TFT_SCLK, -1, TFT_MOSI, TFT_CS);
  tft.init(240, 320);
  tft.setRotation(1);
}

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

void tftFaceBase(){
  tft.fillScreen(ST77XX_BLACK);
  tft.setTextWrap(false);
}

void drawEyes(int x1,int y1,int r1,int x2,int y2,int r2, uint16_t c){
  tft.fillCircle(x1,y1,r1,c);
  tft.fillCircle(x2,y2,r2,c);
}

void drawMouthLine(int x,int y,int w,int h, uint16_t c){
  tft.fillRoundRect(x,y,w,h, h/2, c);
}

void exprNeutral(){
  tftFaceBase();
  drawEyes(90, 80, 18, 150, 80, 18, ST77XX_WHITE);
  drawMouthLine(95, 140, 50, 10, ST77XX_WHITE);
  tft.setTextColor(ST77XX_YELLOW);
  tft.setTextSize(2);
  tft.setCursor(10, 10);
  tft.print("NEUTRAL");
}

void exprHappy(){
  tftFaceBase();
  tft.drawFastHLine(72, 75, 35, ST77XX_WHITE);
  tft.drawFastHLine(132, 75, 35, ST77XX_WHITE);
  tft.drawFastHLine(72, 76, 35, ST77XX_WHITE);
  tft.drawFastHLine(132, 76, 35, ST77XX_WHITE);

  tft.drawCircle(120, 140, 28, ST77XX_WHITE);
  tft.fillRect(60, 112, 120, 28, ST77XX_BLACK);

  tft.setTextColor(ST77XX_YELLOW);
  tft.setTextSize(2);
  tft.setCursor(10, 10);
  tft.print("HAPPY");
}

void exprAngry(){
  tftFaceBase();
  tft.drawLine(60, 60, 105, 75, ST77XX_RED);
  tft.drawLine(180, 60, 135, 75, ST77XX_RED);
  drawEyes(90, 85, 16, 150, 85, 16, ST77XX_WHITE);
  drawMouthLine(90, 145, 60, 10, ST77XX_RED);

  tft.setTextColor(ST77XX_YELLOW);
  tft.setTextSize(2);
  tft.setCursor(10, 10);
  tft.print("ANGRY");
}

void exprSleepy(){
  tftFaceBase();
  tft.drawFastHLine(70, 85, 40, ST77XX_WHITE);
  tft.drawFastHLine(130, 85, 40, ST77XX_WHITE);
  tft.drawFastHLine(70, 86, 40, ST77XX_WHITE);
  tft.drawFastHLine(130, 86, 40, ST77XX_WHITE);
  drawMouthLine(105, 145, 30, 8, ST77XX_WHITE);

  tft.setTextColor(ST77XX_CYAN);
  tft.setTextSize(2);
  tft.setCursor(170, 30); tft.print("Z");
  tft.setCursor(190, 50); tft.print("Z");

  tft.setTextColor(ST77XX_YELLOW);
  tft.setCursor(10, 10);
  tft.print("SLEEPY");
}

// 你的虎嘴（保留）
static inline uint16_t RGB565(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

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");
}

bool mouthOpen=false;

void showExpression(const String& name){
  if (name == "happy") exprHappy();
  else if (name == "angry") exprAngry();
  else if (name == "sleepy") exprSleepy();
  else if (name == "mouth_open") { mouthOpen=true;  drawTigerMouth(true); }
  else if (name == "mouth_close"){ mouthOpen=false; drawTigerMouth(false); }
  else exprNeutral();
}

// ===================== I2S (speaker only) =====================
const i2s_port_t I2S_PORT = I2S_NUM_0;
static SemaphoreHandle_t i2sMutex = nullptr;
static void lockI2S()   { if (i2sMutex) xSemaphoreTake(i2sMutex, portMAX_DELAY); }
static void unlockI2S() { if (i2sMutex) xSemaphoreGive(i2sMutex); }

bool i2sInitSpeaker() {
  i2s_driver_uninstall(I2S_PORT);

  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
  };

  if (i2s_driver_install(I2S_PORT, &cfg, 0, NULL) != ESP_OK) return false;
  if (i2s_set_pin(I2S_PORT, &pin_cfg) != ESP_OK) return false;
  i2s_zero_dma_buffer(I2S_PORT);
  i2s_start(I2S_PORT);
  return true;
}

void playBeep1kHz_200ms_locked() {
  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;
  }
}

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

void playRoarOnce_locked() {
  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;

      int32_t v = (int32_t)(out * 0.85f * 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;
  }
}

// ===================== Actions =====================
void actionHappy(){
  showExpression("happy");
  servoSpeed = SPD_FAST;
  servoMoveAllTo(70, 300);
  servoMoveAllTo(110, 300);
  servoMoveAllTo(90, 250);

  showExpression("mouth_open");
  lockI2S(); playRoarOnce_locked(); unlockI2S();
  delay(120);
  showExpression("happy");
}

void actionAngry(){
  showExpression("angry");
  servoSpeed = SPD_FAST;
  servoMoveAllTo(100, 250);
  servoMoveAllTo(80, 250);
  servoMoveAllTo(100, 250);
  lockI2S(); playBeep1kHz_200ms_locked(); unlockI2S();
}

void actionSleepy(){
  showExpression("sleepy");
  servoSpeed = SPD_SLOW;
  servoMoveAllTo(90, 600);
}

void actionNeutral(){
  showExpression("neutral");
  servoSpeed = SPD_NORMAL;
  servoMoveAllTo(90, 300);
}

// ===================== MQTT parsing =====================
String getKV(const String& msg, const String& key){
  int p = msg.indexOf(key + "=");
  if (p < 0) return "";
  int s = p + key.length() + 1;
  int e1 = msg.indexOf(';', s);
  int e2 = msg.indexOf('&', s);
  int e = -1;
  if (e1 >= 0 && e2 >= 0) e = min(e1,e2);
  else if (e1 >= 0) e = e1;
  else if (e2 >= 0) e = e2;
  else e = msg.length();
  return msg.substring(s, e);
}

void mqttPublishAck(const String& s){
  mqtt.publish(MQTT_TOPIC_ACK, s.c_str());
}

void mqttPublishStatus(){
  String st = String("{\"expr\":\"") + (mouthOpen ? "mouth_open" : "neutral") +
              String("\",\"ip\":\"") + WiFi.localIP().toString() + "\"}";
  mqtt.publish(MQTT_TOPIC_STATUS, st.c_str());
}

void applyCommand(const String& msg){
  String action = getKV(msg, "action");
  String expr   = getKV(msg, "expr");
  String spk    = getKV(msg, "spk");
  String speed  = getKV(msg, "servo_speed");

  if (speed.length()){
    if (speed=="slow") servoSpeed = SPD_SLOW;
    else if (speed=="fast") servoSpeed = SPD_FAST;
    else servoSpeed = SPD_NORMAL;
  }

  if (action.length()){
    if (action=="happy") actionHappy();
    else if (action=="angry") actionAngry();
    else if (action=="sleepy") actionSleepy();
    else actionNeutral();
    mqttPublishAck(String("OK action=")+action);
    return;
  }

  if (expr.length()){
    showExpression(expr);
    mqttPublishAck(String("OK expr=")+expr);
  }

  if (spk.length()){
    lockI2S();
    if (spk=="roar") playRoarOnce_locked();
    else playBeep1kHz_200ms_locked();
    unlockI2S();
    mqttPublishAck(String("OK spk=")+spk);
  }
}

void mqttCallback(char* topic, byte* payload, unsigned int len){
  String msg;
  msg.reserve(len+1);
  for(unsigned int i=0;i<len;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");
  while (WiFi.status() != WL_CONNECTED){
    delay(300);
    Serial.print(".");
  }
  Serial.println();
  Serial.print("WiFi OK IP=");
  Serial.println(WiFi.localIP());
}

void connectMQTT(){
  mqtt.setServer(MQTT_HOST, MQTT_PORT);
  mqtt.setCallback(mqttCallback);

  while(!mqtt.connected()){
    Serial.print("MQTT connecting... ");
    String cid = "OTTOGO_" + String((uint32_t)ESP.getEfuseMac(), HEX);
    bool ok = mqtt.connect(cid.c_str());
    Serial.println(ok ? "OK" : "FAIL");
    if(!ok) delay(800);
  }
  mqtt.subscribe(MQTT_TOPIC_CMD);
  mqttPublishAck("READY");
}

// ===================== Setup/Loop =====================
void setup() {
  Serial.begin(115200);
  delay(200);
  Serial.println("[BOOT] start");

  // TFT：只初始化一次
  tftBeginOnce();
  tft.fillScreen(ST77XX_RED);   delay(150);
  tft.fillScreen(ST77XX_GREEN); delay(150);
  tft.fillScreen(ST77XX_BLUE);  delay(150);
  tftHello();
  delay(300);
  exprNeutral();
  Serial.println("[TFT] ok");

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

  // Servos
  s1.attach(SERVO1, 600, 2400);
  s2.attach(SERVO2, 600, 2400);
  s3.attach(SERVO3, 600, 2400);
  s4.attach(SERVO4, 600, 2400);
  servoWriteAll(90);

  // I2S speaker init
  i2sMutex = xSemaphoreCreateMutex();
  lockI2S();
  bool spk_ok = i2sInitSpeaker();
  if (spk_ok) playBeep1kHz_200ms_locked();
  unlockI2S();

  // WiFi/MQTT
  connectWiFi();
  connectMQTT();

  Serial.println("----- MQTT Topics -----");
  Serial.print("CMD    : "); Serial.println(MQTT_TOPIC_CMD);
  Serial.print("ACK    : "); Serial.println(MQTT_TOPIC_ACK);
  Serial.print("STATUS : "); Serial.println(MQTT_TOPIC_STATUS);
  Serial.println("-----------------------");
}

unsigned long lastStatusMs = 0;

void loop() {
  if(!mqtt.connected()){
    connectMQTT();
  }
  mqtt.loop();

  unsigned long now = millis();
  if(now - lastStatusMs > 2000){
    lastStatusMs = now;
    mqttPublishStatus();
  }
}

智慧生活科技專業社群

2026年2月14日星期六

[OTTO GO] MQTT整合表情和動作

MQTT Topics

指令格式

支援的命令一覽

1) `servo_speed`

2) `action`

3) `expr`

4) `spk`

ACK / STATUS 會回什麼

沒有留言:

張貼留言

2026年2月14日 星期六

[OTTO GO] MQTT整合表情和動作

MQTT Topics

指令格式

支援的命令一覽

1) servo_speed

2) action

3) expr

4) spk

ACK / STATUS 會回什麼

沒有留言:

張貼留言

2026年2月14日星期六

1) `servo_speed`

2) `action`

3) `expr`

4) `spk`