From 09f959544fd3d9d193a653dd72974474a222b6c8 Mon Sep 17 00:00:00 2001 From: Atsushi Morimoto <74th.tech@gmail.com> Date: Fri, 13 Mar 2026 23:41:15 +0900 Subject: [PATCH 1/5] feat: add servo control functionality and protocol definitions --- docs/protocols.md | 24 +++ example_apps/echo.py | 15 +- example_apps/echo_with_move.py | 79 ++++++++ firmware/include/protocols.hpp | 13 ++ firmware/include/servo.hpp | 61 +++++++ firmware/src/idf_component.yml | 2 + firmware/src/main.cpp | 36 ++++ firmware/src/servo.cpp | 318 +++++++++++++++++++++++++++++++++ platformio.ini | 1 + stackchan_server/ws_proxy.py | 133 +++++++++++++- tmp/main.cpp | 252 ++++++++++++++++++++++++++ 11 files changed, 924 insertions(+), 10 deletions(-) create mode 100644 example_apps/echo_with_move.py create mode 100644 firmware/include/servo.hpp create mode 100644 firmware/src/idf_component.yml create mode 100644 firmware/src/servo.cpp create mode 100644 tmp/main.cpp diff --git a/docs/protocols.md b/docs/protocols.md index 8994325..fd6573a 100644 --- a/docs/protocols.md +++ b/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 つの「サーボ動作シーケンス」をまとめて送る + - `` + - 続いて `command_count` 個のコマンド + - `Sleep (op=0)`: `` + - `MoveX (op=1)`: `` + - `MoveY (op=2)`: `` +- ファームウェアは受信後すぐにキューへ積み、`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） diff --git a/example_apps/echo.py b/example_apps/echo.py index 64a84f2..3cf7c9e 100644 --- a/example_apps/echo.py +++ b/example_apps/echo.py @@ -5,7 +5,10 @@ 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, + WhisperServerSpeechToText, +) from stackchan_server.speech_synthesis import VoiceVoxSpeechSynthesizer from stackchan_server.ws_proxy import EmptyTranscriptError, WsProxy @@ -20,11 +23,11 @@ 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 whisper_server_url or whisper_server_port: + # return StackChanApp( + # speech_recognizer=WhisperServerSpeechToText(server_url=whisper_server_url), + # speech_synthesizer=VoiceVoxSpeechSynthesizer(), + # ) if whisper_model: return StackChanApp( speech_recognizer=WhisperCppSpeechToText( diff --git a/example_apps/echo_with_move.py b/example_apps/echo_with_move.py new file mode 100644 index 0000000..39478bf --- /dev/null +++ b/example_apps/echo_with_move.py @@ -0,0 +1,79 @@ +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, + WhisperServerSpeechToText, +) +from stackchan_server.speech_synthesis import VoiceVoxSpeechSynthesizer +from stackchan_server.ws_proxy import EmptyTranscriptError, 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_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 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([("move_y", 90, 100)]) + + +@app.talk_session +async def talk_session(proxy: WsProxy): + while True: + try: + await proxy.move_servo([("move_y", 80, 100)]) + + text = await proxy.listen() + + await proxy.move_servo([ + ("move_y", 100, 100), + ("sleep", 200), + ("move_y", 90, 100), + ("sleep", 200), + ("move_y", 100, 100), + ("sleep", 200), + ("move_y", 90, 100), + ]) + + except EmptyTranscriptError: + await proxy.move_servo([("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) diff --git a/firmware/include/protocols.hpp b/firmware/include/protocols.hpp index df44eca..b39838d 100644 --- a/firmware/include/protocols.hpp +++ b/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 +// +// command op=Sleep: +// command op=MoveX/Y: +enum class ServoCommandOp : uint8_t +{ + Sleep = 0, + MoveX = 1, + MoveY = 2, +}; diff --git a/firmware/include/servo.hpp b/firmware/include/servo.hpp new file mode 100644 index 0000000..48a80a8 --- /dev/null +++ b/firmware/include/servo.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "protocols.hpp" + +class AsyncServo +{ +public: + AsyncServo() = default; + + void init(); + void loop(); + void resetSequence(); + + bool enqueueSequence(const uint8_t *payload, size_t payload_len); + bool isBusy() const; + void setCompletionCallback(std::function 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 steps_{}; + size_t current_step_index_ = 0; + bool sequence_active_ = false; + bool step_started_ = false; + uint32_t sleep_deadline_ms_ = 0; + std::function on_complete_{}; +}; diff --git a/firmware/src/idf_component.yml b/firmware/src/idf_component.yml new file mode 100644 index 0000000..d752765 --- /dev/null +++ b/firmware/src/idf_component.yml @@ -0,0 +1,2 @@ +dependencies: + idf: '>=5.1' diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index 44d7741..8ade6be 100644 --- a/firmware/src/main.cpp +++ b/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 AsyncServo 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(rx.messageType)); } break; + case MessageKind::ServoCmd: + if (static_cast(rx.messageType) == MessageType::DATA) + { + applyServoCommand(body, rx_payload_len); + } + else + { + log_w("ServoCmd unsupported msgType=%u", static_cast(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) diff --git a/firmware/src/servo.cpp b/firmware/src/servo.cpp new file mode 100644 index 0000000..acd4be5 --- /dev/null +++ b/firmware/src/servo.cpp @@ -0,0 +1,318 @@ +#include "servo.hpp" + +#include + +#include +#include +#include + +namespace +{ +constexpr int kServoXPin = 6; +constexpr int kServoYPin = 7; +constexpr int kServoPulseMinUs = 500; +constexpr int kServoPulseMaxUs = 2400; +constexpr int kServoFrequencyHz = 50; +constexpr uint32_t kEasingDivisionMs = 10; + +int16_t clampDegree(int16_t degree) +{ + return std::clamp(degree, 0, 180); +} + +uint32_t clampDuration(int16_t duration_ms) +{ + return duration_ms <= 0 ? 0U : static_cast(duration_ms); +} + +int16_t readInt16Le(const uint8_t *src) +{ + int16_t value = 0; + memcpy(&value, src, sizeof(value)); + return value; +} +} // namespace + +void AsyncServo::init() +{ + if (!ensureAttached()) + { + log_w("Failed to attach servos"); + return; + } + + axis_x_.servo.write(axis_x_.current_degree); + axis_y_.servo.write(axis_y_.current_degree); +} + +void AsyncServo::loop() +{ + if (!attached_) + { + return; + } + + uint32_t now = millis(); + updateAxis(axis_x_, now); + updateAxis(axis_y_, now); + + if (!sequence_active_ || current_step_index_ >= steps_.size()) + { + return; + } + + if (!step_started_) + { + startCurrentStep(now); + } + + if (!sequence_active_ || current_step_index_ >= steps_.size()) + { + return; + } + + const Step &step = steps_[current_step_index_]; + bool finished = false; + switch (step.op) + { + case ServoCommandOp::Sleep: + finished = static_cast(now - sleep_deadline_ms_) >= 0; + break; + case ServoCommandOp::MoveX: + finished = !axis_x_.moving; + break; + case ServoCommandOp::MoveY: + finished = !axis_y_.moving; + break; + default: + log_w("Unknown servo step op=%u", static_cast(step.op)); + finished = true; + break; + } + + if (finished) + { + advanceStep(); + } +} + +void AsyncServo::resetSequence() +{ + steps_.clear(); + current_step_index_ = 0; + sequence_active_ = false; + step_started_ = false; + sleep_deadline_ms_ = 0; + axis_x_.moving = false; + axis_y_.moving = false; +} + +bool AsyncServo::enqueueSequence(const uint8_t *payload, size_t payload_len) +{ + if (!ensureAttached()) + { + return false; + } + if (payload == nullptr || payload_len < 1) + { + log_w("ServoCmd payload too short: %u", static_cast(payload_len)); + return false; + } + + const uint8_t command_count = payload[0]; + size_t offset = 1; + std::vector parsed_steps; + parsed_steps.reserve(command_count); + + for (uint8_t i = 0; i < command_count; ++i) + { + if (offset >= payload_len) + { + log_w("ServoCmd truncated at command=%u", static_cast(i)); + return false; + } + + const ServoCommandOp op = static_cast(payload[offset++]); + Step step{}; + step.op = op; + + switch (op) + { + case ServoCommandOp::Sleep: + if (offset + sizeof(int16_t) > payload_len) + { + log_w("ServoCmd sleep truncated at command=%u", static_cast(i)); + return false; + } + step.duration_ms = readInt16Le(payload + offset); + offset += sizeof(int16_t); + break; + case ServoCommandOp::MoveX: + case ServoCommandOp::MoveY: + if (offset + sizeof(int8_t) + sizeof(int16_t) > payload_len) + { + log_w("ServoCmd move truncated at command=%u", static_cast(i)); + return false; + } + step.angle = static_cast(payload[offset]); + offset += sizeof(int8_t); + step.duration_ms = readInt16Le(payload + offset); + offset += sizeof(int16_t); + break; + default: + log_w("ServoCmd unknown op=%u", static_cast(op)); + return false; + } + + parsed_steps.push_back(step); + } + + if (offset != payload_len) + { + log_w("ServoCmd payload has %u trailing bytes", static_cast(payload_len - offset)); + return false; + } + + resetSequence(); + steps_ = std::move(parsed_steps); + + if (steps_.empty()) + { + completeSequence(); + return true; + } + + current_step_index_ = 0; + sequence_active_ = true; + step_started_ = false; + log_i("Accepted servo sequence commands=%u", static_cast(command_count)); + return true; +} + +bool AsyncServo::isBusy() const +{ + return sequence_active_ || axis_x_.moving || axis_y_.moving; +} + +void AsyncServo::setCompletionCallback(std::function cb) +{ + on_complete_ = std::move(cb); +} + +bool AsyncServo::ensureAttached() +{ + if (attached_) + { + return true; + } + + axis_x_.servo.setPeriodHertz(kServoFrequencyHz); + axis_y_.servo.setPeriodHertz(kServoFrequencyHz); + + const bool x_ok = axis_x_.servo.attach(kServoXPin, kServoPulseMinUs, kServoPulseMaxUs) > 0; + const bool y_ok = axis_y_.servo.attach(kServoYPin, kServoPulseMinUs, kServoPulseMaxUs) > 0; + attached_ = x_ok && y_ok; + return attached_; +} + +void AsyncServo::updateAxis(AxisMotion &axis, uint32_t now) +{ + if (!axis.moving) + { + return; + } + + const uint32_t elapsed = now - axis.move_start_ms; + if ((now - axis.last_update_ms) < kEasingDivisionMs && elapsed < axis.move_duration_ms) + { + return; + } + + if (elapsed >= axis.move_duration_ms) + { + axis.current_degree = axis.target_degree; + axis.servo.write(axis.current_degree); + axis.moving = false; + axis.last_update_ms = now; + return; + } + + const float progress = static_cast(elapsed) / static_cast(axis.move_duration_ms); + axis.current_degree = axis.start_degree + static_cast((axis.target_degree - axis.start_degree) * progress); + axis.servo.write(axis.current_degree); + axis.last_update_ms = now; +} + +void AsyncServo::startMove(AxisMotion &axis, int8_t degree, int16_t duration_ms) +{ + axis.target_degree = clampDegree(degree); + axis.start_degree = axis.current_degree; + axis.move_start_ms = millis(); + axis.last_update_ms = axis.move_start_ms; + axis.move_duration_ms = clampDuration(duration_ms); + + if (axis.move_duration_ms == 0 || axis.start_degree == axis.target_degree) + { + axis.current_degree = axis.target_degree; + axis.servo.write(axis.current_degree); + axis.moving = false; + return; + } + + axis.moving = true; +} + +void AsyncServo::startCurrentStep(uint32_t now) +{ + if (!sequence_active_ || current_step_index_ >= steps_.size()) + { + return; + } + + const Step &step = steps_[current_step_index_]; + step_started_ = true; + switch (step.op) + { + case ServoCommandOp::Sleep: + sleep_deadline_ms_ = now + clampDuration(step.duration_ms); + break; + case ServoCommandOp::MoveX: + startMove(axis_x_, step.angle, step.duration_ms); + break; + case ServoCommandOp::MoveY: + startMove(axis_y_, step.angle, step.duration_ms); + break; + default: + advanceStep(); + break; + } +} + +void AsyncServo::advanceStep() +{ + if (!sequence_active_) + { + return; + } + + ++current_step_index_; + step_started_ = false; + if (current_step_index_ >= steps_.size()) + { + completeSequence(); + } +} + +void AsyncServo::completeSequence() +{ + steps_.clear(); + current_step_index_ = 0; + sequence_active_ = false; + step_started_ = false; + sleep_deadline_ms_ = 0; + log_i("Servo sequence completed"); + if (on_complete_) + { + on_complete_(); + } +} diff --git a/platformio.ini b/platformio.ini index cf49f67..c97916c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -60,6 +60,7 @@ lib_deps = adafruit/Adafruit NeoPixel@^1.15.2 Links2004/WebSockets@^2.7.2 ESP32Async/AsyncTCP@^3.4.10 + madhephaestus/ESP32Servo@^3.1.3 lib_ldf_mode = deep diff --git a/stackchan_server/ws_proxy.py b/stackchan_server/ws_proxy.py index 6550485..6e5484c 100644 --- a/stackchan_server/ws_proxy.py +++ b/stackchan_server/ws_proxy.py @@ -3,11 +3,12 @@ import asyncio import os import struct +from collections import deque from contextlib import suppress from enum import IntEnum from logging import getLogger from pathlib import Path -from typing import Optional +from typing import Literal, Optional, Sequence, TypeAlias from fastapi import WebSocket, WebSocketDisconnect @@ -45,6 +46,8 @@ class _WsKind(IntEnum): WAKEWORD_EVT = 4 STATE_EVT = 5 SPEAK_DONE_EVT = 6 + SERVO_CMD = 7 + SERVO_DONE_EVT = 8 class _WsMsgType(IntEnum): @@ -52,6 +55,56 @@ class _WsMsgType(IntEnum): DATA = 2 END = 3 + +class _ServoOp(IntEnum): + SLEEP = 0 + MOVE_X = 1 + MOVE_Y = 2 + + +ServoMoveCommand: TypeAlias = tuple[Literal["move_x", "move_y"], int, int] +ServoSleepCommand: TypeAlias = tuple[Literal["sleep"], int] +ServoCommand: TypeAlias = ServoMoveCommand | ServoSleepCommand + + +def _ensure_range(value: int, *, minimum: int, maximum: int, label: str) -> int: + if not minimum <= value <= maximum: + raise ValueError(f"{label} must be between {minimum} and {maximum}: {value}") + return value + + +def _encode_servo_commands(commands: Sequence[ServoCommand]) -> bytes: + normalized = list(commands) + _ensure_range(len(normalized), minimum=0, maximum=255, label="servo command count") + + payload = bytearray() + payload.append(len(normalized)) + + for index, command in enumerate(normalized): + name = command[0] + if name == "sleep": + if len(command) != 2: + raise ValueError(f"sleep command at index {index} must be ('sleep', duration_ms)") + duration_ms = _ensure_range(int(command[1]), minimum=-32768, maximum=32767, label="sleep duration") + payload.append(_ServoOp.SLEEP) + payload.extend(struct.pack(" bool: @@ -145,6 +201,34 @@ async def send_state_command(self, state_id: int | FirmwareState) -> None: async def reset_state(self) -> None: await self.send_state_command(FirmwareState.IDLE) + async def move_servo(self, commands: Sequence[ServoCommand]) -> None: + payload = _encode_servo_commands(commands) + previous_counter = self._servo_sent_counter + target_counter = previous_counter + 1 + self._servo_sent_counter = target_counter + self._pending_servo_wait_targets.append(target_counter) + try: + await self._send_packet(_WsKind.SERVO_CMD, _WsMsgType.DATA, payload) + except Exception: + if self._pending_servo_wait_targets and self._pending_servo_wait_targets[-1] == target_counter: + self._pending_servo_wait_targets.pop() + self._servo_sent_counter = previous_counter + raise + + async def wait_servo_complete(self, timeout_seconds: float | None = 120.0) -> None: + target_counter = ( + self._pending_servo_wait_targets.popleft() + if self._pending_servo_wait_targets + else self._servo_done_counter + 1 + ) + await self._wait_for_counter( + current=lambda: self._servo_done_counter, + min_counter=target_counter, + timeout_seconds=timeout_seconds, + is_closed=lambda: self._closed, + label="servo completed event", + ) + async def start(self) -> None: if self._receiving_task is None: self._receiving_task = asyncio.create_task(self._receive_loop()) @@ -213,6 +297,10 @@ async def _receive_loop(self) -> None: self._handle_speak_done_event(msg_type, payload) continue + if kind == _WsKind.SERVO_DONE_EVT: + self._handle_servo_done_event(msg_type, payload) + continue + await self.ws.close(code=1003, reason="unsupported kind") break except WebSocketDisconnect: @@ -248,12 +336,23 @@ def _handle_speak_done_event(self, msg_type: int, payload: bytes) -> None: return self._speaker.handle_speak_done_event() + def _handle_servo_done_event(self, msg_type: int, payload: bytes) -> None: + if msg_type != _WsMsgType.DATA: + return + if len(payload) < 1: + return + self._servo_done_counter += 1 + logger.info("Received servo done event") + async def _send_state_command(self, state_id: int | FirmwareState) -> None: payload = struct.pack(" None: hdr = struct.pack( _WS_HEADER_FMT, - _WsKind.STATE_CMD.value, - _WsMsgType.DATA.value, + int(kind), + int(msg_type), 0, self._down_seq, len(payload), @@ -261,10 +360,36 @@ async def _send_state_command(self, state_id: int | FirmwareState) -> None: await self.ws.send_bytes(hdr + payload) self._down_seq += 1 + async def _wait_for_counter( + self, + *, + current, + min_counter: int, + timeout_seconds: float | None, + is_closed, + label: str, + ) -> None: + loop = asyncio.get_running_loop() + deadline = (loop.time() + timeout_seconds) if timeout_seconds else None + while True: + if current() >= min_counter: + return + if is_closed(): + raise WebSocketDisconnect() + if deadline and loop.time() >= deadline: + raise TimeoutError(f"Timed out waiting for {label}") + await asyncio.sleep(0.05) + def _next_down_seq(self) -> int: seq = self._down_seq self._down_seq += 1 return seq -__all__ = ["WsProxy", "FirmwareState", "TimeoutError", "EmptyTranscriptError"] +__all__ = [ + "WsProxy", + "FirmwareState", + "TimeoutError", + "EmptyTranscriptError", + "ServoCommand", +] diff --git a/tmp/main.cpp b/tmp/main.cpp new file mode 100644 index 0000000..bb9f91d --- /dev/null +++ b/tmp/main.cpp @@ -0,0 +1,252 @@ +#include +#include +#include + +// CoreS3 Port A +// static const int SERVO_PIN1 = 1; +// static const int SERVO_PIN2 = 2; + +// CoreS3 with m5-pantilt +const int SERVOX_PIN = 6; +const int SERVOY_PIN = 7; + +#define DELAY_MS 40 + +namespace test4 +{ + class LServo + { + protected: + Servo servoX; + Servo servoY; + + int8_t current_degree_x; + int8_t current_degree_y; + int8_t start_degree_x; + int8_t start_degree_y; + int8_t target_degree_x; + int8_t target_degree_y; + uint32_t move_start_time_x; + uint32_t move_start_time_y; + uint32_t move_duration_ms_x; + uint32_t move_duration_ms_y; + uint32_t last_update_time_ms_x; + uint32_t last_update_time_ms_y; + uint32_t easing_division_ms = 50; + bool moving_x; + bool moving_y; + + public: + LServo() + : current_degree_x(90), current_degree_y(90), + start_degree_x(90), start_degree_y(90), + target_degree_x(90), target_degree_y(90), + move_start_time_x(0), move_start_time_y(0), + move_duration_ms_x(0), move_duration_ms_y(0), + last_update_time_ms_x(0), last_update_time_ms_y(0), + moving_x(false), moving_y(false) { + }; + void begin(int x_pin, int y_pin) + { + servoX.setPeriodHertz(50); + servoX.attach(x_pin, 500, 2400); + servoY.setPeriodHertz(50); + servoY.attach(y_pin, 500, 2400); + servoX.write(current_degree_x); + servoY.write(current_degree_y); + } + void moveX(int16_t degree, uint32_t duration_ms) + { + target_degree_x = constrain(degree, 0, 180); + start_degree_x = current_degree_x; + move_start_time_x = millis(); + last_update_time_ms_x = move_start_time_x; + move_duration_ms_x = duration_ms; + + if (duration_ms == 0 || start_degree_x == target_degree_x) + { + current_degree_x = target_degree_x; + servoX.write(current_degree_x); + moving_x = false; + return; + } + + moving_x = true; + } + void moveY(int16_t degree, uint32_t duration_ms) + { + target_degree_y = constrain(degree, 0, 180); + start_degree_y = current_degree_y; + move_start_time_y = millis(); + last_update_time_ms_y = move_start_time_y; + move_duration_ms_y = duration_ms; + + if (duration_ms == 0 || start_degree_y == target_degree_y) + { + current_degree_y = target_degree_y; + servoY.write(current_degree_y); + moving_y = false; + return; + } + + moving_y = true; + } + void loop() + { + uint32_t now = millis(); + + if (moving_x && now - last_update_time_ms_x >= easing_division_ms) + { + // Serial.println("@@X"); + uint32_t elapsed_ms = now - move_start_time_x; + if (elapsed_ms >= move_duration_ms_x) + { + current_degree_x = target_degree_x; + servoX.write(current_degree_x); + moving_x = false; + } + else + { + float progress = static_cast(elapsed_ms) / static_cast(move_duration_ms_x); + current_degree_x = start_degree_x + static_cast((target_degree_x - start_degree_x) * progress); + servoX.write(current_degree_x); + last_update_time_ms_x = now; + } + } + + if (moving_y && now - last_update_time_ms_y >= easing_division_ms) + { + uint32_t elapsed_ms = now - move_start_time_y; + if (elapsed_ms >= move_duration_ms_y) + { + current_degree_y = target_degree_y; + servoY.write(current_degree_y); + moving_y = false; + } + else + { + float progress = static_cast(elapsed_ms) / static_cast(move_duration_ms_y); + current_degree_y = start_degree_y + static_cast((target_degree_y - start_degree_y) * progress); + servoY.write(current_degree_y); + last_update_time_ms_y = now; + } + } + } + }; + + LServo servo; + uint32_t loop_start_time = 0; + int8_t state = 0; + + void setup() + { + auto cfg = M5.config(); + M5.begin(cfg); + Serial.begin(115200); + + M5.Display.setTextSize(2); + + servo.begin(SERVOX_PIN, SERVOY_PIN); + + M5.Display.println("SG90 Servo test (ServoEasing)"); + Serial.println("SG90 Servo test (ServoEasing)"); + + delay(5000); + + loop_start_time = millis(); + } + + void loop() + { + M5.update(); + + uint32_t now = millis(); + uint32_t elapsed_ms = now - loop_start_time; + + if (state == 0 && elapsed_ms >= 0) + { + Serial.println("@@1"); + servo.moveX(110, 300); + state = 1; + loop_start_time = now; + } + else if (state == 1 && elapsed_ms >= 1000) + { + Serial.println("@@2"); + servo.moveX(70, 1000); + state = 2; + loop_start_time = now; + } + else if (state == 2 && elapsed_ms >= 2000) + { + Serial.println("@@3"); + servo.moveX(90, 300); + state = 3; + loop_start_time = now; + } + else if (state == 3 && elapsed_ms >= 2000) + { + Serial.println("@@4"); + servo.moveY(100, 300); + state = 4; + loop_start_time = now; + } + else if (state == 4 && elapsed_ms >= 1000) + { + Serial.println("@@5"); + servo.moveY(80, 1000); + state = 5; + loop_start_time = now; + } + else if (state == 5 && elapsed_ms >= 2000) + { + Serial.println("@@6"); + servo.moveY(90, 300); + state = 6; + loop_start_time = now; + } + else if (state == 6 && elapsed_ms >= 2000) + { + Serial.println("@@7"); + servo.moveY(80, 200); + state = 7; + loop_start_time = now; + } + else if (state == 7 && elapsed_ms >= 3000) + { + Serial.println("@@8"); + servo.moveY(100, 100); + state = 8; + loop_start_time = now; + } + else if (state == 8 && elapsed_ms >= 300) + { + Serial.println("@@9"); + servo.moveY(90, 100); + state = 9; + loop_start_time = now; + } + else if (state == 9 && elapsed_ms >= 300) + { + Serial.println("@@10"); + servo.moveY(100, 100); + state = 10; + loop_start_time = now; + } + else if (state == 10 && elapsed_ms >= 300) + { + Serial.println("@@11"); + servo.moveY(90, 100); + state = 11; + loop_start_time = now; + } + else if (state == 11 && elapsed_ms >= 5000) + { + Serial.println("@@12"); + state = 0; + loop_start_time = now; + } + + servo.loop(); + } +} From 13130c47b814b9fe3a9325d59c114d269e43fd73 Mon Sep 17 00:00:00 2001 From: Atsushi Morimoto <74th.tech@gmail.com> Date: Sat, 14 Mar 2026 08:33:43 +0900 Subject: [PATCH 2/5] feat: refactor servo command handling with new types and improve readability --- example_apps/echo_with_move.py | 27 +++++++++------- stackchan_server/ws_proxy.py | 59 ++++++++++++++++++++++++++-------- 2 files changed, 62 insertions(+), 24 deletions(-) diff --git a/example_apps/echo_with_move.py b/example_apps/echo_with_move.py index 39478bf..d7b091f 100644 --- a/example_apps/echo_with_move.py +++ b/example_apps/echo_with_move.py @@ -10,7 +10,12 @@ WhisperServerSpeechToText, ) from stackchan_server.speech_synthesis import VoiceVoxSpeechSynthesizer -from stackchan_server.ws_proxy import EmptyTranscriptError, WsProxy +from stackchan_server.ws_proxy import ( + EmptyTranscriptError, + ServoMoveType, + ServoWaitType, + WsProxy, +) logger = getLogger(__name__) logging.basicConfig( @@ -44,29 +49,29 @@ def _create_app() -> StackChanApp: @app.setup async def setup(proxy: WsProxy): logger.info("WebSocket connected") - await proxy.move_servo([("move_y", 90, 100)]) + 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([("move_y", 80, 100)]) + await proxy.move_servo([(ServoMoveType.MOVE_Y, 80, 100)]) text = await proxy.listen() await proxy.move_servo([ - ("move_y", 100, 100), - ("sleep", 200), - ("move_y", 90, 100), - ("sleep", 200), - ("move_y", 100, 100), - ("sleep", 200), - ("move_y", 90, 100), + (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([("move_y", 90, 100)]) + await proxy.move_servo([(ServoMoveType.MOVE_Y, 90, 100)]) return logger.info("Heard: %s", text) await proxy.speak(text) diff --git a/stackchan_server/ws_proxy.py b/stackchan_server/ws_proxy.py index 6e5484c..af91ca5 100644 --- a/stackchan_server/ws_proxy.py +++ b/stackchan_server/ws_proxy.py @@ -5,7 +5,7 @@ import struct from collections import deque from contextlib import suppress -from enum import IntEnum +from enum import IntEnum, StrEnum from logging import getLogger from pathlib import Path from typing import Literal, Optional, Sequence, TypeAlias @@ -26,8 +26,12 @@ _WS_HEADER_SIZE = struct.calcsize(_WS_HEADER_FMT) _DOWN_WAV_CHUNK = 4096 # bytes per WebSocket frame for synthesized audio (raw PCM) -_DOWN_SEGMENT_MILLIS = 2000 # duration of a single START-DATA-END segment in milliseconds -_DOWN_SEGMENT_STAGGER_MILLIS = _DOWN_SEGMENT_MILLIS // 2 # half interval for the second segment start +_DOWN_SEGMENT_MILLIS = ( + 2000 # duration of a single START-DATA-END segment in milliseconds +) +_DOWN_SEGMENT_STAGGER_MILLIS = ( + _DOWN_SEGMENT_MILLIS // 2 +) # half interval for the second segment start _LISTEN_AUDIO_TIMEOUT_SECONDS = 10.0 _DEBUG_RECORDING_ENABLED = os.getenv("DEBUG_RECODING") == "1" @@ -62,8 +66,19 @@ class _ServoOp(IntEnum): MOVE_Y = 2 -ServoMoveCommand: TypeAlias = tuple[Literal["move_x", "move_y"], int, int] -ServoSleepCommand: TypeAlias = tuple[Literal["sleep"], int] +class ServoMoveType(StrEnum): + MOVE_X = "move_x" + MOVE_Y = "move_y" + + +class ServoWaitType(StrEnum): + SLEEP = "sleep" + + +ServoMoveCommand: TypeAlias = tuple[ + Literal["move_x", "move_y"] | ServoMoveType, int, int +] +ServoSleepCommand: TypeAlias = tuple[Literal["sleep"] | ServoWaitType, int] ServoCommand: TypeAlias = ServoMoveCommand | ServoSleepCommand @@ -81,11 +96,15 @@ def _encode_servo_commands(commands: Sequence[ServoCommand]) -> bytes: payload.append(len(normalized)) for index, command in enumerate(normalized): - name = command[0] + name = str(command[0]) if name == "sleep": if len(command) != 2: - raise ValueError(f"sleep command at index {index} must be ('sleep', duration_ms)") - duration_ms = _ensure_range(int(command[1]), minimum=-32768, maximum=32767, label="sleep duration") + raise ValueError( + f"sleep command at index {index} must be ('sleep', duration_ms)" + ) + duration_ms = _ensure_range( + int(command[1]), minimum=-32768, maximum=32767, label="sleep duration" + ) payload.append(_ServoOp.SLEEP) payload.extend(struct.pack(" bytes: raise ValueError( f"{name} command at index {index} must be ('{name}', angle, duration_ms)" ) - angle = _ensure_range(int(command[1]), minimum=-128, maximum=127, label="servo angle") - duration_ms = _ensure_range(int(command[2]), minimum=-32768, maximum=32767, label="servo duration") + angle = _ensure_range( + int(command[1]), minimum=-128, maximum=127, label="servo angle" + ) + duration_ms = _ensure_range( + int(command[2]), minimum=-32768, maximum=32767, label="servo duration" + ) payload.append(_ServoOp.MOVE_X if name == "move_x" else _ServoOp.MOVE_Y) payload.extend(struct.pack(" bytes: return bytes(payload) + class WsProxy: def __init__( self, @@ -210,7 +234,10 @@ async def move_servo(self, commands: Sequence[ServoCommand]) -> None: try: await self._send_packet(_WsKind.SERVO_CMD, _WsMsgType.DATA, payload) except Exception: - if self._pending_servo_wait_targets and self._pending_servo_wait_targets[-1] == target_counter: + if ( + self._pending_servo_wait_targets + and self._pending_servo_wait_targets[-1] == target_counter + ): self._pending_servo_wait_targets.pop() self._servo_sent_counter = previous_counter raise @@ -268,7 +295,9 @@ async def _receive_loop(self) -> None: continue if msg_type == _WsMsgType.DATA: - if not await self._listener.handle_data(self.ws, payload_bytes, payload): + if not await self._listener.handle_data( + self.ws, payload_bytes, payload + ): break continue @@ -348,7 +377,9 @@ async def _send_state_command(self, state_id: int | FirmwareState) -> None: payload = struct.pack(" None: + async def _send_packet( + self, kind: _WsKind, msg_type: _WsMsgType, payload: bytes = b"" + ) -> None: hdr = struct.pack( _WS_HEADER_FMT, int(kind), @@ -392,4 +423,6 @@ def _next_down_seq(self) -> int: "TimeoutError", "EmptyTranscriptError", "ServoCommand", + "ServoMoveType", + "ServoWaitType", ] From 9e35b50529b9f4fab2fb71c759b21aafb726d274 Mon Sep 17 00:00:00 2001 From: Atsushi Morimoto <74th.tech@gmail.com> Date: Sat, 14 Mar 2026 09:01:08 +0900 Subject: [PATCH 3/5] feat: remove main.cpp file containing servo control implementation --- tmp/main.cpp | 252 --------------------------------------------------- 1 file changed, 252 deletions(-) delete mode 100644 tmp/main.cpp diff --git a/tmp/main.cpp b/tmp/main.cpp deleted file mode 100644 index bb9f91d..0000000 --- a/tmp/main.cpp +++ /dev/null @@ -1,252 +0,0 @@ -#include -#include -#include - -// CoreS3 Port A -// static const int SERVO_PIN1 = 1; -// static const int SERVO_PIN2 = 2; - -// CoreS3 with m5-pantilt -const int SERVOX_PIN = 6; -const int SERVOY_PIN = 7; - -#define DELAY_MS 40 - -namespace test4 -{ - class LServo - { - protected: - Servo servoX; - Servo servoY; - - int8_t current_degree_x; - int8_t current_degree_y; - int8_t start_degree_x; - int8_t start_degree_y; - int8_t target_degree_x; - int8_t target_degree_y; - uint32_t move_start_time_x; - uint32_t move_start_time_y; - uint32_t move_duration_ms_x; - uint32_t move_duration_ms_y; - uint32_t last_update_time_ms_x; - uint32_t last_update_time_ms_y; - uint32_t easing_division_ms = 50; - bool moving_x; - bool moving_y; - - public: - LServo() - : current_degree_x(90), current_degree_y(90), - start_degree_x(90), start_degree_y(90), - target_degree_x(90), target_degree_y(90), - move_start_time_x(0), move_start_time_y(0), - move_duration_ms_x(0), move_duration_ms_y(0), - last_update_time_ms_x(0), last_update_time_ms_y(0), - moving_x(false), moving_y(false) { - }; - void begin(int x_pin, int y_pin) - { - servoX.setPeriodHertz(50); - servoX.attach(x_pin, 500, 2400); - servoY.setPeriodHertz(50); - servoY.attach(y_pin, 500, 2400); - servoX.write(current_degree_x); - servoY.write(current_degree_y); - } - void moveX(int16_t degree, uint32_t duration_ms) - { - target_degree_x = constrain(degree, 0, 180); - start_degree_x = current_degree_x; - move_start_time_x = millis(); - last_update_time_ms_x = move_start_time_x; - move_duration_ms_x = duration_ms; - - if (duration_ms == 0 || start_degree_x == target_degree_x) - { - current_degree_x = target_degree_x; - servoX.write(current_degree_x); - moving_x = false; - return; - } - - moving_x = true; - } - void moveY(int16_t degree, uint32_t duration_ms) - { - target_degree_y = constrain(degree, 0, 180); - start_degree_y = current_degree_y; - move_start_time_y = millis(); - last_update_time_ms_y = move_start_time_y; - move_duration_ms_y = duration_ms; - - if (duration_ms == 0 || start_degree_y == target_degree_y) - { - current_degree_y = target_degree_y; - servoY.write(current_degree_y); - moving_y = false; - return; - } - - moving_y = true; - } - void loop() - { - uint32_t now = millis(); - - if (moving_x && now - last_update_time_ms_x >= easing_division_ms) - { - // Serial.println("@@X"); - uint32_t elapsed_ms = now - move_start_time_x; - if (elapsed_ms >= move_duration_ms_x) - { - current_degree_x = target_degree_x; - servoX.write(current_degree_x); - moving_x = false; - } - else - { - float progress = static_cast(elapsed_ms) / static_cast(move_duration_ms_x); - current_degree_x = start_degree_x + static_cast((target_degree_x - start_degree_x) * progress); - servoX.write(current_degree_x); - last_update_time_ms_x = now; - } - } - - if (moving_y && now - last_update_time_ms_y >= easing_division_ms) - { - uint32_t elapsed_ms = now - move_start_time_y; - if (elapsed_ms >= move_duration_ms_y) - { - current_degree_y = target_degree_y; - servoY.write(current_degree_y); - moving_y = false; - } - else - { - float progress = static_cast(elapsed_ms) / static_cast(move_duration_ms_y); - current_degree_y = start_degree_y + static_cast((target_degree_y - start_degree_y) * progress); - servoY.write(current_degree_y); - last_update_time_ms_y = now; - } - } - } - }; - - LServo servo; - uint32_t loop_start_time = 0; - int8_t state = 0; - - void setup() - { - auto cfg = M5.config(); - M5.begin(cfg); - Serial.begin(115200); - - M5.Display.setTextSize(2); - - servo.begin(SERVOX_PIN, SERVOY_PIN); - - M5.Display.println("SG90 Servo test (ServoEasing)"); - Serial.println("SG90 Servo test (ServoEasing)"); - - delay(5000); - - loop_start_time = millis(); - } - - void loop() - { - M5.update(); - - uint32_t now = millis(); - uint32_t elapsed_ms = now - loop_start_time; - - if (state == 0 && elapsed_ms >= 0) - { - Serial.println("@@1"); - servo.moveX(110, 300); - state = 1; - loop_start_time = now; - } - else if (state == 1 && elapsed_ms >= 1000) - { - Serial.println("@@2"); - servo.moveX(70, 1000); - state = 2; - loop_start_time = now; - } - else if (state == 2 && elapsed_ms >= 2000) - { - Serial.println("@@3"); - servo.moveX(90, 300); - state = 3; - loop_start_time = now; - } - else if (state == 3 && elapsed_ms >= 2000) - { - Serial.println("@@4"); - servo.moveY(100, 300); - state = 4; - loop_start_time = now; - } - else if (state == 4 && elapsed_ms >= 1000) - { - Serial.println("@@5"); - servo.moveY(80, 1000); - state = 5; - loop_start_time = now; - } - else if (state == 5 && elapsed_ms >= 2000) - { - Serial.println("@@6"); - servo.moveY(90, 300); - state = 6; - loop_start_time = now; - } - else if (state == 6 && elapsed_ms >= 2000) - { - Serial.println("@@7"); - servo.moveY(80, 200); - state = 7; - loop_start_time = now; - } - else if (state == 7 && elapsed_ms >= 3000) - { - Serial.println("@@8"); - servo.moveY(100, 100); - state = 8; - loop_start_time = now; - } - else if (state == 8 && elapsed_ms >= 300) - { - Serial.println("@@9"); - servo.moveY(90, 100); - state = 9; - loop_start_time = now; - } - else if (state == 9 && elapsed_ms >= 300) - { - Serial.println("@@10"); - servo.moveY(100, 100); - state = 10; - loop_start_time = now; - } - else if (state == 10 && elapsed_ms >= 300) - { - Serial.println("@@11"); - servo.moveY(90, 100); - state = 11; - loop_start_time = now; - } - else if (state == 11 && elapsed_ms >= 5000) - { - Serial.println("@@12"); - state = 0; - loop_start_time = now; - } - - servo.loop(); - } -} From a5742eec18f2e42c6300bc02ba59325eefc9a39e Mon Sep 17 00:00:00 2001 From: Atsushi Morimoto <74th.tech@gmail.com> Date: Sat, 14 Mar 2026 09:05:23 +0900 Subject: [PATCH 4/5] feat: rename AsyncServo to BodyServo for clarity and consistency --- firmware/include/servo.hpp | 4 ++-- firmware/src/main.cpp | 2 +- firmware/src/servo.cpp | 24 ++++++++++++------------ 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/firmware/include/servo.hpp b/firmware/include/servo.hpp index 48a80a8..eff200f 100644 --- a/firmware/include/servo.hpp +++ b/firmware/include/servo.hpp @@ -8,10 +8,10 @@ #include "protocols.hpp" -class AsyncServo +class BodyServo { public: - AsyncServo() = default; + BodyServo() = default; void init(); void loop(); diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index 8ade6be..34fb063 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/main.cpp @@ -32,7 +32,7 @@ static Speaking speaking(stateMachine); static Listening listening(wsClient, stateMachine, SAMPLE_RATE); static WakeUpWord wakeUpWord(stateMachine, SAMPLE_RATE); static Display display(stateMachine); -static AsyncServo servo; +static BodyServo servo; // Protocol types are defined in include/protocols.hpp namespace diff --git a/firmware/src/servo.cpp b/firmware/src/servo.cpp index acd4be5..ffe5782 100644 --- a/firmware/src/servo.cpp +++ b/firmware/src/servo.cpp @@ -33,7 +33,7 @@ int16_t readInt16Le(const uint8_t *src) } } // namespace -void AsyncServo::init() +void BodyServo::init() { if (!ensureAttached()) { @@ -45,7 +45,7 @@ void AsyncServo::init() axis_y_.servo.write(axis_y_.current_degree); } -void AsyncServo::loop() +void BodyServo::loop() { if (!attached_) { @@ -96,7 +96,7 @@ void AsyncServo::loop() } } -void AsyncServo::resetSequence() +void BodyServo::resetSequence() { steps_.clear(); current_step_index_ = 0; @@ -107,7 +107,7 @@ void AsyncServo::resetSequence() axis_y_.moving = false; } -bool AsyncServo::enqueueSequence(const uint8_t *payload, size_t payload_len) +bool BodyServo::enqueueSequence(const uint8_t *payload, size_t payload_len) { if (!ensureAttached()) { @@ -189,17 +189,17 @@ bool AsyncServo::enqueueSequence(const uint8_t *payload, size_t payload_len) return true; } -bool AsyncServo::isBusy() const +bool BodyServo::isBusy() const { return sequence_active_ || axis_x_.moving || axis_y_.moving; } -void AsyncServo::setCompletionCallback(std::function cb) +void BodyServo::setCompletionCallback(std::function cb) { on_complete_ = std::move(cb); } -bool AsyncServo::ensureAttached() +bool BodyServo::ensureAttached() { if (attached_) { @@ -215,7 +215,7 @@ bool AsyncServo::ensureAttached() return attached_; } -void AsyncServo::updateAxis(AxisMotion &axis, uint32_t now) +void BodyServo::updateAxis(AxisMotion &axis, uint32_t now) { if (!axis.moving) { @@ -243,7 +243,7 @@ void AsyncServo::updateAxis(AxisMotion &axis, uint32_t now) axis.last_update_ms = now; } -void AsyncServo::startMove(AxisMotion &axis, int8_t degree, int16_t duration_ms) +void BodyServo::startMove(AxisMotion &axis, int8_t degree, int16_t duration_ms) { axis.target_degree = clampDegree(degree); axis.start_degree = axis.current_degree; @@ -262,7 +262,7 @@ void AsyncServo::startMove(AxisMotion &axis, int8_t degree, int16_t duration_ms) axis.moving = true; } -void AsyncServo::startCurrentStep(uint32_t now) +void BodyServo::startCurrentStep(uint32_t now) { if (!sequence_active_ || current_step_index_ >= steps_.size()) { @@ -288,7 +288,7 @@ void AsyncServo::startCurrentStep(uint32_t now) } } -void AsyncServo::advanceStep() +void BodyServo::advanceStep() { if (!sequence_active_) { @@ -303,7 +303,7 @@ void AsyncServo::advanceStep() } } -void AsyncServo::completeSequence() +void BodyServo::completeSequence() { steps_.clear(); current_step_index_ = 0; From a8d7687b5b208f9c55d5360d114244ea35f1ac8b Mon Sep 17 00:00:00 2001 From: Atsushi Morimoto <74th.tech@gmail.com> Date: Sat, 14 Mar 2026 09:37:37 +0900 Subject: [PATCH 5/5] feat: add lint-fix target to Makefile and clean up echo app imports --- .github/prompts/fix-lint.md | 1 + Makefile | 4 ++++ example_apps/echo.py | 5 +---- example_apps/echo_with_move.py | 5 +---- stackchan_server/ws_proxy.py | 35 ++++++++++++++++++++++------------ 5 files changed, 30 insertions(+), 20 deletions(-) create mode 100644 .github/prompts/fix-lint.md diff --git a/.github/prompts/fix-lint.md b/.github/prompts/fix-lint.md new file mode 100644 index 0000000..4ea0636 --- /dev/null +++ b/.github/prompts/fix-lint.md @@ -0,0 +1 @@ +make lint を実行して、検出されたエラーを修正して diff --git a/Makefile b/Makefile index 89a9ae3..318a6f4 100644 --- a/Makefile +++ b/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 diff --git a/example_apps/echo.py b/example_apps/echo.py index 3cf7c9e..7218540 100644 --- a/example_apps/echo.py +++ b/example_apps/echo.py @@ -7,7 +7,6 @@ from stackchan_server.app import StackChanApp from stackchan_server.speech_recognition import ( WhisperCppSpeechToText, - WhisperServerSpeechToText, ) from stackchan_server.speech_synthesis import VoiceVoxSpeechSynthesizer from stackchan_server.ws_proxy import EmptyTranscriptError, WsProxy @@ -20,10 +19,8 @@ ) 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: + # 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(), diff --git a/example_apps/echo_with_move.py b/example_apps/echo_with_move.py index d7b091f..104b91e 100644 --- a/example_apps/echo_with_move.py +++ b/example_apps/echo_with_move.py @@ -7,7 +7,6 @@ from stackchan_server.app import StackChanApp from stackchan_server.speech_recognition import ( WhisperCppSpeechToText, - WhisperServerSpeechToText, ) from stackchan_server.speech_synthesis import VoiceVoxSpeechSynthesizer from stackchan_server.ws_proxy import ( @@ -25,10 +24,8 @@ ) 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: + # 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(), diff --git a/stackchan_server/ws_proxy.py b/stackchan_server/ws_proxy.py index af91ca5..b92ef41 100644 --- a/stackchan_server/ws_proxy.py +++ b/stackchan_server/ws_proxy.py @@ -8,7 +8,7 @@ from enum import IntEnum, StrEnum from logging import getLogger from pathlib import Path -from typing import Literal, Optional, Sequence, TypeAlias +from typing import Literal, Optional, Sequence, TypeAlias, cast from fastapi import WebSocket, WebSocketDisconnect @@ -96,35 +96,46 @@ def _encode_servo_commands(commands: Sequence[ServoCommand]) -> bytes: payload.append(len(normalized)) for index, command in enumerate(normalized): - name = str(command[0]) - if name == "sleep": - if len(command) != 2: + if len(command) == 2: + sleep_command = cast(ServoSleepCommand, command) + name, raw_duration_ms = sleep_command + name = str(name) + if name != "sleep": raise ValueError( - f"sleep command at index {index} must be ('sleep', duration_ms)" + f"unsupported servo command at index {index}: {name}" ) duration_ms = _ensure_range( - int(command[1]), minimum=-32768, maximum=32767, label="sleep duration" + int(raw_duration_ms), + minimum=-32768, + maximum=32767, + label="sleep duration", ) payload.append(_ServoOp.SLEEP) payload.extend(struct.pack("