Add and refactor servo control functionality and protocols

.github/prompts/fix-lint.md

@@ -0,0 +1 @@
+make lint を実行して、検出されたエラーを修正して

Makefile

@@ -1,3 +1,7 @@
 lint:
 	uv run ruff check stackchan_server example_apps
 	uv run ty check stackchan_server example_apps
+
+lint-fix:
+	uv run ruff check --fix stackchan_server example_apps
+	uv run ty check stackchan_server example_apps

docs/protocols.md

@@ -18,6 +18,8 @@ enum class MessageKind : uint8_t {
   WakeWordEvt = 4, // クライアント→サーバ（wake word 検知通知）
   StateEvt = 5, // クライアント→サーバ（現在状態通知）
   SpeakDoneEvt = 6, // クライアント→サーバ（発話完了通知）
+  ServoCmd = 7, // サーバ→クライアント（サーボ動作シーケンス指示）
+  ServoDoneEvt = 8, // クライアント→サーバ（サーボ動作シーケンス完了通知）
 };
 
 enum class MessageType : uint8_t {
@@ -92,6 +94,26 @@ struct __attribute__((packed)) WsHeader {
 - payload: 1 byte（`1=done`）
 - 役割: TTS再生が完了したことを通知する。`Idle` 遷移とは独立に扱える。
 
+### Downlink: kind = ServoCmd (7)
+
+- 方向: サーバー -> クライアント
+- メッセージ種別: `DATA` のみ使用
+- payload: 1 つの「サーボ動作シーケンス」をまとめて送る
+  - `<uint8 command_count>`
+  - 続いて `command_count` 個のコマンド
+    - `Sleep (op=0)`: `<uint8 op><int16 duration_ms>`
+    - `MoveX (op=1)`: `<uint8 op><int8 angle><int16 duration_ms>`
+    - `MoveY (op=2)`: `<uint8 op><int8 angle><int16 duration_ms>`
+- ファームウェアは受信後すぐにキューへ積み、`loop()` 内で非同期に順次実行する。
+- 新しい `ServoCmd` を受信した場合、現在のシーケンスは置き換える。
+
+### Uplink: kind = ServoDoneEvt (8)
+
+- 方向: クライアント -> サーバー
+- メッセージ種別: `DATA` のみ使用
+- payload: 1 byte（`1=done`）
+- 役割: 直前に受信した `ServoCmd` のシーケンス全体が完了したことを通知する。
+
 ### kind の拡張例
 
 - AudioPcm (1): 現行の PCM16LE アップリンク
@@ -100,6 +122,8 @@ struct __attribute__((packed)) WsHeader {
 - WakeWordEvt (4): wake word 検知通知
 - StateEvt (5): 現在状態通知
 - SpeakDoneEvt (6): 発話完了通知
+- ServoCmd (7): サーボ動作シーケンス指示
+- ServoDoneEvt (8): サーボ動作シーケンス完了通知
 
 ### 簡易バイト例（AudioPcm / DATA）
 

example_apps/echo.py

@@ -5,7 +5,9 @@
 from logging import getLogger
 
 from stackchan_server.app import StackChanApp
-from stackchan_server.speech_recognition import WhisperCppSpeechToText, WhisperServerSpeechToText
+from stackchan_server.speech_recognition import (
+    WhisperCppSpeechToText,
+)
 from stackchan_server.speech_synthesis import VoiceVoxSpeechSynthesizer
 from stackchan_server.ws_proxy import EmptyTranscriptError, WsProxy
 
@@ -17,14 +19,12 @@
 )
 
 def _create_app() -> StackChanApp:
-    whisper_server_url = os.getenv("STACKCHAN_WHISPER_SERVER_URL")
-    whisper_server_port = os.getenv("STACKCHAN_WHISPER_SERVER_PORT")
     whisper_model = os.getenv("STACKCHAN_WHISPER_MODEL")
-    if whisper_server_url or whisper_server_port:
-        return StackChanApp(
-            speech_recognizer=WhisperServerSpeechToText(server_url=whisper_server_url),
-            speech_synthesizer=VoiceVoxSpeechSynthesizer(),
-        )
+    # if os.getenv("STACKCHAN_WHISPER_SERVER_URL") or os.getenv("STACKCHAN_WHISPER_SERVER_PORT"):
+    #     return StackChanApp(
+    #         speech_recognizer=WhisperServerSpeechToText(server_url=whisper_server_url),
+    #         speech_synthesizer=VoiceVoxSpeechSynthesizer(),
+    #     )
     if whisper_model:
         return StackChanApp(
             speech_recognizer=WhisperCppSpeechToText(

example_apps/echo_with_move.py

@@ -0,0 +1,81 @@
+from __future__ import annotations
+
+import logging
+import os
+from logging import getLogger
+
+from stackchan_server.app import StackChanApp
+from stackchan_server.speech_recognition import (
+    WhisperCppSpeechToText,
+)
+from stackchan_server.speech_synthesis import VoiceVoxSpeechSynthesizer
+from stackchan_server.ws_proxy import (
+    EmptyTranscriptError,
+    ServoMoveType,
+    ServoWaitType,
+    WsProxy,
+)
+
+logger = getLogger(__name__)
+logging.basicConfig(
+    level=os.getenv("STACKCHAN_LOG_LEVEL", "INFO"),
+    format="%(asctime)s.%(msecs)03d %(levelname)s:%(name)s:%(message)s",
+    datefmt="%H:%M:%S",
+)
+
+def _create_app() -> StackChanApp:
+    whisper_model = os.getenv("STACKCHAN_WHISPER_MODEL")
+    # if os.getenv("STACKCHAN_WHISPER_SERVER_URL") or os.getenv("STACKCHAN_WHISPER_SERVER_PORT"):
+    #     return StackChanApp(
+    #         speech_recognizer=WhisperServerSpeechToText(server_url=whisper_server_url),
+    #         speech_synthesizer=VoiceVoxSpeechSynthesizer(),
+    #     )
+    if whisper_model:
+        return StackChanApp(
+            speech_recognizer=WhisperCppSpeechToText(
+                model_path=whisper_model,
+            ),
+            speech_synthesizer=VoiceVoxSpeechSynthesizer(),
+        )
+    return StackChanApp()
+
+
+app = _create_app()
+
+
+@app.setup
+async def setup(proxy: WsProxy):
+    logger.info("WebSocket connected")
+    await proxy.move_servo([(ServoMoveType.MOVE_Y, 90, 100)])
+
+
+@app.talk_session
+async def talk_session(proxy: WsProxy):
+    while True:
+        try:
+            await proxy.move_servo([(ServoMoveType.MOVE_Y, 80, 100)])
+
+            text = await proxy.listen()
+
+            await proxy.move_servo([
+                (ServoMoveType.MOVE_Y, 100, 100),
+                (ServoWaitType.SLEEP, 200),
+                (ServoMoveType.MOVE_Y, 90, 100),
+                (ServoWaitType.SLEEP, 200),
+                (ServoMoveType.MOVE_Y, 100, 100),
+                (ServoWaitType.SLEEP, 200),
+                (ServoMoveType.MOVE_Y, 90, 100),
+            ])
+
+        except EmptyTranscriptError:
+            await proxy.move_servo([(ServoMoveType.MOVE_Y, 90, 100)])
+            return
+        logger.info("Heard: %s", text)
+        await proxy.speak(text)
+
+
+
+if __name__ == "__main__":
+    import uvicorn
+
+    uvicorn.run("example_apps.echo:app.fastapi", host="0.0.0.0", port=8000, reload=True)

firmware/include/protocols.hpp

@@ -19,6 +19,8 @@ enum class MessageKind : uint8_t
 	WakeWordEvt = 4, // wake word event (client -> server)
 	StateEvt = 5, // current state event (client -> server)
 	SpeakDoneEvt = 6, // speaking completed event (client -> server)
+	ServoCmd = 7, // servo command sequence (server -> client)
+	ServoDoneEvt = 8, // servo sequence completed event (client -> server)
 };
 
 enum class MessageType : uint8_t
@@ -46,3 +48,14 @@ enum class RemoteState : uint8_t
 	Thinking = 2,
 	Speaking = 3,
 };
+
+// payload for kind=ServoCmd, messageType=DATA
+// <uint8_t command_count><commands...>
+//   command op=Sleep: <uint8_t op><int16_t duration_ms>
+//   command op=MoveX/Y: <uint8_t op><int8_t angle><int16_t duration_ms>
+enum class ServoCommandOp : uint8_t
+{
+	Sleep = 0,
+	MoveX = 1,
+	MoveY = 2,
+};

firmware/include/servo.hpp

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <ESP32Servo.h>
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+#include <vector>
+
+#include "protocols.hpp"
+
+class BodyServo
+{
+public:
+  BodyServo() = default;
+
+  void init();
+  void loop();
+  void resetSequence();
+
+  bool enqueueSequence(const uint8_t *payload, size_t payload_len);
+  bool isBusy() const;
+  void setCompletionCallback(std::function<void()> cb);
+
+private:
+  struct AxisMotion
+  {
+    Servo servo;
+    int16_t current_degree = 90;
+    int16_t start_degree = 90;
+    int16_t target_degree = 90;
+    uint32_t move_start_ms = 0;
+    uint32_t move_duration_ms = 0;
+    uint32_t last_update_ms = 0;
+    bool moving = false;
+  };
+
+  struct Step
+  {
+    ServoCommandOp op;
+    int8_t angle = 0;
+    int16_t duration_ms = 0;
+  };
+
+  bool ensureAttached();
+  void updateAxis(AxisMotion &axis, uint32_t now);
+  void startMove(AxisMotion &axis, int8_t degree, int16_t duration_ms);
+  void startCurrentStep(uint32_t now);
+  void advanceStep();
+  void completeSequence();
+
+  AxisMotion axis_x_{};
+  AxisMotion axis_y_{};
+  bool attached_ = false;
+
+  std::vector<Step> steps_{};
+  size_t current_step_index_ = 0;
+  bool sequence_active_ = false;
+  bool step_started_ = false;
+  uint32_t sleep_deadline_ms_ = 0;
+  std::function<void()> on_complete_{};
+};

firmware/src/idf_component.yml

@@ -0,0 +1,2 @@
+dependencies:
+  idf: '>=5.1'

firmware/src/main.cpp

@@ -14,6 +14,7 @@
 #include "../include/listening.hpp"
 #include "../include/wake_up_word.hpp"
 #include "../include/display.hpp"
+#include "../include/servo.hpp"
 
 //////////////////// 設定 ////////////////////
 const char *WIFI_SSID = WIFI_SSID_H;
@@ -31,6 +32,7 @@ static Speaking speaking(stateMachine);
 static Listening listening(wsClient, stateMachine, SAMPLE_RATE);
 static WakeUpWord wakeUpWord(stateMachine, SAMPLE_RATE);
 static Display display(stateMachine);
+static BodyServo servo;
 
 // Protocol types are defined in include/protocols.hpp
 namespace
@@ -116,6 +118,15 @@ void notifySpeakDone()
   }
 }
 
