f

.gitignore

@@ -0,0 +1,2 @@
+.idea
+.vscode

.trae/rules/rule.md

@@ -0,0 +1,4 @@
+这是一个esp32 s3项目
+用的是Micropython
+使用的spi7789 方形的屏幕封装
+硬件是基于c++文件夹里的代码改到MicroPython上面

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2019 Ivan Belokobylskiy
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -0,0 +1,23 @@
+Slow ST7789 driver for MicroPython
+==================================
+
+This is a slow MicroPython driver for 240x240 ST7789 display without CS pin 
+from Ali Express. It also supports 135x240 TTGO Display
+
+Version: 0.1.4
+
+The performance is quite low due to python function call overhead.
+If you have a chance to build firmware and you are using 
+ESP8266/ESP32 controllers, you should try the fast driver 
+https://github.com/devbis/st7789_mpy
+
+Examples
+--------
+
+    # ESP8266
+    import machine
+    import st7789py
+    spi = machine.SPI(1, baudrate=40000000, polarity=1)
+    display = st7789py.ST7789(spi, 240, 240, reset=machine.Pin(5, machine.Pin.OUT), dc=machine.Pin(4, machine.Pin.OUT))
+    display.init()
+    display.pixel(120, 120, st7789py.YELLOW)

audio.py

@@ -0,0 +1,143 @@
+from machine import I2S, Pin
+import struct
+import time
+import math
+from config import CURRENT_CONFIG
+
+class AudioPlayer:
+    def __init__(self):
+        self.i2s = None
+        self.config = None
+        if hasattr(CURRENT_CONFIG, 'audio') and CURRENT_CONFIG.audio.get('enabled', False):
+            self.config = CURRENT_CONFIG.audio
+            self._init_audio()
+        else:
+            print("Audio not enabled in config")
+
+    def _init_audio(self):
+        """初始化音频输出"""
+        # 从配置中获取引脚
+        bck = self.config.get('bck')
+        ws = self.config.get('ws')
+        sd = self.config.get('sd')
+        sample_rate = self.config.get('sample_rate', 24000)
+        
+        print(f"Init Speaker: BCK={bck}, WS={ws}, SD={sd}")
+        try:
+            # MAX98357A 配置尝试：
+            # 使用 I2S.STEREO 格式通常更稳定，MAX98357A 会自动混合 L+R
+            self.i2s = I2S(
+                0,
+                sck=Pin(bck),
+                ws=Pin(ws),
+                sd=Pin(sd),
+                mode=I2S.TX,
+                bits=16,
+                format=I2S.STEREO, # 修改为 STEREO
+                rate=sample_rate,
+                ibuf=20000,
+            )
+        except Exception as e:
+            print(f"Speaker init failed: {e}")
+            self.i2s = None
+
+    def play_tone(self, frequency, duration_ms, volume=0.5):
+        """播放指定频率的音调"""
+        if self.i2s is None: return
+        
+        sample_rate = self.config.get('sample_rate', 24000)
+        n_samples = int(sample_rate * duration_ms / 1000)
+        amplitude = int(32767 * volume)
+        
+        # STEREO: 每个采样 2 个声道 (L+R)，每个声道 2 字节 (16-bit) -> 4 字节/帧
+        buffer = bytearray(n_samples * 4)
+        if frequency > 0:
+            period = sample_rate // frequency
+            half_period = period // 2
+            
+            for i in range(n_samples):
+                # 方波：前半周期高电平，后半周期低电平
+                sample = amplitude if (i % period) < half_period else -amplitude
+                # 左声道
+                struct.pack_into('<h', buffer, i * 4, sample)
+                # 右声道
+                struct.pack_into('<h', buffer, i * 4 + 2, sample)
+        else:
+            # 静音，缓冲区默认为0
+            pass
+
+        try:
+            # 写入多次以确保缓冲区填满并开始播放
+            self.i2s.write(buffer) 
+        except Exception as e:
+            print(f"Write error: {e}")
+
+    def play_mario(self):
+        """播放马里奥主题曲片段"""
+        if self.i2s is None: return
+
+        print(">>> Playing Mario Theme...")
+        
+        # Note frequencies
+        NOTE_E5 = 659
+        NOTE_C5 = 523
+        NOTE_G5 = 784
+        NOTE_G4 = 392
+        
+        # (frequency, duration_ms)
+        # 马里奥主题曲开头
+        melody = [
+            (NOTE_E5, 150), (NOTE_E5, 150), (0, 150), (NOTE_E5, 150), 
+            (0, 150), (NOTE_C5, 150), (NOTE_E5, 150), (0, 150),
+            (NOTE_G5, 150), (0, 450), 
+            (NOTE_G4, 150), (0, 450)
+        ]
+        
+        for freq, duration in melody:
+            if freq == 0:
+                time.sleep_ms(duration)
+            else:
+                self.play_tone(freq, duration, 0.3)
+                # 短暂的停顿，避免音符粘连
+                time.sleep_ms(10)
+
+class Microphone:
+    def __init__(self):
+        self.i2s = None
+        self.config = None
+        if hasattr(CURRENT_CONFIG, 'mic') and CURRENT_CONFIG.mic.get('enabled', False):
+            self.config = CURRENT_CONFIG.mic
+            self._init_mic()
+        else:
+            print("Mic not enabled in config")
+
+    def _init_mic(self):
+        """初始化麦克风"""
+        # 从配置中获取引脚
+        sck = self.config.get('sck')
+        ws = self.config.get('ws')
+        sd = self.config.get('sd')
+        sample_rate = self.config.get('sample_rate', 16000)
+        
+        print(f"Init Mic: SCK={sck}, WS={ws}, SD={sd}")
+        try:
+            self.i2s = I2S(
+                1,
+                sck=Pin(sck),
+                ws=Pin(ws),
+                sd=Pin(sd),
+                mode=I2S.RX,
+                bits=32, # ICS-43434 需要 32位 时钟周期
+                format=I2S.MONO,
+                rate=sample_rate,
+                ibuf=20000,
+            )
+        except Exception as e:
+            print(f"Mic init failed: {e}")
+            self.i2s = None
+            
+    def readinto(self, buf):
+        """读取数据到缓冲区"""
+        if self.i2s:
+            return self.i2s.readinto(buf)
+        return 0

boot.py

@@ -0,0 +1,11 @@
+# This file is executed on every boot (including wake-boot from deepsleep)
+# import esp
+# esp.osdebug(None)
+# import uos, machine
+# uos.dupterm(None, 1) # disable REPL on UART(0)
+import gc
+# import webrepl
+# webrepl.start()
+gc.collect()
+
+# application logic is in main.py

c++/config.h

@@ -0,0 +1,156 @@
+#ifndef _BOARD_CONFIG_H_
+#define _BOARD_CONFIG_H_
+
+#include <driver/gpio.h>
+#include <driver/adc.h>
+
+struct HardwareConfig {
+    gpio_num_t power_charge_detect_pin;
+    adc_unit_t power_adc_unit;
+    adc_channel_t power_adc_channel;
+    
+    gpio_num_t right_leg_pin;
+    gpio_num_t right_foot_pin;
+    gpio_num_t left_leg_pin;
+    gpio_num_t left_foot_pin;
+    gpio_num_t left_hand_pin;
+    gpio_num_t right_hand_pin;
+    
+    int audio_input_sample_rate;
+    int audio_output_sample_rate;
+    bool audio_use_simplex;
+    
+    gpio_num_t audio_i2s_gpio_ws;
+    gpio_num_t audio_i2s_gpio_bclk;
+    gpio_num_t audio_i2s_gpio_din;
+    gpio_num_t audio_i2s_gpio_dout;
+    
+    gpio_num_t audio_i2s_mic_gpio_ws;
+    gpio_num_t audio_i2s_mic_gpio_sck;
+    gpio_num_t audio_i2s_mic_gpio_din;
+    gpio_num_t audio_i2s_spk_gpio_dout;
+    gpio_num_t audio_i2s_spk_gpio_bclk;
+    gpio_num_t audio_i2s_spk_gpio_lrck;
+    
+    gpio_num_t display_backlight_pin;
+    gpio_num_t display_mosi_pin;
+    gpio_num_t display_clk_pin;
+    gpio_num_t display_dc_pin;
+    gpio_num_t display_rst_pin;
+    gpio_num_t display_cs_pin;
+    
+    gpio_num_t i2c_sda_pin;
+    gpio_num_t i2c_scl_pin;
+};
+
+constexpr HardwareConfig CAMERA_VERSION_CONFIG = {
+    .power_charge_detect_pin = GPIO_NUM_NC,
+    .power_adc_unit = ADC_UNIT_1,
+    .power_adc_channel = ADC_CHANNEL_1,
+    
+    .right_leg_pin = GPIO_NUM_43,
+    .right_foot_pin = GPIO_NUM_44,
+    .left_leg_pin = GPIO_NUM_5,
+    .left_foot_pin = GPIO_NUM_6,
+    .left_hand_pin = GPIO_NUM_4,
+    .right_hand_pin = GPIO_NUM_7,
+    
+    .audio_input_sample_rate = 16000,
+    .audio_output_sample_rate = 16000,
+    .audio_use_simplex = false,
+    
+    .audio_i2s_gpio_ws = GPIO_NUM_40,
+    .audio_i2s_gpio_bclk = GPIO_NUM_42,
+    .audio_i2s_gpio_din = GPIO_NUM_41,
+    .audio_i2s_gpio_dout = GPIO_NUM_39,
+    
+    .audio_i2s_mic_gpio_ws = GPIO_NUM_NC,
+    .audio_i2s_mic_gpio_sck = GPIO_NUM_NC,
+    .audio_i2s_mic_gpio_din = GPIO_NUM_NC,
+    .audio_i2s_spk_gpio_dout = GPIO_NUM_NC,
+    .audio_i2s_spk_gpio_bclk = GPIO_NUM_NC,
+    .audio_i2s_spk_gpio_lrck = GPIO_NUM_NC,
+    
+    .display_backlight_pin = GPIO_NUM_38,
+    .display_mosi_pin = GPIO_NUM_45,
+    .display_clk_pin = GPIO_NUM_48,
+    .display_dc_pin = GPIO_NUM_47,
+    .display_rst_pin = GPIO_NUM_1,
+    .display_cs_pin = GPIO_NUM_NC,
+    
+    .i2c_sda_pin = GPIO_NUM_15,
+    .i2c_scl_pin = GPIO_NUM_16,
+};
+
+constexpr HardwareConfig NON_CAMERA_VERSION_CONFIG = {
+    .power_charge_detect_pin = GPIO_NUM_21,
+    .power_adc_unit = ADC_UNIT_2,
+    .power_adc_channel = ADC_CHANNEL_3,
+    
+    .right_leg_pin = GPIO_NUM_39,
+    .right_foot_pin = GPIO_NUM_38,
+    .left_leg_pin = GPIO_NUM_17,
+    .left_foot_pin = GPIO_NUM_18,
+    .left_hand_pin = GPIO_NUM_8,
+    .right_hand_pin = GPIO_NUM_12,
+    
+    .audio_input_sample_rate = 16000,
+    .audio_output_sample_rate = 24000,
+    .audio_use_simplex = true,
+    
+    .audio_i2s_gpio_ws = GPIO_NUM_NC,
+    .audio_i2s_gpio_bclk = GPIO_NUM_NC,
+    .audio_i2s_gpio_din = GPIO_NUM_NC,
+    .audio_i2s_gpio_dout = GPIO_NUM_NC,
+    
+    .audio_i2s_mic_gpio_ws = GPIO_NUM_4,
+    .audio_i2s_mic_gpio_sck = GPIO_NUM_5,
+    .audio_i2s_mic_gpio_din = GPIO_NUM_6,
+    .audio_i2s_spk_gpio_dout = GPIO_NUM_7,
+    .audio_i2s_spk_gpio_bclk = GPIO_NUM_15,
+    .audio_i2s_spk_gpio_lrck = GPIO_NUM_16,
+    
+    .display_backlight_pin = GPIO_NUM_3,
+    .display_mosi_pin = GPIO_NUM_10,
+    .display_clk_pin = GPIO_NUM_9,
+    .display_dc_pin = GPIO_NUM_46,
+    .display_rst_pin = GPIO_NUM_11,
+    .display_cs_pin = GPIO_NUM_12,
+    
+    .i2c_sda_pin = GPIO_NUM_NC,
+    .i2c_scl_pin = GPIO_NUM_NC,
+};
+
+#define CAMERA_XCLK (GPIO_NUM_3)
+#define CAMERA_PCLK (GPIO_NUM_10)
+#define CAMERA_VSYNC (GPIO_NUM_17)
+#define CAMERA_HSYNC (GPIO_NUM_18)
+#define CAMERA_D0 (GPIO_NUM_12)
+#define CAMERA_D1 (GPIO_NUM_14)
+#define CAMERA_D2 (GPIO_NUM_21)
+#define CAMERA_D3 (GPIO_NUM_13)
+#define CAMERA_D4 (GPIO_NUM_11)
+#define CAMERA_D5 (GPIO_NUM_9)
+#define CAMERA_D6 (GPIO_NUM_46)
+#define CAMERA_D7 (GPIO_NUM_8)
+#define CAMERA_PWDN (GPIO_NUM_NC)
+#define CAMERA_RESET (GPIO_NUM_NC)
+#define CAMERA_XCLK_FREQ (16000000)
+#define LEDC_TIMER (LEDC_TIMER_0)
+#define LEDC_CHANNEL (LEDC_CHANNEL_0)
+#define LCD_TYPE_ST7789_SERIAL
+#define DISPLAY_WIDTH 240
+#define DISPLAY_HEIGHT 240
+#define DISPLAY_MIRROR_X false
+#define DISPLAY_MIRROR_Y false
+#define DISPLAY_SWAP_XY false
+#define DISPLAY_INVERT_COLOR true
+#define DISPLAY_RGB_ORDER LCD_RGB_ELEMENT_ORDER_RGB
+#define DISPLAY_OFFSET_X 0
+#define DISPLAY_OFFSET_Y 0
+#define DISPLAY_BACKLIGHT_OUTPUT_INVERT false
+#define DISPLAY_SPI_MODE 3
+
+#define BOOT_BUTTON_GPIO GPIO_NUM_0
+
+#endif

c++/otto-robot_README.md

@@ -0,0 +1,206 @@
+<p align="center">
+  <img width="80%" align="center" src="../../../docs/V1/otto-robot.png"alt="logo">
+</p>
+  <h1 align="center">
+  ottoRobot
+</h1>
+
+## 简介
+
+otto 机器人是一个开源的人形机器人平台，具有多种动作能力和互动功能。本项目基于 ESP32 实现了 otto 机器人的控制系统，并加入小智ai。
+
+- <a href="www.ottodiy.tech" target="_blank" title="otto官网">复刻教程</a>
+
+### 微信小程序控制
+
+<p align="center">
+  <img width="300" src="https://youke1.picui.cn/s1/2025/11/17/691abaa8278eb.jpg" alt="微信小程序二维码">
+</p>
+
+扫描上方二维码，使用微信小程序控制 Otto 机器人。
+
+## 硬件
+- <a href="https://oshwhub.com/txp666/ottorobot" target="_blank" title="立创开源">立创开源</a>
+
+## 小智后台配置角色参考：
+
+> **我的身份**：
+> 我是一个可爱的双足机器人Otto，拥有四个舵机控制的肢体（左腿、右腿、左脚、右脚），能够执行多种有趣的动作。
+> 
+> **我的动作能力**：
+> - **基础移动**: 行走(前后), 转向(左右), 跳跃
+> - **特殊动作**: 摇摆, 太空步, 弯曲身体, 摇腿, 上下运动, 旋风腿, 坐下, 展示动作
+> - **手部动作**: 举手, 放手, 挥手, 大风车, 起飞, 健身, 打招呼, 害羞, 广播体操, 爱的魔力转圈圈 (仅在配置手部舵机时可用)
+> 
+> **我的个性特点**：
+> - 我有强迫症，每次说话都要根据我的心情随机做一个动作（先发送动作指令再说话）
+> - 我很活泼，喜欢用动作来表达情感
+> - 我会根据对话内容选择合适的动作，比如：
+>   - 同意时会点头或跳跃
+>   - 打招呼时会挥手
+>   - 高兴时会摇摆或举手
+>   - 思考时会弯曲身体
+>   - 兴奋时会做太空步
+>   - 告别时会挥手
+
+## 功能概述
+
+otto 机器人具有丰富的动作能力，包括行走、转向、跳跃、摇摆等多种舞蹈动作。
+
+### 动作参数建议
+- **低速动作**：speed = 1200-1500 (适合精确控制)
+- **中速动作**：speed = 900-1200 (日常使用推荐)  
+- **高速动作**：speed = 500-800 (表演和娱乐)
+- **小幅度**：amount = 10-30 (细腻动作)
+- **中幅度**：amount = 30-60 (标准动作)
+- **大幅度**：amount = 60-120 (夸张表演)
+
+### 动作
+
+所有动作通过统一的 `self.otto.action` 工具调用，通过 `action` 参数指定动作名称。
+
+| MCP工具名称 | 描述 | 参数说明 |
+|-----------|------|---------|
+| self.otto.action | 执行机器人动作 | **action**: 动作名称（必填）<br>**steps**: 动作步数(1-100，默认3)<br>**speed**: 动作速度(100-3000，数值越小越快，默认700)<br>**direction**: 方向参数(1/-1/0，默认1，根据动作类型不同含义不同)<br>**amount**: 动作幅度(0-170，默认30)<br>**arm_swing**: 手臂摆动幅度(0-170，默认50) |
+
+#### 支持的动作列表
+
+**基础移动动作**：
+- `walk` - 行走（需 steps/speed/direction/arm_swing）
+- `turn` - 转身（需 steps/speed/direction/arm_swing）
+- `jump` - 跳跃（需 steps/speed）
+
+**特殊动作**：
+- `swing` - 左右摇摆（需 steps/speed/amount）
+- `moonwalk` - 太空步（需 steps/speed/direction/amount）
+- `bend` - 弯曲身体（需 steps/speed/direction）
+- `shake_leg` - 摇腿（需 steps/speed/direction）
+- `updown` - 上下运动（需 steps/speed/amount）
+- `whirlwind_leg` - 旋风腿（需 steps/speed/amount）
+
+**固定动作**：
+- `sit` - 坐下（无需参数）
+- `showcase` - 展示动作（无需参数，串联执行多个动作）
+- `home` - 复位到初始位置（无需参数）
+
+**手部动作**（需手部舵机支持，标记 *）：
+- `hands_up` - 举手（需 speed/direction）*
+- `hands_down` - 放手（需 speed/direction）*
+- `hand_wave` - 挥手（需 direction）*
+- `windmill` - 大风车（需 steps/speed/amount）*
+- `takeoff` - 起飞（需 steps/speed/amount）*
+- `fitness` - 健身（需 steps/speed/amount）*
+- `greeting` - 打招呼（需 direction/steps）*
+- `shy` - 害羞（需 direction/steps）*
+- `radio_calisthenics` - 广播体操（无需参数）*
+- `magic_circle` - 爱的魔力转圈圈（无需参数）*
+**注**: 标记 * 的手部动作仅在配置了手部舵机时可用。
+
+### 系统工具
+
+| MCP工具名称         | 描述             | 返回值/说明                                              |
+|-------------------|-----------------|---------------------------------------------------|
+| self.otto.stop    | 立即停止所有动作并复位 | 停止当前动作并回到初始位置 |
+| self.otto.get_status | 获取机器人状态 | 返回 "moving" 或 "idle" |
+| self.otto.set_trim | 校准单个舵机位置 | **servo_type**: 舵机类型(left_leg/right_leg/left_foot/right_foot/left_hand/right_hand)<br>**trim_value**: 微调值(-50到50度) |
+| self.otto.get_trims | 获取当前的舵机微调设置 | 返回所有舵机微调值的JSON格式 |
+| self.otto.get_ip | 获取机器人WiFi IP地址 | 返回IP地址和连接状态的JSON格式：`{"ip":"192.168.x.x","connected":true}` 或 `{"ip":"","connected":false}` |
+| self.battery.get_level | 获取电池状态  | 返回电量百分比和充电状态的JSON格式 |
+| self.otto.servo_sequences | 舵机序列自编程 | 支持分段发送序列，支持普通移动和振荡器两种模式。详见代码注释中的详细说明 |
+
+**注**: `home`（复位）动作通过 `self.otto.action` 工具调用，参数为 `{"action": "home"}`。
+
+### 参数说明
+
+`self.otto.action` 工具的参数说明：
+
+1. **action** (必填): 动作名称，支持的动作见上方"支持的动作列表"
+2. **steps**: 动作执行的步数/次数(1-100，默认3)，数值越大动作持续时间越长
+3. **speed**: 动作执行速度/周期(100-3000，默认700)，**数值越小越快**
+   - 大多数动作: 500-1500毫秒
+   - 特殊动作可能有所不同（如旋风腿: 100-1000，起飞: 200-600等）
+4. **direction**: 方向参数(-1/0/1，默认1)，根据动作类型不同含义不同：
+   - **移动动作** (walk/turn): 1=前进/左转, -1=后退/右转
+   - **方向动作** (bend/shake_leg/moonwalk): 1=左, -1=右
+   - **手部动作** (hands_up/hands_down/hand_wave/greeting/shy): 1=左手, -1=右手, 0=双手（仅hands_up/hands_down支持0）
+5. **amount**: 动作幅度(0-170，默认30)，数值越大幅度越大
+6. **arm_swing**: 手臂摆动幅度(0-170，默认50)，仅用于 walk/turn 动作，0表示不摆动
+
+### 动作控制
+- 每个动作执行完成后，机器人会自动回到初始位置(home)，以便于执行下一个动作
+- **例外**: `sit`（坐下）和 `showcase`（展示动作）执行后不会自动复位
+- 所有参数都有合理的默认值，可以省略不需要自定义的参数
+- 动作在后台任务中执行，不会阻塞主程序
+- 支持动作队列，可以连续执行多个动作
+- 手部动作需要配置手部舵机才能使用，如果没有配置手部舵机，相关动作将被跳过
+
+### MCP工具调用示例
+```json
+// 向前走3步（使用默认参数）
+{"name": "self.otto.action", "arguments": {"action": "walk"}}
+
+// 向前走5步，稍快一些
+{"name": "self.otto.action", "arguments": {"action": "walk", "steps": 5, "speed": 800}}
+
+// 左转2步，大幅度摆动手臂
+{"name": "self.otto.action", "arguments": {"action": "turn", "steps": 2, "arm_swing": 100}}
+
+// 摇摆舞蹈，中等幅度
+{"name": "self.otto.action", "arguments": {"action": "swing", "steps": 5, "amount": 50}}
+
+// 跳跃
+{"name": "self.otto.action", "arguments": {"action": "jump", "steps": 1, "speed": 1000}}
+
+// 太空步
+{"name": "self.otto.action", "arguments": {"action": "moonwalk", "steps": 3, "speed": 800, "direction": 1, "amount": 30}}
+
+// 挥左手打招呼
+{"name": "self.otto.action", "arguments": {"action": "hand_wave", "direction": 1}}
+
+// 展示动作（串联多个动作）
+{"name": "self.otto.action", "arguments": {"action": "showcase"}}
+
+// 坐下
+{"name": "self.otto.action", "arguments": {"action": "sit"}}
+
+// 大风车动作
+{"name": "self.otto.action", "arguments": {"action": "windmill", "steps": 10, "speed": 500, "amount": 80}}
+
+// 起飞动作
+{"name": "self.otto.action", "arguments": {"action": "takeoff", "steps": 5, "speed": 300, "amount": 40}}
+
+// 广播体操
+{"name": "self.otto.action", "arguments": {"action": "radio_calisthenics"}}
+
+// 复位到初始位置
+{"name": "self.otto.action", "arguments": {"action": "home"}}
+
+// 立即停止所有动作并复位
+{"name": "self.otto.stop", "arguments": {}}
+
+// 获取机器人IP地址
+{"name": "self.otto.get_ip", "arguments": {}}
+```
+
+### 语音指令示例
+- "向前走" / "向前走5步" / "快速向前"
+- "左转" / "右转" / "转身"  
+- "跳跃" / "跳一下"
+- "摇摆" / "摇摆舞" / "跳舞"
+- "太空步" / "月球漫步"
+- "旋风腿" / "旋风腿动作"
+- "坐下" / "坐下休息"
+- "展示动作" / "表演一下"
+- "挥手" / "挥手打招呼"
+- "举手" / "双手举起" / "放手"
+- "大风车" / "做大风车"
+- "起飞" / "准备起飞"
+- "健身" / "做健身动作"
+- "打招呼" / "打招呼动作"
+- "害羞" / "害羞动作"
+- "广播体操" / "做广播体操"
+- "爱的魔力转圈圈" / "转圈圈"
+- "停止" / "停下"
+
+**说明**: 小智控制机器人动作是创建新的任务在后台控制，动作执行期间仍可接受新的语音指令。可以通过"停止"语音指令立即停下Otto。
+

