quant-speed-AI/V2_micropython
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

f

Browse Source
This commit is contained in:
jeremygan2021
2026-03-02 21:14:05 +08:00
commit 252a430466
26 changed files with 4591 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
c++/config.h Normal file
View File

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

206
c++/otto-robot_README.md Normal file
View File

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

156
c++/otto-robot_config.h Normal file
View File

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

14
c++/otto-robot_config.json Normal file
View File

@@ -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"
]
}
]
}

161
c++/otto-robot_oscillator.cc Normal file
View File

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

91
c++/otto-robot_oscillator.h Normal file
View File

@@ -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__

846
c++/otto-robot_otto_controller.cc Normal file
View File

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

165
c++/otto-robot_otto_emoji_display.cc Normal file
View File

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

23
c++/otto-robot_otto_emoji_display.h Normal file
View File

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

121
c++/otto-robot_otto_icon_font.c Normal file
View File

@@ -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*/

964
c++/otto-robot_otto_movements.cc Normal file
View File

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

113
c++/otto-robot_otto_movements.h Normal file
View File

@@ -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__

341
c++/otto-robot_otto_robot.cc Normal file
View File

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

149
c++/otto-robot_power_manager.h Normal file
View File

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

191
c++/otto-robot_websocket_control_server.cc Normal file
View File

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

33
c++/otto-robot_websocket_control_server.h Normal file
View File

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