+void notifyServoDone()
+{
+  const uint8_t payload = 1; // done
+  if (!sendUplinkPacket(MessageKind::ServoDoneEvt, MessageType::DATA, &payload, sizeof(payload)))
+  {
+    log_w("Failed to send ServoDoneEvt");
+  }
+}
+
 bool applyRemoteStateCommand(const uint8_t *body, size_t bodyLen)
 {
   if (body == nullptr || bodyLen < 1)
@@ -144,6 +155,16 @@ bool applyRemoteStateCommand(const uint8_t *body, size_t bodyLen)
     return false;
   }
 }
+
+bool applyServoCommand(const uint8_t *body, size_t bodyLen)
+{
+  if (!servo.enqueueSequence(body, bodyLen))
+  {
+    log_w("Failed to apply servo command");
+    return false;
+  }
+  return true;
+}
 } // namespace
 
 void connectWiFi()
@@ -221,6 +242,16 @@ void handleWsEvent(WStype_t type, uint8_t *payload, size_t length)
         log_w("StateCmd unsupported msgType=%u", static_cast<unsigned>(rx.messageType));
       }
       break;
+    case MessageKind::ServoCmd:
+      if (static_cast<MessageType>(rx.messageType) == MessageType::DATA)
+      {
+        applyServoCommand(body, rx_payload_len);
+      }
+      else
+      {
+        log_w("ServoCmd unsupported msgType=%u", static_cast<unsigned>(rx.messageType));
+      }
+      break;
     default:
       // M5.Display.printf("WS bin kind=%u len=%d\n", (unsigned)rx.kind, (int)length);
       break;
@@ -249,6 +280,10 @@ void setup()
   speaking.setSpeakFinishedCallback([]() {
     notifySpeakDone();
   });
+  servo.init();
+  servo.setCompletionCallback([]() {
+    notifyServoDone();
+  });
   wakeUpWord.init();
   wakeUpWord.setWakeWordDetectedCallback([]() {
     notifyWakeWordDetected();
@@ -304,6 +339,7 @@ void loop()
   M5.update();
   wsClient.loop();
   handleCommunicationTimeout();
+  servo.loop();
 
   StateMachine::State current = stateMachine.getState();
   switch (current)