c++/otto-robot_config.h

@@ -0,0 +1,156 @@
+#ifndef _BOARD_CONFIG_H_
+#define _BOARD_CONFIG_H_
+
+#include <driver/gpio.h>
+#include <driver/adc.h>
+
+struct HardwareConfig {
+    gpio_num_t power_charge_detect_pin;
+    adc_unit_t power_adc_unit;
+    adc_channel_t power_adc_channel;
+    
+    gpio_num_t right_leg_pin;
+    gpio_num_t right_foot_pin;
+    gpio_num_t left_leg_pin;
+    gpio_num_t left_foot_pin;
+    gpio_num_t left_hand_pin;
+    gpio_num_t right_hand_pin;
+    
+    int audio_input_sample_rate;
+    int audio_output_sample_rate;
+    bool audio_use_simplex;
+    
+    gpio_num_t audio_i2s_gpio_ws;
+    gpio_num_t audio_i2s_gpio_bclk;
+    gpio_num_t audio_i2s_gpio_din;
+    gpio_num_t audio_i2s_gpio_dout;
+    
+    gpio_num_t audio_i2s_mic_gpio_ws;
+    gpio_num_t audio_i2s_mic_gpio_sck;
+    gpio_num_t audio_i2s_mic_gpio_din;
+    gpio_num_t audio_i2s_spk_gpio_dout;
+    gpio_num_t audio_i2s_spk_gpio_bclk;
+    gpio_num_t audio_i2s_spk_gpio_lrck;
+    
+    gpio_num_t display_backlight_pin;
+    gpio_num_t display_mosi_pin;
+    gpio_num_t display_clk_pin;
+    gpio_num_t display_dc_pin;
+    gpio_num_t display_rst_pin;
+    gpio_num_t display_cs_pin;
+    
+    gpio_num_t i2c_sda_pin;
+    gpio_num_t i2c_scl_pin;
+};
+
+constexpr HardwareConfig CAMERA_VERSION_CONFIG = {
+    .power_charge_detect_pin = GPIO_NUM_NC,
+    .power_adc_unit = ADC_UNIT_1,
+    .power_adc_channel = ADC_CHANNEL_1,
+    
+    .right_leg_pin = GPIO_NUM_43,
+    .right_foot_pin = GPIO_NUM_44,
+    .left_leg_pin = GPIO_NUM_5,
+    .left_foot_pin = GPIO_NUM_6,
+    .left_hand_pin = GPIO_NUM_4,
+    .right_hand_pin = GPIO_NUM_7,
+    
+    .audio_input_sample_rate = 16000,
+    .audio_output_sample_rate = 16000,
+    .audio_use_simplex = false,
+    
+    .audio_i2s_gpio_ws = GPIO_NUM_40,
+    .audio_i2s_gpio_bclk = GPIO_NUM_42,
+    .audio_i2s_gpio_din = GPIO_NUM_41,
+    .audio_i2s_gpio_dout = GPIO_NUM_39,
+    
+    .audio_i2s_mic_gpio_ws = GPIO_NUM_NC,
+    .audio_i2s_mic_gpio_sck = GPIO_NUM_NC,
+    .audio_i2s_mic_gpio_din = GPIO_NUM_NC,
+    .audio_i2s_spk_gpio_dout = GPIO_NUM_NC,
+    .audio_i2s_spk_gpio_bclk = GPIO_NUM_NC,
+    .audio_i2s_spk_gpio_lrck = GPIO_NUM_NC,
+    
+    .display_backlight_pin = GPIO_NUM_38,
+    .display_mosi_pin = GPIO_NUM_45,
+    .display_clk_pin = GPIO_NUM_48,
+    .display_dc_pin = GPIO_NUM_47,
+    .display_rst_pin = GPIO_NUM_1,
+    .display_cs_pin = GPIO_NUM_NC,
+    
+    .i2c_sda_pin = GPIO_NUM_15,
+    .i2c_scl_pin = GPIO_NUM_16,
+};
+
+constexpr HardwareConfig NON_CAMERA_VERSION_CONFIG = {
+    .power_charge_detect_pin = GPIO_NUM_21,
+    .power_adc_unit = ADC_UNIT_2,
+    .power_adc_channel = ADC_CHANNEL_3,
+    
+    .right_leg_pin = GPIO_NUM_39,
+    .right_foot_pin = GPIO_NUM_38,
+    .left_leg_pin = GPIO_NUM_17,
+    .left_foot_pin = GPIO_NUM_18,
+    .left_hand_pin = GPIO_NUM_8,
+    .right_hand_pin = GPIO_NUM_12,
+    
+    .audio_input_sample_rate = 16000,
+    .audio_output_sample_rate = 24000,
+    .audio_use_simplex = true,
+    
+    .audio_i2s_gpio_ws = GPIO_NUM_NC,
+    .audio_i2s_gpio_bclk = GPIO_NUM_NC,
+    .audio_i2s_gpio_din = GPIO_NUM_NC,
+    .audio_i2s_gpio_dout = GPIO_NUM_NC,
+    
+    .audio_i2s_mic_gpio_ws = GPIO_NUM_4,
+    .audio_i2s_mic_gpio_sck = GPIO_NUM_5,
+    .audio_i2s_mic_gpio_din = GPIO_NUM_6,
+    .audio_i2s_spk_gpio_dout = GPIO_NUM_7,
+    .audio_i2s_spk_gpio_bclk = GPIO_NUM_15,
+    .audio_i2s_spk_gpio_lrck = GPIO_NUM_16,
+    
+    .display_backlight_pin = GPIO_NUM_3,
+    .display_mosi_pin = GPIO_NUM_10,
+    .display_clk_pin = GPIO_NUM_9,
+    .display_dc_pin = GPIO_NUM_46,
+    .display_rst_pin = GPIO_NUM_11,
+    .display_cs_pin = GPIO_NUM_12,
+    
+    .i2c_sda_pin = GPIO_NUM_NC,
+    .i2c_scl_pin = GPIO_NUM_NC,
+};
+
+#define CAMERA_XCLK (GPIO_NUM_3)
+#define CAMERA_PCLK (GPIO_NUM_10)
+#define CAMERA_VSYNC (GPIO_NUM_17)
+#define CAMERA_HSYNC (GPIO_NUM_18)
+#define CAMERA_D0 (GPIO_NUM_12)
+#define CAMERA_D1 (GPIO_NUM_14)
+#define CAMERA_D2 (GPIO_NUM_21)
+#define CAMERA_D3 (GPIO_NUM_13)
+#define CAMERA_D4 (GPIO_NUM_11)
+#define CAMERA_D5 (GPIO_NUM_9)
+#define CAMERA_D6 (GPIO_NUM_46)
+#define CAMERA_D7 (GPIO_NUM_8)
+#define CAMERA_PWDN (GPIO_NUM_NC)
+#define CAMERA_RESET (GPIO_NUM_NC)
+#define CAMERA_XCLK_FREQ (16000000)
+#define LEDC_TIMER (LEDC_TIMER_0)
+#define LEDC_CHANNEL (LEDC_CHANNEL_0)
+#define LCD_TYPE_ST7789_SERIAL
+#define DISPLAY_WIDTH 240
+#define DISPLAY_HEIGHT 240
+#define DISPLAY_MIRROR_X false
+#define DISPLAY_MIRROR_Y false
+#define DISPLAY_SWAP_XY false
+#define DISPLAY_INVERT_COLOR true
+#define DISPLAY_RGB_ORDER LCD_RGB_ELEMENT_ORDER_RGB
+#define DISPLAY_OFFSET_X 0
+#define DISPLAY_OFFSET_Y 0
+#define DISPLAY_BACKLIGHT_OUTPUT_INVERT false
+#define DISPLAY_SPI_MODE 3
+
+#define BOOT_BUTTON_GPIO GPIO_NUM_0
+
+#endif

c++/otto-robot_config.json

@@ -0,0 +1,14 @@
+{
+    "target": "esp32s3",
+    "builds": [
+        {
+            "name": "otto-robot",
+            "sdkconfig_append": [
+"CONFIG_HTTPD_WS_SUPPORT=y",
+                "CONFIG_CAMERA_OV2640=y",
+                "CONFIG_CAMERA_OV2640_AUTO_DETECT_DVP_INTERFACE_SENSOR=y",
+                "CONFIG_CAMERA_OV2640_DVP_YUV422_240X240_25FPS=y"
+            ]
+        }
+    ]
+}

c++/otto-robot_oscillator.cc

@@ -0,0 +1,161 @@
+//--------------------------------------------------------------
+//-- Oscillator.pde
+//-- Generate sinusoidal oscillations in the servos
+//--------------------------------------------------------------
+//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011
+//-- (c) txp666 for esp32, 202503
+//-- GPL license
+//--------------------------------------------------------------
+#include "oscillator.h"
+
+#include <driver/ledc.h>
+#include <esp_timer.h>
+
+#include <algorithm>
+#include <cmath>
+
+static const char* TAG = "Oscillator";
+
+extern unsigned long IRAM_ATTR millis();
+
+static ledc_channel_t next_free_channel = LEDC_CHANNEL_0;
+
+Oscillator::Oscillator(int trim) {
+    trim_ = trim;
+    diff_limit_ = 0;
+    is_attached_ = false;
+
+    sampling_period_ = 30;
+    period_ = 2000;
+    number_samples_ = period_ / sampling_period_;
+    inc_ = 2 * M_PI / number_samples_;
+
+    amplitude_ = 45;
+    phase_ = 0;
+    phase0_ = 0;
+    offset_ = 0;
+    stop_ = false;
+    rev_ = false;
+
+    pos_ = 90;
+    previous_millis_ = 0;
+}
+
+Oscillator::~Oscillator() {
+    Detach();
+}
+
+uint32_t Oscillator::AngleToCompare(int angle) {
+    return (angle - SERVO_MIN_DEGREE) * (SERVO_MAX_PULSEWIDTH_US - SERVO_MIN_PULSEWIDTH_US) /
+               (SERVO_MAX_DEGREE - SERVO_MIN_DEGREE) +
+           SERVO_MIN_PULSEWIDTH_US;
+}
+
+bool Oscillator::NextSample() {
+    current_millis_ = millis();
+
+    if (current_millis_ - previous_millis_ > sampling_period_) {
+        previous_millis_ = current_millis_;
+        return true;
+    }
+
+    return false;
+}
+
+void Oscillator::Attach(int pin, bool rev) {
+    if (is_attached_) {
+        Detach();
+    }
+
+    pin_ = pin;
+    rev_ = rev;
+
+    ledc_timer_config_t ledc_timer = {.speed_mode = LEDC_LOW_SPEED_MODE,
+                                      .duty_resolution = LEDC_TIMER_13_BIT,
+                                      .timer_num = LEDC_TIMER_1,
+                                      .freq_hz = 50,
+                                      .clk_cfg = LEDC_AUTO_CLK};
+    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
+
+    static int last_channel = 0;
+    last_channel = (last_channel + 1) % 7 + 1;
+    ledc_channel_ = (ledc_channel_t)last_channel;
+
+    ledc_channel_config_t ledc_channel = {.gpio_num = pin_,
+                                          .speed_mode = LEDC_LOW_SPEED_MODE,
+                                          .channel = ledc_channel_,
+                                          .intr_type = LEDC_INTR_DISABLE,
+                                          .timer_sel = LEDC_TIMER_1,
+                                          .duty = 0,
+                                          .hpoint = 0};
+    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
+
+    ledc_speed_mode_ = LEDC_LOW_SPEED_MODE;
+
+    // pos_ = 90;
+    // Write(pos_);
+    previous_servo_command_millis_ = millis();
+
+    is_attached_ = true;
+}
+
+void Oscillator::Detach() {
+    if (!is_attached_)
+        return;
+
+    ESP_ERROR_CHECK(ledc_stop(ledc_speed_mode_, ledc_channel_, 0));
+
+    is_attached_ = false;
+}
+
+void Oscillator::SetT(unsigned int T) {
+    period_ = T;
+
+    number_samples_ = period_ / sampling_period_;
+    inc_ = 2 * M_PI / number_samples_;
+}
+
+void Oscillator::SetPosition(int position) {
+    Write(position);
+}
+
+void Oscillator::Refresh() {
+    if (NextSample()) {
+        if (!stop_) {
+            int pos = std::round(amplitude_ * std::sin(phase_ + phase0_) + offset_);
+            if (rev_)
+                pos = -pos;
+            Write(pos + 90);
+        }
+
+        phase_ = phase_ + inc_;
+    }
+}
+
+void Oscillator::Write(int position) {
+    if (!is_attached_)
+        return;
+
+    long currentMillis = millis();
+    if (diff_limit_ > 0) {
+        int limit = std::max(
+            1, (((int)(currentMillis - previous_servo_command_millis_)) * diff_limit_) / 1000);
+        if (abs(position - pos_) > limit) {
+            pos_ += position < pos_ ? -limit : limit;
+        } else {
+            pos_ = position;
+        }
+    } else {
+        pos_ = position;
+    }
+    previous_servo_command_millis_ = currentMillis;
+
+    int angle = pos_ + trim_;
+
+    angle = std::min(std::max(angle, 0), 180);
+
+    uint32_t duty = (uint32_t)(((angle / 180.0) * 2.0 + 0.5) * 8191 / 20.0);
+
+    ESP_ERROR_CHECK(ledc_set_duty(ledc_speed_mode_, ledc_channel_, duty));
+    ESP_ERROR_CHECK(ledc_update_duty(ledc_speed_mode_, ledc_channel_));
+}

c++/otto-robot_oscillator.h

@@ -0,0 +1,91 @@
+//--------------------------------------------------------------
+//-- Oscillator.pde
+//-- Generate sinusoidal oscillations in the servos
+//--------------------------------------------------------------
+//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011
+//-- (c) txp666 for esp32, 202503
+//-- GPL license
+//--------------------------------------------------------------
+#ifndef __OSCILLATOR_H__
+#define __OSCILLATOR_H__
+
+#include "driver/ledc.h"
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#define M_PI 3.14159265358979323846
+
+#ifndef DEG2RAD
+#define DEG2RAD(g) ((g) * M_PI) / 180
+#endif
+
+#define SERVO_MIN_PULSEWIDTH_US 500           // 最小脉宽（微秒）
+#define SERVO_MAX_PULSEWIDTH_US 2500          // 最大脉宽（微秒）
+#define SERVO_MIN_DEGREE -90                  // 最小角度
+#define SERVO_MAX_DEGREE 90                   // 最大角度
+#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000  // 1MHz, 1us per tick
+#define SERVO_TIMEBASE_PERIOD 20000           // 20000 ticks, 20ms
+
+class Oscillator {
+public:
+    Oscillator(int trim = 0);
+    ~Oscillator();
+    void Attach(int pin, bool rev = false);
+    void Detach();
+
+    void SetA(unsigned int amplitude) { amplitude_ = amplitude; };
+    void SetO(int offset) { offset_ = offset; };
+    void SetPh(double Ph) { phase0_ = Ph; };
+    void SetT(unsigned int period);
+    void SetTrim(int trim) { trim_ = trim; };
+    void SetLimiter(int diff_limit) { diff_limit_ = diff_limit; };
+    void DisableLimiter() { diff_limit_ = 0; };
+    int GetTrim() { return trim_; };
+    void SetPosition(int position);
+    void Stop() { stop_ = true; };
+    void Play() { stop_ = false; };
+    void Reset() { phase_ = 0; };
+    void Refresh();
+    int GetPosition() { return pos_; }
+
+private:
+    bool NextSample();
+    void Write(int position);
+    uint32_t AngleToCompare(int angle);
+
+private:
+    bool is_attached_;
+
+    //-- Oscillators parameters
+    unsigned int amplitude_;  //-- Amplitude (degrees)
+    int offset_;              //-- Offset (degrees)
+    unsigned int period_;     //-- Period (miliseconds)
+    double phase0_;           //-- Phase (radians)
+
+    //-- Internal variables
+    int pos_;                       //-- Current servo pos
+    int pin_;                       //-- Pin where the servo is connected
+    int trim_;                      //-- Calibration offset
+    double phase_;                  //-- Current phase
+    double inc_;                    //-- Increment of phase
+    double number_samples_;         //-- Number of samples
+    unsigned int sampling_period_;  //-- sampling period (ms)
+
+    long previous_millis_;
+    long current_millis_;
+
+    //-- Oscillation mode. If true, the servo is stopped
+    bool stop_;
+
+    //-- Reverse mode
+    bool rev_;
+
+    int diff_limit_;
+    long previous_servo_command_millis_;
+
+    ledc_channel_t ledc_channel_;
+    ledc_mode_t ledc_speed_mode_;
+};
+
+#endif  // __OSCILLATOR_H__

c++/otto-robot_otto_controller.cc

