commit 252a4304666f92c4228310b27b3f5f3600755b88 Author: jeremygan2021 Date: Mon Mar 2 21:14:05 2026 +0800 f diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..706fd07 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.idea +.vscode diff --git a/.trae/rules/rule.md b/.trae/rules/rule.md new file mode 100644 index 0000000..9601240 --- /dev/null +++ b/.trae/rules/rule.md @@ -0,0 +1,4 @@ +这是一个esp32 s3项目 +用的是Micropython +使用的spi7789 方形的屏幕封装 +硬件是基于c++文件夹里的代码改到MicroPython上面 \ No newline at end of file diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..ecd5bbf --- /dev/null +++ b/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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..bd1b493 --- /dev/null +++ b/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) diff --git a/audio.py b/audio.py new file mode 100644 index 0000000..f3c7eb1 --- /dev/null +++ b/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('>> 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 diff --git a/boot.py b/boot.py new file mode 100644 index 0000000..8c750fd --- /dev/null +++ b/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 diff --git a/c++/config.h b/c++/config.h new file mode 100644 index 0000000..c299812 --- /dev/null +++ b/c++/config.h @@ -0,0 +1,156 @@ +#ifndef _BOARD_CONFIG_H_ +#define _BOARD_CONFIG_H_ + +#include +#include + +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 diff --git a/c++/otto-robot_README.md b/c++/otto-robot_README.md new file mode 100644 index 0000000..fe31798 --- /dev/null +++ b/c++/otto-robot_README.md @@ -0,0 +1,206 @@ +

+ logo +

+

+ ottoRobot +

+ +## 简介 + +otto 机器人是一个开源的人形机器人平台，具有多种动作能力和互动功能。本项目基于 ESP32 实现了 otto 机器人的控制系统，并加入小智ai。 + +- 复刻教程 + +### 微信小程序控制 + +

+ 微信小程序二维码 +

+ +扫描上方二维码，使用微信小程序控制 Otto 机器人。 + +## 硬件 +- 立创开源 + +## 小智后台配置角色参考： + +> **我的身份**： +> 我是一个可爱的双足机器人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**: 动作名称（必填）
**steps**: 动作步数(1-100，默认3)
**speed**: 动作速度(100-3000，数值越小越快，默认700)
**direction**: 方向参数(1/-1/0，默认1，根据动作类型不同含义不同)
**amount**: 动作幅度(0-170，默认30)
**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)
**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。 + diff --git a/c++/otto-robot_config.h b/c++/otto-robot_config.h new file mode 100644 index 0000000..c299812 --- /dev/null +++ b/c++/otto-robot_config.h @@ -0,0 +1,156 @@ +#ifndef _BOARD_CONFIG_H_ +#define _BOARD_CONFIG_H_ + +#include +#include + +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 diff --git a/c++/otto-robot_config.json b/c++/otto-robot_config.json new file mode 100644 index 0000000..fae2541 --- /dev/null +++ b/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" + ] + } + ] +} \ No newline at end of file diff --git a/c++/otto-robot_oscillator.cc b/c++/otto-robot_oscillator.cc new file mode 100644 index 0000000..2cd9cba --- /dev/null +++ b/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 +#include + +#include +#include + +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_)); +} diff --git a/c++/otto-robot_oscillator.h b/c++/otto-robot_oscillator.h new file mode 100644 index 0000000..1478b4f --- /dev/null +++ b/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__ \ No newline at end of file diff --git a/c++/otto-robot_otto_controller.cc b/c++/otto-robot_otto_controller.cc new file mode 100644 index 0000000..522f294 --- /dev/null +++ b/c++/otto-robot_otto_controller.cc @@ -0,0 +1,846 @@ +/* + Otto机器人控制器 - MCP协议版本 +*/ + +#include +#include + +#include +#include + +#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 +#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(arg); + OttoActionParams params; + controller->otto_.AttachServos(); + + while (true) { + if (xQueueReceive(controller->action_queue_, ¶ms, 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_, ¶ms, 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_, ¶ms, 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(); + // 所有参数都有默认值，直接访问即可 + int steps = properties["steps"].value(); + int speed = properties["speed"].value(); + int direction = properties["direction"].value(); + int amount = properties["amount"].value(); + int arm_swing = properties["arm_swing"].value(); + + // 基础移动动作 + 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(); + // 检查是否是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(); + int trim_value = properties["trim_value"].value(); + + 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工具"); + } +} diff --git a/c++/otto-robot_otto_emoji_display.cc b/c++/otto-robot_otto_emoji_display.cc new file mode 100644 index 0000000..5906113 --- /dev/null +++ b/c++/otto-robot_otto_emoji_display.cc @@ -0,0 +1,165 @@ +#include "otto_emoji_display.h" + +#include + +#include + +#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(); + + // 中性/平静类表情 -> staticstate + otto_emoji_collection->AddEmoji("staticstate", std::make_shared((void*)staticstate.data, staticstate.data_size)); + otto_emoji_collection->AddEmoji("neutral", std::make_shared((void*)staticstate.data, staticstate.data_size)); + otto_emoji_collection->AddEmoji("relaxed", std::make_shared((void*)staticstate.data, staticstate.data_size)); + otto_emoji_collection->AddEmoji("sleepy", std::make_shared((void*)staticstate.data, staticstate.data_size)); + otto_emoji_collection->AddEmoji("idle", std::make_shared((void*)staticstate.data, staticstate.data_size)); + + // 积极/开心类表情 -> happy + otto_emoji_collection->AddEmoji("happy", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("laughing", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("funny", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("loving", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("confident", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("winking", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("cool", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("delicious", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("kissy", std::make_shared((void*)happy.data, happy.data_size)); + otto_emoji_collection->AddEmoji("silly", std::make_shared((void*)happy.data, happy.data_size)); + + // 悲伤类表情 -> sad + otto_emoji_collection->AddEmoji("sad", std::make_shared((void*)sad.data, sad.data_size)); + otto_emoji_collection->AddEmoji("crying", std::make_shared((void*)sad.data, sad.data_size)); + + // 愤怒类表情 -> anger + otto_emoji_collection->AddEmoji("anger", std::make_shared((void*)anger.data, anger.data_size)); + otto_emoji_collection->AddEmoji("angry", std::make_shared((void*)anger.data, anger.data_size)); + + // 惊讶类表情 -> scare + otto_emoji_collection->AddEmoji("scare", std::make_shared((void*)scare.data, scare.data_size)); + otto_emoji_collection->AddEmoji("surprised", std::make_shared((void*)scare.data, scare.data_size)); + otto_emoji_collection->AddEmoji("shocked", std::make_shared((void*)scare.data, scare.data_size)); + + // 思考/困惑类表情 -> buxue + otto_emoji_collection->AddEmoji("buxue", std::make_shared((void*)buxue.data, buxue.data_size)); + otto_emoji_collection->AddEmoji("thinking", std::make_shared((void*)buxue.data, buxue.data_size)); + otto_emoji_collection->AddEmoji("confused", std::make_shared((void*)buxue.data, buxue.data_size)); + otto_emoji_collection->AddEmoji("embarrassed", std::make_shared((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(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 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)); +} diff --git a/c++/otto-robot_otto_emoji_display.h b/c++/otto-robot_otto_emoji_display.h new file mode 100644 index 0000000..a7a3ddc --- /dev/null +++ b/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 image) override; + + private: + void InitializeOttoEmojis(); + void SetupPreviewImage(); +}; \ No newline at end of file diff --git a/c++/otto-robot_otto_icon_font.c b/c++/otto-robot_otto_icon_font.c new file mode 100644 index 0000000..f041116 --- /dev/null +++ b/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*/ diff --git a/c++/otto-robot_otto_movements.cc b/c++/otto-robot_otto_movements.cc new file mode 100644 index 0000000..da64b68 --- /dev/null +++ b/c++/otto-robot_otto_movements.cc @@ -0,0 +1,964 @@ +#include "otto_movements.h" + +#include + +#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(); + } + } +} diff --git a/c++/otto-robot_otto_movements.h b/c++/otto-robot_otto_movements.h new file mode 100644 index 0000000..c6994ea --- /dev/null +++ b/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__ \ No newline at end of file diff --git a/c++/otto-robot_otto_robot.cc b/c++/otto-robot_otto_robot.cc new file mode 100644 index 0000000..cf062fc --- /dev/null +++ b/c++/otto-robot_otto_robot.cc @@ -0,0 +1,341 @@ +#include +#include +#include +#include +#include +#include +#include +#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, ®_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); diff --git a/c++/otto-robot_power_manager.h b/c++/otto-robot_power_manager.h new file mode 100644 index 0000000..790bea0 --- /dev/null +++ b/c++/otto-robot_power_manager.h @@ -0,0 +1,149 @@ +#ifndef __POWER_MANAGER_H__ +#define __POWER_MANAGER_H__ + +#include +#include +#include +#include + +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(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(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__ \ No newline at end of file diff --git a/c++/otto-robot_websocket_control_server.cc b/c++/otto-robot_websocket_control_server.cc new file mode 100644 index 0000000..4bc2b66 --- /dev/null +++ b/c++/otto-robot_websocket_control_server.cc @@ -0,0 +1,191 @@ +#include "websocket_control_server.h" +#include "mcp_server.h" +#include +#include +#include +#include +#include +#include + +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(); +} diff --git a/c++/otto-robot_websocket_control_server.h b/c++/otto-robot_websocket_control_server.h new file mode 100644 index 0000000..3d5dc0a --- /dev/null +++ b/c++/otto-robot_websocket_control_server.h @@ -0,0 +1,33 @@ +#ifndef WEBSOCKET_CONTROL_SERVER_H +#define WEBSOCKET_CONTROL_SERVER_H + +#include +#include +#include +#include + +class WebSocketControlServer { +public: + WebSocketControlServer(); + ~WebSocketControlServer(); + + bool Start(int port = 8080); + + void Stop(); + + size_t GetClientCount() const; + +private: + httpd_handle_t server_handle_; + std::map 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 + diff --git a/config.py b/config.py new file mode 100644 index 0000000..cc0ef74 --- /dev/null +++ b/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 diff --git a/display.py b/display.py new file mode 100644 index 0000000..bf10b30 --- /dev/null +++ b/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 diff --git a/main.py b/main.py new file mode 100644 index 0000000..1786af0 --- /dev/null +++ b/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('> 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() diff --git a/st7789py.py b/st7789py.py new file mode 100644 index 0000000..d37301c --- /dev/null +++ b/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)