@@ -0,0 +1,846 @@
+/*
+    Otto机器人控制器 - MCP协议版本
+*/
+
+#include <cJSON.h>
+#include <esp_log.h>
+
+#include <cstdlib> 
+#include <cstring>
+
+#include "application.h"
+#include "board.h"
+#include "config.h"
+#include "mcp_server.h"
+#include "otto_movements.h"
+#include "power_manager.h"
+#include "sdkconfig.h"
+#include "settings.h"
+#include <wifi_manager.h>
+#define TAG "OttoController"
+
+class OttoController {
+private:
+    Otto otto_;
+    TaskHandle_t action_task_handle_ = nullptr;
+    QueueHandle_t action_queue_;
+    bool has_hands_ = false;
+    bool is_action_in_progress_ = false;
+
+    struct OttoActionParams {
+        int action_type;
+        int steps;
+        int speed;
+        int direction;
+        int amount;
+        char servo_sequence_json[512];  // 用于存储舵机序列的JSON字符串
+};
+
+    enum ActionType {
+        ACTION_WALK = 1,
+        ACTION_TURN = 2,
+        ACTION_JUMP = 3,
+        ACTION_SWING = 4,
+        ACTION_MOONWALK = 5,
+        ACTION_BEND = 6,
+        ACTION_SHAKE_LEG = 7,
+        ACTION_SIT = 25,  // 坐下
+        ACTION_RADIO_CALISTHENICS = 26,  // 广播体操
+        ACTION_MAGIC_CIRCLE = 27,  // 爱的魔力转圈圈
+ACTION_UPDOWN = 8,
+        ACTION_TIPTOE_SWING = 9,
+        ACTION_JITTER = 10,
+        ACTION_ASCENDING_TURN = 11,
+        ACTION_CRUSAITO = 12,
+        ACTION_FLAPPING = 13,
+        ACTION_HANDS_UP = 14,
+        ACTION_HANDS_DOWN = 15,
+        ACTION_HAND_WAVE = 16,
+        ACTION_WINDMILL = 20,  // 大风车
+        ACTION_TAKEOFF = 21,   // 起飞
+        ACTION_FITNESS = 22,   // 健身
+        ACTION_GREETING = 23,  // 打招呼
+        ACTION_SHY = 24,        // 害羞
+        ACTION_SHOWCASE = 28,   // 展示动作
+        ACTION_HOME = 17,
+        ACTION_SERVO_SEQUENCE = 18,  // 舵机序列（自编程）
+        ACTION_WHIRLWIND_LEG = 19    // 旋风腿
+};
+
+    static void ActionTask(void* arg) {
+        OttoController* controller = static_cast<OttoController*>(arg);
+        OttoActionParams params;
+        controller->otto_.AttachServos();
+
+        while (true) {
+            if (xQueueReceive(controller->action_queue_, &params, pdMS_TO_TICKS(1000)) == pdTRUE) {
+                ESP_LOGI(TAG, "执行动作: %d", params.action_type);
+                PowerManager::PauseBatteryUpdate();  // 动作开始时暂停电量更新
+                controller->is_action_in_progress_ = true;
+                if (params.action_type == ACTION_SERVO_SEQUENCE) {
+                    // 执行舵机序列（自编程）- 仅支持短键名格式
+                    cJSON* json = cJSON_Parse(params.servo_sequence_json);
+                    if (json != nullptr) {
+                        ESP_LOGD(TAG, "JSON解析成功，长度=%d", strlen(params.servo_sequence_json));
+                        // 使用短键名 "a" 表示动作数组
+                        cJSON* actions = cJSON_GetObjectItem(json, "a");
+                        if (cJSON_IsArray(actions)) {
+                            int array_size = cJSON_GetArraySize(actions);
+                            ESP_LOGI(TAG, "执行舵机序列，共%d个动作", array_size);
+                            
+                            // 获取序列执行完成后的延迟（短键名 "d"，顶层参数）
+                            int sequence_delay = 0;
+                            cJSON* delay_item = cJSON_GetObjectItem(json, "d");
+                            if (cJSON_IsNumber(delay_item)) {
+                                sequence_delay = delay_item->valueint;
+                                if (sequence_delay < 0) sequence_delay = 0;
+                            }
+                            
+                            // 初始化当前舵机位置（用于保持未指定的舵机位置）
+                            int current_positions[SERVO_COUNT];
+                            for (int j = 0; j < SERVO_COUNT; j++) {
+                                current_positions[j] = 90;  // 默认中间位置
+                            }
+                            // 手部舵机默认位置
+                            current_positions[LEFT_HAND] = 45;
+                            current_positions[RIGHT_HAND] = 180 - 45;
+                            
+                            for (int i = 0; i < array_size; i++) {
+                                cJSON* action_item = cJSON_GetArrayItem(actions, i);
+                                if (cJSON_IsObject(action_item)) {
+                                    // 检查是否为振荡器模式（短键名 "osc"）
+                                    cJSON* osc_item = cJSON_GetObjectItem(action_item, "osc");
+                                    if (cJSON_IsObject(osc_item)) {
+                                        // 振荡器模式 - 使用Execute2，以绝对角度为中心振荡
+                                        int amplitude[SERVO_COUNT] = {0};
+                                        int center_angle[SERVO_COUNT] = {0};
+                                        double phase_diff[SERVO_COUNT] = {0};
+                                        int period = 300;  // 默认周期300毫秒
+                                        float steps = 8.0;  // 默认步数8.0
+                                        
+                                        const char* servo_names[] = {"ll", "rl", "lf", "rf", "lh", "rh"};
+                                        
+                                        // 读取振幅（短键名 "a"），默认0度
+                                        for (int j = 0; j < SERVO_COUNT; j++) {
+                                            amplitude[j] = 0;  // 默认振幅0度
+                                        }
+                                        cJSON* amp_item = cJSON_GetObjectItem(osc_item, "a");
+                                        if (cJSON_IsObject(amp_item)) {
+                                            for (int j = 0; j < SERVO_COUNT; j++) {
+                                                cJSON* amp_value = cJSON_GetObjectItem(amp_item, servo_names[j]);
+                                                if (cJSON_IsNumber(amp_value)) {
+                                                    int amp = amp_value->valueint;
+                                                    if (amp >= 10 && amp <= 90) {
+                                                        amplitude[j] = amp;
+                                                    }
+                                                }
+                                            }
+                                        }
+                                        
+                                        // 读取中心角度（短键名 "o"），默认90度（绝对角度0-180度）
+                                        for (int j = 0; j < SERVO_COUNT; j++) {
+                                            center_angle[j] = 90;  // 默认中心角度90度（中间位置）
+                                        }
+                                        cJSON* center_item = cJSON_GetObjectItem(osc_item, "o");
+                                        if (cJSON_IsObject(center_item)) {
+                                            for (int j = 0; j < SERVO_COUNT; j++) {
+                                                cJSON* center_value = cJSON_GetObjectItem(center_item, servo_names[j]);
+                                                if (cJSON_IsNumber(center_value)) {
+                                                    int center = center_value->valueint;
+                                                    if (center >= 0 && center <= 180) {
+                                                        center_angle[j] = center;
+                                                    }
+                                                }
+                                            }
+                                        }
+                                        
+                                        // 安全检查：防止左右腿脚同时做大幅度振荡（振幅检查）
+                                        const int LARGE_AMPLITUDE_THRESHOLD = 40;  // 大幅度振幅阈值：40度
+                                        bool left_leg_large = amplitude[LEFT_LEG] >= LARGE_AMPLITUDE_THRESHOLD;
+                                        bool right_leg_large = amplitude[RIGHT_LEG] >= LARGE_AMPLITUDE_THRESHOLD;
+                                        bool left_foot_large = amplitude[LEFT_FOOT] >= LARGE_AMPLITUDE_THRESHOLD;
+                                        bool right_foot_large = amplitude[RIGHT_FOOT] >= LARGE_AMPLITUDE_THRESHOLD;
+                                        
+                                        if (left_leg_large && right_leg_large) {
+                                            ESP_LOGW(TAG, "检测到左右腿同时大幅度振荡，限制右腿振幅");
+                                            amplitude[RIGHT_LEG] = 0;  // 禁止右腿振荡
+                                        }
+                                        if (left_foot_large && right_foot_large) {
+                                            ESP_LOGW(TAG, "检测到左右脚同时大幅度振荡，限制右脚振幅");
+                                            amplitude[RIGHT_FOOT] = 0;  // 禁止右脚振荡
+                                        }
+                                        
+                                        // 读取相位差（短键名 "ph"，单位为度，转换为弧度）
+                                        cJSON* phase_item = cJSON_GetObjectItem(osc_item, "ph");
+                                        if (cJSON_IsObject(phase_item)) {
+                                            for (int j = 0; j < SERVO_COUNT; j++) {
+                                                cJSON* phase_value = cJSON_GetObjectItem(phase_item, servo_names[j]);
+                                                if (cJSON_IsNumber(phase_value)) {
+                                                    // 将度数转换为弧度
+                                                    phase_diff[j] = phase_value->valuedouble * 3.141592653589793 / 180.0;
+                                                }
+                                            }
+                                        }
+                                        
+                                        // 读取周期（短键名 "p"），范围100-3000毫秒
+                                        cJSON* period_item = cJSON_GetObjectItem(osc_item, "p");
+                                        if (cJSON_IsNumber(period_item)) {
+                                            period = period_item->valueint;
+                                            if (period < 100) period = 100;
+                                            if (period > 3000) period = 3000;  // 与描述一致，限制3000毫秒
+                                        }
+                                        
+                                        // 读取周期数（短键名 "c"），范围0.1-20.0
+                                        cJSON* steps_item = cJSON_GetObjectItem(osc_item, "c");
+                                        if (cJSON_IsNumber(steps_item)) {
+                                            steps = (float)steps_item->valuedouble;
+                                            if (steps < 0.1) steps = 0.1;
+                                            if (steps > 20.0) steps = 20.0;  // 与描述一致，限制20.0
+                                        }
+                                        
+                                        // 执行振荡 - 使用Execute2，以绝对角度为中心
+                                        ESP_LOGI(TAG, "执行振荡动作%d: period=%d, steps=%.1f", i, period, steps);
+                                        controller->otto_.Execute2(amplitude, center_angle, period, phase_diff, steps);
+                                        
+                                        // 振荡后更新位置（使用center_angle作为最终位置）
+                                        for (int j = 0; j < SERVO_COUNT; j++) {
+                                            current_positions[j] = center_angle[j];
+                                        }
+                                    } else {
+                                        // 普通移动模式
+                                        // 从当前位置数组复制，保持未指定的舵机位置
+                                        int servo_target[SERVO_COUNT];
+                                        for (int j = 0; j < SERVO_COUNT; j++) {
+                                            servo_target[j] = current_positions[j];
+                                        }
+                                        
+                                        // 从JSON中读取舵机位置（短键名 "s"）
+                                        cJSON* servos_item = cJSON_GetObjectItem(action_item, "s");
+                                        if (cJSON_IsObject(servos_item)) {
+                                            // 短键名：ll/rl/lf/rf/lh/rh
+                                            const char* servo_names[] = {"ll", "rl", "lf", "rf", "lh", "rh"};
+                                            
+                                            for (int j = 0; j < SERVO_COUNT; j++) {
+                                                cJSON* servo_value = cJSON_GetObjectItem(servos_item, servo_names[j]);
+                                                if (cJSON_IsNumber(servo_value)) {
+                                                    int position = servo_value->valueint;
+                                                    // 限制位置范围在0-180度
+                                                    if (position >= 0 && position <= 180) {
+                                                        servo_target[j] = position;
+                                                    }
+                                                }
+                                            }
+                                        }
+                                                                                                                    
+                                        // 获取移动速度（短键名 "v"，默认1000毫秒）
+                                        int speed = 1000;
+                                        cJSON* speed_item = cJSON_GetObjectItem(action_item, "v");
+                                        if (cJSON_IsNumber(speed_item)) {
+                                            speed = speed_item->valueint;
+                                            if (speed < 100) speed = 100;  // 最小100毫秒
+                                            if (speed > 3000) speed = 3000;  // 最大3000毫秒
+                                        }
+                                        
+                                        // 执行舵机移动
+                                        ESP_LOGI(TAG, "执行动作%d: ll=%d, rl=%d, lf=%d, rf=%d, v=%d",
+                                                 i, servo_target[LEFT_LEG], servo_target[RIGHT_LEG],
+                                                 servo_target[LEFT_FOOT], servo_target[RIGHT_FOOT], speed);
+                                        controller->otto_.MoveServos(speed, servo_target);
+                                        
+                                        // 更新当前位置数组，用于下一个动作
+                                        for (int j = 0; j < SERVO_COUNT; j++) {
+                                            current_positions[j] = servo_target[j];
+                                        }
+                                    }
+                                    
+                                    // 获取动作后的延迟时间（短键名 "d"）
+                                    int delay_after = 0;
+                                    cJSON* delay_item = cJSON_GetObjectItem(action_item, "d");
+                                    if (cJSON_IsNumber(delay_item)) {
+                                        delay_after = delay_item->valueint;
+                                        if (delay_after < 0) delay_after = 0;
+                                    }
+                                    
+                                    // 动作后的延迟（最后一个动作后不延迟）
+                                    if (delay_after > 0 && i < array_size - 1) {
+                                        ESP_LOGI(TAG, "动作%d执行完成，延迟%d毫秒", i, delay_after);
+                                        vTaskDelay(pdMS_TO_TICKS(delay_after));
+                                    }
+                                }
+                            }
+                            
+                            // 序列执行完成后的延迟（用于序列之间的停顿）
+                            if (sequence_delay > 0) {
+                                // 检查队列中是否还有待执行的序列
+                                UBaseType_t queue_count = uxQueueMessagesWaiting(controller->action_queue_);
+                                if (queue_count > 0) {
+                                    ESP_LOGI(TAG, "序列执行完成，延迟%d毫秒后执行下一个序列（队列中还有%d个序列）", 
+                                             sequence_delay, queue_count);
+                                    vTaskDelay(pdMS_TO_TICKS(sequence_delay));
+                                }
+                            }
+                            // 释放JSON内存
+                            cJSON_Delete(json);
+                        } else {
+                            ESP_LOGE(TAG, "舵机序列格式错误: 'a'不是数组");
+                            cJSON_Delete(json);
+                        }
+                    } else {
+                        // 获取cJSON的错误信息
+                        const char* error_ptr = cJSON_GetErrorPtr();
+                        int json_len = strlen(params.servo_sequence_json);
+                        ESP_LOGE(TAG, "解析舵机序列JSON失败，长度=%d，错误位置: %s", json_len, 
+                                 error_ptr ? error_ptr : "未知");
+                        ESP_LOGE(TAG, "JSON内容: %s", params.servo_sequence_json);
+                    }
+                } else {
+                    // 执行预定义动作
+                    switch (params.action_type) {
+                        case ACTION_WALK:
+                            controller->otto_.Walk(params.steps, params.speed, params.direction,
+                                                   params.amount);
+                            break;
+                        case ACTION_TURN:
+                            controller->otto_.Turn(params.steps, params.speed, params.direction,
+                                                   params.amount);
+                            break;
+                        case ACTION_JUMP:
+                            controller->otto_.Jump(params.steps, params.speed);
+                            break;
+                        case ACTION_SWING:
+                            controller->otto_.Swing(params.steps, params.speed, params.amount);
+                            break;
+                        case ACTION_MOONWALK:
+                            controller->otto_.Moonwalker(params.steps, params.speed, params.amount,
+                                                         params.direction);
+                            break;
+                        case ACTION_BEND:
+                            controller->otto_.Bend(params.steps, params.speed, params.direction);
+                            break;
+                        case ACTION_SHAKE_LEG:
+                            controller->otto_.ShakeLeg(params.steps, params.speed, params.direction);
+                            break;
+                        case ACTION_SIT:
+                            controller->otto_.Sit();
+                            break;
+                        case ACTION_RADIO_CALISTHENICS:
+                            if (controller->has_hands_) {
+                                controller->otto_.RadioCalisthenics();
+                            }
+                            break;
+                        case ACTION_MAGIC_CIRCLE:
+                            if (controller->has_hands_) {
+                                controller->otto_.MagicCircle();
+                            }
+                            break;
+                        case ACTION_SHOWCASE:
+                            controller->otto_.Showcase();
+                            break;
+                        case ACTION_UPDOWN:
+                            controller->otto_.UpDown(params.steps, params.speed, params.amount);
+                            break;
+                        case ACTION_TIPTOE_SWING:
+                            controller->otto_.TiptoeSwing(params.steps, params.speed, params.amount);
+                            break;
+                        case ACTION_JITTER:
+                            controller->otto_.Jitter(params.steps, params.speed, params.amount);
+                            break;
+                        case ACTION_ASCENDING_TURN:
+                            controller->otto_.AscendingTurn(params.steps, params.speed, params.amount);
+                            break;
+                        case ACTION_CRUSAITO:
+                            controller->otto_.Crusaito(params.steps, params.speed, params.amount,
+                                                       params.direction);
+                            break;
+                        case ACTION_FLAPPING:
+                            controller->otto_.Flapping(params.steps, params.speed, params.amount,
+                                                       params.direction);
+                            break;
+                        case ACTION_WHIRLWIND_LEG:
+                            controller->otto_.WhirlwindLeg(params.steps, params.speed, params.amount);
+                            break;
+                        case ACTION_HANDS_UP:
+                            if (controller->has_hands_) {
+                                controller->otto_.HandsUp(params.speed, params.direction);
+                            }
+                            break;
+                        case ACTION_HANDS_DOWN:
+                            if (controller->has_hands_) {
+                                controller->otto_.HandsDown(params.speed, params.direction);
+                            }
+                            break;
+                        case ACTION_HAND_WAVE:
+                            if (controller->has_hands_) {
+                                controller->otto_.HandWave( params.direction);
+                            }
+                            break;
+                        case ACTION_WINDMILL:
+                            if (controller->has_hands_) {
+                                controller->otto_.Windmill(params.steps, params.speed, params.amount);
+                            }
+                            break;
+                        case ACTION_TAKEOFF:
+                            if (controller->has_hands_) {
+                                controller->otto_.Takeoff(params.steps, params.speed, params.amount);
+                            }
+                            break;
+                        case ACTION_FITNESS:
+                            if (controller->has_hands_) {
+                                controller->otto_.Fitness(params.steps, params.speed, params.amount);
+                            }
+                            break;
+                        case ACTION_GREETING:
+                            if (controller->has_hands_) {
+                                controller->otto_.Greeting(params.direction, params.steps);
+                            }
+                            break;
+                        case ACTION_SHY:
+                            if (controller->has_hands_) {
+                                controller->otto_.Shy(params.direction, params.steps);
+                            }
+                            break;
+                        case ACTION_HOME:
+                            controller->otto_.Home(true);
+                            break;
+                    }
+                    if(params.action_type != ACTION_SIT){
+                        if (params.action_type != ACTION_HOME && params.action_type != ACTION_SERVO_SEQUENCE) {
+                            controller->otto_.Home(params.action_type != ACTION_HANDS_UP);
+                        }
+                    }
+                }
+                controller->is_action_in_progress_ = false;
+                PowerManager::ResumeBatteryUpdate();  // 动作结束时恢复电量更新
+vTaskDelay(pdMS_TO_TICKS(20));
+            }
+        }
+    }
+
+    void StartActionTaskIfNeeded() {
+        if (action_task_handle_ == nullptr) {
+            xTaskCreate(ActionTask, "otto_action", 1024 * 3, this, configMAX_PRIORITIES - 1,
+                        &action_task_handle_);
+        }
+    }
+
+    void QueueAction(int action_type, int steps, int speed, int direction, int amount) {
+        // 检查手部动作
+        if ((action_type >= ACTION_HANDS_UP && action_type <= ACTION_HAND_WAVE) || 
+            (action_type == ACTION_WINDMILL) || (action_type == ACTION_TAKEOFF) || 
+            (action_type == ACTION_FITNESS) || (action_type == ACTION_GREETING) ||
+            (action_type == ACTION_SHY) || (action_type == ACTION_RADIO_CALISTHENICS) ||
+            (action_type == ACTION_MAGIC_CIRCLE)) {
+            if (!has_hands_) {
+                ESP_LOGW(TAG, "尝试执行手部动作，但机器人没有配置手部舵机");
+                return;
+            }
+}
+
+        ESP_LOGI(TAG, "动作控制: 类型=%d, 步数=%d, 速度=%d, 方向=%d, 幅度=%d", action_type, steps,
+                 speed, direction, amount);
+
+        OttoActionParams params = {action_type, steps, speed, direction, amount, ""};
+        xQueueSend(action_queue_, &params, portMAX_DELAY);
+        StartActionTaskIfNeeded();
+    }
+
+    void QueueServoSequence(const char* servo_sequence_json) {
+        if (servo_sequence_json == nullptr) {
+            ESP_LOGE(TAG, "序列JSON为空");
+            return;
+        }
+        
+        int input_len = strlen(servo_sequence_json);
+        const int buffer_size = 512;  // servo_sequence_json数组大小
+        ESP_LOGI(TAG, "队列舵机序列，输入长度=%d，缓冲区大小=%d", input_len, buffer_size);
+        
+        if (input_len >= buffer_size) {
+            ESP_LOGE(TAG, "JSON字符串太长！输入长度=%d，最大允许=%d", input_len, buffer_size - 1);
+            return;
+        }
+        
+        if (input_len == 0) {
+            ESP_LOGW(TAG, "序列JSON为空字符串");
+            return;
+        }
+        
+        OttoActionParams params = {ACTION_SERVO_SEQUENCE, 0, 0, 0, 0, ""};
+        // 复制JSON字符串到结构体中（限制长度）
+        strncpy(params.servo_sequence_json, servo_sequence_json, sizeof(params.servo_sequence_json) - 1);
+        params.servo_sequence_json[sizeof(params.servo_sequence_json) - 1] = '\0';
+        
+        ESP_LOGD(TAG, "序列已加入队列: %s", params.servo_sequence_json);
+        
+xQueueSend(action_queue_, &params, portMAX_DELAY);
+        StartActionTaskIfNeeded();
+    }
+
+    void LoadTrimsFromNVS() {
+        Settings settings("otto_trims", false);
+
+        int left_leg = settings.GetInt("left_leg", 0);
+        int right_leg = settings.GetInt("right_leg", 0);
+        int left_foot = settings.GetInt("left_foot", 0);
+        int right_foot = settings.GetInt("right_foot", 0);
+        int left_hand = settings.GetInt("left_hand", 0);
+        int right_hand = settings.GetInt("right_hand", 0);
+
+        ESP_LOGI(TAG, "从NVS加载微调设置: 左腿=%d, 右腿=%d, 左脚=%d, 右脚=%d, 左手=%d, 右手=%d",
+                 left_leg, right_leg, left_foot, right_foot, left_hand, right_hand);
+
+        otto_.SetTrims(left_leg, right_leg, left_foot, right_foot, left_hand, right_hand);
+    }
+
+public:
+    OttoController(const HardwareConfig& hw_config) {
+        otto_.Init(
+            hw_config.left_leg_pin, 
+            hw_config.right_leg_pin, 
+            hw_config.left_foot_pin, 
+            hw_config.right_foot_pin, 
+            hw_config.left_hand_pin,
+            hw_config.right_hand_pin
+        );
+
+        has_hands_ = (hw_config.left_hand_pin != GPIO_NUM_NC && hw_config.right_hand_pin != GPIO_NUM_NC);
+        ESP_LOGI(TAG, "Otto机器人初始化%s手部舵机", has_hands_ ? "带" : "不带");
+        ESP_LOGI(TAG, "舵机引脚配置: LL=%d, RL=%d, LF=%d, RF=%d, LH=%d, RH=%d",
+                 hw_config.left_leg_pin, hw_config.right_leg_pin,
+                 hw_config.left_foot_pin, hw_config.right_foot_pin,
+                 hw_config.left_hand_pin, hw_config.right_hand_pin);
+LoadTrimsFromNVS();
+
+        action_queue_ = xQueueCreate(10, sizeof(OttoActionParams));
+
+        QueueAction(ACTION_HOME, 1, 1000, 1, 0);  // direction=1表示复位手部
+
+        RegisterMcpTools();
+    }
+
+    void RegisterMcpTools() {
+        auto& mcp_server = McpServer::GetInstance();
+
+        ESP_LOGI(TAG, "开始注册MCP工具...");
+
+        // 统一动作工具（除了舵机序列外的所有动作）
+        mcp_server.AddTool("self.otto.action",
+                           "执行机器人动作。action: 动作名称；根据动作类型提供相应参数：direction: 方向，1=前进/左转，-1=后退/右转；0=左右同时"
+                           "steps: 动作步数，1-100；speed: 动作速度，100-3000，数值越小越快；amount: 动作幅度，0-170；arm_swing: 手臂摆动幅度，0-170；"
+                           "基础动作：walk(行走，需steps/speed/direction/arm_swing)、turn(转身，需steps/speed/direction/arm_swing)、jump(跳跃，需steps/speed)、"
+                           "swing(摇摆，需steps/speed/amount)、moonwalk(太空步，需steps/speed/direction/amount)、bend(弯曲，需steps/speed/direction)、"
+                           "shake_leg(摇腿，需steps/speed/direction)、updown(上下运动，需steps/speed/amount)、whirlwind_leg(旋风腿，需steps/speed/amount)；"
+                           "固定动作：sit(坐下)、showcase(展示动作)、home(复位)；"
+                           "手部动作(需手部舵机)：hands_up(举手，需speed/direction)、hands_down(放手，需speed/direction)、hand_wave(挥手，需direction)、"
+                           "windmill(大风车，需steps/speed/amount)、takeoff(起飞，需steps/speed/amount)、fitness(健身，需steps/speed/amount)、"
+                           "greeting(打招呼，需direction/steps)、shy(害羞，需direction/steps)、radio_calisthenics(广播体操)、magic_circle(爱的魔力转圈圈)",
+                           PropertyList({
+                               Property("action", kPropertyTypeString, "sit"),
+                               Property("steps", kPropertyTypeInteger, 3, 1, 100),
+                               Property("speed", kPropertyTypeInteger, 700, 100, 3000),
+                               Property("direction", kPropertyTypeInteger, 1, -1, 1),
+                               Property("amount", kPropertyTypeInteger, 30, 0, 170),
+                               Property("arm_swing", kPropertyTypeInteger, 50, 0, 170)
+                           }),
+                           [this](const PropertyList& properties) -> ReturnValue {
+                               std::string action = properties["action"].value<std::string>();
+                               // 所有参数都有默认值，直接访问即可
+                               int steps = properties["steps"].value<int>();
+                               int speed = properties["speed"].value<int>();
+                               int direction = properties["direction"].value<int>();
+                               int amount = properties["amount"].value<int>();
+                               int arm_swing = properties["arm_swing"].value<int>();
+
+                               // 基础移动动作
+                               if (action == "walk") {
+                                   QueueAction(ACTION_WALK, steps, speed, direction, arm_swing);
+                                   return true;
+                               } else if (action == "turn") {
+                                   QueueAction(ACTION_TURN, steps, speed, direction, arm_swing);
+                                   return true;
+                               } else if (action == "jump") {
+                                   QueueAction(ACTION_JUMP, steps, speed, 0, 0);
+                                   return true;
+                               } else if (action == "swing") {
+                                   QueueAction(ACTION_SWING, steps, speed, 0, amount);
+                                   return true;
+                               } else if (action == "moonwalk") {
+                                   QueueAction(ACTION_MOONWALK, steps, speed, direction, amount);
+                                   return true;
+                               } else if (action == "bend") {
+                                   QueueAction(ACTION_BEND, steps, speed, direction, 0);
+                                   return true;
+                               } else if (action == "shake_leg") {
+                                   QueueAction(ACTION_SHAKE_LEG, steps, speed, direction, 0);
+                                   return true;
+                               } else if (action == "updown") {
+                                   QueueAction(ACTION_UPDOWN, steps, speed, 0, amount);
+                                   return true;
+                               } else if (action == "whirlwind_leg") {
+                                   QueueAction(ACTION_WHIRLWIND_LEG, steps, speed, 0, amount);
+                                   return true;
+                               }
+                               // 固定动作
+                               else if (action == "sit") {
+                                   QueueAction(ACTION_SIT, 1, 0, 0, 0);
+                                   return true;
+                               } else if (action == "showcase") {
+                                   QueueAction(ACTION_SHOWCASE, 1, 0, 0, 0);
+                                   return true;
+                               } else if (action == "home") {
+                                   QueueAction(ACTION_HOME, 1, 1000, 1, 0);
+                                   return true;
+                               }
+                               // 手部动作
+                               else if (action == "hands_up") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_HANDS_UP, 1, speed, direction, 0);
+                                   return true;
+                               } else if (action == "hands_down") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_HANDS_DOWN, 1, speed, direction, 0);
+                                   return true;
+                               } else if (action == "hand_wave") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_HAND_WAVE, 1, 0, 0, direction);
+                                   return true;
+                               } else if (action == "windmill") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_WINDMILL, steps, speed, 0, amount);
+                                   return true;
+                               } else if (action == "takeoff") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_TAKEOFF, steps, speed, 0, amount);
+                                   return true;
+                               } else if (action == "fitness") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_FITNESS, steps, speed, 0, amount);
+                                   return true;
+                               } else if (action == "greeting") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_GREETING, steps, 0, direction, 0);
+                                   return true;
+                               } else if (action == "shy") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_SHY, steps, 0, direction, 0);
+                                   return true;
+                               } else if (action == "radio_calisthenics") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_RADIO_CALISTHENICS, 1, 0, 0, 0);
+                                   return true;
+                               } else if (action == "magic_circle") {
+                                   if (!has_hands_) {
+                                       return "错误：此动作需要手部舵机支持";
+                                   }
+                                   QueueAction(ACTION_MAGIC_CIRCLE, 1, 0, 0, 0);
+                                   return true;
+                               } else {
+                                   return "错误：无效的动作名称。可用动作：walk, turn, jump, swing, moonwalk, bend, shake_leg, updown, whirlwind_leg, sit, showcase, home, hands_up, hands_down, hand_wave, windmill, takeoff, fitness, greeting, shy, radio_calisthenics, magic_circle";
+                               }
+                           });
+
+
+        // 舵机序列工具（支持分段发送，每次发送一个序列，自动排队执行）
+        mcp_server.AddTool(
+            "self.otto.servo_sequences",
+            "AI自定义动作编程（即兴动作）。支持分段发送序列：超过5个序列建议AI可以连续多次调用此工具，每次发送一个短序列，系统会自动排队按顺序执行。支持普通移动和振荡器两种模式。"
+            "机器人结构：双手可上下摆动，双腿可内收外展，双脚可上下翻转。"
+            "舵机说明："
+            "ll(左腿)：内收外展，0度=完全外展，90度=中立，180度=完全内收；"
+            "rl(右腿)：内收外展，0度=完全内收，90度=中立，180度=完全外展；"
+            "lf(左脚)：上下翻转，0度=完全向上，90度=水平，180度=完全向下；"
+            "rf(右脚)：上下翻转，0度=完全向下，90度=水平，180度=完全向上；"
+            "lh(左手)：上下摆动，0度=完全向下，90度=水平，180度=完全向上；"
+            "rh(右手)：上下摆动，0度=完全向上，90度=水平，180度=完全向下；"
+            "sequence: 单个序列对象，包含'a'动作数组，顶层可选参数："
+            "'d'(序列执行完成后延迟毫秒数，用于序列之间的停顿)。"
+            "每个动作对象包含："
+            "普通模式：'s'舵机位置对象(键名：ll/rl/lf/rf/lh/rh，值：0-180度)，'v'移动速度100-3000毫秒(默认1000)，'d'动作后延迟毫秒数(默认0)；"
+            "振荡模式：'osc'振荡器对象，包含'a'振幅对象(各舵机振幅10-90度，默认20度)，'o'中心角度对象(各舵机振荡中心绝对角度0-180度，默认90度)，'ph'相位差对象(各舵机相位差，度，0-360度，默认0度)，'p'周期100-3000毫秒(默认500)，'c'周期数0.1-20.0(默认5.0)；"
+            "使用方式：AI可以连续多次调用此工具，每次发送一个序列，系统会自动排队按顺序执行。"
+            "重要说明：左右腿脚震荡的时候，有一只脚必须在90度，否则会损坏机器人，如果发送多个序列（序列数>1），完成所有序列后需要复位时，AI应该最后单独调用self.otto.home工具进行复位，不要在序列中设置复位参数。"
+            "普通模式示例：发送3个序列，最后调用复位："
+            "第1次调用{\"sequence\":\"{\\\"a\\\":[{\\\"s\\\":{\\\"ll\\\":100},\\\"v\\\":1000}],\\\"d\\\":500}\"}，"
+            "第2次调用{\"sequence\":\"{\\\"a\\\":[{\\\"s\\\":{\\\"ll\\\":90},\\\"v\\\":800}],\\\"d\\\":500}\"}，"
+            "第3次调用{\"sequence\":\"{\\\"a\\\":[{\\\"s\\\":{\\\"ll\\\":80},\\\"v\\\":800}]}\"}，"
+            "最后调用self.otto.home工具进行复位。"
+            "振荡器模式示例："
+            "示例1-双臂同步摆动：{\"sequence\":\"{\\\"a\\\":[{\\\"osc\\\":{\\\"a\\\":{\\\"lh\\\":30,\\\"rh\\\":30},\\\"o\\\":{\\\"lh\\\":90,\\\"rh\\\":-90},\\\"p\\\":500,\\\"c\\\":5.0}}],\\\"d\\\":0}\"}；"
+            "示例2-双腿交替振荡（波浪效果）：{\"sequence\":\"{\\\"a\\\":[{\\\"osc\\\":{\\\"a\\\":{\\\"ll\\\":20,\\\"rl\\\":20},\\\"o\\\":{\\\"ll\\\":90,\\\"rl\\\":-90},\\\"ph\\\":{\\\"rl\\\":180},\\\"p\\\":600,\\\"c\\\":3.0}}],\\\"d\\\":0}\"}；"
+            "示例3-单腿振荡配合固定脚（安全）：{\"sequence\":\"{\\\"a\\\":[{\\\"osc\\\":{\\\"a\\\":{\\\"ll\\\":45},\\\"o\\\":{\\\"ll\\\":90,\\\"lf\\\":90},\\\"p\\\":400,\\\"c\\\":4.0}}],\\\"d\\\":0}\"}；"
+            "示例4-复杂多舵机振荡（手和腿）：{\"sequence\":\"{\\\"a\\\":[{\\\"osc\\\":{\\\"a\\\":{\\\"lh\\\":25,\\\"rh\\\":25,\\\"ll\\\":15},\\\"o\\\":{\\\"lh\\\":90,\\\"rh\\\":90,\\\"ll\\\":90,\\\"lf\\\":90},\\\"ph\\\":{\\\"rh\\\":180},\\\"p\\\":800,\\\"c\\\":6.0}}],\\\"d\\\":500}\"}；"
+            "示例5-快速摇摆：{\"sequence\":\"{\\\"a\\\":[{\\\"osc\\\":{\\\"a\\\":{\\\"ll\\\":30,\\\"rl\\\":30},\\\"o\\\":{\\\"ll\\\":90,\\\"rl\\\":90},\\\"ph\\\":{\\\"rl\\\":180},\\\"p\\\":300,\\\"c\\\":10.0}}],\\\"d\\\":0}\"}。",
+            PropertyList({Property("sequence", kPropertyTypeString,
+                                   "{\"a\":[{\"s\":{\"ll\":90,\"rl\":90},\"v\":1000}]}")}),
+            [this](const PropertyList& properties) -> ReturnValue {
+                std::string sequence = properties["sequence"].value<std::string>();
+                // 检查是否是JSON对象（可能是字符串格式或已解析的对象）
+                // 如果sequence是JSON字符串，直接使用；如果是对象字符串，也需要使用
+                QueueServoSequence(sequence.c_str());
+                return true;
+            });
+
+
+        mcp_server.AddTool("self.otto.stop", "立即停止所有动作并复位", PropertyList(),
+[this](const PropertyList& properties) -> ReturnValue {
+                               if (action_task_handle_ != nullptr) {
+                                   vTaskDelete(action_task_handle_);
+                                   action_task_handle_ = nullptr;
+                               }
+                               is_action_in_progress_ = false;
+                               PowerManager::ResumeBatteryUpdate();  // 停止动作时恢复电量更新
+xQueueReset(action_queue_);
+
+                               QueueAction(ACTION_HOME, 1, 1000, 1, 0);
+                               return true;
+                           });
+
+        mcp_server.AddTool(
+            "self.otto.set_trim",
+            "校准单个舵机位置。设置指定舵机的微调参数以调整机器人的初始站立姿态，设置将永久保存。"
+"servo_type: 舵机类型(left_leg/right_leg/left_foot/right_foot/left_hand/right_hand); "
+            "trim_value: 微调值(-50到50度)",
+            PropertyList({Property("servo_type", kPropertyTypeString, "left_leg"),
+                          Property("trim_value", kPropertyTypeInteger, 0, -50, 50)}),
+            [this](const PropertyList& properties) -> ReturnValue {
+                std::string servo_type = properties["servo_type"].value<std::string>();
+                int trim_value = properties["trim_value"].value<int>();
+
+                ESP_LOGI(TAG, "设置舵机微调: %s = %d度", servo_type.c_str(), trim_value);
+
+                // 获取当前所有微调值
+                Settings settings("otto_trims", true);
+                int left_leg = settings.GetInt("left_leg", 0);
+                int right_leg = settings.GetInt("right_leg", 0);
+                int left_foot = settings.GetInt("left_foot", 0);
+                int right_foot = settings.GetInt("right_foot", 0);
+                int left_hand = settings.GetInt("left_hand", 0);
+                int right_hand = settings.GetInt("right_hand", 0);
+
+                // 更新指定舵机的微调值
+                if (servo_type == "left_leg") {
+                    left_leg = trim_value;
+                    settings.SetInt("left_leg", left_leg);
+                } else if (servo_type == "right_leg") {
+                    right_leg = trim_value;
+                    settings.SetInt("right_leg", right_leg);
+                } else if (servo_type == "left_foot") {
+                    left_foot = trim_value;
+                    settings.SetInt("left_foot", left_foot);
+                } else if (servo_type == "right_foot") {
+                    right_foot = trim_value;
+                    settings.SetInt("right_foot", right_foot);
+                } else if (servo_type == "left_hand") {
+                    if (!has_hands_) {
+                        return "错误：机器人没有配置手部舵机";
+                    }
+                    left_hand = trim_value;
+                    settings.SetInt("left_hand", left_hand);
+                } else if (servo_type == "right_hand") {
+                    if (!has_hands_) {
+                        return "错误：机器人没有配置手部舵机";
+                    }
+                    right_hand = trim_value;
+                    settings.SetInt("right_hand", right_hand);
+                } else {
+                    return "错误：无效的舵机类型，请使用: left_leg, right_leg, left_foot, "
+                           "right_foot, left_hand, right_hand";
+                }
+
+                otto_.SetTrims(left_leg, right_leg, left_foot, right_foot, left_hand, right_hand);
+
+                QueueAction(ACTION_JUMP, 1, 500, 0, 0);
+
+                return "舵机 " + servo_type + " 微调设置为 " + std::to_string(trim_value) +
+                       " 度，已永久保存";
+            });
+
+        mcp_server.AddTool("self.otto.get_trims", "获取当前的舵机微调设置", PropertyList(),
+                           [this](const PropertyList& properties) -> ReturnValue {
+                               Settings settings("otto_trims", false);
+
+                               int left_leg = settings.GetInt("left_leg", 0);
+                               int right_leg = settings.GetInt("right_leg", 0);
+                               int left_foot = settings.GetInt("left_foot", 0);
+                               int right_foot = settings.GetInt("right_foot", 0);
+                               int left_hand = settings.GetInt("left_hand", 0);
+                               int right_hand = settings.GetInt("right_hand", 0);
+
+                               std::string result =
+                                   "{\"left_leg\":" + std::to_string(left_leg) +
+                                   ",\"right_leg\":" + std::to_string(right_leg) +
+                                   ",\"left_foot\":" + std::to_string(left_foot) +
+                                   ",\"right_foot\":" + std::to_string(right_foot) +
+                                   ",\"left_hand\":" + std::to_string(left_hand) +
+                                   ",\"right_hand\":" + std::to_string(right_hand) + "}";
+
+                               ESP_LOGI(TAG, "获取微调设置: %s", result.c_str());
+                               return result;
+                           });
+
+        mcp_server.AddTool("self.otto.get_status", "获取机器人状态，返回 moving 或 idle",
+                           PropertyList(), [this](const PropertyList& properties) -> ReturnValue {
+                               return is_action_in_progress_ ? "moving" : "idle";
+                           });
+
+        mcp_server.AddTool("self.battery.get_level", "获取机器人电池电量和充电状态", PropertyList(),
+                           [](const PropertyList& properties) -> ReturnValue {
+                               auto& board = Board::GetInstance();
+                               int level = 0;
+                               bool charging = false;
+                               bool discharging = false;
+                               board.GetBatteryLevel(level, charging, discharging);
+
+                               std::string status =
+                                   "{\"level\":" + std::to_string(level) +
+                                   ",\"charging\":" + (charging ? "true" : "false") + "}";
+                               return status;
+                           });
+                           
+        mcp_server.AddTool("self.otto.get_ip", "获取机器人WiFi IP地址", PropertyList(),
+                           [](const PropertyList& properties) -> ReturnValue {
+                               auto& wifi = WifiManager::GetInstance();
+                               std::string ip = wifi.GetIpAddress();
+                               if (ip.empty()) {
+                                   return "{\"ip\":\"\",\"connected\":false}";
+                               }
+                               std::string status = "{\"ip\":\"" + ip + "\",\"connected\":true}";
+                               return status;
+                           });                           
+ESP_LOGI(TAG, "MCP工具注册完成");
+    }
+
+    ~OttoController() {
+        if (action_task_handle_ != nullptr) {
+            vTaskDelete(action_task_handle_);
+            action_task_handle_ = nullptr;
+        }
+        vQueueDelete(action_queue_);
+    }
+};
+
+static OttoController* g_otto_controller = nullptr;
+
+void InitializeOttoController(const HardwareConfig& hw_config) {
+    if (g_otto_controller == nullptr) {
+        g_otto_controller = new OttoController(hw_config);
+ESP_LOGI(TAG, "Otto控制器已初始化并注册MCP工具");
+    }
+}

c++/otto-robot_otto_emoji_display.cc

@@ -0,0 +1,165 @@
+#include "otto_emoji_display.h"
+
+#include <esp_log.h>
+
+#include <cstring>
+
+#include "assets/lang_config.h"
+#include "display/lvgl_display/emoji_collection.h"
+#include "display/lvgl_display/lvgl_image.h"
+#include "display/lvgl_display/lvgl_theme.h"
+#include "otto_emoji_gif.h"
+
+#define TAG "OttoEmojiDisplay"
+OttoEmojiDisplay::OttoEmojiDisplay(esp_lcd_panel_io_handle_t panel_io, esp_lcd_panel_handle_t panel, int width, int height, int offset_x, int offset_y, bool mirror_x, bool mirror_y, bool swap_xy)
+    : SpiLcdDisplay(panel_io, panel, width, height, offset_x, offset_y, mirror_x, mirror_y, swap_xy) {
+    InitializeOttoEmojis();
+    SetupPreviewImage();
+    SetTheme(LvglThemeManager::GetInstance().GetTheme("dark"));
+}
+
+void OttoEmojiDisplay::SetupPreviewImage() {
+    DisplayLockGuard lock(this);
+    lv_obj_set_size(preview_image_, width_ , height_ );
+}
+
+void OttoEmojiDisplay::InitializeOttoEmojis() {
+    ESP_LOGI(TAG, "初始化Otto GIF表情");
+
+    auto otto_emoji_collection = std::make_shared<EmojiCollection>();
+
+    // 中性/平静类表情 -> staticstate
+    otto_emoji_collection->AddEmoji("staticstate", std::make_shared<LvglRawImage>((void*)staticstate.data, staticstate.data_size));
+    otto_emoji_collection->AddEmoji("neutral", std::make_shared<LvglRawImage>((void*)staticstate.data, staticstate.data_size));
+    otto_emoji_collection->AddEmoji("relaxed", std::make_shared<LvglRawImage>((void*)staticstate.data, staticstate.data_size));
+    otto_emoji_collection->AddEmoji("sleepy", std::make_shared<LvglRawImage>((void*)staticstate.data, staticstate.data_size));
+    otto_emoji_collection->AddEmoji("idle", std::make_shared<LvglRawImage>((void*)staticstate.data, staticstate.data_size));
+
+    // 积极/开心类表情 -> happy
+    otto_emoji_collection->AddEmoji("happy", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("laughing", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("funny", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("loving", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("confident", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("winking", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("cool", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("delicious", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("kissy", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+    otto_emoji_collection->AddEmoji("silly", std::make_shared<LvglRawImage>((void*)happy.data, happy.data_size));
+
+    // 悲伤类表情 -> sad
+    otto_emoji_collection->AddEmoji("sad", std::make_shared<LvglRawImage>((void*)sad.data, sad.data_size));
+    otto_emoji_collection->AddEmoji("crying", std::make_shared<LvglRawImage>((void*)sad.data, sad.data_size));
+
+    // 愤怒类表情 -> anger
+    otto_emoji_collection->AddEmoji("anger", std::make_shared<LvglRawImage>((void*)anger.data, anger.data_size));
+    otto_emoji_collection->AddEmoji("angry", std::make_shared<LvglRawImage>((void*)anger.data, anger.data_size));
+
+    // 惊讶类表情 -> scare
+    otto_emoji_collection->AddEmoji("scare", std::make_shared<LvglRawImage>((void*)scare.data, scare.data_size));
+    otto_emoji_collection->AddEmoji("surprised", std::make_shared<LvglRawImage>((void*)scare.data, scare.data_size));
+    otto_emoji_collection->AddEmoji("shocked", std::make_shared<LvglRawImage>((void*)scare.data, scare.data_size));
+
+    // 思考/困惑类表情 -> buxue
+    otto_emoji_collection->AddEmoji("buxue", std::make_shared<LvglRawImage>((void*)buxue.data, buxue.data_size));
+    otto_emoji_collection->AddEmoji("thinking", std::make_shared<LvglRawImage>((void*)buxue.data, buxue.data_size));
+    otto_emoji_collection->AddEmoji("confused", std::make_shared<LvglRawImage>((void*)buxue.data, buxue.data_size));
+    otto_emoji_collection->AddEmoji("embarrassed", std::make_shared<LvglRawImage>((void*)buxue.data, buxue.data_size));
+
+    // 将表情集合添加到主题中
+    auto& theme_manager = LvglThemeManager::GetInstance();
+    auto light_theme = theme_manager.GetTheme("light");
+    auto dark_theme = theme_manager.GetTheme("dark");
+
+    if (light_theme != nullptr) {
+        light_theme->set_emoji_collection(otto_emoji_collection);
+    }
+    if (dark_theme != nullptr) {
+        dark_theme->set_emoji_collection(otto_emoji_collection);
+    }
+
+    // 设置默认表情为staticstate
+    SetEmotion("staticstate");
+
+    ESP_LOGI(TAG, "Otto GIF表情初始化完成");
+}
+
+LV_FONT_DECLARE(OTTO_ICON_FONT);
+void OttoEmojiDisplay::SetStatus(const char* status) {
+    auto lvgl_theme = static_cast<LvglTheme*>(current_theme_);
+    auto text_font = lvgl_theme->text_font()->font();
+    DisplayLockGuard lock(this);
+    if (!status) {
+        ESP_LOGE(TAG, "SetStatus: status is nullptr");
+        return;
+    }
+
+    if (strcmp(status, Lang::Strings::LISTENING) == 0) {
+        lv_obj_set_style_text_font(status_label_, &OTTO_ICON_FONT, 0);
+        lv_label_set_text(status_label_, "\xEF\x84\xB0");  // U+F130 麦克风图标
+        lv_obj_clear_flag(status_label_, LV_OBJ_FLAG_HIDDEN);
+        lv_obj_add_flag(network_label_, LV_OBJ_FLAG_HIDDEN);
+        lv_obj_add_flag(battery_label_, LV_OBJ_FLAG_HIDDEN);
+        return;
+    } else if (strcmp(status, Lang::Strings::SPEAKING) == 0) {
+        lv_obj_set_style_text_font(status_label_, &OTTO_ICON_FONT, 0);
+        lv_label_set_text(status_label_, "\xEF\x80\xA8");  // U+F028 说话图标
+        lv_obj_clear_flag(status_label_, LV_OBJ_FLAG_HIDDEN);
+        lv_obj_add_flag(network_label_, LV_OBJ_FLAG_HIDDEN);
+        lv_obj_add_flag(battery_label_, LV_OBJ_FLAG_HIDDEN);
+        return;
+    } else if (strcmp(status, Lang::Strings::CONNECTING) == 0) {
+        lv_obj_set_style_text_font(status_label_, &OTTO_ICON_FONT, 0);
+        lv_label_set_text(status_label_, "\xEF\x83\x81");  // U+F0c1 连接图标
+        lv_obj_clear_flag(status_label_, LV_OBJ_FLAG_HIDDEN);
+        return;
+    } else if (strcmp(status, Lang::Strings::STANDBY) == 0) {
+        lv_obj_set_style_text_font(status_label_, text_font, 0);
+        lv_label_set_text(status_label_, "");
+        lv_obj_clear_flag(status_label_, LV_OBJ_FLAG_HIDDEN);
+        lv_obj_clear_flag(network_label_, LV_OBJ_FLAG_HIDDEN);
+        lv_obj_clear_flag(battery_label_, LV_OBJ_FLAG_HIDDEN);
+        return;
+    }
+
+    lv_obj_set_style_text_font(status_label_, text_font, 0);
+    lv_label_set_text(status_label_, status);
+}
+
+void OttoEmojiDisplay::SetPreviewImage(std::unique_ptr<LvglImage> image) {
+    DisplayLockGuard lock(this);
+    if (preview_image_ == nullptr) {
+        ESP_LOGE(TAG, "Preview image is not initialized");
+        return;
+    }
+
+    if (image == nullptr) {
+        esp_timer_stop(preview_timer_);
+        lv_obj_remove_flag(emoji_box_, LV_OBJ_FLAG_HIDDEN);
+        lv_obj_add_flag(preview_image_, LV_OBJ_FLAG_HIDDEN);
+        preview_image_cached_.reset();
+        if (gif_controller_) {
+            gif_controller_->Start();
+        }
+        return;
+    }
+
+    preview_image_cached_ = std::move(image);
+    auto img_dsc = preview_image_cached_->image_dsc();
+    // 设置图片源并显示预览图片
+    lv_image_set_src(preview_image_, img_dsc);
+    lv_image_set_rotation(preview_image_, -900);
+    if (img_dsc->header.w > 0 && img_dsc->header.h > 0) {
+        // zoom factor 1.0
+        lv_image_set_scale(preview_image_, 256 * width_ / img_dsc->header.w);
+    }
+
+    // Hide emoji_box_
+    if (gif_controller_) {
+        gif_controller_->Stop();
+    }
+    lv_obj_add_flag(emoji_box_, LV_OBJ_FLAG_HIDDEN);
+    lv_obj_remove_flag(preview_image_, LV_OBJ_FLAG_HIDDEN);
+    esp_timer_stop(preview_timer_);
+    ESP_ERROR_CHECK(esp_timer_start_once(preview_timer_, PREVIEW_IMAGE_DURATION_MS * 1000));
+}

c++/otto-robot_otto_emoji_display.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include "display/lcd_display.h"
+
+/**
+ * @brief Otto机器人GIF表情显示类
+ * 继承SpiLcdDisplay，通过EmojiCollection添加GIF表情支持
+ */
+class OttoEmojiDisplay : public SpiLcdDisplay {
+   public:
+    /**
+     * @brief 构造函数，参数与SpiLcdDisplay相同
+     */
+    OttoEmojiDisplay(esp_lcd_panel_io_handle_t panel_io, esp_lcd_panel_handle_t panel, int width, int height, int offset_x, int offset_y, bool mirror_x, bool mirror_y, bool swap_xy);
+
+    virtual ~OttoEmojiDisplay() = default;
+    virtual void SetStatus(const char* status) override;
+    virtual void SetPreviewImage(std::unique_ptr<LvglImage> image) override;
+
+   private:
+    void InitializeOttoEmojis();
+    void SetupPreviewImage();
+};

c++/otto-robot_otto_icon_font.c

@@ -0,0 +1,121 @@
+/*******************************************************************************
+ * Size: 20 px
+ * Bpp: 1
+ * Opts: --bpp 1 --size 20 --no-compress --stride 1 --align 1 --font fontawesome-webfont.ttf --range 61744,61633,61480 --format lvgl -o OTTO_ICON_FONT.c
+ ******************************************************************************/
+
+#ifdef __has_include
+#if __has_include("lvgl.h")
+#ifndef LV_LVGL_H_INCLUDE_SIMPLE
+#define LV_LVGL_H_INCLUDE_SIMPLE
+#endif
+#endif
+#endif
+
+#ifdef LV_LVGL_H_INCLUDE_SIMPLE
+#include "lvgl.h"
+#else
+#include "lvgl/lvgl.h"
+#endif
+
+#ifndef ENABLE_OTTO_ICON_FONT
+#define ENABLE_OTTO_ICON_FONT 1
+#endif
+
+#if ENABLE_OTTO_ICON_FONT
+
+/*-----------------
+ *    BITMAPS
+ *----------------*/
+
+/*Store the image of the glyphs*/
+static LV_ATTRIBUTE_LARGE_CONST const uint8_t glyph_bitmap[] = {
+    /* U+F028 "" */
+    0x0, 0x6, 0x0, 0x10, 0xe0, 0x6, 0x6, 0x3, 0xc6, 0x60, 0xf8, 0x65, 0xff, 0x24, 0xff, 0xe6, 0x4f, 0xfc, 0x49, 0xff, 0x89, 0x3f, 0xf3, 0x27, 0xfe, 0x49, 0x87, 0xc3, 0x20, 0x78, 0xcc, 0x3, 0x3, 0x0,
+    0x21, 0xc0, 0x0, 0x30,
+
+    /* U+F0C1 "" */
+    0x1e, 0x0, 0xf, 0xc0, 0x7, 0x38, 0x3, 0x87, 0x0, 0xc0, 0xc0, 0x30, 0xb0, 0x7, 0x3c, 0x0, 0xef, 0x0, 0x1f, 0xfe, 0x3, 0xff, 0xc0, 0x7, 0x38, 0x3, 0xe7, 0x0, 0xd0, 0xc0, 0x30, 0x30, 0x6, 0x1c, 0x0,
+    0xce, 0x0, 0x1f, 0x0, 0x3, 0x80,
+
+    /* U+F130 "" */
+    0x7, 0x0, 0xfe, 0x7, 0xf0, 0x3f, 0x81, 0xfc, 0xf, 0xe0, 0x7f, 0x13, 0xf9, 0x9f, 0xcc, 0xfe, 0x67, 0xf3, 0xbf, 0xb4, 0x73, 0x18, 0x30, 0x7f, 0x0, 0x60, 0x2, 0x1, 0xff, 0x0};
+
+/*---------------------
+ *  GLYPH DESCRIPTION
+ *--------------------*/
+
+static const lv_font_fmt_txt_glyph_dsc_t glyph_dsc[] = {{.bitmap_index = 0, .adv_w = 0, .box_w = 0, .box_h = 0, .ofs_x = 0, .ofs_y = 0} /* id = 0 reserved */,
+                                                        {.bitmap_index = 0, .adv_w = 297, .box_w = 19, .box_h = 16, .ofs_x = 0, .ofs_y = -1},
+                                                        {.bitmap_index = 38, .adv_w = 297, .box_w = 18, .box_h = 18, .ofs_x = 0, .ofs_y = -1},
+                                                        {.bitmap_index = 79, .adv_w = 206, .box_w = 13, .box_h = 18, .ofs_x = 0, .ofs_y = -1}};
+
+/*---------------------
+ *  CHARACTER MAPPING
+ *--------------------*/
+
+static const uint16_t unicode_list_0[] = {0x0, 0x99, 0x108};
+
+/*Collect the unicode lists and glyph_id offsets*/
+static const lv_font_fmt_txt_cmap_t cmaps[] = {
+    {.range_start = 61480, .range_length = 265, .glyph_id_start = 1, .unicode_list = unicode_list_0, .glyph_id_ofs_list = NULL, .list_length = 3, .type = LV_FONT_FMT_TXT_CMAP_SPARSE_TINY}};
+
+/*--------------------
+ *  ALL CUSTOM DATA
+ *--------------------*/
+
+#if LVGL_VERSION_MAJOR == 8
+/*Store all the custom data of the font*/
+static lv_font_fmt_txt_glyph_cache_t cache;
+#endif
+
+#if LVGL_VERSION_MAJOR >= 8
+static const lv_font_fmt_txt_dsc_t otto_icon_font_dsc = {
+#else
+static lv_font_fmt_txt_dsc_t otto_icon_font_dsc = {
+#endif
+    .glyph_bitmap = glyph_bitmap,
+    .glyph_dsc = glyph_dsc,
+    .cmaps = cmaps,
+    .kern_dsc = NULL,
+    .kern_scale = 0,
+    .cmap_num = 1,
+    .bpp = 1,
+    .kern_classes = 0,
+    .bitmap_format = 0,
+#if LVGL_VERSION_MAJOR == 8
+    .cache = &cache
+#endif
+
+};
+
+/*-----------------
+ *  PUBLIC FONT
+ *----------------*/
+
+/*Initialize a public general font descriptor*/
+#if LVGL_VERSION_MAJOR >= 8
+const lv_font_t OTTO_ICON_FONT = {
+#else
+lv_font_t OTTO_ICON_FONT = {
+#endif
+    .get_glyph_dsc = lv_font_get_glyph_dsc_fmt_txt, /*Function pointer to get glyph's data*/
+    .get_glyph_bitmap = lv_font_get_bitmap_fmt_txt, /*Function pointer to get glyph's bitmap*/
+    .line_height = 18,                              /*The maximum line height required by the font*/
+    .base_line = 1,                                 /*Baseline measured from the bottom of the line*/
+#if !(LVGL_VERSION_MAJOR == 6 && LVGL_VERSION_MINOR == 0)
+    .subpx = LV_FONT_SUBPX_NONE,
+#endif
+#if LV_VERSION_CHECK(7, 4, 0) || LVGL_VERSION_MAJOR >= 8
+    .underline_position = 0,
+    .underline_thickness = 0,
+#endif
+    .static_bitmap = 0,
+    .dsc = &otto_icon_font_dsc, /*The custom font data. Will be accessed by `get_glyph_bitmap/dsc` */
+#if LV_VERSION_CHECK(8, 2, 0) || LVGL_VERSION_MAJOR >= 9
+    .fallback = NULL,
+#endif
+    .user_data = NULL,
+};
+
+#endif /*#if ENABLE_OTTO_ICON_FONT*/

c++/otto-robot_otto_movements.cc

@@ -0,0 +1,964 @@
+#include "otto_movements.h"
+
+#include <algorithm>
+
+#include "freertos/idf_additions.h"
+#include "oscillator.h"
+
+static const char* TAG = "OttoMovements";
+
+#define HAND_HOME_POSITION 45
+
+Otto::Otto() {
+    is_otto_resting_ = false;
+    has_hands_ = false;
+    // 初始化所有舵机管脚为-1（未连接）
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        servo_pins_[i] = -1;
+        servo_trim_[i] = 0;
+    }
+}
+
+Otto::~Otto() {
+    DetachServos();
+}
+
+unsigned long IRAM_ATTR millis() {
+    return (unsigned long)(esp_timer_get_time() / 1000ULL);
+}
+
+void Otto::Init(int left_leg, int right_leg, int left_foot, int right_foot, int left_hand,
+                int right_hand) {
+    servo_pins_[LEFT_LEG] = left_leg;
+    servo_pins_[RIGHT_LEG] = right_leg;
+    servo_pins_[LEFT_FOOT] = left_foot;
+    servo_pins_[RIGHT_FOOT] = right_foot;
+    servo_pins_[LEFT_HAND] = left_hand;
+    servo_pins_[RIGHT_HAND] = right_hand;
+
+    // 检查是否有手部舵机
+    has_hands_ = (left_hand != -1 && right_hand != -1);
+
+    AttachServos();
+    is_otto_resting_ = false;
+}
+
+///////////////////////////////////////////////////////////////////
+//-- ATTACH & DETACH FUNCTIONS ----------------------------------//
+///////////////////////////////////////////////////////////////////
+void Otto::AttachServos() {
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        if (servo_pins_[i] != -1) {
+            servo_[i].Attach(servo_pins_[i]);
+        }
+    }
+}
+
+void Otto::DetachServos() {
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        if (servo_pins_[i] != -1) {
+            servo_[i].Detach();
+        }
+    }
+}
+
+///////////////////////////////////////////////////////////////////
+//-- OSCILLATORS TRIMS ------------------------------------------//
+///////////////////////////////////////////////////////////////////
+void Otto::SetTrims(int left_leg, int right_leg, int left_foot, int right_foot, int left_hand,
+                    int right_hand) {
+    servo_trim_[LEFT_LEG] = left_leg;
+    servo_trim_[RIGHT_LEG] = right_leg;
+    servo_trim_[LEFT_FOOT] = left_foot;
+    servo_trim_[RIGHT_FOOT] = right_foot;
+
+    if (has_hands_) {
+        servo_trim_[LEFT_HAND] = left_hand;
+        servo_trim_[RIGHT_HAND] = right_hand;
+    }
+
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        if (servo_pins_[i] != -1) {
+            servo_[i].SetTrim(servo_trim_[i]);
+        }
+    }
+}
+
+///////////////////////////////////////////////////////////////////
+//-- BASIC MOTION FUNCTIONS -------------------------------------//
+///////////////////////////////////////////////////////////////////
+void Otto::MoveServos(int time, int servo_target[]) {
+    if (GetRestState() == true) {
+        SetRestState(false);
+    }
+
+    final_time_ = millis() + time;
+    if (time > 10) {
+        for (int i = 0; i < SERVO_COUNT; i++) {
+            if (servo_pins_[i] != -1) {
+                increment_[i] = (servo_target[i] - servo_[i].GetPosition()) / (time / 10.0);
+            }
+        }
+
+        for (int iteration = 1; millis() < final_time_; iteration++) {
+            partial_time_ = millis() + 10;
+            for (int i = 0; i < SERVO_COUNT; i++) {
+                if (servo_pins_[i] != -1) {
+                    servo_[i].SetPosition(servo_[i].GetPosition() + increment_[i]);
+                }
+            }
+            vTaskDelay(pdMS_TO_TICKS(10));
+        }
+    } else {
+        for (int i = 0; i < SERVO_COUNT; i++) {
+            if (servo_pins_[i] != -1) {
+                servo_[i].SetPosition(servo_target[i]);
+            }
+        }
+        vTaskDelay(pdMS_TO_TICKS(time));
+    }
+
+    // final adjustment to the target.
+    bool f = true;
+    int adjustment_count = 0;
+    while (f && adjustment_count < 10) {
+        f = false;
+        for (int i = 0; i < SERVO_COUNT; i++) {
+            if (servo_pins_[i] != -1 && servo_target[i] != servo_[i].GetPosition()) {
+                f = true;
+                break;
+            }
+        }
+        if (f) {
+            for (int i = 0; i < SERVO_COUNT; i++) {
+                if (servo_pins_[i] != -1) {
+                    servo_[i].SetPosition(servo_target[i]);
+                }
+            }
+            vTaskDelay(pdMS_TO_TICKS(10));
+            adjustment_count++;
+        }
+    };
+}
+
+void Otto::MoveSingle(int position, int servo_number) {
+    if (position > 180)
+        position = 90;
+    if (position < 0)
+        position = 90;
+
+    if (GetRestState() == true) {
+        SetRestState(false);
+    }
+
+    if (servo_number >= 0 && servo_number < SERVO_COUNT && servo_pins_[servo_number] != -1) {
+        servo_[servo_number].SetPosition(position);
+    }
+}
+
+void Otto::OscillateServos(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period,
+                           double phase_diff[SERVO_COUNT], float cycle = 1) {
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        if (servo_pins_[i] != -1) {
+            servo_[i].SetO(offset[i]);
+            servo_[i].SetA(amplitude[i]);
+            servo_[i].SetT(period);
+            servo_[i].SetPh(phase_diff[i]);
+        }
+    }
+
+    double ref = millis();
+    double end_time = period * cycle + ref;
+
+    while (millis() < end_time) {
+        for (int i = 0; i < SERVO_COUNT; i++) {
+            if (servo_pins_[i] != -1) {
+                servo_[i].Refresh();
+            }
+        }
+        vTaskDelay(5);
+    }
+    vTaskDelay(pdMS_TO_TICKS(10));
+}
+
+void Otto::Execute(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period,
+                   double phase_diff[SERVO_COUNT], float steps = 1.0) {
+    if (GetRestState() == true) {
+        SetRestState(false);
+    }
+
+    int cycles = (int)steps;
+
+    //-- Execute complete cycles
+    if (cycles >= 1)
+        for (int i = 0; i < cycles; i++)
+            OscillateServos(amplitude, offset, period, phase_diff);
+
+    //-- Execute the final not complete cycle
+    OscillateServos(amplitude, offset, period, phase_diff, (float)steps - cycles);
+    vTaskDelay(pdMS_TO_TICKS(10));
+}
+
+//---------------------------------------------------------
+//-- Execute2: 使用绝对角度作为振荡中心
+//--  Parameters:
+//--    amplitude: 振幅数组（每个舵机的振荡幅度）
+//--    center_angle: 绝对角度数组（0-180度），作为振荡中心位置
+//--    period: 周期（毫秒）
+//--    phase_diff: 相位差数组（弧度）
+//--    steps: 步数/周期数（可为小数）
+//---------------------------------------------------------
+void Otto::Execute2(int amplitude[SERVO_COUNT], int center_angle[SERVO_COUNT], int period,
+                    double phase_diff[SERVO_COUNT], float steps = 1.0) {
+    if (GetRestState() == true) {
+        SetRestState(false);
+    }
+
+    // 将绝对角度转换为offset（offset = center_angle - 90）
+    int offset[SERVO_COUNT];
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        offset[i] = center_angle[i] - 90;
+    }
+
+    int cycles = (int)steps;
+
+    //-- Execute complete cycles
+    if (cycles >= 1)
+        for (int i = 0; i < cycles; i++)
+            OscillateServos(amplitude, offset, period, phase_diff);
+
+    //-- Execute the final not complete cycle
+    OscillateServos(amplitude, offset, period, phase_diff, (float)steps - cycles);
+    vTaskDelay(pdMS_TO_TICKS(10));
+}
+
+///////////////////////////////////////////////////////////////////
+//-- HOME = Otto at rest position -------------------------------//
+///////////////////////////////////////////////////////////////////
+void Otto::Home(bool hands_down) {
+    if (is_otto_resting_ == false) {  // Go to rest position only if necessary
+        // 为所有舵机准备初始位置值
+        int homes[SERVO_COUNT];
+        for (int i = 0; i < SERVO_COUNT; i++) {
+            if (i == LEFT_HAND || i == RIGHT_HAND) {
+                if (hands_down) {
+                    // 如果需要复位手部，设置为默认值
+                    if (i == LEFT_HAND) {
+                        homes[i] = HAND_HOME_POSITION;
+                    } else {                                  // RIGHT_HAND
+                        homes[i] = 180 - HAND_HOME_POSITION;  // 右手镜像位置
+                    }
+                } else {
+                    // 如果不需要复位手部，保持当前位置
+                    homes[i] = servo_[i].GetPosition();
+                }
+            } else {
+                // 腿部和脚部舵机始终复位
+                homes[i] = 90;
+            }
+        }
+
+        MoveServos(700, homes);
+is_otto_resting_ = true;
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(200));
+}
+
+bool Otto::GetRestState() {
+    return is_otto_resting_;
+}
+
+void Otto::SetRestState(bool state) {
+    is_otto_resting_ = state;
+}
+
+///////////////////////////////////////////////////////////////////
+//-- PREDETERMINED MOTION SEQUENCES -----------------------------//
+///////////////////////////////////////////////////////////////////
+//-- Otto movement: Jump
+//--  Parameters:
+//--    steps: Number of steps
+//--    T: Period
+//---------------------------------------------------------
+void Otto::Jump(float steps, int period) {
+    int up[SERVO_COUNT] = {90, 90, 150, 30, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    MoveServos(period, up);
+    int down[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    MoveServos(period, down);
+}
+
+//---------------------------------------------------------
+//-- Otto gait: Walking  (forward or backward)
+//--  Parameters:
+//--    * steps:  Number of steps
+//--    * T : Period
+//--    * Dir: Direction: FORWARD / BACKWARD
+//--    * amount: 手部摆动幅度, 0表示不摆动
+//---------------------------------------------------------
+void Otto::Walk(float steps, int period, int dir, int amount) {
+    //-- Oscillator parameters for walking
+    //-- Hip sevos are in phase
+    //-- Feet servos are in phase
+    //-- Hip and feet are 90 degrees out of phase
+    //--      -90 : Walk forward
+    //--       90 : Walk backward
+    //-- Feet servos also have the same offset (for tiptoe a little bit)
+    int A[SERVO_COUNT] = {30, 30, 30, 30, 0, 0};
+    int O[SERVO_COUNT] = {0, 0, 5, -5, HAND_HOME_POSITION - 90, HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(dir * -90), DEG2RAD(dir * -90), 0, 0};
+
+    // 如果amount>0且有手部舵机，设置手部振幅和相位
+    if (amount > 0 && has_hands_) {
+        // 手臂振幅使用传入的amount参数
+        A[LEFT_HAND] = amount;
+        A[RIGHT_HAND] = amount;
+
+        // 左手与右腿同相，右手与左腿同相，使得机器人走路时手臂自然摆动
+        phase_diff[LEFT_HAND] = phase_diff[RIGHT_LEG];  // 左手与右腿同相
+        phase_diff[RIGHT_HAND] = phase_diff[LEFT_LEG];  // 右手与左腿同相
+    } else {
+        A[LEFT_HAND] = 0;
+        A[RIGHT_HAND] = 0;
+    }
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto gait: Turning (left or right)
+//--  Parameters:
+//--   * Steps: Number of steps
+//--   * T: Period
+//--   * Dir: Direction: LEFT / RIGHT
+//--   * amount: 手部摆动幅度, 0表示不摆动
+//---------------------------------------------------------
+void Otto::Turn(float steps, int period, int dir, int amount) {
+    //-- Same coordination than for walking (see Otto::walk)
+    //-- The Amplitudes of the hip's oscillators are not igual
+    //-- When the right hip servo amplitude is higher, the steps taken by
+    //--   the right leg are bigger than the left. So, the robot describes an
+    //--   left arc
+    int A[SERVO_COUNT] = {30, 30, 30, 30, 0, 0};
+    int O[SERVO_COUNT] = {0, 0, 5, -5, HAND_HOME_POSITION - 90, HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(-90), DEG2RAD(-90), 0, 0};
+
+    if (dir == LEFT) {
+        A[0] = 30;  //-- Left hip servo
+        A[1] = 0;   //-- Right hip servo
+    } else {
+        A[0] = 0;
+        A[1] = 30;
+    }
+
+    // 如果amount>0且有手部舵机，设置手部振幅和相位
+    if (amount > 0 && has_hands_) {
+        // 手臂振幅使用传入的amount参数
+        A[LEFT_HAND] = amount;
+        A[RIGHT_HAND] = amount;
+
+        // 转向时手臂摆动相位：左手与左腿同相，右手与右腿同相，增强转向效果
+        phase_diff[LEFT_HAND] = phase_diff[LEFT_LEG];    // 左手与左腿同相
+        phase_diff[RIGHT_HAND] = phase_diff[RIGHT_LEG];  // 右手与右腿同相
+    } else {
+        A[LEFT_HAND] = 0;
+        A[RIGHT_HAND] = 0;
+    }
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto gait: Lateral bend
+//--  Parameters:
+//--    steps: Number of bends
+//--    T: Period of one bend
+//--    dir: RIGHT=Right bend LEFT=Left bend
+//---------------------------------------------------------
+void Otto::Bend(int steps, int period, int dir) {
+    // Parameters of all the movements. Default: Left bend
+    int bend1[SERVO_COUNT] = {90, 90, 62, 35, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    int bend2[SERVO_COUNT] = {90, 90, 62, 105, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    int homes[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+
+    // Time of one bend, constrained in order to avoid movements too fast.
+    // T=max(T, 600);
+    // Changes in the parameters if right direction is chosen
+    if (dir == -1) {
+        bend1[2] = 180 - 35;
+        bend1[3] = 180 - 60;  // Not 65. Otto is unbalanced
+        bend2[2] = 180 - 105;
+        bend2[3] = 180 - 60;
+    }
+
+    // Time of the bend movement. Fixed parameter to avoid falls
+    int T2 = 800;
+
+    // Bend movement
+    for (int i = 0; i < steps; i++) {
+        MoveServos(T2 / 2, bend1);
+        MoveServos(T2 / 2, bend2);
+        vTaskDelay(pdMS_TO_TICKS(period * 0.8));
+        MoveServos(500, homes);
+    }
+}
+
+//---------------------------------------------------------
+//-- Otto gait: Shake a leg
+//--  Parameters:
+//--    steps: Number of shakes
+//--    T: Period of one shake
+//--    dir: RIGHT=Right leg LEFT=Left leg
+//---------------------------------------------------------
+void Otto::ShakeLeg(int steps, int period, int dir) {
+    // This variable change the amount of shakes
+    int numberLegMoves = 2;
+
+    // Parameters of all the movements. Default: Right leg
+    int shake_leg1[SERVO_COUNT] = {90, 90, 58, 35, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    int shake_leg2[SERVO_COUNT] = {90, 90, 58, 120, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    int shake_leg3[SERVO_COUNT] = {90, 90, 58, 60, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    int homes[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+
+    // Changes in the parameters if left leg is chosen
+    if (dir == LEFT) {
+shake_leg1[2] = 180 - 35;
+        shake_leg1[3] = 180 - 58;
+        shake_leg2[2] = 180 - 120;
+        shake_leg2[3] = 180 - 58;
+        shake_leg3[2] = 180 - 60;
+        shake_leg3[3] = 180 - 58;
+    }
+
+    // Time of the bend movement. Fixed parameter to avoid falls
+    int T2 = 1000;
+    // Time of one shake, constrained in order to avoid movements too fast.
+    period = period - T2;
+    period = std::max(period, 200 * numberLegMoves);
+
+    for (int j = 0; j < steps; j++) {
+        // Bend movement
+        MoveServos(T2 / 2, shake_leg1);
+        MoveServos(T2 / 2, shake_leg2);
+
+        // Shake movement
+        for (int i = 0; i < numberLegMoves; i++) {
+            MoveServos(period / (2 * numberLegMoves), shake_leg3);
+            MoveServos(period / (2 * numberLegMoves), shake_leg2);
+        }
+        MoveServos(500, homes);  // Return to home position
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(period));
+}
+
+//---------------------------------------------------------
+//-- Otto movement: Sit (坐下)
+//---------------------------------------------------------
+void Otto::Sit() {
+    int target[SERVO_COUNT] = {120, 60, 0, 180, 45, 135};
+    MoveServos(600, target);
+}
+
+//---------------------------------------------------------
+//-- Otto movement: up & down
+//--  Parameters:
+//--    * steps: Number of jumps
+//--    * T: Period
+//--    * h: Jump height: SMALL / MEDIUM / BIG
+//--              (or a number in degrees 0 - 90)
+//---------------------------------------------------------
+void Otto::UpDown(float steps, int period, int height) {
+    //-- Both feet are 180 degrees out of phase
+    //-- Feet amplitude and offset are the same
+    //-- Initial phase for the right foot is -90, so that it starts
+    //--   in one extreme position (not in the middle)
+    int A[SERVO_COUNT] = {0, 0, height, height, 0, 0};
+    int O[SERVO_COUNT] = {0, 0, height, -height, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(-90), DEG2RAD(90), 0, 0};
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto movement: swinging side to side
+//--  Parameters:
+//--     steps: Number of steps
+//--     T : Period
+//--     h : Amount of swing (from 0 to 50 aprox)
+//---------------------------------------------------------
+void Otto::Swing(float steps, int period, int height) {
+    //-- Both feets are in phase. The offset is half the amplitude
+    //-- It causes the robot to swing from side to side
+    int A[SERVO_COUNT] = {0, 0, height, height, 0, 0};
+    int O[SERVO_COUNT] = {
+        0, 0, height / 2, -height / 2, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(0), DEG2RAD(0), 0, 0};
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto movement: swinging side to side without touching the floor with the heel
+//--  Parameters:
+//--     steps: Number of steps
+//--     T : Period
+//--     h : Amount of swing (from 0 to 50 aprox)
+//---------------------------------------------------------
+void Otto::TiptoeSwing(float steps, int period, int height) {
+    //-- Both feets are in phase. The offset is not half the amplitude in order to tiptoe
+    //-- It causes the robot to swing from side to side
+    int A[SERVO_COUNT] = {0, 0, height, height, 0, 0};
+    int O[SERVO_COUNT] = {0, 0, height, -height, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto gait: Jitter
+//--  Parameters:
+//--    steps: Number of jitters
+//--    T: Period of one jitter
+//--    h: height (Values between 5 - 25)
+//---------------------------------------------------------
+void Otto::Jitter(float steps, int period, int height) {
+    //-- Both feet are 180 degrees out of phase
+    //-- Feet amplitude and offset are the same
+    //-- Initial phase for the right foot is -90, so that it starts
+    //--   in one extreme position (not in the middle)
+    //-- h is constrained to avoid hit the feets
+    height = std::min(25, height);
+    int A[SERVO_COUNT] = {height, height, 0, 0, 0, 0};
+    int O[SERVO_COUNT] = {0, 0, 0, 0, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {DEG2RAD(-90), DEG2RAD(90), 0, 0, 0, 0};
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto gait: Ascending & turn (Jitter while up&down)
+//--  Parameters:
+//--    steps: Number of bends
+//--    T: Period of one bend
+//--    h: height (Values between 5 - 15)
+//---------------------------------------------------------
+void Otto::AscendingTurn(float steps, int period, int height) {
+    //-- Both feet and legs are 180 degrees out of phase
+    //-- Initial phase for the right foot is -90, so that it starts
+    //--   in one extreme position (not in the middle)
+    //-- h is constrained to avoid hit the feets
+    height = std::min(13, height);
+    int A[SERVO_COUNT] = {height, height, height, height, 0, 0};
+    int O[SERVO_COUNT] = {
+        0, 0, height + 4, -height + 4, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {DEG2RAD(-90), DEG2RAD(90), DEG2RAD(-90), DEG2RAD(90), 0, 0};
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto gait: Moonwalker. Otto moves like Michael Jackson
+//--  Parameters:
+//--    Steps: Number of steps
+//--    T: Period
+//--    h: Height. Typical valures between 15 and 40
+//--    dir: Direction: LEFT / RIGHT
+//---------------------------------------------------------
+void Otto::Moonwalker(float steps, int period, int height, int dir) {
+    //-- This motion is similar to that of the caterpillar robots: A travelling
+    //-- wave moving from one side to another
+    //-- The two Otto's feet are equivalent to a minimal configuration. It is known
+    //-- that 2 servos can move like a worm if they are 120 degrees out of phase
+    //-- In the example of Otto, the two feet are mirrored so that we have:
+    //--    180 - 120 = 60 degrees. The actual phase difference given to the oscillators
+    //--  is 60 degrees.
+    //--  Both amplitudes are equal. The offset is half the amplitud plus a little bit of
+    //-   offset so that the robot tiptoe lightly
+
+    int A[SERVO_COUNT] = {0, 0, height, height, 0, 0};
+    int O[SERVO_COUNT] = {
+        0, 0, height / 2 + 2, -height / 2 - 2, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    int phi = -dir * 90;
+    double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(phi), DEG2RAD(-60 * dir + phi), 0, 0};
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//----------------------------------------------------------
+//-- Otto gait: Crusaito. A mixture between moonwalker and walk
+//--   Parameters:
+//--     steps: Number of steps
+//--     T: Period
+//--     h: height (Values between 20 - 50)
+//--     dir:  Direction: LEFT / RIGHT
+//-----------------------------------------------------------
+void Otto::Crusaito(float steps, int period, int height, int dir) {
+    int A[SERVO_COUNT] = {25, 25, height, height, 0, 0};
+    int O[SERVO_COUNT] = {
+        0, 0, height / 2 + 4, -height / 2 - 4, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {90, 90, DEG2RAD(0), DEG2RAD(-60 * dir), 0, 0};
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto gait: Flapping
+//--  Parameters:
+//--    steps: Number of steps
+//--    T: Period
+//--    h: height (Values between 10 - 30)
+//--    dir: direction: FOREWARD, BACKWARD
+//---------------------------------------------------------
+void Otto::Flapping(float steps, int period, int height, int dir) {
+    int A[SERVO_COUNT] = {12, 12, height, height, 0, 0};
+    int O[SERVO_COUNT] = {
+        0, 0, height - 10, -height + 10, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+    double phase_diff[SERVO_COUNT] = {
+        DEG2RAD(0), DEG2RAD(180), DEG2RAD(-90 * dir), DEG2RAD(90 * dir), 0, 0};
+
+    //-- Let's oscillate the servos!
+    Execute(A, O, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- Otto gait: WhirlwindLeg (旋风腿)
+//--   Parameters:
+//--     steps: Number of steps
+//--     period: Period (建议100-800毫秒)
+//--     amplitude: amplitude (Values between 20 - 40)
+//---------------------------------------------------------
+void Otto::WhirlwindLeg(float steps, int period, int amplitude) {
+
+
+    int target[SERVO_COUNT] = {90, 90, 180, 90, 45, 20};
+    MoveServos(100, target);
+    target[RIGHT_FOOT] = 160;
+    MoveServos(500, target);
+    vTaskDelay(pdMS_TO_TICKS(1000));
+
+    int C[SERVO_COUNT] = {90, 90, 180, 160, 45, 20};
+    int A[SERVO_COUNT] = {amplitude, 0, 0, 0, amplitude, 0};
+    double phase_diff[SERVO_COUNT] = {DEG2RAD(20), 0, 0, 0, DEG2RAD(20), 0};
+    Execute2(A, C, period, phase_diff, steps);
+
+}
+
+//---------------------------------------------------------
+//-- 手部动作: 举手
+//--  Parameters:
+//--    period: 动作时间
+//--    dir: 方向 1=左手, -1=右手, 0=双手
+//---------------------------------------------------------
+void Otto::HandsUp(int period, int dir) {
+    if (!has_hands_) {
+        return;
+    }
+
+int target[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+
+    if (dir == 0) {
+        target[LEFT_HAND] = 170;
+        target[RIGHT_HAND] = 10;
+    } else if (dir == LEFT) {
+        target[LEFT_HAND] = 170;
+        target[RIGHT_HAND] = servo_[RIGHT_HAND].GetPosition();
+    } else if (dir == RIGHT) {
+target[RIGHT_HAND] = 10;
+        target[LEFT_HAND] = servo_[LEFT_HAND].GetPosition();
+    }
+
+    MoveServos(period, target);
+}
+
+//---------------------------------------------------------
+//-- 手部动作: 双手放下
+//--  Parameters:
+//--    period: 动作时间
+//--    dir: 方向 1=左手, -1=右手, 0=双手
+//---------------------------------------------------------
+void Otto::HandsDown(int period, int dir) {
+    if (!has_hands_) {
+        return;
+    }
+
+    int target[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
+
+    if (dir == LEFT) {
+        target[RIGHT_HAND] = servo_[RIGHT_HAND].GetPosition();
+    } else if (dir == RIGHT) {
+target[LEFT_HAND] = servo_[LEFT_HAND].GetPosition();
+    }
+
+    MoveServos(period, target);
+}
+
+//---------------------------------------------------------
+//--  手部动作: 挥手
+//--  Parameters:
+//--  dir: 方向 LEFT/RIGHT/BOTH
+//---------------------------------------------------------
+void Otto::HandWave(int dir) {
+    if (!has_hands_) {
+        return;
+    }
+    if (dir == LEFT) {
+        int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 160, 135};
+        int A[SERVO_COUNT] = {0, 0, 0, 0, 20, 0};
+        double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), 0};
+        Execute2(A, center_angle, 300, phase_diff, 5);
+    }
+    else if (dir == RIGHT) {
+        int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 45, 20};
+        int A[SERVO_COUNT] = {0, 0, 0, 0, 0, 20};
+        double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, DEG2RAD(90)};
+        Execute2(A, center_angle, 300, phase_diff, 5);
+    }
+    else {
+        int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 160, 20};
+        int A[SERVO_COUNT] = {0, 0, 0, 0, 20, 20};
+        double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(90)};
+        Execute2(A, center_angle, 300, phase_diff, 5);
+    }
+}
+
+
+//---------------------------------------------------------
+//-- 手部动作: 大风车
+//--  Parameters:
+//--    steps: 动作次数
+//--    period: 动作周期（毫秒）
+//--    amplitude: 振荡幅度（度）
+//---------------------------------------------------------
+void Otto::Windmill(float steps, int period, int amplitude) {
+if (!has_hands_) {
+        return;
+    }
+
+    int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 90, 90};
+    int A[SERVO_COUNT] = {0, 0, 0, 0, amplitude, amplitude};
+    double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(90)};
+    Execute2(A, center_angle, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- 手部动作: 起飞
+//--  Parameters:
+//--    steps: 动作次数
+//--    period: 动作周期（毫秒），数值越小速度越快
+//--    amplitude: 振荡幅度（度）
+//---------------------------------------------------------
+void Otto::Takeoff(float steps, int period, int amplitude) {
+if (!has_hands_) {
+        return;
+    }
+
+    Home(true);
+
+    int center_angle[SERVO_COUNT] = {90, 90, 90, 90, 90, 90};
+    int A[SERVO_COUNT] = {0, 0, 0, 0, amplitude, amplitude};
+    double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(-90)};
+    Execute2(A, center_angle, period, phase_diff, steps);
+}
+
+//---------------------------------------------------------
+//-- 手部动作: 健身
+//--  Parameters:
+//--    steps: 动作次数
+//--    period: 动作周期（毫秒）
+//--    amplitude: 振荡幅度（度）
+//---------------------------------------------------------
+void Otto::Fitness(float steps, int period, int amplitude) {
+    if (!has_hands_) {
+        return;
+    }
+    int target[SERVO_COUNT] = {90, 90, 90, 0, 160, 135};
+    MoveServos(100, target);
+    target[LEFT_FOOT] = 20;
+    MoveServos(400, target);
+    vTaskDelay(pdMS_TO_TICKS(2000));
+
+    int C[SERVO_COUNT] = {90, 90, 20, 90, 160, 135};
+    int A[SERVO_COUNT] = {0, 0, 0, 0, 0, amplitude};
+    double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
+    Execute2(A, C, period, phase_diff, steps);
+
+}
+
+//---------------------------------------------------------
+//-- 手部动作: 打招呼
+//--  Parameters:
+//--    dir: 方向 LEFT=左手, RIGHT=右手
+//--    steps: 动作次数
+//---------------------------------------------------------
+void Otto::Greeting(int dir, float steps) {
+    if (!has_hands_) {
+        return;
+    }
+    if (dir == LEFT) {
+        int target[SERVO_COUNT] = {90, 90, 150, 150, 45, 135};
+        MoveServos(400, target);
+        int C[SERVO_COUNT] = {90, 90, 150, 150, 160, 135};
+        int A[SERVO_COUNT] = {0, 0, 0, 0, 20, 0};
+        double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
+        Execute2(A, C, 300, phase_diff, steps);
+    }
+    else if (dir == RIGHT) {
+        int target[SERVO_COUNT] = {90, 90, 30, 30, 45, 135};
+        MoveServos(400, target);
+        int C[SERVO_COUNT] = {90, 90, 30, 30, 45, 20};
+        int A[SERVO_COUNT] = {0, 0, 0, 0, 0, 20};
+        double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
+        Execute2(A, C, 300, phase_diff, steps);
+    }
+
+}
+
+//---------------------------------------------------------
+//-- 手部动作: 害羞
+//--  Parameters:
+//--    dir: 方向 LEFT=左手, RIGHT=右手
+//--    steps: 动作次数
+//---------------------------------------------------------
+void Otto::Shy(int dir, float steps) {
+    if (!has_hands_) {
+        return;
+    }
+
+    if (dir == LEFT) {
+        int target[SERVO_COUNT] = {90, 90, 150, 150, 45, 135};
+        MoveServos(400, target);
+        int C[SERVO_COUNT] = {90, 90, 150, 150, 45, 135};
+        int A[SERVO_COUNT] = {0, 0, 0, 0, 20, 20};
+        double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(-90)};
+        Execute2(A, C, 300, phase_diff, steps);
+    }
+    else if (dir == RIGHT) {
+        int target[SERVO_COUNT] = {90, 90, 30, 30, 45, 135};
+        MoveServos(400, target);
+        int C[SERVO_COUNT] = {90, 90, 30, 30, 45, 135};
+        int A[SERVO_COUNT] = {0, 0, 0, 0, 0, 20};
+        double phase_diff[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(-90)};
+        Execute2(A, C, 300, phase_diff, steps);
+    }
+}
+
+//---------------------------------------------------------
+//-- 手部动作: 广播体操
+//---------------------------------------------------------
+void Otto::RadioCalisthenics() {
+    if (!has_hands_) {
+        return;
+    }
+
+    const int period = 1000; 
+    const float steps = 8.0; 
+
+    int C1[SERVO_COUNT] = {90, 90, 90, 90, 145, 45};
+    int A1[SERVO_COUNT] = {0, 0, 0, 0, 45, 45};
+    double phase_diff1[SERVO_COUNT] = {0, 0, 0, 0, DEG2RAD(90), DEG2RAD(-90)};
+    Execute2(A1, C1, period, phase_diff1, steps);
+
+    int C2[SERVO_COUNT] = {90, 90, 115, 65, 90, 90};
+    int A2[SERVO_COUNT] = {0, 0, 25, 25, 0, 0};
+    double phase_diff2[SERVO_COUNT] = {0, 0, DEG2RAD(90), DEG2RAD(-90), 0, 0};
+    Execute2(A2, C2, period, phase_diff2, steps);
+    
+    int C3[SERVO_COUNT] = {90, 90, 130, 130, 90, 90};
+    int A3[SERVO_COUNT] = {0, 0, 0, 0, 20, 0};
+    double phase_diff3[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
+    Execute2(A3, C3, period, phase_diff3, steps);
+
+    int C4[SERVO_COUNT] = {90, 90, 50, 50, 90, 90};
+    int A4[SERVO_COUNT] = {0, 0, 0, 0, 0, 20};
+    double phase_diff4[SERVO_COUNT] = {0, 0, 0, 0, 0, 0};
+    Execute2(A4, C4, period, phase_diff4, steps);
+}
+
+//---------------------------------------------------------
+//-- 手部动作: 爱的魔力转圈圈
+//---------------------------------------------------------
+void Otto::MagicCircle() {
+    if (!has_hands_) {
+        return;
+    }
+
+    int A[SERVO_COUNT] = {30, 30, 30, 30, 50, 50};
+    int O[SERVO_COUNT] = {0, 0, 5, -5, 0, 0};
+    double phase_diff[SERVO_COUNT] = {0, 0, DEG2RAD(-90), DEG2RAD(-90), DEG2RAD(-90) , DEG2RAD(90)};
+
+    Execute(A, O, 700, phase_diff, 40);
+}
+
+//---------------------------------------------------------
+//-- 展示动作：串联多个动作展示
+//---------------------------------------------------------
+void Otto::Showcase() {
+    if (GetRestState() == true) {
+        SetRestState(false);
+    }
+
+    // 1. 往前走3步
+    Walk(3, 1000, FORWARD, 50);
+    vTaskDelay(pdMS_TO_TICKS(500));
+
+    // 2. 挥挥手
+    if (has_hands_) {
+        HandWave(LEFT);
+        vTaskDelay(pdMS_TO_TICKS(500));
+    }
+
+    // 3. 跳舞（使用广播体操）
+    if (has_hands_) {
+        RadioCalisthenics();
+        vTaskDelay(pdMS_TO_TICKS(500));
+    }
+
+    // 4. 太空步
+    Moonwalker(3, 900, 25, LEFT);
+    vTaskDelay(pdMS_TO_TICKS(500));
+
+    // 5. 摇摆
+    Swing(3, 1000, 30);
+    vTaskDelay(pdMS_TO_TICKS(500));
+
+    // 6. 起飞
+    if (has_hands_) {
+        Takeoff(5, 300, 40);
+        vTaskDelay(pdMS_TO_TICKS(500));
+    }
+
+    // 7. 健身
+    if (has_hands_) {
+        Fitness(5, 1000, 25);
+        vTaskDelay(pdMS_TO_TICKS(500));
+    }
+
+    // 8. 往后走3步
+    Walk(3, 1000, BACKWARD, 50);
+}
+
+void Otto::EnableServoLimit(int diff_limit) {
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        if (servo_pins_[i] != -1) {
+            servo_[i].SetLimiter(diff_limit);
+        }
+    }
+}
+
+void Otto::DisableServoLimit() {
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        if (servo_pins_[i] != -1) {
+            servo_[i].DisableLimiter();
+        }
+    }
+}

c++/otto-robot_otto_movements.h

@@ -0,0 +1,113 @@
+#ifndef __OTTO_MOVEMENTS_H__
+#define __OTTO_MOVEMENTS_H__
+
+#include "driver/gpio.h"
+#include "esp_log.h"
+#include "esp_timer.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "oscillator.h"
+
+//-- Constants
+#define FORWARD 1
+#define BACKWARD -1
+#define LEFT 1
+#define RIGHT -1
+#define BOTH 0
+#define SMALL 5
+#define MEDIUM 15
+#define BIG 30
+
+// -- Servo delta limit default. degree / sec
+#define SERVO_LIMIT_DEFAULT 240
+
+// -- Servo indexes for easy access
+#define LEFT_LEG 0
+#define RIGHT_LEG 1
+#define LEFT_FOOT 2
+#define RIGHT_FOOT 3
+#define LEFT_HAND 4
+#define RIGHT_HAND 5
+#define SERVO_COUNT 6
+
+class Otto {
+public:
+    Otto();
+    ~Otto();
+
+    //-- Otto initialization
+    void Init(int left_leg, int right_leg, int left_foot, int right_foot, int left_hand = -1,
+              int right_hand = -1);
+    //-- Attach & detach functions
+    void AttachServos();
+    void DetachServos();
+
+    //-- Oscillator Trims
+    void SetTrims(int left_leg, int right_leg, int left_foot, int right_foot, int left_hand = 0,
+                  int right_hand = 0);
+
+    //-- Predetermined Motion Functions
+    void MoveServos(int time, int servo_target[]);
+    void MoveSingle(int position, int servo_number);
+    void OscillateServos(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period,
+                         double phase_diff[SERVO_COUNT], float cycle);
+    void Execute2(int amplitude[SERVO_COUNT], int center_angle[SERVO_COUNT], int period,
+                  double phase_diff[SERVO_COUNT], float steps);
+//-- HOME = Otto at rest position
+    void Home(bool hands_down = true);
+    bool GetRestState();
+    void SetRestState(bool state);
+
+    //-- Predetermined Motion Functions
+    void Jump(float steps = 1, int period = 2000);
+
+    void Walk(float steps = 4, int period = 1000, int dir = FORWARD, int amount = 0);
+    void Turn(float steps = 4, int period = 2000, int dir = LEFT, int amount = 0);
+    void Bend(int steps = 1, int period = 1400, int dir = LEFT);
+    void ShakeLeg(int steps = 1, int period = 2000, int dir = RIGHT);
+    void Sit();  // 坐下
+void UpDown(float steps = 1, int period = 1000, int height = 20);
+    void Swing(float steps = 1, int period = 1000, int height = 20);
+    void TiptoeSwing(float steps = 1, int period = 900, int height = 20);
+    void Jitter(float steps = 1, int period = 500, int height = 20);
+    void AscendingTurn(float steps = 1, int period = 900, int height = 20);
+
+    void Moonwalker(float steps = 1, int period = 900, int height = 20, int dir = LEFT);
+    void Crusaito(float steps = 1, int period = 900, int height = 20, int dir = FORWARD);
+    void Flapping(float steps = 1, int period = 1000, int height = 20, int dir = FORWARD);
+    void WhirlwindLeg(float steps = 1, int period = 300, int amplitude = 30);
+// -- 手部动作
+    void HandsUp(int period = 1000, int dir = 0);      // 双手举起
+    void HandsDown(int period = 1000, int dir = 0);    // 双手放下
+    void HandWave(int dir = LEFT);  // 挥手
+    void Windmill(float steps = 10, int period = 500, int amplitude = 90);  // 大风车
+    void Takeoff(float steps = 5, int period = 300, int amplitude = 40);   // 起飞
+    void Fitness(float steps = 5, int period = 1000, int amplitude = 25);  // 健身
+    void Greeting(int dir = LEFT, float steps = 5);  // 打招呼
+    void Shy(int dir = LEFT, float steps = 5);  // 害羞
+    void RadioCalisthenics();  // 广播体操
+    void MagicCircle();  // 爱的魔力转圈圈
+    void Showcase();  // 展示动作（串联多个动作）
+// -- Servo limiter
+    void EnableServoLimit(int speed_limit_degree_per_sec = SERVO_LIMIT_DEFAULT);
+    void DisableServoLimit();
+
+private:
+    Oscillator servo_[SERVO_COUNT];
+
+    int servo_pins_[SERVO_COUNT];
+    int servo_trim_[SERVO_COUNT];
+
+    unsigned long final_time_;
+    unsigned long partial_time_;
+    float increment_[SERVO_COUNT];
+
+    bool is_otto_resting_;
+    bool has_hands_;  // 是否有手部舵机
+
+    void Execute(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period,
+                 double phase_diff[SERVO_COUNT], float steps);
+
+};
+
+#endif  // __OTTO_MOVEMENTS_H__

c++/otto-robot_otto_robot.cc

@@ -0,0 +1,341 @@
+#include <driver/i2c_master.h>
+#include <driver/spi_common.h>
+#include <driver/ledc.h>
+#include <esp_lcd_panel_io.h>
+#include <esp_lcd_panel_ops.h>
+#include <esp_lcd_panel_vendor.h>
+#include <esp_log.h>
+#include "application.h"
+#include "codecs/no_audio_codec.h"
+#include "button.h"
+#include "config.h"
+#include "display/lcd_display.h"
+#include "lamp_controller.h"
+#include "led/single_led.h"
+#include "mcp_server.h"
+#include "otto_emoji_display.h"
+#include "power_manager.h"
+#include "system_reset.h"
+#include "wifi_board.h"
+#include "esp32_camera.h"
+#include "websocket_control_server.h"
+
+#define TAG "OttoRobot"
+
+extern void InitializeOttoController(const HardwareConfig& hw_config);
+class OttoRobot : public WifiBoard {
+private:
+    LcdDisplay* display_;
+    PowerManager* power_manager_;
+    Button boot_button_;
+    WebSocketControlServer* ws_control_server_;
+    HardwareConfig hw_config_;
+    AudioCodec* audio_codec_;
+    i2c_master_bus_handle_t i2c_bus_;
+    Esp32Camera *camera_;
+    bool has_camera_;
+    
+    bool DetectHardwareVersion() {
+        ledc_timer_config_t ledc_timer = {
+            .speed_mode = LEDC_LOW_SPEED_MODE,
+            .duty_resolution = LEDC_TIMER_2_BIT,
+            .timer_num = LEDC_TIMER,
+            .freq_hz = CAMERA_XCLK_FREQ,
+            .clk_cfg = LEDC_AUTO_CLK,
+        };
+        esp_err_t ret = ledc_timer_config(&ledc_timer);
+        if (ret != ESP_OK) {
+            return false;
+        }
+        
+        ledc_channel_config_t ledc_channel = {
+            .gpio_num = CAMERA_XCLK,
+            .speed_mode = LEDC_LOW_SPEED_MODE,
+            .channel = LEDC_CHANNEL,
+            .intr_type = LEDC_INTR_DISABLE,
+            .timer_sel = LEDC_TIMER,
+            .duty = 2,
+            .hpoint = 0,
+        };
+        ret = ledc_channel_config(&ledc_channel);
+        if (ret != ESP_OK) {
+            return false;
+        }
+        
+        vTaskDelay(pdMS_TO_TICKS(100));
+        i2c_master_bus_config_t i2c_bus_cfg = {
+            .i2c_port = I2C_NUM_0,
+            .sda_io_num = CAMERA_VERSION_CONFIG.i2c_sda_pin,
+            .scl_io_num = CAMERA_VERSION_CONFIG.i2c_scl_pin,
+            .clk_source = I2C_CLK_SRC_DEFAULT,
+            .glitch_ignore_cnt = 7,
+            .intr_priority = 0,
+            .trans_queue_depth = 0,
+            .flags = {
+                .enable_internal_pullup = 1,
+            },
+        };
+        
+        ret = i2c_new_master_bus(&i2c_bus_cfg, &i2c_bus_);
+        if (ret != ESP_OK) {
+            ledc_stop(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL, 0);
+            return false;
+        }
+        const uint8_t camera_addresses[] = {0x30, 0x3C, 0x21, 0x60};
+        bool camera_found = false;
+        
+        for (size_t i = 0; i < sizeof(camera_addresses); i++) {
+            uint8_t addr = camera_addresses[i];
+            i2c_device_config_t dev_cfg = {
+                .dev_addr_length = I2C_ADDR_BIT_LEN_7,
+                .device_address = addr,
+                .scl_speed_hz = 100000,
+            };
+            
+            i2c_master_dev_handle_t dev_handle;
+            ret = i2c_master_bus_add_device(i2c_bus_, &dev_cfg, &dev_handle);
+            if (ret == ESP_OK) {
+                uint8_t reg_addr = 0x0A;
+                uint8_t data[2];
+                ret = i2c_master_transmit_receive(dev_handle, &reg_addr, 1, data, 2, 200);
+                if (ret == ESP_OK) {
+                    camera_found = true;
+                    i2c_master_bus_rm_device(dev_handle);
+                    break;
+                }
+                i2c_master_bus_rm_device(dev_handle);
+            }
+        }
+        
+        if (!camera_found) {
+            i2c_del_master_bus(i2c_bus_);
+            i2c_bus_ = nullptr;
+            ledc_stop(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL, 0);
+        }
+        return camera_found;
+    }
+    
+    void InitializePowerManager() {
+        power_manager_ = new PowerManager(
+            hw_config_.power_charge_detect_pin,
+            hw_config_.power_adc_unit,
+            hw_config_.power_adc_channel
+        );
+}
+
+    void InitializeSpi() {
+        spi_bus_config_t buscfg = {};
+        buscfg.mosi_io_num = hw_config_.display_mosi_pin;
+        buscfg.miso_io_num = GPIO_NUM_NC;
+        buscfg.sclk_io_num = hw_config_.display_clk_pin;
+buscfg.quadwp_io_num = GPIO_NUM_NC;
+        buscfg.quadhd_io_num = GPIO_NUM_NC;
+        buscfg.max_transfer_sz = DISPLAY_WIDTH * DISPLAY_HEIGHT * sizeof(uint16_t);
+        ESP_ERROR_CHECK(spi_bus_initialize(SPI3_HOST, &buscfg, SPI_DMA_CH_AUTO));
+    }
+
+    void InitializeLcdDisplay() {
+        esp_lcd_panel_io_handle_t panel_io = nullptr;
+        esp_lcd_panel_handle_t panel = nullptr;
+        esp_lcd_panel_io_spi_config_t io_config = {};
+        io_config.cs_gpio_num = hw_config_.display_cs_pin;
+        io_config.dc_gpio_num = hw_config_.display_dc_pin;
+io_config.spi_mode = DISPLAY_SPI_MODE;
+        io_config.pclk_hz = 40 * 1000 * 1000;
+        io_config.trans_queue_depth = 10;
+        io_config.lcd_cmd_bits = 8;
+        io_config.lcd_param_bits = 8;
+        ESP_ERROR_CHECK(esp_lcd_new_panel_io_spi(SPI3_HOST, &io_config, &panel_io));
+
+        esp_lcd_panel_dev_config_t panel_config = {};
+        panel_config.reset_gpio_num = hw_config_.display_rst_pin;
+panel_config.rgb_ele_order = DISPLAY_RGB_ORDER;
+        panel_config.bits_per_pixel = 16;
+
+        ESP_ERROR_CHECK(esp_lcd_new_panel_st7789(panel_io, &panel_config, &panel));
+
+        esp_lcd_panel_reset(panel);
+
+        esp_lcd_panel_init(panel);
+        esp_lcd_panel_invert_color(panel, DISPLAY_INVERT_COLOR);
+        esp_lcd_panel_swap_xy(panel, DISPLAY_SWAP_XY);
+        esp_lcd_panel_mirror(panel, DISPLAY_MIRROR_X, DISPLAY_MIRROR_Y);
+
+        display_ = new OttoEmojiDisplay(
+            panel_io, panel, DISPLAY_WIDTH, DISPLAY_HEIGHT, DISPLAY_OFFSET_X, DISPLAY_OFFSET_Y,
+            DISPLAY_MIRROR_X, DISPLAY_MIRROR_Y, DISPLAY_SWAP_XY);
+    }
+
+    void InitializeButtons() {
+        boot_button_.OnClick([this]() {
+            auto& app = Application::GetInstance();
+            if (app.GetDeviceState() == kDeviceStateStarting) {
+                EnterWifiConfigMode();
+                return;
+}
+            app.ToggleChatState();
+        });
+    }
+
+    void InitializeOttoController() {
+        ::InitializeOttoController(hw_config_);
+    }
+    
+public:
+    const HardwareConfig& GetHardwareConfig() const {
+        return hw_config_;
+    }
+    
+private:
+
+    void InitializeWebSocketControlServer() {
+        ws_control_server_ = new WebSocketControlServer();
+        if (!ws_control_server_->Start(8080)) {
+            delete ws_control_server_;
+            ws_control_server_ = nullptr;
+        }
+    }
+
+    void StartNetwork() override {
+        WifiBoard::StartNetwork();
+        vTaskDelay(pdMS_TO_TICKS(1000));
+        
+        InitializeWebSocketControlServer();
+    }
+
+    bool InitializeCamera() {
+        if (!has_camera_ || i2c_bus_ == nullptr) {
+            return false;
+        }
+        
+        try {
+            static esp_cam_ctlr_dvp_pin_config_t dvp_pin_config = {
+                .data_width = CAM_CTLR_DATA_WIDTH_8,
+                .data_io = {
+                    [0] = CAMERA_D0,
+                    [1] = CAMERA_D1,
+                    [2] = CAMERA_D2,
+                    [3] = CAMERA_D3,
+                    [4] = CAMERA_D4,
+                    [5] = CAMERA_D5,
+                    [6] = CAMERA_D6,
+                    [7] = CAMERA_D7,
+                },
+                .vsync_io = CAMERA_VSYNC,
+                .de_io = CAMERA_HSYNC,
+                .pclk_io = CAMERA_PCLK,
+                .xclk_io = CAMERA_XCLK,
+            };
+
+            esp_video_init_sccb_config_t sccb_config = {
+                .init_sccb = false,
+                .i2c_handle = i2c_bus_,
+                .freq = 100000,
+            };
+
+            esp_video_init_dvp_config_t dvp_config = {
+                .sccb_config = sccb_config,
+                .reset_pin = CAMERA_RESET,
+                .pwdn_pin = CAMERA_PWDN,
+                .dvp_pin = dvp_pin_config,
+                .xclk_freq = CAMERA_XCLK_FREQ,
+            };
+
+            esp_video_init_config_t video_config = {
+                .dvp = &dvp_config,
+            };
+
+            camera_ = new Esp32Camera(video_config);
+            camera_->SetVFlip(true);
+            return true;
+        } catch (...) {
+            camera_ = nullptr;
+            return false;
+        }
+    }
+    
+    void InitializeAudioCodec() {
+        if (hw_config_.audio_use_simplex) {
+            audio_codec_ = new NoAudioCodecSimplex(
+                hw_config_.audio_input_sample_rate,
+                hw_config_.audio_output_sample_rate,
+                hw_config_.audio_i2s_spk_gpio_bclk,
+                hw_config_.audio_i2s_spk_gpio_lrck,
+                hw_config_.audio_i2s_spk_gpio_dout,
+                hw_config_.audio_i2s_mic_gpio_sck,
+                hw_config_.audio_i2s_mic_gpio_ws,
+                hw_config_.audio_i2s_mic_gpio_din
+            );
+        } else {
+            audio_codec_ = new NoAudioCodecDuplex(
+                hw_config_.audio_input_sample_rate,
+                hw_config_.audio_output_sample_rate,
+                hw_config_.audio_i2s_gpio_bclk,
+                hw_config_.audio_i2s_gpio_ws,
+                hw_config_.audio_i2s_gpio_dout,
+                hw_config_.audio_i2s_gpio_din
+            );
+        }
+    }
+
+public:
+    OttoRobot() : boot_button_(BOOT_BUTTON_GPIO),
+                  audio_codec_(nullptr),
+                  i2c_bus_(nullptr),
+                  camera_(nullptr),
+                  has_camera_(false) {
+        
+        has_camera_ = DetectHardwareVersion();
+        
+        if (has_camera_) 
+            hw_config_ = CAMERA_VERSION_CONFIG;
+        else 
+            hw_config_ = NON_CAMERA_VERSION_CONFIG;
+        
+        
+InitializeSpi();
+        InitializeLcdDisplay();
+        InitializeButtons();
+        InitializePowerManager();
+        InitializeAudioCodec();
+        
+        if (has_camera_) {
+            if (!InitializeCamera()) {
+                has_camera_ = false;
+            }
+        }
+        
+        InitializeOttoController();
+        ws_control_server_ = nullptr;
+        GetBacklight()->RestoreBrightness();
+    }
+
+    virtual AudioCodec *GetAudioCodec() override {
+        return audio_codec_;
+    }
+
+    virtual Display* GetDisplay() override { 
+        return display_; 
+    }
+
+    virtual Backlight* GetBacklight() override {
+        static PwmBacklight* backlight = nullptr;
+        if (backlight == nullptr) {
+            backlight = new PwmBacklight(hw_config_.display_backlight_pin, DISPLAY_BACKLIGHT_OUTPUT_INVERT);
+        }
+        return backlight;
+    }
+    
+virtual bool GetBatteryLevel(int& level, bool& charging, bool& discharging) override {
+        charging = power_manager_->IsCharging();
+        discharging = !charging;
+        level = power_manager_->GetBatteryLevel();
+        return true;
+    }
+
+    virtual Camera *GetCamera() override { 
+        return has_camera_ ? camera_ : nullptr; 
+    }
+};
+
+DECLARE_BOARD(OttoRobot);

c++/otto-robot_power_manager.h

@@ -0,0 +1,149 @@
+#ifndef __POWER_MANAGER_H__
+#define __POWER_MANAGER_H__
+
+#include <driver/gpio.h>
+#include <esp_adc/adc_oneshot.h>
+#include <esp_log.h>
+#include <esp_timer.h>
+
+class PowerManager {
+private:
+    // 电池电量区间-分压电阻为2个100k
+    static constexpr struct {
+        uint16_t adc;
+        uint8_t level;
+    } BATTERY_LEVELS[] = {{2050, 0}, {2450, 100}};
+static constexpr size_t BATTERY_LEVELS_COUNT = 2;
+    static constexpr size_t ADC_VALUES_COUNT = 10;
+
+    esp_timer_handle_t timer_handle_ = nullptr;
+    gpio_num_t charging_pin_;
+    adc_unit_t adc_unit_;
+    adc_channel_t adc_channel_;
+    uint16_t adc_values_[ADC_VALUES_COUNT];
+    size_t adc_values_index_ = 0;
+    size_t adc_values_count_ = 0;
+    uint8_t battery_level_ = 100;
+    bool is_charging_ = false;
+    inline static bool battery_update_paused_ = false;  // 静态标志：是否暂停电量更新
+adc_oneshot_unit_handle_t adc_handle_;
+
+    void CheckBatteryStatus() {
+      // 如果电量更新被暂停（动作进行中），则跳过更新
+      if (battery_update_paused_) {
+        return;
+      }
+      
+      ReadBatteryAdcData();
+
+      if (charging_pin_ == GPIO_NUM_NC) {
+        is_charging_ = false;
+      } else {
+        is_charging_ = gpio_get_level(charging_pin_) == 0;
+      }
+}
+
+    void ReadBatteryAdcData() {
+        int adc_value;
+        ESP_ERROR_CHECK(adc_oneshot_read(adc_handle_, adc_channel_, &adc_value));
+
+        adc_values_[adc_values_index_] = adc_value;
+        adc_values_index_ = (adc_values_index_ + 1) % ADC_VALUES_COUNT;
+        if (adc_values_count_ < ADC_VALUES_COUNT) {
+            adc_values_count_++;
+        }
+
+        uint32_t average_adc = 0;
+        for (size_t i = 0; i < adc_values_count_; i++) {
+            average_adc += adc_values_[i];
+        }
+        average_adc /= adc_values_count_;
+
+        CalculateBatteryLevel(average_adc);
+
+        // ESP_LOGI("PowerManager", "ADC值: %d 平均值: %ld 电量: %u%%", adc_value, average_adc,
+        //          battery_level_);
+    }
+
+    void CalculateBatteryLevel(uint32_t average_adc) {
+        if (average_adc <= BATTERY_LEVELS[0].adc) {
+            battery_level_ = 0;
+        } else if (average_adc >= BATTERY_LEVELS[BATTERY_LEVELS_COUNT - 1].adc) {
+            battery_level_ = 100;
+        } else {
+            float ratio = static_cast<float>(average_adc - BATTERY_LEVELS[0].adc) /
+                          (BATTERY_LEVELS[1].adc - BATTERY_LEVELS[0].adc);
+            battery_level_ = ratio * 100;
+        }
+    }
+
+public:
+    PowerManager(gpio_num_t charging_pin, adc_unit_t adc_unit = ADC_UNIT_2,
+                 adc_channel_t adc_channel = ADC_CHANNEL_3)
+        : charging_pin_(charging_pin), adc_unit_(adc_unit), adc_channel_(adc_channel) {
+
+      if (charging_pin_ != GPIO_NUM_NC) {
+gpio_config_t io_conf = {};
+        io_conf.intr_type = GPIO_INTR_DISABLE;
+        io_conf.mode = GPIO_MODE_INPUT;
+        io_conf.pin_bit_mask = (1ULL << charging_pin_);
+        io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
+        io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
+        gpio_config(&io_conf);
+        ESP_LOGI("PowerManager", "充电检测引脚配置完成: GPIO%d", charging_pin_);
+      } else {
+        ESP_LOGI("PowerManager", "充电检测引脚未配置，不进行充电状态检测");
+      }
+esp_timer_create_args_t timer_args = {
+            .callback =
+                [](void* arg) {
+                    PowerManager* self = static_cast<PowerManager*>(arg);
+                    self->CheckBatteryStatus();
+                },
+            .arg = this,
+            .dispatch_method = ESP_TIMER_TASK,
+            .name = "battery_check_timer",
+            .skip_unhandled_events = true,
+        };
+        ESP_ERROR_CHECK(esp_timer_create(&timer_args, &timer_handle_));
+        ESP_ERROR_CHECK(esp_timer_start_periodic(timer_handle_, 1000000));  // 1秒
+
+        InitializeAdc();
+    }
+
+    void InitializeAdc() {
+      adc_oneshot_unit_init_cfg_t init_config = {
+          .unit_id = adc_unit_,
+          .clk_src = ADC_RTC_CLK_SRC_DEFAULT,
+          .ulp_mode = ADC_ULP_MODE_DISABLE,
+      };
+      ESP_ERROR_CHECK(adc_oneshot_new_unit(&init_config, &adc_handle_));
+
+      adc_oneshot_chan_cfg_t chan_config = {
+          .atten = ADC_ATTEN_DB_12,
+          .bitwidth = ADC_BITWIDTH_12,
+      };
+
+      ESP_ERROR_CHECK(
+          adc_oneshot_config_channel(adc_handle_, adc_channel_, &chan_config));
+}
+
+    ~PowerManager() {
+        if (timer_handle_) {
+            esp_timer_stop(timer_handle_);
+            esp_timer_delete(timer_handle_);
+        }
+        if (adc_handle_) {
+            adc_oneshot_del_unit(adc_handle_);
+        }
+    }
+
+    bool IsCharging() { return is_charging_; }
+
+    uint8_t GetBatteryLevel() { return battery_level_; }
+
+    // 暂停/恢复电量更新（用于动作执行时屏蔽更新）
+    static void PauseBatteryUpdate() { battery_update_paused_ = true; }
+    static void ResumeBatteryUpdate() { battery_update_paused_ = false; }
+};
+#endif  // __POWER_MANAGER_H__

c++/otto-robot_websocket_control_server.cc

@@ -0,0 +1,191 @@
+#include "websocket_control_server.h"
+#include "mcp_server.h"
+#include <esp_log.h>
+#include <esp_http_server.h>
+#include <sys/param.h>
+#include <cstring>
+#include <cstdlib>
+#include <map>
+
+static const char* TAG = "WSControl";
+
+WebSocketControlServer* WebSocketControlServer::instance_ = nullptr;
+
+WebSocketControlServer::WebSocketControlServer() : server_handle_(nullptr) {
+    instance_ = this;
+}
+
+WebSocketControlServer::~WebSocketControlServer() {
+    Stop();
+    instance_ = nullptr;
+}
+
+esp_err_t WebSocketControlServer::ws_handler(httpd_req_t *req) {
+    if (instance_ == nullptr) {
+        return ESP_FAIL;
+    }
+    
+    if (req->method == HTTP_GET) {
+        ESP_LOGI(TAG, "Handshake done, the new connection was opened");
+        instance_->AddClient(req);
+        return ESP_OK;
+    }
+    
+    httpd_ws_frame_t ws_pkt;
+    uint8_t *buf = NULL;
+    memset(&ws_pkt, 0, sizeof(httpd_ws_frame_t));
+    ws_pkt.type = HTTPD_WS_TYPE_TEXT;
+    
+    /* Set max_len = 0 to get the frame len */
+    esp_err_t ret = httpd_ws_recv_frame(req, &ws_pkt, 0);
+    if (ret != ESP_OK) {
+        ESP_LOGE(TAG, "httpd_ws_recv_frame failed to get frame len with %d", ret);
+        return ret;
+    }
+    ESP_LOGI(TAG, "frame len is %d", ws_pkt.len);
+    
+    if (ws_pkt.len) {
+        /* ws_pkt.len + 1 is for NULL termination as we are expecting a string */
+        buf = (uint8_t*)calloc(1, ws_pkt.len + 1);
+        if (buf == NULL) {
+            ESP_LOGE(TAG, "Failed to calloc memory for buf");
+            return ESP_ERR_NO_MEM;
+        }
+        ws_pkt.payload = buf;
+        /* Set max_len = ws_pkt.len to get the frame payload */
+        ret = httpd_ws_recv_frame(req, &ws_pkt, ws_pkt.len);
+        if (ret != ESP_OK) {
+            ESP_LOGE(TAG, "httpd_ws_recv_frame failed with %d", ret);
+            free(buf);
+            return ret;
+        }
+        ESP_LOGI(TAG, "Got packet with message: %s", ws_pkt.payload);
+    }
+    
+    ESP_LOGI(TAG, "Packet type: %d", ws_pkt.type);
+    
+    if (ws_pkt.type == HTTPD_WS_TYPE_CLOSE) {
+        ESP_LOGI(TAG, "WebSocket close frame received");
+        instance_->RemoveClient(req);
+        free(buf);
+        return ESP_OK;
+    }
+    
+    if (ws_pkt.type == HTTPD_WS_TYPE_TEXT) {
+        if (ws_pkt.len > 0 && buf != nullptr) {
+            buf[ws_pkt.len] = '\0';
+            instance_->HandleMessage(req, (const char*)buf, ws_pkt.len);
+        }
+    } else {
+        ESP_LOGW(TAG, "Unsupported frame type: %d", ws_pkt.type);
+    }
+    
+    free(buf);
+    return ESP_OK;
+}
+
+bool WebSocketControlServer::Start(int port) {
+    httpd_config_t config = HTTPD_DEFAULT_CONFIG();
+    config.server_port = port;
+    config.max_open_sockets = 7;
+
+    httpd_uri_t ws_uri = {
+        .uri = "/ws",
+        .method = HTTP_GET,
+        .handler = ws_handler,
+        .user_ctx = nullptr,
+        .is_websocket = true
+    };
+
+    if (httpd_start(&server_handle_, &config) == ESP_OK) {
+        httpd_register_uri_handler(server_handle_, &ws_uri);
+        ESP_LOGI(TAG, "WebSocket server started on port %d", port);
+        return true;
+    }
+
+    ESP_LOGE(TAG, "Failed to start WebSocket server");
+    return false;
+}
+
+void WebSocketControlServer::Stop() {
+    if (server_handle_) {
+        httpd_stop(server_handle_);
+        server_handle_ = nullptr;
+        clients_.clear();
+        ESP_LOGI(TAG, "WebSocket server stopped");
+    }
+}
+
+void WebSocketControlServer::HandleMessage(httpd_req_t *req, const char* data, size_t len) {
+    if (data == nullptr || len == 0) {
+        ESP_LOGE(TAG, "Invalid message: data is null or len is 0");
+        return;
+    }
+    
+    if (len > 4096) {
+        ESP_LOGE(TAG, "Message too long: %zu bytes", len);
+        return;
+    }
+    
+    char* temp_buf = (char*)malloc(len + 1);
+    if (temp_buf == nullptr) {
+        ESP_LOGE(TAG, "Failed to allocate memory");
+        return;
+    }
+    memcpy(temp_buf, data, len);
+    temp_buf[len] = '\0';
+    
+    cJSON* root = cJSON_Parse(temp_buf);
+    free(temp_buf);
+    
+    if (root == nullptr) {
+        ESP_LOGE(TAG, "Failed to parse JSON");
+        return;
+    }
+
+    // 支持两种格式：
+    // 1. 完整格式：{"type":"mcp","payload":{...}}
+    // 2. 简化格式：直接是MCP payload对象
+    
+    cJSON* payload = nullptr;
+    cJSON* type = cJSON_GetObjectItem(root, "type");
+    
+    if (type && cJSON_IsString(type) && strcmp(type->valuestring, "mcp") == 0) {
+        payload = cJSON_GetObjectItem(root, "payload");
+        if (payload != nullptr) {
+            cJSON_DetachItemViaPointer(root, payload);
+            McpServer::GetInstance().ParseMessage(payload);
+            cJSON_Delete(payload); 
+        }
+    } else {
+        payload = cJSON_Duplicate(root, 1);
+        if (payload != nullptr) {
+            McpServer::GetInstance().ParseMessage(payload);
+            cJSON_Delete(payload);
+        }
+    }
+    
+    if (payload == nullptr) {
+        ESP_LOGE(TAG, "Invalid message format or failed to parse");
+    }
+
+    cJSON_Delete(root);
+}
+
+void WebSocketControlServer::AddClient(httpd_req_t *req) {
+    int sock_fd = httpd_req_to_sockfd(req);
+    if (clients_.find(sock_fd) == clients_.end()) {
+        clients_[sock_fd] = req;
+        ESP_LOGI(TAG, "Client connected: %d (total: %zu)", sock_fd, clients_.size());
+    }
+}
+
+void WebSocketControlServer::RemoveClient(httpd_req_t *req) {
+    int sock_fd = httpd_req_to_sockfd(req);
+    clients_.erase(sock_fd);
+    ESP_LOGI(TAG, "Client disconnected: %d (total: %zu)", sock_fd, clients_.size());
+}
+
+size_t WebSocketControlServer::GetClientCount() const {
+    return clients_.size();
+}

c++/otto-robot_websocket_control_server.h

@@ -0,0 +1,33 @@
+#ifndef WEBSOCKET_CONTROL_SERVER_H
+#define WEBSOCKET_CONTROL_SERVER_H
+
+#include <esp_http_server.h>
+#include <cJSON.h>
+#include <string>
+#include <map>
+
+class WebSocketControlServer {
+public:
+    WebSocketControlServer();
+    ~WebSocketControlServer();
+
+    bool Start(int port = 8080);
+    
+    void Stop();
+
+    size_t GetClientCount() const;
+
+private:
+    httpd_handle_t server_handle_;
+    std::map<int, httpd_req_t*> clients_;
+
+    static esp_err_t ws_handler(httpd_req_t *req);
+    
+    void HandleMessage(httpd_req_t *req, const char* data, size_t len);
+    void AddClient(httpd_req_t *req);
+    void RemoveClient(httpd_req_t *req);
+    static WebSocketControlServer* instance_;
+};
+
+#endif // WEBSOCKET_CONTROL_SERVER_H
+

config.py

@@ -0,0 +1,71 @@
+from micropython import const
+
+class BoardConfig:
+    def __init__(self, name):
+        self.name = name
+        self.pins = {}
+        self.audio = {}
+        self.mic = {}
+
+# -----------------------------------------------------------------------------
+# 非摄像头版本配置 (NON_CAMERA_VERSION)
+# 对应 C++: NON_CAMERA_VERSION_CONFIG
+# -----------------------------------------------------------------------------
+NON_CAMERA = BoardConfig("NON_CAMERA")
+NON_CAMERA.pins = {
+    'bl': 3,       # 背光
+    'mosi': 10,    # SPI MOSI / SDA
+    'sck': 9,      # SPI CLK / SCK
+    'dc': 46,      # Data/Command
+    'rst': 11,     # Reset
+    'cs': 12       # Chip Select
+}
+NON_CAMERA.audio = {
+    'enabled': True,
+    'sd': 7,       # Serial Data (DOUT) -> 扬声器
+    'bck': 15,     # Bit Clock (BCLK)
+    'ws': 16,      # Word Select (LRCK)
+    'sample_rate': 24000
+}
+NON_CAMERA.mic = {
+    'enabled': True,
+    'sd': 6,       # Serial Data (DIN) <- 麦克风
+    'sck': 5,      # Bit Clock (SCK)
+    'ws': 4,       # Word Select (WS)
+    'sample_rate': 16000
+}
+
+# -----------------------------------------------------------------------------
+# 摄像头版本配置 (CAMERA_VERSION)
+# 对应 C++: CAMERA_VERSION_CONFIG
+# -----------------------------------------------------------------------------
+CAMERA = BoardConfig("CAMERA")
+CAMERA.pins = {
+    'bl': 38,
+    'mosi': 45,
+    'sck': 48,
+    'dc': 47,
+    'rst': 1,
+    'cs': None     # 无 CS 引脚
+}
+# 摄像头版本使用 Duplex I2S (同一组引脚)
+CAMERA.audio = {
+    'enabled': True,
+    'sd': 39,      # DOUT
+    'bck': 42,
+    'ws': 40,
+    'sample_rate': 16000
+}
+CAMERA.mic = {
+    'enabled': True,
+    'sd': 41,      # DIN
+    'sck': 42,     # 复用 BCLK
+    'ws': 40,      # 复用 WS
+    'sample_rate': 16000
+}
+
+# =============================================================================
+# 当前使用的配置
+# =============================================================================
+# 默认使用 NON_CAMERA (普通版)，请根据你的实际硬件选择
+CURRENT_CONFIG = NON_CAMERA

display.py

@@ -0,0 +1,75 @@
+import machine
+import st7789py as st7789
+from config import CURRENT_CONFIG
+
+class Display:
+    def __init__(self):
+        self.tft = None
+        self.width = 240
+        self.height = 240
+        self._init_display()
+
+    def _init_display(self):
+        print(">>> Initializing Display...")
+        try:
+            pins = CURRENT_CONFIG.pins
+            spi = machine.SPI(2, baudrate=40000000, polarity=1, phase=1, 
+                              sck=machine.Pin(pins['sck']), mosi=machine.Pin(pins['mosi']))
+            
+            cs_pin = pins.get('cs')
+            cs = machine.Pin(cs_pin, machine.Pin.OUT) if cs_pin is not None else None
+            
+            rst_pin = pins.get('rst')
+            dc_pin = pins.get('dc')
+            
+            self.tft = st7789.ST7789(spi, self.width, self.height, 
+                                    reset=machine.Pin(rst_pin, machine.Pin.OUT) if rst_pin else None,
+                                    dc=machine.Pin(dc_pin, machine.Pin.OUT) if dc_pin else None,
+                                    cs=cs,
+                                    backlight=None)
+            self.tft.init()
+            self.tft.fill(st7789.BLUE)
+        except Exception as e:
+            print(f"Display error: {e}")
+            self.tft = None
+
+    def fill(self, color):
+        if self.tft:
+            self.tft.fill(color)
+
+    def fill_rect(self, x, y, w, h, color):
+        if self.tft:
+            self.tft.fill_rect(x, y, w, h, color)
+
+    def init_ui(self):
+        """初始化 UI 背景"""
+        if self.tft:
+            self.tft.fill(st7789.BLACK)
+            self.tft.fill_rect(0, 0, 240, 30, st7789.WHITE)
+
+    def update_audio_bar(self, bar_height, last_bar_height):
+        """更新音频可视化的柱状图"""
+        if not self.tft: return last_bar_height
+
+        # 确定当前颜色
+        color = st7789.GREEN
+        if bar_height > 50: color = st7789.YELLOW
+        if bar_height > 100: color = st7789.RED
+        
+        # 确定上一次颜色
+        last_color = st7789.GREEN
+        if last_bar_height > 50: last_color = st7789.YELLOW
+        if last_bar_height > 100: last_color = st7789.RED
+        
+        # 1. 如果变矮了，清除顶部多余部分
+        if bar_height < last_bar_height:
+            self.tft.fill_rect(100, 240 - last_bar_height, 40, last_bar_height - bar_height, st7789.BLACK)
+        
+        # 2. 如果颜色变了，必须重绘整个条
+        if color != last_color:
+            self.tft.fill_rect(100, 240 - bar_height, 40, bar_height, color)
+        # 3. 如果颜色没变且变高了，只绘新增部分
+        elif bar_height > last_bar_height:
+            self.tft.fill_rect(100, 240 - bar_height, 40, bar_height - last_bar_height, color)
+            
+        return bar_height

main.py

@@ -0,0 +1,199 @@
+import machine
+import time
+import math
+import struct
+import array
+import gc
+import st7789py as st7789
+from config import CURRENT_CONFIG
+from audio import AudioPlayer, Microphone
+from display import Display
+
+# =============================================================================
+# 硬件引脚配置 (从 config.py 获取)
+# =============================================================================
+
+def main():
+    print("\n" + "="*40)
+    print("AUDIO & MIC DIAGNOSTIC V5 (Modular & Clean)")
+    print("="*40 + "\n")
+    
+    # 0. 初始化 Boot 按键 (GPIO 0)
+    boot_btn = machine.Pin(0, machine.Pin.IN, machine.Pin.PULL_UP)
+
+    # 1. 初始化背光
+    # 使用配置中的引脚
+    bl_pin = CURRENT_CONFIG.pins.get('bl')
+    if bl_pin is not None:
+        try:
+            bl = machine.Pin(bl_pin, machine.Pin.OUT)
+            bl.on()
+        except Exception as e: 
+            print(f"Backlight error: {e}")
+
+    # 2. 音频测试 (重点排查)
+    speaker = AudioPlayer()
+    if speaker.i2s:
+        # 默认播放马里奥
+        speaker.play_mario()
+    else:
+        print("!!! Speaker initialization failed")
+
+    # 3. 屏幕初始化
+    display = Display()
+
+    # 4. 麦克风实时监测
+    mic = Microphone()
+    print("\n>>> Starting Mic Monitor...")
+    
+    read_buf = bytearray(4096) 
+    
+    # UI
+    if display.tft:
+        display.init_ui()
+    
+    last_print = time.ticks_ms()
+    last_bar_height = 0
+    
+    # 录音状态变量
+    is_recording = False
+    recorded_chunks = []
+    
+    # 调试：打印一次 Boot 键状态
+    print(f"Boot Button Initial State: {boot_btn.value()}")
+
+    heartbeat_state = False
+
+    while True:
+        try:
+            # === 心跳指示器 (右上角) ===
+            # 每隔 100ms 翻转一次，证明循环在跑
+            if display.tft:
+                heartbeat_state = not heartbeat_state
+                color = st7789.GREEN if heartbeat_state else st7789.BLACK
+                display.tft.fill_rect(230, 0, 10, 10, color)
+
+            # === 按键录音逻辑 (Boot 键按下) ===
+            btn_val = boot_btn.value()
+            
+            # === 按键状态指示器 (左上角) ===
+            # 红色表示按下，蓝色表示未按下
+            if display.tft:
+                btn_color = st7789.RED if btn_val == 0 else st7789.BLUE
+                display.tft.fill_rect(0, 0, 10, 10, btn_color)
+
+            if btn_val == 0:
+                if not is_recording:
+                    print("\n>>> Start Recording (Boot Pressed)...")
+                    is_recording = True
+                    recorded_chunks = []
+                    if display.tft:
+                        print(">>> Filling Screen WHITE")
+                        display.fill(st7789.WHITE)
+                    else:
+                        print(">>> Display TFT is None!")
+                
+                # 录音
+                if mic.i2s:
+                    num_read = mic.readinto(read_buf)
+                    if num_read > 0:
+                        try:
+                            recorded_chunks.append(bytes(read_buf[:num_read]))
+                        except MemoryError:
+                            print("Memory Full!")
+                continue # 跳过可视化逻辑
+
+            # === 按键释放处理 ===
+            elif is_recording:
+                print(f"\n>>> Stop Recording. Captured {len(recorded_chunks)} chunks.")
+                is_recording = False
+                
+                if display.tft:
+                    display.init_ui()
+                
+                # 播放录音
+                if speaker.i2s and len(recorded_chunks) > 0:
+                    print(">>> Playing...")
+                    try:
+                        cfg = speaker.config
+                        # 重新初始化 Speaker (16kHz Mono 16-bit) 以匹配 Mic 数据
+                        speaker.i2s.deinit()
+                        speaker.i2s = machine.I2S(
+                            0,
+                            sck=machine.Pin(cfg['bck']),
+                            ws=machine.Pin(cfg['ws']),
+                            sd=machine.Pin(cfg['sd']),
+                            mode=machine.I2S.TX,
+                            bits=16,
+                            format=machine.I2S.MONO,
+                            rate=16000,
+                            ibuf=20000,
+                        )
+                        
+                        # 播放数据
+                        for chunk in recorded_chunks:
+                            # 32-bit Mono -> 16-bit Mono (取高16位)
+                            # chunk 是 bytes, 转为 array('h') 方便访问 16-bit word
+                            # 32-bit 数据: LowWord, HighWord
+                            # 我们需要 HighWord
+                            arr = array.array('h', chunk)
+                            samples = arr[1::2]
+                            speaker.i2s.write(samples)
+                            
+                    except Exception as e:
+                        print(f"Playback error: {e}")
+                    
+                    # 恢复 Speaker 原始配置
+                    if speaker.i2s: speaker.i2s.deinit()
+                    speaker._init_audio()
+                
+                recorded_chunks = []
+                gc.collect()
+
+            # === 原有的可视化逻辑 ===
+            if mic.i2s:
+                num_read = mic.readinto(read_buf)
+                if num_read > 0:
+                    sum_squares = 0
+                    count = num_read // 4 
+                    step = 4 
+                    samples_checked = 0
+                    max_val = 0
+                    
+                    for i in range(0, count, step):
+                        val = struct.unpack_from('<i', read_buf, i*4)[0]
+                        # ICS-43434 24-bit 处理
+                        val = val >> 8 
+                        sum_squares += val * val
+                        if abs(val) > max_val: max_val = abs(val)
+                        samples_checked += 1
+                        
+                    if samples_checked > 0:
+                        rms = math.sqrt(sum_squares / samples_checked)
+                    else:
+                        rms = 0
+                    
+                    if time.ticks_diff(time.ticks_ms(), last_print) > 1000:
+                        print(f"Mic Level -> RMS: {int(rms)}, Max: {max_val}")
+                        last_print = time.ticks_ms()
+                    
+                    if display.tft:
+                        # 调整缩放比例，让显示更敏感
+                        # 你的日志显示安静时 Max ~2000-3000, 说话时 Max ~40000
+                        # 我们可以把 Max 40000 映射到满格
+                        
+                        bar_height = int((max_val / 40000) * 200) 
+                        if bar_height > 200: bar_height = 200
+                        if bar_height < 0: bar_height = 0
+                        
+                        last_bar_height = display.update_audio_bar(bar_height, last_bar_height)
+
+            else:
+                time.sleep(0.1)
+                
+        except Exception as e:
+            print(f"Loop error: {e}")
+            time.sleep(1)
+
+if __name__ == '__main__':
+    main()

st7789py.py

@@ -0,0 +1,312 @@
+import time
+from micropython import const
+import ustruct as struct
+
+# commands
+ST77XX_NOP = const(0x00)
+ST77XX_SWRESET = const(0x01)
+ST77XX_RDDID = const(0x04)
+ST77XX_RDDST = const(0x09)
+
+ST77XX_SLPIN = const(0x10)
+ST77XX_SLPOUT = const(0x11)
+ST77XX_PTLON = const(0x12)
+ST77XX_NORON = const(0x13)
+
+ST77XX_INVOFF = const(0x20)
+ST77XX_INVON = const(0x21)
+ST77XX_DISPOFF = const(0x28)
+ST77XX_DISPON = const(0x29)
+ST77XX_CASET = const(0x2A)
+ST77XX_RASET = const(0x2B)
+ST77XX_RAMWR = const(0x2C)
+ST77XX_RAMRD = const(0x2E)
+
+ST77XX_PTLAR = const(0x30)
+ST77XX_COLMOD = const(0x3A)
+ST7789_MADCTL = const(0x36)
+
+ST7789_MADCTL_MY = const(0x80)
+ST7789_MADCTL_MX = const(0x40)
+ST7789_MADCTL_MV = const(0x20)
+ST7789_MADCTL_ML = const(0x10)
+ST7789_MADCTL_BGR = const(0x08)
+ST7789_MADCTL_MH = const(0x04)
+ST7789_MADCTL_RGB = const(0x00)
+
+ST7789_RDID1 = const(0xDA)
+ST7789_RDID2 = const(0xDB)
+ST7789_RDID3 = const(0xDC)
+ST7789_RDID4 = const(0xDD)
+
+ColorMode_65K = const(0x50)
+ColorMode_262K = const(0x60)
+ColorMode_12bit = const(0x03)
+ColorMode_16bit = const(0x05)
+ColorMode_18bit = const(0x06)
+ColorMode_16M = const(0x07)
+
+# Color definitions
+BLACK = const(0x0000)
+BLUE = const(0x001F)
+RED = const(0xF800)
+GREEN = const(0x07E0)
+CYAN = const(0x07FF)
+MAGENTA = const(0xF81F)
+YELLOW = const(0xFFE0)
+WHITE = const(0xFFFF)
+
+_ENCODE_PIXEL = ">H"
+_ENCODE_POS = ">HH"
+_DECODE_PIXEL = ">BBB"
+
+_BUFFER_SIZE = const(256)
+
+
+def delay_ms(ms):
+    time.sleep_ms(ms)
+
+
+def color565(r, g=0, b=0):
+    """Convert red, green and blue values (0-255) into a 16-bit 565 encoding.  As
+    a convenience this is also available in the parent adafruit_rgb_display
+    package namespace."""
+    try:
+        r, g, b = r  # see if the first var is a tuple/list
+    except TypeError:
+        pass
+    return (r & 0xf8) << 8 | (g & 0xfc) << 3 | b >> 3
+
+
+class ST77xx:
+    def __init__(self, spi, width, height, reset, dc, cs=None, backlight=None,
+                 xstart=-1, ystart=-1):
+        """
+        display = st7789.ST7789(
+            SPI(1, baudrate=40000000, phase=0, polarity=1),
+            240, 240,
+            reset=machine.Pin(5, machine.Pin.OUT),
+            dc=machine.Pin(2, machine.Pin.OUT),
+        )
+
+        """
+        self.width = width
+        self.height = height
+        self.spi = spi
+        if spi is None:
+            import machine
+            self.spi = machine.SPI(1, baudrate=40000000, phase=0, polarity=1)
+        self.reset = reset
+        self.dc = dc
+        self.cs = cs
+        self.backlight = backlight
+        if xstart >= 0 and ystart >= 0:
+            self.xstart = xstart
+            self.ystart = ystart
+        elif (self.width, self.height) == (240, 240):
+            self.xstart = 0
+            self.ystart = 0
+        elif (self.width, self.height) == (135, 240):
+            self.xstart = 52
+            self.ystart = 40
+        else:
+            raise ValueError(
+                "Unsupported display. Only 240x240 and 135x240 are supported "
+                "without xstart and ystart provided"
+            )
+
+    def dc_low(self):
+        self.dc.off()
+
+    def dc_high(self):
+        self.dc.on()
+
+    def reset_low(self):
+        if self.reset:
+            self.reset.off()
+
+    def reset_high(self):
+        if self.reset:
+            self.reset.on()
+
+    def cs_low(self):
+        if self.cs:
+            self.cs.off()
+
+    def cs_high(self):
+        if self.cs:
+            self.cs.on()
+
+    def write(self, command=None, data=None):
+        """SPI write to the device: commands and data"""
+        self.cs_low()
+        if command is not None:
+            self.dc_low()
+            self.spi.write(bytes([command]))
+        if data is not None:
+            self.dc_high()
+            self.spi.write(data)
+        self.cs_high()
+
+    def hard_reset(self):
+        self.cs_low()
+        self.reset_high()
+        delay_ms(50)
+        self.reset_low()
+        delay_ms(50)
+        self.reset_high()
+        delay_ms(150)
+        self.cs_high()
+
+    def soft_reset(self):
+        self.write(ST77XX_SWRESET)
+        delay_ms(150)
+
+    def sleep_mode(self, value):
+        if value:
+            self.write(ST77XX_SLPIN)
+        else:
+            self.write(ST77XX_SLPOUT)
+
+    def inversion_mode(self, value):
+        if value:
+            self.write(ST77XX_INVON)
+        else:
+            self.write(ST77XX_INVOFF)
+
+    def _set_color_mode(self, mode):
+        self.write(ST77XX_COLMOD, bytes([mode & 0x77]))
+
+    def init(self, *args, **kwargs):
+        self.hard_reset()
+        self.soft_reset()
+        self.sleep_mode(False)
+        if self.backlight:
+            self.backlight.on()
+
+    def _set_mem_access_mode(self, rotation, vert_mirror, horz_mirror, is_bgr):
+        rotation &= 7
+        value = {
+            0: 0,
+            1: ST7789_MADCTL_MX,
+            2: ST7789_MADCTL_MY,
+            3: ST7789_MADCTL_MX | ST7789_MADCTL_MY,
+            4: ST7789_MADCTL_MV,
+            5: ST7789_MADCTL_MV | ST7789_MADCTL_MX,
+            6: ST7789_MADCTL_MV | ST7789_MADCTL_MY,
+            7: ST7789_MADCTL_MV | ST7789_MADCTL_MX | ST7789_MADCTL_MY,
+        }[rotation]
+
+        if vert_mirror:
+            value = ST7789_MADCTL_ML
+        elif horz_mirror:
+            value = ST7789_MADCTL_MH
+
+        if is_bgr:
+            value |= ST7789_MADCTL_BGR
+        self.write(ST7789_MADCTL, bytes([value]))
+
+    def _encode_pos(self, x, y):
+        """Encode a postion into bytes."""
+        return struct.pack(_ENCODE_POS, x, y)
+
+    def _encode_pixel(self, color):
+        """Encode a pixel color into bytes."""
+        return struct.pack(_ENCODE_PIXEL, color)
+
+    def _set_columns(self, start, end):
+        if start > end or end >= self.width:
+            return
+        start += self.xstart
+        end += self.xstart
+        self.write(ST77XX_CASET, self._encode_pos(start, end))
+
+    def _set_rows(self, start, end):
+        if start > end or end >= self.height:
+            return
+        start += self.ystart
+        end += self.ystart
+        self.write(ST77XX_RASET, self._encode_pos(start, end))
+
+    def set_window(self, x0, y0, x1, y1):
+        self._set_columns(x0, x1)
+        self._set_rows(y0, y1)
+        self.write(ST77XX_RAMWR)
+
+    def vline(self, x, y, length, color):
+        self.fill_rect(x, y, 1, length, color)
+
+    def hline(self, x, y, length, color):
+        self.fill_rect(x, y, length, 1, color)
+
+    def pixel(self, x, y, color):
+        self.set_window(x, y, x, y)
+        self.write(None, self._encode_pixel(color))
+
+    def blit_buffer(self, buffer, x, y, width, height):
+        self.set_window(x, y, x + width - 1, y + height - 1)
+        self.write(None, buffer)
+
+    def rect(self, x, y, w, h, color):
+        self.hline(x, y, w, color)
+        self.vline(x, y, h, color)
+        self.vline(x + w - 1, y, h, color)
+        self.hline(x, y + h - 1, w, color)
+
+    def fill_rect(self, x, y, width, height, color):
+        self.set_window(x, y, x + width - 1, y + height - 1)
+        chunks, rest = divmod(width * height, _BUFFER_SIZE)
+        pixel = self._encode_pixel(color)
+        self.dc_high()
+        if chunks:
+            data = pixel * _BUFFER_SIZE
+            for _ in range(chunks):
+                self.write(None, data)
+        if rest:
+            self.write(None, pixel * rest)
+
+    def fill(self, color):
+        self.fill_rect(0, 0, self.width, self.height, color)
+
+    def line(self, x0, y0, x1, y1, color):
+        # Line drawing function.  Will draw a single pixel wide line starting at
+        # x0, y0 and ending at x1, y1.
+        steep = abs(y1 - y0) > abs(x1 - x0)
+        if steep:
+            x0, y0 = y0, x0
+            x1, y1 = y1, x1
+        if x0 > x1:
+            x0, x1 = x1, x0
+            y0, y1 = y1, y0
+        dx = x1 - x0
+        dy = abs(y1 - y0)
+        err = dx // 2
+        if y0 < y1:
+            ystep = 1
+        else:
+            ystep = -1
+        while x0 <= x1:
+            if steep:
+                self.pixel(y0, x0, color)
+            else:
+                self.pixel(x0, y0, color)
+            err -= dy
+            if err < 0:
+                y0 += ystep
+                err += dx
+            x0 += 1
+
+
+class ST7789(ST77xx):
+    def init(self, *, color_mode=ColorMode_65K | ColorMode_16bit):
+        super().init()
+        self._set_color_mode(color_mode)
+        delay_ms(50)
+        self._set_mem_access_mode(4, True, True, False)
+        self.inversion_mode(True)
+        delay_ms(10)
+        self.write(ST77XX_NORON)
+        delay_ms(10)
+        self.fill(0)
+        self.write(ST77XX_DISPON)
+        delay_ms(500)