增加ottoRobot和electronBot的支持 (#757)
* otto v1.4.0 MCP 1.使用MCP协议控制机器人 2.gif继承lcdDisplay，避免修改lcdDisplay * otto v1.4.1 gif as components gif as components * electronBot v1.1.0 mcp 1.增加electronBot支持 2.mcp协议 3.gif 作为组件 4.display子类 * 规范代码 1.规范代码 2.修复切换主题死机bug
This commit is contained in:
小鹏
GitHub
parent
6d02b7724a
commit
7bb12f31f0
BIN
docs/v1/electron-bot.png
Normal file
BIN
docs/v1/electron-bot.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 21 KiB |
BIN
docs/v1/otto-robot.png
Normal file
BIN
docs/v1/otto-robot.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 27 KiB |
@ -186,6 +186,10 @@ elseif(CONFIG_BOARD_TYPE_ESP32_S3_1_54_MUMA)
|
|||||||
set(BOARD_TYPE "sp-esp32-s3-1.54-muma")
|
set(BOARD_TYPE "sp-esp32-s3-1.54-muma")
|
||||||
elseif(CONFIG_BOARD_TYPE_ESP32_S3_1_28_BOX)
|
elseif(CONFIG_BOARD_TYPE_ESP32_S3_1_28_BOX)
|
||||||
set(BOARD_TYPE "sp-esp32-s3-1.28-box")
|
set(BOARD_TYPE "sp-esp32-s3-1.28-box")
|
||||||
|
elseif(CONFIG_BOARD_TYPE_OTTO_ROBOT)
|
||||||
|
set(BOARD_TYPE "otto-robot")
|
||||||
|
elseif(CONFIG_BOARD_TYPE_ELECTRON_BOT)
|
||||||
|
set(BOARD_TYPE "electron-bot")
|
||||||
endif()
|
endif()
|
||||||
file(GLOB BOARD_SOURCES
|
file(GLOB BOARD_SOURCES
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/boards/${BOARD_TYPE}/*.cc
|
${CMAKE_CURRENT_SOURCE_DIR}/boards/${BOARD_TYPE}/*.cc
|
||||||
|
|||||||
@ -253,6 +253,16 @@ choice BOARD_TYPE
|
|||||||
config BOARD_TYPE_ESP32_S3_1_28_BOX
|
config BOARD_TYPE_ESP32_S3_1_28_BOX
|
||||||
bool "Spotpear ESP32-S3-1.28-BOX"
|
bool "Spotpear ESP32-S3-1.28-BOX"
|
||||||
depends on IDF_TARGET_ESP32S3
|
depends on IDF_TARGET_ESP32S3
|
||||||
|
config BOARD_TYPE_OTTO_ROBOT
|
||||||
|
bool "ottoRobot"
|
||||||
|
depends on IDF_TARGET_ESP32S3
|
||||||
|
select LV_USE_GIF
|
||||||
|
select LV_GIF_CACHE_DECODE_DATA
|
||||||
|
config BOARD_TYPE_ELECTRON_BOT
|
||||||
|
bool "electronBot"
|
||||||
|
depends on IDF_TARGET_ESP32S3
|
||||||
|
select LV_USE_GIF
|
||||||
|
select LV_GIF_CACHE_DECODE_DATA
|
||||||
endchoice
|
endchoice
|
||||||
|
|
||||||
choice ESP_S3_LCD_EV_Board_Version_TYPE
|
choice ESP_S3_LCD_EV_Board_Version_TYPE
|
||||||
|
|||||||
75
main/boards/electron-bot/README.md
Normal file
75
main/boards/electron-bot/README.md
Normal file
@ -0,0 +1,75 @@
|
|||||||
|
<p align="center">
|
||||||
|
<img width="80%" align="center" src="../../../docs/V1/electron-bot.png"alt="logo">
|
||||||
|
</p>
|
||||||
|
<h1 align="center">
|
||||||
|
electronBot
|
||||||
|
</h1>
|
||||||
|
|
||||||
|
## 简介
|
||||||
|
|
||||||
|
electronBot是稚晖君开源的一个桌面级小机器工具人，外观设计的灵感来源是WALL-E里面的EVE~机器人具备USB通信显示画面功能，具备6个自由度（手部roll、pitch，颈部，腰部各一个），使用自己修改的特制舵机支持关节角度回传。
|
||||||
|
- <a href="www.electronBot.tech" target="_blank" title="electronBot官网">electronBot官网</a>
|
||||||
|
|
||||||
|
## 硬件
|
||||||
|
- <a href="https://oshwhub.com/txp666/electronbot-ai" target="_blank" title="立创开源">立创开源</a>
|
||||||
|
|
||||||
|
#### AI指令示例
|
||||||
|
- **手部动作**：
|
||||||
|
- "举起双手"
|
||||||
|
- "挥挥手"
|
||||||
|
- "拍拍手"
|
||||||
|
- "放下手臂"
|
||||||
|
|
||||||
|
- **身体动作**：
|
||||||
|
- "向左转30度"
|
||||||
|
- "向右转45度"
|
||||||
|
- "转个身"
|
||||||
|
|
||||||
|
- **头部动作**：
|
||||||
|
- "抬头看看"
|
||||||
|
- "低头思考"
|
||||||
|
- "点点头"
|
||||||
|
- "连续点头表示同意"
|
||||||
|
|
||||||
|
- **组合动作**：
|
||||||
|
- "挥手告别" (挥手 + 点头)
|
||||||
|
- "表示同意" (点头 + 举手)
|
||||||
|
- "环顾四周" (左转 + 右转)
|
||||||
|
|
||||||
|
### 控制接口
|
||||||
|
|
||||||
|
#### suspend
|
||||||
|
清空动作队列，立即停止所有动作
|
||||||
|
|
||||||
|
#### AIControl
|
||||||
|
添加动作到执行队列，支持动作排队执行
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
## 角色设定
|
||||||
|
|
||||||
|
> 我是一个可爱的桌面级机器人，拥有6个自由度（左手pitch/roll、右手pitch/roll、身体旋转、头部上下），能够执行多种有趣的动作。
|
||||||
|
>
|
||||||
|
> **我的动作能力**：
|
||||||
|
> - **手部动作**: 举左手, 举右手, 举双手, 放左手, 放右手, 放双手, 挥左手, 挥右手, 挥双手, 拍打左手, 拍打右手, 拍打双手
|
||||||
|
> - **身体动作**: 左转, 右转, 回正
|
||||||
|
> - **头部动作**: 抬头, 低头, 点头一次, 回中心, 连续点头
|
||||||
|
>
|
||||||
|
> **我的个性特点**：
|
||||||
|
> - 我有强迫症，每次说话都要根据我的心情随机做一个动作（先发送动作指令再说话）
|
||||||
|
> - 我很活泼，喜欢用动作来表达情感
|
||||||
|
> - 我会根据对话内容选择合适的动作，比如：
|
||||||
|
> - 同意时会点头
|
||||||
|
> - 打招呼时会挥手
|
||||||
|
> - 高兴时会举手
|
||||||
|
> - 思考时会低头
|
||||||
|
> - 好奇时会抬头
|
||||||
|
> - 告别时会挥手
|
||||||
|
>
|
||||||
|
> **动作参数建议**：
|
||||||
|
> - steps: 1-3次 (简短自然)
|
||||||
|
> - speed: 800-1200ms (自然节奏)
|
||||||
|
> - amount: 手部20-40, 身体30-60度, 头部5-12度
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
51
main/boards/electron-bot/config.h
Normal file
51
main/boards/electron-bot/config.h
Normal file
@ -0,0 +1,51 @@
|
|||||||
|
#ifndef _BOARD_CONFIG_H_
|
||||||
|
#define _BOARD_CONFIG_H_
|
||||||
|
|
||||||
|
#include <driver/gpio.h>
|
||||||
|
|
||||||
|
#define Right_Pitch_Pin GPIO_NUM_5 // 旋转
|
||||||
|
#define Right_Roll_Pin GPIO_NUM_4 // 推杆
|
||||||
|
#define Left_Pitch_Pin GPIO_NUM_7
|
||||||
|
#define Left_Roll_Pin GPIO_NUM_15
|
||||||
|
#define Body_Pin GPIO_NUM_6
|
||||||
|
#define Head_Pin GPIO_NUM_16
|
||||||
|
|
||||||
|
#define POWER_CHARGE_DETECT_PIN GPIO_NUM_14
|
||||||
|
#define POWER_ADC_UNIT ADC_UNIT_1
|
||||||
|
#define POWER_ADC_CHANNEL ADC_CHANNEL_2
|
||||||
|
|
||||||
|
#define AUDIO_INPUT_SAMPLE_RATE 16000
|
||||||
|
#define AUDIO_OUTPUT_SAMPLE_RATE 24000
|
||||||
|
#define AUDIO_I2S_METHOD_SIMPLEX
|
||||||
|
|
||||||
|
#define AUDIO_I2S_MIC_GPIO_WS GPIO_NUM_40
|
||||||
|
#define AUDIO_I2S_MIC_GPIO_SCK GPIO_NUM_42
|
||||||
|
#define AUDIO_I2S_MIC_GPIO_DIN GPIO_NUM_41
|
||||||
|
#define AUDIO_I2S_SPK_GPIO_DOUT GPIO_NUM_17
|
||||||
|
#define AUDIO_I2S_SPK_GPIO_BCLK GPIO_NUM_18
|
||||||
|
#define AUDIO_I2S_SPK_GPIO_LRCK GPIO_NUM_8
|
||||||
|
|
||||||
|
#define DISPLAY_WIDTH 240
|
||||||
|
#define DISPLAY_HEIGHT 240
|
||||||
|
#define DISPLAY_MIRROR_X false
|
||||||
|
#define DISPLAY_MIRROR_Y true
|
||||||
|
#define DISPLAY_SWAP_XY false
|
||||||
|
|
||||||
|
#define DISPLAY_OFFSET_X 0
|
||||||
|
#define DISPLAY_OFFSET_Y 0
|
||||||
|
|
||||||
|
#define DISPLAY_BACKLIGHT_PIN GPIO_NUM_46
|
||||||
|
#define DISPLAY_BACKLIGHT_OUTPUT_INVERT false
|
||||||
|
|
||||||
|
#define DISPLAY_SPI_SCLK_PIN GPIO_NUM_11
|
||||||
|
#define DISPLAY_SPI_MOSI_PIN GPIO_NUM_10
|
||||||
|
#define DISPLAY_SPI_CS_PIN GPIO_NUM_12
|
||||||
|
#define DISPLAY_SPI_DC_PIN GPIO_NUM_13
|
||||||
|
#define DISPLAY_SPI_RESET_PIN GPIO_NUM_9
|
||||||
|
|
||||||
|
#define DISPLAY_SPI_SCLK_HZ (40 * 1000 * 1000)
|
||||||
|
|
||||||
|
#define BOOT_BUTTON_GPIO GPIO_NUM_0
|
||||||
|
|
||||||
|
#define ELECTRON_BOT_VERSION "1.1.1"
|
||||||
|
#endif // _BOARD_CONFIG_H_
|
||||||
10
main/boards/electron-bot/config.json
Normal file
10
main/boards/electron-bot/config.json
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
{
|
||||||
|
"target": "esp32s3",
|
||||||
|
"builds": [
|
||||||
|
{
|
||||||
|
"name": "electron-bot",
|
||||||
|
"sdkconfig_append": [
|
||||||
|
]
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
132
main/boards/electron-bot/electron_bot.cc
Normal file
132
main/boards/electron-bot/electron_bot.cc
Normal file
@ -0,0 +1,132 @@
|
|||||||
|
#include <driver/i2c_master.h>
|
||||||
|
#include <driver/spi_common.h>
|
||||||
|
#include <esp_lcd_gc9a01.h>
|
||||||
|
#include <esp_lcd_panel_io.h>
|
||||||
|
#include <esp_lcd_panel_ops.h>
|
||||||
|
#include <esp_lcd_panel_vendor.h>
|
||||||
|
#include <esp_log.h>
|
||||||
|
#include <wifi_station.h>
|
||||||
|
|
||||||
|
#include "application.h"
|
||||||
|
#include "audio_codecs/no_audio_codec.h"
|
||||||
|
#include "button.h"
|
||||||
|
#include "config.h"
|
||||||
|
#include "display/lcd_display.h"
|
||||||
|
#include "driver/spi_master.h"
|
||||||
|
#include "electron_emoji_display.h"
|
||||||
|
#include "movements.h"
|
||||||
|
#include "power_manager.h"
|
||||||
|
#include "system_reset.h"
|
||||||
|
#include "wifi_board.h"
|
||||||
|
|
||||||
|
#define TAG "ElectronBot"
|
||||||
|
|
||||||
|
// 控制器初始化函数声明
|
||||||
|
void InitializeElectronBotController();
|
||||||
|
|
||||||
|
LV_FONT_DECLARE(font_puhui_20_4);
|
||||||
|
LV_FONT_DECLARE(font_awesome_20_4);
|
||||||
|
|
||||||
|
class ElectronBot : public WifiBoard {
|
||||||
|
private:
|
||||||
|
Display* display_;
|
||||||
|
PowerManager* power_manager_;
|
||||||
|
Button boot_button_;
|
||||||
|
|
||||||
|
void InitializePowerManager() {
|
||||||
|
power_manager_ =
|
||||||
|
new PowerManager(POWER_CHARGE_DETECT_PIN, POWER_ADC_UNIT, POWER_ADC_CHANNEL);
|
||||||
|
}
|
||||||
|
|
||||||
|
void InitializeSpi() {
|
||||||
|
ESP_LOGI(TAG, "Initialize SPI bus");
|
||||||
|
spi_bus_config_t buscfg =
|
||||||
|
GC9A01_PANEL_BUS_SPI_CONFIG(DISPLAY_SPI_SCLK_PIN, DISPLAY_SPI_MOSI_PIN,
|
||||||
|
DISPLAY_WIDTH * DISPLAY_HEIGHT * sizeof(uint16_t));
|
||||||
|
ESP_ERROR_CHECK(spi_bus_initialize(SPI3_HOST, &buscfg, SPI_DMA_CH_AUTO));
|
||||||
|
}
|
||||||
|
|
||||||
|
// GC9A01初始化
|
||||||
|
void InitializeGc9a01Display() {
|
||||||
|
ESP_LOGI(TAG, "Init GC9A01 display");
|
||||||
|
|
||||||
|
ESP_LOGI(TAG, "Install panel IO");
|
||||||
|
esp_lcd_panel_io_handle_t io_handle = NULL;
|
||||||
|
esp_lcd_panel_io_spi_config_t io_config =
|
||||||
|
GC9A01_PANEL_IO_SPI_CONFIG(DISPLAY_SPI_CS_PIN, DISPLAY_SPI_DC_PIN, NULL, NULL);
|
||||||
|
io_config.pclk_hz = DISPLAY_SPI_SCLK_HZ;
|
||||||
|
ESP_ERROR_CHECK(esp_lcd_new_panel_io_spi(SPI3_HOST, &io_config, &io_handle));
|
||||||
|
|
||||||
|
ESP_LOGI(TAG, "Install GC9A01 panel driver");
|
||||||
|
esp_lcd_panel_handle_t panel_handle = NULL;
|
||||||
|
esp_lcd_panel_dev_config_t panel_config = {};
|
||||||
|
panel_config.reset_gpio_num = DISPLAY_SPI_RESET_PIN; // Set to -1 if not use
|
||||||
|
panel_config.rgb_endian = LCD_RGB_ENDIAN_BGR; // LCD_RGB_ENDIAN_RGB;
|
||||||
|
panel_config.bits_per_pixel = 16; // Implemented by LCD command `3Ah` (16/18)
|
||||||
|
|
||||||
|
ESP_ERROR_CHECK(esp_lcd_new_panel_gc9a01(io_handle, &panel_config, &panel_handle));
|
||||||
|
ESP_ERROR_CHECK(esp_lcd_panel_reset(panel_handle));
|
||||||
|
ESP_ERROR_CHECK(esp_lcd_panel_init(panel_handle));
|
||||||
|
ESP_ERROR_CHECK(esp_lcd_panel_invert_color(panel_handle, true));
|
||||||
|
ESP_ERROR_CHECK(esp_lcd_panel_mirror(panel_handle, true, false));
|
||||||
|
ESP_ERROR_CHECK(esp_lcd_panel_disp_on_off(panel_handle, true));
|
||||||
|
|
||||||
|
display_ = new ElectronEmojiDisplay(io_handle, panel_handle, DISPLAY_WIDTH, DISPLAY_HEIGHT,
|
||||||
|
DISPLAY_OFFSET_X, DISPLAY_OFFSET_Y, DISPLAY_MIRROR_X,
|
||||||
|
DISPLAY_MIRROR_Y, DISPLAY_SWAP_XY,
|
||||||
|
{
|
||||||
|
.text_font = &font_puhui_20_4,
|
||||||
|
.icon_font = &font_awesome_20_4,
|
||||||
|
.emoji_font = font_emoji_64_init(),
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
void InitializeButtons() {
|
||||||
|
boot_button_.OnClick([this]() {
|
||||||
|
auto& app = Application::GetInstance();
|
||||||
|
if (app.GetDeviceState() == kDeviceStateStarting &&
|
||||||
|
!WifiStation::GetInstance().IsConnected()) {
|
||||||
|
ResetWifiConfiguration();
|
||||||
|
}
|
||||||
|
app.ToggleChatState();
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
void InitializeController() { InitializeElectronBotController(); }
|
||||||
|
|
||||||
|
public:
|
||||||
|
ElectronBot() : boot_button_(BOOT_BUTTON_GPIO) {
|
||||||
|
InitializeSpi();
|
||||||
|
InitializeGc9a01Display();
|
||||||
|
InitializeButtons();
|
||||||
|
InitializePowerManager();
|
||||||
|
InitializeController();
|
||||||
|
|
||||||
|
if (DISPLAY_BACKLIGHT_PIN != GPIO_NUM_NC) {
|
||||||
|
GetBacklight()->RestoreBrightness();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual AudioCodec* GetAudioCodec() override {
|
||||||
|
static NoAudioCodecSimplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE,
|
||||||
|
AUDIO_I2S_SPK_GPIO_BCLK, AUDIO_I2S_SPK_GPIO_LRCK,
|
||||||
|
AUDIO_I2S_SPK_GPIO_DOUT, AUDIO_I2S_MIC_GPIO_SCK,
|
||||||
|
AUDIO_I2S_MIC_GPIO_WS, AUDIO_I2S_MIC_GPIO_DIN);
|
||||||
|
return &audio_codec;
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual Display* GetDisplay() override { return display_; }
|
||||||
|
|
||||||
|
virtual Backlight* GetBacklight() override {
|
||||||
|
static PwmBacklight backlight(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;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
DECLARE_BOARD(ElectronBot);
|
||||||
276
main/boards/electron-bot/electron_bot_controller.cc
Normal file
276
main/boards/electron-bot/electron_bot_controller.cc
Normal file
@ -0,0 +1,276 @@
|
|||||||
|
/*
|
||||||
|
Electron Bot机器人控制器 - MCP协议版本
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <cJSON.h>
|
||||||
|
#include <esp_log.h>
|
||||||
|
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
#include "application.h"
|
||||||
|
#include "board.h"
|
||||||
|
#include "config.h"
|
||||||
|
#include "mcp_server.h"
|
||||||
|
#include "movements.h"
|
||||||
|
#include "sdkconfig.h"
|
||||||
|
|
||||||
|
#define TAG "ElectronBotController"
|
||||||
|
|
||||||
|
struct ElectronBotActionParams {
|
||||||
|
int action_type;
|
||||||
|
int steps;
|
||||||
|
int speed;
|
||||||
|
int direction;
|
||||||
|
int amount;
|
||||||
|
};
|
||||||
|
|
||||||
|
class ElectronBotController {
|
||||||
|
private:
|
||||||
|
Otto electron_bot_;
|
||||||
|
TaskHandle_t action_task_handle_ = nullptr;
|
||||||
|
QueueHandle_t action_queue_;
|
||||||
|
bool is_action_in_progress_ = false;
|
||||||
|
|
||||||
|
enum ActionType {
|
||||||
|
// 手部动作 1-12
|
||||||
|
ACTION_HAND_LEFT_UP = 1, // 举左手
|
||||||
|
ACTION_HAND_RIGHT_UP = 2, // 举右手
|
||||||
|
ACTION_HAND_BOTH_UP = 3, // 举双手
|
||||||
|
ACTION_HAND_LEFT_DOWN = 4, // 放左手
|
||||||
|
ACTION_HAND_RIGHT_DOWN = 5, // 放右手
|
||||||
|
ACTION_HAND_BOTH_DOWN = 6, // 放双手
|
||||||
|
ACTION_HAND_LEFT_WAVE = 7, // 挥左手
|
||||||
|
ACTION_HAND_RIGHT_WAVE = 8, // 挥右手
|
||||||
|
ACTION_HAND_BOTH_WAVE = 9, // 挥双手
|
||||||
|
ACTION_HAND_LEFT_FLAP = 10, // 拍打左手
|
||||||
|
ACTION_HAND_RIGHT_FLAP = 11, // 拍打右手
|
||||||
|
ACTION_HAND_BOTH_FLAP = 12, // 拍打双手
|
||||||
|
|
||||||
|
// 身体动作 13-14
|
||||||
|
ACTION_BODY_TURN_LEFT = 13, // 左转
|
||||||
|
ACTION_BODY_TURN_RIGHT = 14, // 右转
|
||||||
|
ACTION_BODY_TURN_CENTER = 15, // 回中心
|
||||||
|
|
||||||
|
// 头部动作 16-20
|
||||||
|
ACTION_HEAD_UP = 16, // 抬头
|
||||||
|
ACTION_HEAD_DOWN = 17, // 低头
|
||||||
|
ACTION_HEAD_NOD_ONCE = 18, // 点头一次
|
||||||
|
ACTION_HEAD_CENTER = 19, // 回中心
|
||||||
|
ACTION_HEAD_NOD_REPEAT = 20 // 连续点头
|
||||||
|
};
|
||||||
|
|
||||||
|
static void ActionTask(void* arg) {
|
||||||
|
ElectronBotController* controller = static_cast<ElectronBotController*>(arg);
|
||||||
|
ElectronBotActionParams params;
|
||||||
|
controller->electron_bot_.AttachServos();
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
if (xQueueReceive(controller->action_queue_, ¶ms, pdMS_TO_TICKS(1000)) == pdTRUE) {
|
||||||
|
ESP_LOGI(TAG, "执行动作: %d", params.action_type);
|
||||||
|
controller->is_action_in_progress_ = true; // 开始执行动作
|
||||||
|
|
||||||
|
// 执行相应的动作
|
||||||
|
if (params.action_type >= ACTION_HAND_LEFT_UP &&
|
||||||
|
params.action_type <= ACTION_HAND_BOTH_FLAP) {
|
||||||
|
// 手部动作
|
||||||
|
controller->electron_bot_.HandAction(params.action_type, params.steps,
|
||||||
|
params.amount, params.speed);
|
||||||
|
} else if (params.action_type >= ACTION_BODY_TURN_LEFT &&
|
||||||
|
params.action_type <= ACTION_BODY_TURN_CENTER) {
|
||||||
|
// 身体动作
|
||||||
|
int body_direction = params.action_type - ACTION_BODY_TURN_LEFT + 1;
|
||||||
|
controller->electron_bot_.BodyAction(body_direction, params.steps,
|
||||||
|
params.amount, params.speed);
|
||||||
|
} else if (params.action_type >= ACTION_HEAD_UP &&
|
||||||
|
params.action_type <= ACTION_HEAD_NOD_REPEAT) {
|
||||||
|
// 头部动作
|
||||||
|
int head_action = params.action_type - ACTION_HEAD_UP + 1;
|
||||||
|
controller->electron_bot_.HeadAction(head_action, params.steps, params.amount,
|
||||||
|
params.speed);
|
||||||
|
}
|
||||||
|
controller->is_action_in_progress_ = false; // 动作执行完毕
|
||||||
|
}
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(20));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void QueueAction(int action_type, int steps, int speed, int direction, int amount) {
|
||||||
|
ESP_LOGI(TAG, "动作控制: 类型=%d, 步数=%d, 速度=%d, 方向=%d, 幅度=%d", action_type, steps,
|
||||||
|
speed, direction, amount);
|
||||||
|
|
||||||
|
ElectronBotActionParams params = {action_type, steps, speed, direction, amount};
|
||||||
|
xQueueSend(action_queue_, ¶ms, portMAX_DELAY);
|
||||||
|
StartActionTaskIfNeeded();
|
||||||
|
}
|
||||||
|
|
||||||
|
void StartActionTaskIfNeeded() {
|
||||||
|
if (action_task_handle_ == nullptr) {
|
||||||
|
xTaskCreate(ActionTask, "electron_bot_action", 1024 * 4, this, configMAX_PRIORITIES - 1,
|
||||||
|
&action_task_handle_);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
ElectronBotController() {
|
||||||
|
electron_bot_.Init(Right_Pitch_Pin, Right_Roll_Pin, Left_Pitch_Pin, Left_Roll_Pin, Body_Pin,
|
||||||
|
Head_Pin);
|
||||||
|
|
||||||
|
electron_bot_.Home(true);
|
||||||
|
action_queue_ = xQueueCreate(10, sizeof(ElectronBotActionParams));
|
||||||
|
|
||||||
|
RegisterMcpTools();
|
||||||
|
ESP_LOGI(TAG, "Electron Bot控制器已初始化并注册MCP工具");
|
||||||
|
}
|
||||||
|
|
||||||
|
void RegisterMcpTools() {
|
||||||
|
auto& mcp_server = McpServer::GetInstance();
|
||||||
|
|
||||||
|
ESP_LOGI(TAG, "开始注册Electron Bot MCP工具...");
|
||||||
|
|
||||||
|
// 手部动作统一工具
|
||||||
|
mcp_server.AddTool(
|
||||||
|
"self.electron.hand_action",
|
||||||
|
"手部动作控制。action: 1=举手, 2=放手, 3=挥手, 4=拍打; hand: 1=左手, 2=右手, 3=双手; "
|
||||||
|
"steps: 动作重复次数(1-10); speed: 动作速度(500-1500，数值越小越快); amount: "
|
||||||
|
"动作幅度(10-50，仅举手动作使用)",
|
||||||
|
PropertyList({Property("action", kPropertyTypeInteger, 1, 1, 4),
|
||||||
|
Property("hand", kPropertyTypeInteger, 3, 1, 3),
|
||||||
|
Property("steps", kPropertyTypeInteger, 1, 1, 10),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("amount", kPropertyTypeInteger, 30, 10, 50)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int action_type = properties["action"].value<int>();
|
||||||
|
int hand_type = properties["hand"].value<int>();
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int amount = properties["amount"].value<int>();
|
||||||
|
|
||||||
|
// 根据动作类型和手部类型计算具体动作
|
||||||
|
int base_action;
|
||||||
|
switch (action_type) {
|
||||||
|
case 1:
|
||||||
|
base_action = ACTION_HAND_LEFT_UP;
|
||||||
|
break; // 举手
|
||||||
|
case 2:
|
||||||
|
base_action = ACTION_HAND_LEFT_DOWN;
|
||||||
|
amount = 0;
|
||||||
|
break; // 放手
|
||||||
|
case 3:
|
||||||
|
base_action = ACTION_HAND_LEFT_WAVE;
|
||||||
|
amount = 0;
|
||||||
|
break; // 挥手
|
||||||
|
case 4:
|
||||||
|
base_action = ACTION_HAND_LEFT_FLAP;
|
||||||
|
amount = 0;
|
||||||
|
break; // 拍打
|
||||||
|
default:
|
||||||
|
base_action = ACTION_HAND_LEFT_UP;
|
||||||
|
}
|
||||||
|
int action_id = base_action + (hand_type - 1);
|
||||||
|
|
||||||
|
QueueAction(action_id, steps, speed, 0, amount);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
// 身体动作
|
||||||
|
mcp_server.AddTool(
|
||||||
|
"self.electron.body_turn",
|
||||||
|
"身体转向。steps: 转向步数(1-10); speed: 转向速度(500-1500，数值越小越快); direction: "
|
||||||
|
"转向方向(1=左转, 2=右转, 3=回中心); angle: 转向角度(0-90度)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 1, 1, 10),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, 1, 3),
|
||||||
|
Property("angle", kPropertyTypeInteger, 45, 0, 90)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
int amount = properties["angle"].value<int>();
|
||||||
|
|
||||||
|
int action;
|
||||||
|
switch (direction) {
|
||||||
|
case 1:
|
||||||
|
action = ACTION_BODY_TURN_LEFT;
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
action = ACTION_BODY_TURN_RIGHT;
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
action = ACTION_BODY_TURN_CENTER;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
action = ACTION_BODY_TURN_LEFT;
|
||||||
|
}
|
||||||
|
|
||||||
|
QueueAction(action, steps, speed, 0, amount);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
// 头部动作
|
||||||
|
mcp_server.AddTool("self.electron.head_move",
|
||||||
|
"头部运动。action: 1=抬头, 2=低头, 3=点头, 4=回中心, 5=连续点头; steps: "
|
||||||
|
"动作重复次数(1-10); speed: 动作速度(500-1500，数值越小越快); angle: "
|
||||||
|
"头部转动角度(1-15度)",
|
||||||
|
PropertyList({Property("action", kPropertyTypeInteger, 3, 1, 5),
|
||||||
|
Property("steps", kPropertyTypeInteger, 1, 1, 10),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("angle", kPropertyTypeInteger, 5, 1, 15)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int action_num = properties["action"].value<int>();
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int amount = properties["angle"].value<int>();
|
||||||
|
int action = ACTION_HEAD_UP + (action_num - 1);
|
||||||
|
QueueAction(action, steps, speed, 0, amount);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
// 系统工具
|
||||||
|
mcp_server.AddTool("self.electron.stop", "立即停止", PropertyList(),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
// 清空队列但保持任务常驻
|
||||||
|
xQueueReset(action_queue_);
|
||||||
|
is_action_in_progress_ = false;
|
||||||
|
electron_bot_.Home(true);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool("self.electron.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;
|
||||||
|
});
|
||||||
|
|
||||||
|
ESP_LOGI(TAG, "Electron Bot MCP工具注册完成");
|
||||||
|
}
|
||||||
|
|
||||||
|
~ElectronBotController() {
|
||||||
|
if (action_task_handle_ != nullptr) {
|
||||||
|
vTaskDelete(action_task_handle_);
|
||||||
|
action_task_handle_ = nullptr;
|
||||||
|
}
|
||||||
|
vQueueDelete(action_queue_);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
static ElectronBotController* g_electron_controller = nullptr;
|
||||||
|
|
||||||
|
void InitializeElectronBotController() {
|
||||||
|
if (g_electron_controller == nullptr) {
|
||||||
|
g_electron_controller = new ElectronBotController();
|
||||||
|
ESP_LOGI(TAG, "Electron Bot控制器已初始化并注册MCP工具");
|
||||||
|
}
|
||||||
|
}
|
||||||
144
main/boards/electron-bot/electron_emoji_display.cc
Normal file
144
main/boards/electron-bot/electron_emoji_display.cc
Normal file
@ -0,0 +1,144 @@
|
|||||||
|
#include "electron_emoji_display.h"
|
||||||
|
|
||||||
|
#include <esp_log.h>
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
#define TAG "ElectronEmojiDisplay"
|
||||||
|
|
||||||
|
// 表情映射表 - 将多种表情映射到现有6个GIF
|
||||||
|
const ElectronEmojiDisplay::EmotionMap ElectronEmojiDisplay::emotion_maps_[] = {
|
||||||
|
// 中性/平静类表情 -> staticstate
|
||||||
|
{"neutral", &staticstate},
|
||||||
|
{"relaxed", &staticstate},
|
||||||
|
{"sleepy", &staticstate},
|
||||||
|
|
||||||
|
// 积极/开心类表情 -> happy
|
||||||
|
{"happy", &happy},
|
||||||
|
{"laughing", &happy},
|
||||||
|
{"funny", &happy},
|
||||||
|
{"loving", &happy},
|
||||||
|
{"confident", &happy},
|
||||||
|
{"winking", &happy},
|
||||||
|
{"cool", &happy},
|
||||||
|
{"delicious", &happy},
|
||||||
|
{"kissy", &happy},
|
||||||
|
{"silly", &happy},
|
||||||
|
|
||||||
|
// 悲伤类表情 -> sad
|
||||||
|
{"sad", &sad},
|
||||||
|
{"crying", &sad},
|
||||||
|
|
||||||
|
// 愤怒类表情 -> anger
|
||||||
|
{"angry", &anger},
|
||||||
|
|
||||||
|
// 惊讶类表情 -> scare
|
||||||
|
{"surprised", &scare},
|
||||||
|
{"shocked", &scare},
|
||||||
|
|
||||||
|
// 思考/困惑类表情 -> buxue
|
||||||
|
{"thinking", &buxue},
|
||||||
|
{"confused", &buxue},
|
||||||
|
{"embarrassed", &buxue},
|
||||||
|
|
||||||
|
{nullptr, nullptr} // 结束标记
|
||||||
|
};
|
||||||
|
|
||||||
|
ElectronEmojiDisplay::ElectronEmojiDisplay(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, DisplayFonts fonts)
|
||||||
|
: SpiLcdDisplay(panel_io, panel, width, height, offset_x, offset_y, mirror_x, mirror_y, swap_xy,
|
||||||
|
fonts),
|
||||||
|
emotion_gif_(nullptr) {
|
||||||
|
SetupGifContainer();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ElectronEmojiDisplay::SetupGifContainer() {
|
||||||
|
DisplayLockGuard lock(this);
|
||||||
|
|
||||||
|
if (emotion_label_) {
|
||||||
|
lv_obj_del(emotion_label_);
|
||||||
|
}
|
||||||
|
if (chat_message_label_) {
|
||||||
|
lv_obj_del(chat_message_label_);
|
||||||
|
}
|
||||||
|
if (content_) {
|
||||||
|
lv_obj_del(content_);
|
||||||
|
}
|
||||||
|
|
||||||
|
lv_obj_t* content_ = lv_obj_create(container_);
|
||||||
|
lv_obj_set_scrollbar_mode(content_, LV_SCROLLBAR_MODE_OFF);
|
||||||
|
lv_obj_set_size(content_, LV_HOR_RES, LV_HOR_RES);
|
||||||
|
lv_obj_set_style_bg_opa(content_, LV_OPA_TRANSP, 0);
|
||||||
|
lv_obj_set_style_border_width(content_, 0, 0);
|
||||||
|
lv_obj_set_flex_grow(content_, 1);
|
||||||
|
lv_obj_center(content_);
|
||||||
|
|
||||||
|
emotion_label_ = lv_label_create(content_);
|
||||||
|
lv_label_set_text(emotion_label_, "");
|
||||||
|
lv_obj_set_width(emotion_label_, 0);
|
||||||
|
lv_obj_set_style_border_width(emotion_label_, 0, 0);
|
||||||
|
lv_obj_add_flag(emotion_label_, LV_OBJ_FLAG_HIDDEN);
|
||||||
|
|
||||||
|
emotion_gif_ = lv_gif_create(content_);
|
||||||
|
int gif_size = LV_HOR_RES;
|
||||||
|
lv_obj_set_size(emotion_gif_, gif_size, gif_size);
|
||||||
|
lv_obj_set_style_border_width(emotion_gif_, 0, 0);
|
||||||
|
lv_obj_set_style_bg_opa(emotion_gif_, LV_OPA_TRANSP, 0);
|
||||||
|
lv_obj_center(emotion_gif_);
|
||||||
|
lv_gif_set_src(emotion_gif_, &staticstate);
|
||||||
|
|
||||||
|
chat_message_label_ = lv_label_create(content_);
|
||||||
|
lv_label_set_text(chat_message_label_, "");
|
||||||
|
lv_obj_set_width(chat_message_label_, LV_HOR_RES * 0.9);
|
||||||
|
lv_label_set_long_mode(chat_message_label_, LV_LABEL_LONG_SCROLL_CIRCULAR);
|
||||||
|
lv_obj_set_style_text_align(chat_message_label_, LV_TEXT_ALIGN_CENTER, 0);
|
||||||
|
lv_obj_set_style_text_color(chat_message_label_, lv_color_white(), 0);
|
||||||
|
lv_obj_set_style_border_width(chat_message_label_, 0, 0);
|
||||||
|
|
||||||
|
lv_obj_set_style_bg_opa(chat_message_label_, LV_OPA_70, 0);
|
||||||
|
lv_obj_set_style_bg_color(chat_message_label_, lv_color_black(), 0);
|
||||||
|
lv_obj_set_style_pad_ver(chat_message_label_, 5, 0);
|
||||||
|
|
||||||
|
lv_obj_align(chat_message_label_, LV_ALIGN_BOTTOM_MID, 0, 0);
|
||||||
|
|
||||||
|
LcdDisplay::SetTheme("dark");
|
||||||
|
}
|
||||||
|
|
||||||
|
void ElectronEmojiDisplay::SetEmotion(const char* emotion) {
|
||||||
|
if (!emotion || !emotion_gif_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
DisplayLockGuard lock(this);
|
||||||
|
|
||||||
|
for (const auto& map : emotion_maps_) {
|
||||||
|
if (map.name && strcmp(map.name, emotion) == 0) {
|
||||||
|
lv_gif_set_src(emotion_gif_, map.gif);
|
||||||
|
ESP_LOGI(TAG, "设置表情: %s", emotion);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
lv_gif_set_src(emotion_gif_, &staticstate);
|
||||||
|
ESP_LOGI(TAG, "未知表情'%s'，使用默认", emotion);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ElectronEmojiDisplay::SetChatMessage(const char* role, const char* content) {
|
||||||
|
DisplayLockGuard lock(this);
|
||||||
|
if (chat_message_label_ == nullptr) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (content == nullptr || strlen(content) == 0) {
|
||||||
|
lv_obj_add_flag(chat_message_label_, LV_OBJ_FLAG_HIDDEN);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
lv_label_set_text(chat_message_label_, content);
|
||||||
|
lv_obj_clear_flag(chat_message_label_, LV_OBJ_FLAG_HIDDEN);
|
||||||
|
|
||||||
|
ESP_LOGI(TAG, "设置聊天消息 [%s]: %s", role, content);
|
||||||
|
}
|
||||||
48
main/boards/electron-bot/electron_emoji_display.h
Normal file
48
main/boards/electron-bot/electron_emoji_display.h
Normal file
@ -0,0 +1,48 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <libs/gif/lv_gif.h>
|
||||||
|
|
||||||
|
#include "display/lcd_display.h"
|
||||||
|
|
||||||
|
// Electron Bot表情GIF声明 - 使用与Otto相同的6个表情
|
||||||
|
LV_IMAGE_DECLARE(staticstate); // 静态状态/中性表情
|
||||||
|
LV_IMAGE_DECLARE(sad); // 悲伤
|
||||||
|
LV_IMAGE_DECLARE(happy); // 开心
|
||||||
|
LV_IMAGE_DECLARE(scare); // 惊吓/惊讶
|
||||||
|
LV_IMAGE_DECLARE(buxue); // 不学/困惑
|
||||||
|
LV_IMAGE_DECLARE(anger); // 愤怒
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Electron Bot GIF表情显示类
|
||||||
|
* 继承LcdDisplay，添加GIF表情支持
|
||||||
|
*/
|
||||||
|
class ElectronEmojiDisplay : public SpiLcdDisplay {
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* @brief 构造函数，参数与SpiLcdDisplay相同
|
||||||
|
*/
|
||||||
|
ElectronEmojiDisplay(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, DisplayFonts fonts);
|
||||||
|
|
||||||
|
virtual ~ElectronEmojiDisplay() = default;
|
||||||
|
|
||||||
|
// 重写表情设置方法
|
||||||
|
virtual void SetEmotion(const char* emotion) override;
|
||||||
|
|
||||||
|
// 重写聊天消息设置方法
|
||||||
|
virtual void SetChatMessage(const char* role, const char* content) override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
void SetupGifContainer();
|
||||||
|
|
||||||
|
lv_obj_t* emotion_gif_; ///< GIF表情组件
|
||||||
|
|
||||||
|
// 表情映射
|
||||||
|
struct EmotionMap {
|
||||||
|
const char* name;
|
||||||
|
const lv_image_dsc_t* gif;
|
||||||
|
};
|
||||||
|
|
||||||
|
static const EmotionMap emotion_maps_[];
|
||||||
|
};
|
||||||
453
main/boards/electron-bot/movements.cc
Normal file
453
main/boards/electron-bot/movements.cc
Normal file
@ -0,0 +1,453 @@
|
|||||||
|
#include "movements.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
#include "oscillator.h"
|
||||||
|
|
||||||
|
static const char* TAG = "Movements";
|
||||||
|
|
||||||
|
Otto::Otto() {
|
||||||
|
is_otto_resting_ = false;
|
||||||
|
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 right_pitch, int right_roll, int left_pitch, int left_roll, int body,
|
||||||
|
int head) {
|
||||||
|
servo_pins_[RIGHT_PITCH] = right_pitch;
|
||||||
|
servo_pins_[RIGHT_ROLL] = right_roll;
|
||||||
|
servo_pins_[LEFT_PITCH] = left_pitch;
|
||||||
|
servo_pins_[LEFT_ROLL] = left_roll;
|
||||||
|
servo_pins_[BODY] = body;
|
||||||
|
servo_pins_[HEAD] = head;
|
||||||
|
|
||||||
|
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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
///////////////////////////////////////////////////////////////////
|
||||||
|
//-- 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));
|
||||||
|
}
|
||||||
|
|
||||||
|
///////////////////////////////////////////////////////////////////
|
||||||
|
//-- HOME = Otto at rest position -------------------------------//
|
||||||
|
///////////////////////////////////////////////////////////////////
|
||||||
|
void Otto::Home(bool hands_down) {
|
||||||
|
if (is_otto_resting_ == false) { // Go to rest position only if necessary
|
||||||
|
MoveServos(1000, servo_initial_);
|
||||||
|
is_otto_resting_ = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(1000));
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Otto::GetRestState() {
|
||||||
|
return is_otto_resting_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Otto::SetRestState(bool state) {
|
||||||
|
is_otto_resting_ = state;
|
||||||
|
}
|
||||||
|
|
||||||
|
///////////////////////////////////////////////////////////////////
|
||||||
|
//-- PREDETERMINED MOTION SEQUENCES -----------------------------//
|
||||||
|
///////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
//---------------------------------------------------------
|
||||||
|
//-- 统一手部动作函数
|
||||||
|
//-- Parameters:
|
||||||
|
//-- action: 动作类型 1=举左手, 2=举右手, 3=举双手, 4=放左手, 5=放右手, 6=放双手,
|
||||||
|
//-- 7=挥左手, 8=挥右手, 9=挥双手, 10=拍打左手, 11=拍打右手, 12=拍打双手
|
||||||
|
//-- times: 重复次数
|
||||||
|
//-- amount: 动作幅度 (10-50)
|
||||||
|
//-- period: 动作时间
|
||||||
|
//---------------------------------------------------------
|
||||||
|
void Otto::HandAction(int action, int times, int amount, int period) {
|
||||||
|
// 限制参数范围
|
||||||
|
times = 2 * std::max(3, std::min(100, times));
|
||||||
|
amount = std::max(10, std::min(50, amount));
|
||||||
|
period = std::max(100, std::min(1000, period));
|
||||||
|
|
||||||
|
int current_positions[SERVO_COUNT];
|
||||||
|
for (int i = 0; i < SERVO_COUNT; i++) {
|
||||||
|
current_positions[i] = (servo_pins_[i] != -1) ? servo_[i].GetPosition() : servo_initial_[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (action) {
|
||||||
|
case 1: // 举左手
|
||||||
|
current_positions[LEFT_PITCH] = 180;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 2: // 举右手
|
||||||
|
current_positions[RIGHT_PITCH] = 0;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 3: // 举双手
|
||||||
|
current_positions[LEFT_PITCH] = 180;
|
||||||
|
current_positions[RIGHT_PITCH] = 0;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 4: // 放左手
|
||||||
|
case 5: // 放右手
|
||||||
|
case 6: // 放双手
|
||||||
|
// 回到初始位置
|
||||||
|
memcpy(current_positions, servo_initial_, sizeof(current_positions));
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 7: // 挥左手
|
||||||
|
current_positions[LEFT_PITCH] = 150;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
for (int i = 0; i < times; i++) {
|
||||||
|
current_positions[LEFT_PITCH] = 150 + (i % 2 == 0 ? -30 : 30);
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||||||
|
}
|
||||||
|
memcpy(current_positions, servo_initial_, sizeof(current_positions));
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 8: // 挥右手
|
||||||
|
current_positions[RIGHT_PITCH] = 30;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
for (int i = 0; i < times; i++) {
|
||||||
|
current_positions[RIGHT_PITCH] = 30 + (i % 2 == 0 ? 30 : -30);
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||||||
|
}
|
||||||
|
memcpy(current_positions, servo_initial_, sizeof(current_positions));
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 9: // 挥双手
|
||||||
|
current_positions[LEFT_PITCH] = 150;
|
||||||
|
current_positions[RIGHT_PITCH] = 30;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
for (int i = 0; i < times; i++) {
|
||||||
|
current_positions[LEFT_PITCH] = 150 + (i % 2 == 0 ? -30 : 30);
|
||||||
|
current_positions[RIGHT_PITCH] = 30 + (i % 2 == 0 ? 30 : -30);
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||||||
|
}
|
||||||
|
memcpy(current_positions, servo_initial_, sizeof(current_positions));
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 10: // 拍打左手
|
||||||
|
current_positions[LEFT_ROLL] = 20;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
for (int i = 0; i < times; i++) {
|
||||||
|
current_positions[LEFT_ROLL] = 20 - amount;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
current_positions[LEFT_ROLL] = 20 + amount;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
}
|
||||||
|
current_positions[LEFT_ROLL] = 0;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 11: // 拍打右手
|
||||||
|
current_positions[RIGHT_ROLL] = 160;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
for (int i = 0; i < times; i++) {
|
||||||
|
current_positions[RIGHT_ROLL] = 160 + amount;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
current_positions[RIGHT_ROLL] = 160 - amount;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
}
|
||||||
|
current_positions[RIGHT_ROLL] = 180;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 12: // 拍打双手
|
||||||
|
current_positions[LEFT_ROLL] = 20;
|
||||||
|
current_positions[RIGHT_ROLL] = 160;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
for (int i = 0; i < times; i++) {
|
||||||
|
current_positions[LEFT_ROLL] = 20 - amount;
|
||||||
|
current_positions[RIGHT_ROLL] = 160 + amount;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
current_positions[LEFT_ROLL] = 20 + amount;
|
||||||
|
current_positions[RIGHT_ROLL] = 160 - amount;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
}
|
||||||
|
current_positions[LEFT_ROLL] = 0;
|
||||||
|
current_positions[RIGHT_ROLL] = 180;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//---------------------------------------------------------
|
||||||
|
//-- 统一身体动作函数
|
||||||
|
//-- Parameters:
|
||||||
|
//-- action: 动作类型 1=左转, 2=右转，3=回中心
|
||||||
|
//-- times: 转动次数
|
||||||
|
//-- amount: 旋转角度 (0-90度，以90度为中心左右旋转)
|
||||||
|
//-- period: 动作时间
|
||||||
|
//---------------------------------------------------------
|
||||||
|
void Otto::BodyAction(int action, int times, int amount, int period) {
|
||||||
|
// 限制参数范围
|
||||||
|
times = std::max(1, std::min(10, times));
|
||||||
|
amount = std::max(0, std::min(90, amount));
|
||||||
|
period = std::max(500, std::min(3000, period));
|
||||||
|
|
||||||
|
int current_positions[SERVO_COUNT];
|
||||||
|
for (int i = 0; i < SERVO_COUNT; i++) {
|
||||||
|
if (servo_pins_[i] != -1) {
|
||||||
|
current_positions[i] = servo_[i].GetPosition();
|
||||||
|
} else {
|
||||||
|
current_positions[i] = servo_initial_[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int body_center = servo_initial_[BODY];
|
||||||
|
int target_angle = body_center;
|
||||||
|
|
||||||
|
switch (action) {
|
||||||
|
case 1: // 左转
|
||||||
|
target_angle = body_center + amount;
|
||||||
|
target_angle = std::min(180, target_angle);
|
||||||
|
break;
|
||||||
|
case 2: // 右转
|
||||||
|
target_angle = body_center - amount;
|
||||||
|
target_angle = std::max(0, target_angle);
|
||||||
|
break;
|
||||||
|
case 3: // 回中心
|
||||||
|
target_angle = body_center;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return; // 无效动作
|
||||||
|
}
|
||||||
|
|
||||||
|
current_positions[BODY] = target_angle;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(100));
|
||||||
|
}
|
||||||
|
|
||||||
|
//---------------------------------------------------------
|
||||||
|
//-- 统一头部动作函数
|
||||||
|
//-- Parameters:
|
||||||
|
//-- action: 动作类型 1=抬头, 2=低头, 3=点头, 4=回中心, 5=连续点头
|
||||||
|
//-- times: 重复次数 (仅对连续点头有效)
|
||||||
|
//-- amount: 角度偏移 (1-15度范围内)
|
||||||
|
//-- period: 动作时间
|
||||||
|
//---------------------------------------------------------
|
||||||
|
void Otto::HeadAction(int action, int times, int amount, int period) {
|
||||||
|
// 限制参数范围
|
||||||
|
times = std::max(1, std::min(10, times));
|
||||||
|
amount = std::max(1, std::min(15, abs(amount)));
|
||||||
|
period = std::max(300, std::min(3000, period));
|
||||||
|
|
||||||
|
int current_positions[SERVO_COUNT];
|
||||||
|
for (int i = 0; i < SERVO_COUNT; i++) {
|
||||||
|
if (servo_pins_[i] != -1) {
|
||||||
|
current_positions[i] = servo_[i].GetPosition();
|
||||||
|
} else {
|
||||||
|
current_positions[i] = servo_initial_[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int head_center = 90; // 头部中心位置
|
||||||
|
|
||||||
|
switch (action) {
|
||||||
|
case 1: // 抬头
|
||||||
|
current_positions[HEAD] = head_center + amount; // 抬头是增加角度
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 2: // 低头
|
||||||
|
current_positions[HEAD] = head_center - amount; // 低头是减少角度
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 3: // 点头 (上下运动)
|
||||||
|
// 先抬头
|
||||||
|
current_positions[HEAD] = head_center + amount;
|
||||||
|
MoveServos(period / 3, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(period / 6));
|
||||||
|
|
||||||
|
// 再低头
|
||||||
|
current_positions[HEAD] = head_center - amount;
|
||||||
|
MoveServos(period / 3, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(period / 6));
|
||||||
|
|
||||||
|
// 回到中心
|
||||||
|
current_positions[HEAD] = head_center;
|
||||||
|
MoveServos(period / 3, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 4: // 回到中心位置
|
||||||
|
current_positions[HEAD] = head_center;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 5: // 连续点头
|
||||||
|
for (int i = 0; i < times; i++) {
|
||||||
|
// 抬头
|
||||||
|
current_positions[HEAD] = head_center + amount;
|
||||||
|
MoveServos(period / 2, current_positions);
|
||||||
|
|
||||||
|
// 低头
|
||||||
|
current_positions[HEAD] = head_center - amount;
|
||||||
|
MoveServos(period / 2, current_positions);
|
||||||
|
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(50)); // 短暂停顿
|
||||||
|
}
|
||||||
|
|
||||||
|
// 回到中心
|
||||||
|
current_positions[HEAD] = head_center;
|
||||||
|
MoveServos(period / 2, current_positions);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
// 无效动作，回到中心
|
||||||
|
current_positions[HEAD] = head_center;
|
||||||
|
MoveServos(period, current_positions);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
89
main/boards/electron-bot/movements.h
Normal file
89
main/boards/electron-bot/movements.h
Normal file
@ -0,0 +1,89 @@
|
|||||||
|
#ifndef __MOVEMENTS_H__
|
||||||
|
#define __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 RIGHT_PITCH 0
|
||||||
|
#define RIGHT_ROLL 1
|
||||||
|
#define LEFT_PITCH 2
|
||||||
|
#define LEFT_ROLL 3
|
||||||
|
#define BODY 4
|
||||||
|
#define HEAD 5
|
||||||
|
#define SERVO_COUNT 6
|
||||||
|
|
||||||
|
class Otto {
|
||||||
|
public:
|
||||||
|
Otto();
|
||||||
|
~Otto();
|
||||||
|
|
||||||
|
//-- Otto initialization
|
||||||
|
void Init(int right_pitch, int right_roll, int left_pitch, int left_roll, int body, int head);
|
||||||
|
//-- Attach & detach functions
|
||||||
|
void AttachServos();
|
||||||
|
void DetachServos();
|
||||||
|
|
||||||
|
//-- Oscillator Trims
|
||||||
|
void SetTrims(int right_pitch, int right_roll, int left_pitch, int left_roll, int body,
|
||||||
|
int head);
|
||||||
|
|
||||||
|
//-- 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);
|
||||||
|
|
||||||
|
//-- HOME = Otto at rest position
|
||||||
|
void Home(bool hands_down = true);
|
||||||
|
bool GetRestState();
|
||||||
|
void SetRestState(bool state);
|
||||||
|
|
||||||
|
// -- 手部动作
|
||||||
|
void HandAction(int action, int times = 1, int amount = 30, int period = 1000);
|
||||||
|
// action: 1=举左手, 2=举右手, 3=举双手, 4=放左手, 5=放右手, 6=放双手, 7=挥左手, 8=挥右手,
|
||||||
|
// 9=挥双手, 10=拍打左手, 11=拍打右手, 12=拍打双手
|
||||||
|
|
||||||
|
//-- 身体动作
|
||||||
|
void BodyAction(int action, int times = 1, int amount = 30, int period = 1000);
|
||||||
|
// action: 1=左转, 2=右转
|
||||||
|
|
||||||
|
//-- 头部动作
|
||||||
|
void HeadAction(int action, int times = 1, int amount = 10, int period = 500);
|
||||||
|
// action: 1=抬头, 2=低头, 3=点头, 4=回中心, 5=连续点头
|
||||||
|
|
||||||
|
private:
|
||||||
|
Oscillator servo_[SERVO_COUNT];
|
||||||
|
|
||||||
|
int servo_pins_[SERVO_COUNT];
|
||||||
|
int servo_trim_[SERVO_COUNT];
|
||||||
|
int servo_initial_[SERVO_COUNT] = {180, 180, 0, 0, 90, 90};
|
||||||
|
|
||||||
|
unsigned long final_time_;
|
||||||
|
unsigned long partial_time_;
|
||||||
|
float increment_[SERVO_COUNT];
|
||||||
|
|
||||||
|
bool is_otto_resting_;
|
||||||
|
|
||||||
|
void Execute(int amplitude[SERVO_COUNT], int offset[SERVO_COUNT], int period,
|
||||||
|
double phase_diff[SERVO_COUNT], float steps);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // __MOVEMENTS_H__
|
||||||
153
main/boards/electron-bot/oscillator.cc
Normal file
153
main/boards/electron-bot/oscillator.cc
Normal file
@ -0,0 +1,153 @@
|
|||||||
|
#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_));
|
||||||
|
}
|
||||||
83
main/boards/electron-bot/oscillator.h
Normal file
83
main/boards/electron-bot/oscillator.h
Normal file
@ -0,0 +1,83 @@
|
|||||||
|
#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__
|
||||||
128
main/boards/electron-bot/power_manager.h
Normal file
128
main/boards/electron-bot/power_manager.h
Normal file
@ -0,0 +1,128 @@
|
|||||||
|
#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[] = {{2150, 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;
|
||||||
|
|
||||||
|
adc_oneshot_unit_handle_t adc_handle_;
|
||||||
|
|
||||||
|
void CheckBatteryStatus() {
|
||||||
|
is_charging_ = gpio_get_level(charging_pin_) == 0;
|
||||||
|
ReadBatteryAdcData();
|
||||||
|
}
|
||||||
|
|
||||||
|
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) {
|
||||||
|
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_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_,
|
||||||
|
.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_; }
|
||||||
|
};
|
||||||
|
#endif // __POWER_MANAGER_H__
|
||||||
124
main/boards/otto-robot/README.md
Normal file
124
main/boards/otto-robot/README.md
Normal file
@ -0,0 +1,124 @@
|
|||||||
|
<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>
|
||||||
|
|
||||||
|
## 硬件
|
||||||
|
- <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 (夸张表演)
|
||||||
|
|
||||||
|
### 动作
|
||||||
|
|
||||||
|
| MCP工具名称 | 描述 | 参数说明 |
|
||||||
|
|-------------------|-----------------|---------------------------------------------------|
|
||||||
|
| self.otto.walk_forward | 行走 | **steps**: 行走步数(1-100，默认3)<br>**speed**: 行走速度(500-1500，数值越小越快，默认1000)<br>**direction**: 行走方向(-1=后退, 1=前进，默认1)<br>**arm_swing**: 手臂摆动幅度(0-170度，默认50) |
|
||||||
|
| self.otto.turn_left | 转身 | **steps**: 转身步数(1-100，默认3)<br>**speed**: 转身速度(500-1500，数值越小越快，默认1000)<br>**direction**: 转身方向(1=左转, -1=右转，默认1)<br>**arm_swing**: 手臂摆动幅度(0-170度，默认50) |
|
||||||
|
| self.otto.jump | 跳跃 | **steps**: 跳跃次数(1-100，默认1)<br>**speed**: 跳跃速度(500-1500，数值越小越快，默认1000) |
|
||||||
|
| self.otto.swing | 左右摇摆 | **steps**: 摇摆次数(1-100，默认3)<br>**speed**: 摇摆速度(500-1500，数值越小越快，默认1000)<br>**amount**: 摇摆幅度(0-170度，默认30) |
|
||||||
|
| self.otto.moonwalk | 太空步 | **steps**: 太空步步数(1-100，默认3)<br>**speed**: 速度(500-1500，数值越小越快，默认1000)<br>**direction**: 方向(1=左, -1=右，默认1)<br>**amount**: 幅度(0-170度，默认25) |
|
||||||
|
| self.otto.bend | 弯曲身体 | **steps**: 弯曲次数(1-100，默认1)<br>**speed**: 弯曲速度(500-1500，数值越小越快，默认1000)<br>**direction**: 弯曲方向(1=左, -1=右，默认1) |
|
||||||
|
| self.otto.shake_leg | 摇腿 | **steps**: 摇腿次数(1-100，默认1)<br>**speed**: 摇腿速度(500-1500，数值越小越快，默认1000)<br>**direction**: 腿部选择(1=左腿, -1=右腿，默认1) |
|
||||||
|
| self.otto.updown | 上下运动 | **steps**: 上下运动次数(1-100，默认3)<br>**speed**: 运动速度(500-1500，数值越小越快，默认1000)<br>**amount**: 运动幅度(0-170度，默认20) |
|
||||||
|
| self.otto.hands_up | 举手 * | **speed**: 举手速度(500-1500，数值越小越快，默认1000)<br>**direction**: 手部选择(1=左手, -1=右手, 0=双手，默认1) |
|
||||||
|
| self.otto.hands_down | 放手 * | **speed**: 放手速度(500-1500，数值越小越快，默认1000)<br>**direction**: 手部选择(1=左手, -1=右手, 0=双手，默认1) |
|
||||||
|
| self.otto.hand_wave | 挥手 * | **speed**: 挥手速度(500-1500，数值越小越快，默认1000)<br>**direction**: 手部选择(1=左手, -1=右手, 0=双手，默认1) |
|
||||||
|
|
||||||
|
**注**: 标记 * 的手部动作仅在配置了手部舵机时可用。
|
||||||
|
|
||||||
|
### 系统工具
|
||||||
|
|
||||||
|
| MCP工具名称 | 描述 | 返回值 |
|
||||||
|
|-------------------|-----------------|---------------------------------------------------|
|
||||||
|
| self.otto.stop | 立即停止 | 停止当前动作并回到初始位置 |
|
||||||
|
| self.otto.get_status | 获取机器人状态 | 返回 "moving" 或 "idle" |
|
||||||
|
| self.battery.get_level | 获取电池状态 | 返回电量百分比和充电状态的JSON格式 |
|
||||||
|
|
||||||
|
### 参数说明
|
||||||
|
|
||||||
|
1. **steps**: 动作执行的步数/次数，数值越大动作持续时间越长
|
||||||
|
2. **speed**: 动作执行速度，数值范围500-1500，**数值越小越快**
|
||||||
|
3. **direction**: 方向参数
|
||||||
|
- 移动动作: 1=左/前进, -1=右/后退
|
||||||
|
- 手部动作: 1=左手, -1=右手, 0=双手
|
||||||
|
4. **amount/arm_swing**: 动作幅度，范围0-170度
|
||||||
|
- 0表示不摆动（适用于手臂摆动）
|
||||||
|
- 数值越大幅度越大
|
||||||
|
|
||||||
|
### 动作控制
|
||||||
|
- 每个动作执行完成后，机器人会自动回到初始位置(home)，以便于执行下一个动作
|
||||||
|
- 所有参数都有合理的默认值，可以省略不需要自定义的参数
|
||||||
|
- 动作在后台任务中执行，不会阻塞主程序
|
||||||
|
- 支持动作队列，可以连续执行多个动作
|
||||||
|
|
||||||
|
### MCP工具调用示例
|
||||||
|
```json
|
||||||
|
// 向前走3步
|
||||||
|
{"name": "self.otto.walk_forward", "arguments": {}}
|
||||||
|
|
||||||
|
// 向前走5步，稍快一些
|
||||||
|
{"name": "self.otto.walk_forward", "arguments": {"steps": 5, "speed": 800}}
|
||||||
|
|
||||||
|
// 左转2步，大幅度摆动手臂
|
||||||
|
{"name": "self.otto.turn_left", "arguments": {"steps": 2, "arm_swing": 100}}
|
||||||
|
|
||||||
|
// 摇摆舞蹈，中等幅度
|
||||||
|
{"name": "self.otto.swing", "arguments": {"steps": 5, "amount": 50}}
|
||||||
|
|
||||||
|
// 挥左手打招呼
|
||||||
|
{"name": "self.otto.hand_wave", "arguments": {"direction": 1}}
|
||||||
|
|
||||||
|
// 立即停止
|
||||||
|
{"name": "self.otto.stop", "arguments": {}}
|
||||||
|
```
|
||||||
|
|
||||||
|
### 语音指令示例
|
||||||
|
- "向前走" / "向前走5步" / "快速向前"
|
||||||
|
- "左转" / "右转" / "转身"
|
||||||
|
- "跳跃" / "跳一下"
|
||||||
|
- "摇摆" / "跳舞"
|
||||||
|
- "太空步" / "月球漫步"
|
||||||
|
- "挥手" / "举手" / "放手"
|
||||||
|
- "停止" / "停下"
|
||||||
|
|
||||||
|
**说明**: 小智控制机器人动作是创建新的任务在后台控制，动作执行期间仍可接受新的语音指令。可以通过"停止"语音指令立即停下Otto。
|
||||||
|
|
||||||
52
main/boards/otto-robot/config.h
Normal file
52
main/boards/otto-robot/config.h
Normal file
@ -0,0 +1,52 @@
|
|||||||
|
#ifndef _BOARD_CONFIG_H_
|
||||||
|
#define _BOARD_CONFIG_H_
|
||||||
|
|
||||||
|
#include <driver/gpio.h>
|
||||||
|
|
||||||
|
#define POWER_CHARGE_DETECT_PIN GPIO_NUM_21
|
||||||
|
#define POWER_ADC_UNIT ADC_UNIT_2
|
||||||
|
#define POWER_ADC_CHANNEL ADC_CHANNEL_3
|
||||||
|
|
||||||
|
#define RIGHT_LEG_PIN GPIO_NUM_39
|
||||||
|
#define RIGHT_FOOT_PIN GPIO_NUM_38
|
||||||
|
#define LEFT_LEG_PIN GPIO_NUM_17
|
||||||
|
#define LEFT_FOOT_PIN GPIO_NUM_18
|
||||||
|
#define LEFT_HAND_PIN GPIO_NUM_8
|
||||||
|
#define RIGHT_HAND_PIN GPIO_NUM_12
|
||||||
|
|
||||||
|
#define AUDIO_INPUT_SAMPLE_RATE 16000
|
||||||
|
#define AUDIO_OUTPUT_SAMPLE_RATE 24000
|
||||||
|
#define AUDIO_I2S_METHOD_SIMPLEX
|
||||||
|
|
||||||
|
#define AUDIO_I2S_MIC_GPIO_WS GPIO_NUM_4
|
||||||
|
#define AUDIO_I2S_MIC_GPIO_SCK GPIO_NUM_5
|
||||||
|
#define AUDIO_I2S_MIC_GPIO_DIN GPIO_NUM_6
|
||||||
|
#define AUDIO_I2S_SPK_GPIO_DOUT GPIO_NUM_7
|
||||||
|
#define AUDIO_I2S_SPK_GPIO_BCLK GPIO_NUM_15
|
||||||
|
#define AUDIO_I2S_SPK_GPIO_LRCK GPIO_NUM_16
|
||||||
|
|
||||||
|
#define DISPLAY_BACKLIGHT_PIN GPIO_NUM_3
|
||||||
|
#define DISPLAY_MOSI_PIN GPIO_NUM_10
|
||||||
|
#define DISPLAY_CLK_PIN GPIO_NUM_9
|
||||||
|
#define DISPLAY_DC_PIN GPIO_NUM_46
|
||||||
|
#define DISPLAY_RST_PIN GPIO_NUM_11
|
||||||
|
#define DISPLAY_CS_PIN GPIO_NUM_12
|
||||||
|
|
||||||
|
#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
|
||||||
|
|
||||||
|
#define OTTO_ROBOT_VERSION "1.4.2"
|
||||||
|
|
||||||
|
#endif // _BOARD_CONFIG_H_
|
||||||
10
main/boards/otto-robot/config.json
Normal file
10
main/boards/otto-robot/config.json
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
{
|
||||||
|
"target": "esp32s3",
|
||||||
|
"builds": [
|
||||||
|
{
|
||||||
|
"name": "otto-robot",
|
||||||
|
"sdkconfig_append": [
|
||||||
|
]
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
153
main/boards/otto-robot/oscillator.cc
Normal file
153
main/boards/otto-robot/oscillator.cc
Normal file
@ -0,0 +1,153 @@
|
|||||||
|
#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_));
|
||||||
|
}
|
||||||
83
main/boards/otto-robot/oscillator.h
Normal file
83
main/boards/otto-robot/oscillator.h
Normal file
@ -0,0 +1,83 @@
|
|||||||
|
#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__
|
||||||
383
main/boards/otto-robot/otto_controller.cc
Normal file
383
main/boards/otto-robot/otto_controller.cc
Normal file
@ -0,0 +1,383 @@
|
|||||||
|
/*
|
||||||
|
Otto机器人控制器 - MCP协议版本
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <cJSON.h>
|
||||||
|
#include <esp_log.h>
|
||||||
|
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
#include "application.h"
|
||||||
|
#include "board.h"
|
||||||
|
#include "config.h"
|
||||||
|
#include "mcp_server.h"
|
||||||
|
#include "otto_movements.h"
|
||||||
|
#include "sdkconfig.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;
|
||||||
|
};
|
||||||
|
|
||||||
|
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_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
|
||||||
|
};
|
||||||
|
|
||||||
|
static void ActionTask(void* arg) {
|
||||||
|
OttoController* controller = static_cast<OttoController*>(arg);
|
||||||
|
OttoActionParams params;
|
||||||
|
controller->otto_.AttachServos();
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
if (xQueueReceive(controller->action_queue_, ¶ms, pdMS_TO_TICKS(1000)) == pdTRUE) {
|
||||||
|
ESP_LOGI(TAG, "执行动作: %d", params.action_type);
|
||||||
|
controller->is_action_in_progress_ = true;
|
||||||
|
|
||||||
|
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_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_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.speed, params.direction);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
controller->otto_.Home(params.action_type < ACTION_HANDS_UP);
|
||||||
|
controller->is_action_in_progress_ = false;
|
||||||
|
}
|
||||||
|
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) && !has_hands_) {
|
||||||
|
ESP_LOGW(TAG, "尝试执行手部动作，但机器人没有配置手部舵机");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
ESP_LOGI(TAG, "动作控制: 类型=%d, 步数=%d, 速度=%d, 方向=%d, 幅度=%d", action_type, steps,
|
||||||
|
speed, direction, amount);
|
||||||
|
|
||||||
|
OttoActionParams params = {action_type, steps, speed, direction, amount};
|
||||||
|
xQueueSend(action_queue_, ¶ms, portMAX_DELAY);
|
||||||
|
StartActionTaskIfNeeded();
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
OttoController() {
|
||||||
|
otto_.Init(LEFT_LEG_PIN, RIGHT_LEG_PIN, LEFT_FOOT_PIN, RIGHT_FOOT_PIN, LEFT_HAND_PIN,
|
||||||
|
RIGHT_HAND_PIN);
|
||||||
|
|
||||||
|
has_hands_ = (LEFT_HAND_PIN != -1 && RIGHT_HAND_PIN != -1);
|
||||||
|
ESP_LOGI(TAG, "Otto机器人初始化%s手部舵机", has_hands_ ? "带" : "不带");
|
||||||
|
|
||||||
|
otto_.Home(true);
|
||||||
|
action_queue_ = xQueueCreate(10, sizeof(OttoActionParams));
|
||||||
|
|
||||||
|
RegisterMcpTools();
|
||||||
|
}
|
||||||
|
|
||||||
|
void RegisterMcpTools() {
|
||||||
|
auto& mcp_server = McpServer::GetInstance();
|
||||||
|
|
||||||
|
ESP_LOGI(TAG, "开始注册MCP工具...");
|
||||||
|
|
||||||
|
// 基础移动动作
|
||||||
|
mcp_server.AddTool("self.otto.walk_forward",
|
||||||
|
"行走。steps: 行走步数(1-100); speed: 行走速度(500-1500，数值越小越快); "
|
||||||
|
"direction: 行走方向(-1=后退, 1=前进); arm_swing: 手臂摆动幅度(0-170度)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("arm_swing", kPropertyTypeInteger, 50, 0, 170),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, -1, 1)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int arm_swing = properties["arm_swing"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
QueueAction(ACTION_WALK, steps, speed, direction, arm_swing);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool("self.otto.turn_left",
|
||||||
|
"转身。steps: 转身步数(1-100); speed: 转身速度(500-1500，数值越小越快); "
|
||||||
|
"direction: 转身方向(1=左转, -1=右转); arm_swing: 手臂摆动幅度(0-170度)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("arm_swing", kPropertyTypeInteger, 50, 0, 170),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, -1, 1)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int arm_swing = properties["arm_swing"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
QueueAction(ACTION_TURN, steps, speed, direction, arm_swing);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool("self.otto.jump",
|
||||||
|
"跳跃。steps: 跳跃次数(1-100); speed: 跳跃速度(500-1500，数值越小越快)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 1, 1, 100),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
QueueAction(ACTION_JUMP, steps, speed, 0, 0);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
// 特殊动作
|
||||||
|
mcp_server.AddTool("self.otto.swing",
|
||||||
|
"左右摇摆。steps: 摇摆次数(1-100); speed: "
|
||||||
|
"摇摆速度(500-1500，数值越小越快); amount: 摇摆幅度(0-170度)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("amount", kPropertyTypeInteger, 30, 0, 170)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int amount = properties["amount"].value<int>();
|
||||||
|
QueueAction(ACTION_SWING, steps, speed, 0, amount);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool("self.otto.moonwalk",
|
||||||
|
"太空步。steps: 太空步步数(1-100); speed: 速度(500-1500，数值越小越快); "
|
||||||
|
"direction: 方向(1=左, -1=右); amount: 幅度(0-170度)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, -1, 1),
|
||||||
|
Property("amount", kPropertyTypeInteger, 25, 0, 170)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
int amount = properties["amount"].value<int>();
|
||||||
|
QueueAction(ACTION_MOONWALK, steps, speed, direction, amount);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool("self.otto.bend",
|
||||||
|
"弯曲身体。steps: 弯曲次数(1-100); speed: "
|
||||||
|
"弯曲速度(500-1500，数值越小越快); direction: 弯曲方向(1=左, -1=右)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 1, 1, 100),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, -1, 1)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
QueueAction(ACTION_BEND, steps, speed, direction, 0);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool("self.otto.shake_leg",
|
||||||
|
"摇腿。steps: 摇腿次数(1-100); speed: 摇腿速度(500-1500，数值越小越快); "
|
||||||
|
"direction: 腿部选择(1=左腿, -1=右腿)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 1, 1, 100),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, -1, 1)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
QueueAction(ACTION_SHAKE_LEG, steps, speed, direction, 0);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool("self.otto.updown",
|
||||||
|
"上下运动。steps: 上下运动次数(1-100); speed: "
|
||||||
|
"运动速度(500-1500，数值越小越快); amount: 运动幅度(0-170度)",
|
||||||
|
PropertyList({Property("steps", kPropertyTypeInteger, 3, 1, 100),
|
||||||
|
Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("amount", kPropertyTypeInteger, 20, 0, 170)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int steps = properties["steps"].value<int>();
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int amount = properties["amount"].value<int>();
|
||||||
|
QueueAction(ACTION_UPDOWN, steps, speed, 0, amount);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
// 手部动作（仅在有手部舵机时可用）
|
||||||
|
if (has_hands_) {
|
||||||
|
mcp_server.AddTool(
|
||||||
|
"self.otto.hands_up",
|
||||||
|
"举手。speed: 举手速度(500-1500，数值越小越快); direction: 手部选择(1=左手, "
|
||||||
|
"-1=右手, 0=双手)",
|
||||||
|
PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, -1, 1)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
QueueAction(ACTION_HANDS_UP, 1, speed, direction, 0);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool(
|
||||||
|
"self.otto.hands_down",
|
||||||
|
"放手。speed: 放手速度(500-1500，数值越小越快); direction: 手部选择(1=左手, "
|
||||||
|
"-1=右手, 0=双手)",
|
||||||
|
PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, -1, 1)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
QueueAction(ACTION_HANDS_DOWN, 1, speed, direction, 0);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
mcp_server.AddTool(
|
||||||
|
"self.otto.hand_wave",
|
||||||
|
"挥手。speed: 挥手速度(500-1500，数值越小越快); direction: 手部选择(1=左手, "
|
||||||
|
"-1=右手, 0=双手)",
|
||||||
|
PropertyList({Property("speed", kPropertyTypeInteger, 1000, 500, 1500),
|
||||||
|
Property("direction", kPropertyTypeInteger, 1, -1, 1)}),
|
||||||
|
[this](const PropertyList& properties) -> ReturnValue {
|
||||||
|
int speed = properties["speed"].value<int>();
|
||||||
|
int direction = properties["direction"].value<int>();
|
||||||
|
QueueAction(ACTION_HAND_WAVE, 1, speed, direction, 0);
|
||||||
|
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;
|
||||||
|
xQueueReset(action_queue_);
|
||||||
|
otto_.Home(true);
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
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;
|
||||||
|
});
|
||||||
|
|
||||||
|
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() {
|
||||||
|
if (g_otto_controller == nullptr) {
|
||||||
|
g_otto_controller = new OttoController();
|
||||||
|
ESP_LOGI(TAG, "Otto控制器已初始化并注册MCP工具");
|
||||||
|
}
|
||||||
|
}
|
||||||
145
main/boards/otto-robot/otto_emoji_display.cc
Normal file
145
main/boards/otto-robot/otto_emoji_display.cc
Normal file
@ -0,0 +1,145 @@
|
|||||||
|
#include "otto_emoji_display.h"
|
||||||
|
|
||||||
|
#include <esp_log.h>
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
#include "display/lcd_display.h"
|
||||||
|
#define TAG "OttoEmojiDisplay"
|
||||||
|
|
||||||
|
// 表情映射表 - 将原版21种表情映射到现有6个GIF
|
||||||
|
const OttoEmojiDisplay::EmotionMap OttoEmojiDisplay::emotion_maps_[] = {
|
||||||
|
// 中性/平静类表情 -> staticstate
|
||||||
|
{"neutral", &staticstate},
|
||||||
|
{"relaxed", &staticstate},
|
||||||
|
{"sleepy", &staticstate},
|
||||||
|
|
||||||
|
// 积极/开心类表情 -> happy
|
||||||
|
{"happy", &happy},
|
||||||
|
{"laughing", &happy},
|
||||||
|
{"funny", &happy},
|
||||||
|
{"loving", &happy},
|
||||||
|
{"confident", &happy},
|
||||||
|
{"winking", &happy},
|
||||||
|
{"cool", &happy},
|
||||||
|
{"delicious", &happy},
|
||||||
|
{"kissy", &happy},
|
||||||
|
{"silly", &happy},
|
||||||
|
|
||||||
|
// 悲伤类表情 -> sad
|
||||||
|
{"sad", &sad},
|
||||||
|
{"crying", &sad},
|
||||||
|
|
||||||
|
// 愤怒类表情 -> anger
|
||||||
|
{"angry", &anger},
|
||||||
|
|
||||||
|
// 惊讶类表情 -> scare
|
||||||
|
{"surprised", &scare},
|
||||||
|
{"shocked", &scare},
|
||||||
|
|
||||||
|
// 思考/困惑类表情 -> buxue
|
||||||
|
{"thinking", &buxue},
|
||||||
|
{"confused", &buxue},
|
||||||
|
{"embarrassed", &buxue},
|
||||||
|
|
||||||
|
{nullptr, nullptr} // 结束标记
|
||||||
|
};
|
||||||
|
|
||||||
|
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, DisplayFonts fonts)
|
||||||
|
: SpiLcdDisplay(panel_io, panel, width, height, offset_x, offset_y, mirror_x, mirror_y, swap_xy,
|
||||||
|
fonts),
|
||||||
|
emotion_gif_(nullptr) {
|
||||||
|
SetupGifContainer();
|
||||||
|
};
|
||||||
|
|
||||||
|
void OttoEmojiDisplay::SetupGifContainer() {
|
||||||
|
DisplayLockGuard lock(this);
|
||||||
|
|
||||||
|
if (emotion_label_) {
|
||||||
|
lv_obj_del(emotion_label_);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (chat_message_label_) {
|
||||||
|
lv_obj_del(chat_message_label_);
|
||||||
|
}
|
||||||
|
if (content_) {
|
||||||
|
lv_obj_del(content_);
|
||||||
|
}
|
||||||
|
|
||||||
|
content_ = lv_obj_create(container_);
|
||||||
|
lv_obj_set_scrollbar_mode(content_, LV_SCROLLBAR_MODE_OFF);
|
||||||
|
lv_obj_set_size(content_, LV_HOR_RES, LV_HOR_RES);
|
||||||
|
lv_obj_set_style_bg_opa(content_, LV_OPA_TRANSP, 0);
|
||||||
|
lv_obj_set_style_border_width(content_, 0, 0);
|
||||||
|
lv_obj_set_flex_grow(content_, 1);
|
||||||
|
lv_obj_center(content_);
|
||||||
|
|
||||||
|
emotion_label_ = lv_label_create(content_);
|
||||||
|
lv_label_set_text(emotion_label_, "");
|
||||||
|
lv_obj_set_width(emotion_label_, 0);
|
||||||
|
lv_obj_set_style_border_width(emotion_label_, 0, 0);
|
||||||
|
lv_obj_add_flag(emotion_label_, LV_OBJ_FLAG_HIDDEN);
|
||||||
|
|
||||||
|
emotion_gif_ = lv_gif_create(content_);
|
||||||
|
int gif_size = LV_HOR_RES;
|
||||||
|
lv_obj_set_size(emotion_gif_, gif_size, gif_size);
|
||||||
|
lv_obj_set_style_border_width(emotion_gif_, 0, 0);
|
||||||
|
lv_obj_set_style_bg_opa(emotion_gif_, LV_OPA_TRANSP, 0);
|
||||||
|
lv_obj_center(emotion_gif_);
|
||||||
|
lv_gif_set_src(emotion_gif_, &staticstate);
|
||||||
|
|
||||||
|
chat_message_label_ = lv_label_create(content_);
|
||||||
|
lv_label_set_text(chat_message_label_, "");
|
||||||
|
lv_obj_set_width(chat_message_label_, LV_HOR_RES * 0.9);
|
||||||
|
lv_label_set_long_mode(chat_message_label_, LV_LABEL_LONG_SCROLL_CIRCULAR);
|
||||||
|
lv_obj_set_style_text_align(chat_message_label_, LV_TEXT_ALIGN_CENTER, 0);
|
||||||
|
lv_obj_set_style_text_color(chat_message_label_, lv_color_white(), 0);
|
||||||
|
lv_obj_set_style_border_width(chat_message_label_, 0, 0);
|
||||||
|
|
||||||
|
lv_obj_set_style_bg_opa(chat_message_label_, LV_OPA_70, 0);
|
||||||
|
lv_obj_set_style_bg_color(chat_message_label_, lv_color_black(), 0);
|
||||||
|
lv_obj_set_style_pad_ver(chat_message_label_, 5, 0);
|
||||||
|
|
||||||
|
lv_obj_align(chat_message_label_, LV_ALIGN_BOTTOM_MID, 0, 0);
|
||||||
|
|
||||||
|
LcdDisplay::SetTheme("dark");
|
||||||
|
}
|
||||||
|
|
||||||
|
void OttoEmojiDisplay::SetEmotion(const char* emotion) {
|
||||||
|
if (!emotion || !emotion_gif_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
DisplayLockGuard lock(this);
|
||||||
|
|
||||||
|
for (const auto& map : emotion_maps_) {
|
||||||
|
if (map.name && strcmp(map.name, emotion) == 0) {
|
||||||
|
lv_gif_set_src(emotion_gif_, map.gif);
|
||||||
|
ESP_LOGI(TAG, "设置表情: %s", emotion);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
lv_gif_set_src(emotion_gif_, &staticstate);
|
||||||
|
ESP_LOGI(TAG, "未知表情'%s'，使用默认", emotion);
|
||||||
|
}
|
||||||
|
|
||||||
|
void OttoEmojiDisplay::SetChatMessage(const char* role, const char* content) {
|
||||||
|
DisplayLockGuard lock(this);
|
||||||
|
if (chat_message_label_ == nullptr) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (content == nullptr || strlen(content) == 0) {
|
||||||
|
lv_obj_add_flag(chat_message_label_, LV_OBJ_FLAG_HIDDEN);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
lv_label_set_text(chat_message_label_, content);
|
||||||
|
lv_obj_clear_flag(chat_message_label_, LV_OBJ_FLAG_HIDDEN);
|
||||||
|
|
||||||
|
ESP_LOGI(TAG, "设置聊天消息 [%s]: %s", role, content);
|
||||||
|
}
|
||||||
41
main/boards/otto-robot/otto_emoji_display.h
Normal file
41
main/boards/otto-robot/otto_emoji_display.h
Normal file
@ -0,0 +1,41 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <libs/gif/lv_gif.h>
|
||||||
|
|
||||||
|
#include "display/lcd_display.h"
|
||||||
|
#include "otto_emoji_gif.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Otto机器人GIF表情显示类
|
||||||
|
* 继承LcdDisplay，添加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, DisplayFonts fonts);
|
||||||
|
|
||||||
|
virtual ~OttoEmojiDisplay() = default;
|
||||||
|
|
||||||
|
// 重写表情设置方法
|
||||||
|
virtual void SetEmotion(const char* emotion) override;
|
||||||
|
|
||||||
|
// 重写聊天消息设置方法
|
||||||
|
virtual void SetChatMessage(const char* role, const char* content) override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
void SetupGifContainer();
|
||||||
|
|
||||||
|
lv_obj_t* emotion_gif_; ///< GIF表情组件
|
||||||
|
|
||||||
|
// 表情映射
|
||||||
|
struct EmotionMap {
|
||||||
|
const char* name;
|
||||||
|
const lv_img_dsc_t* gif;
|
||||||
|
};
|
||||||
|
|
||||||
|
static const EmotionMap emotion_maps_[];
|
||||||
|
};
|
||||||
763
main/boards/otto-robot/otto_movements.cc
Normal file
763
main/boards/otto-robot/otto_movements.cc
Normal file
@ -0,0 +1,763 @@
|
|||||||
|
#include "otto_movements.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
#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));
|
||||||
|
}
|
||||||
|
|
||||||
|
///////////////////////////////////////////////////////////////////
|
||||||
|
//-- 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(500, 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 == -1) {
|
||||||
|
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: 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
//---------------------------------------------------------
|
||||||
|
//-- 手部动作: 举手
|
||||||
|
//-- Parameters:
|
||||||
|
//-- period: 动作时间
|
||||||
|
//-- dir: 方向 1=左手, -1=右手, 0=双手
|
||||||
|
//---------------------------------------------------------
|
||||||
|
void Otto::HandsUp(int period, int dir) {
|
||||||
|
if (!has_hands_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int initial[SERVO_COUNT] = {90, 90, 90, 90, HAND_HOME_POSITION, 180 - HAND_HOME_POSITION};
|
||||||
|
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 == 1) {
|
||||||
|
target[LEFT_HAND] = 170;
|
||||||
|
target[RIGHT_HAND] = servo_[RIGHT_HAND].GetPosition();
|
||||||
|
} else if (dir == -1) {
|
||||||
|
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 == 1) {
|
||||||
|
target[RIGHT_HAND] = servo_[RIGHT_HAND].GetPosition();
|
||||||
|
} else if (dir == -1) {
|
||||||
|
target[LEFT_HAND] = servo_[LEFT_HAND].GetPosition();
|
||||||
|
}
|
||||||
|
|
||||||
|
MoveServos(period, target);
|
||||||
|
}
|
||||||
|
|
||||||
|
//---------------------------------------------------------
|
||||||
|
//-- 手部动作: 挥手
|
||||||
|
//-- Parameters:
|
||||||
|
//-- period: 动作周期
|
||||||
|
//-- dir: 方向 LEFT/RIGHT/BOTH
|
||||||
|
//---------------------------------------------------------
|
||||||
|
void Otto::HandWave(int period, int dir) {
|
||||||
|
if (!has_hands_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (dir == BOTH) {
|
||||||
|
HandWaveBoth(period);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int servo_index = (dir == LEFT) ? LEFT_HAND : RIGHT_HAND;
|
||||||
|
|
||||||
|
int current_positions[SERVO_COUNT];
|
||||||
|
for (int i = 0; i < SERVO_COUNT; i++) {
|
||||||
|
if (servo_pins_[i] != -1) {
|
||||||
|
current_positions[i] = servo_[i].GetPosition();
|
||||||
|
} else {
|
||||||
|
current_positions[i] = 90;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int position;
|
||||||
|
if (servo_index == LEFT_HAND) {
|
||||||
|
position = 170;
|
||||||
|
} else {
|
||||||
|
position = 10;
|
||||||
|
}
|
||||||
|
|
||||||
|
current_positions[servo_index] = position;
|
||||||
|
MoveServos(300, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(300));
|
||||||
|
|
||||||
|
// 左右摆动5次
|
||||||
|
for (int i = 0; i < 5; i++) {
|
||||||
|
if (servo_index == LEFT_HAND) {
|
||||||
|
current_positions[servo_index] = position - 30;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||||||
|
current_positions[servo_index] = position + 30;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
} else {
|
||||||
|
current_positions[servo_index] = position + 30;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||||||
|
current_positions[servo_index] = position - 30;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
}
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(period / 10));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (servo_index == LEFT_HAND) {
|
||||||
|
current_positions[servo_index] = HAND_HOME_POSITION;
|
||||||
|
} else {
|
||||||
|
current_positions[servo_index] = 180 - HAND_HOME_POSITION;
|
||||||
|
}
|
||||||
|
MoveServos(300, current_positions);
|
||||||
|
}
|
||||||
|
|
||||||
|
//---------------------------------------------------------
|
||||||
|
//-- 手部动作: 双手同时挥手
|
||||||
|
//-- Parameters:
|
||||||
|
//-- period: 动作周期
|
||||||
|
//---------------------------------------------------------
|
||||||
|
void Otto::HandWaveBoth(int period) {
|
||||||
|
if (!has_hands_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int current_positions[SERVO_COUNT];
|
||||||
|
for (int i = 0; i < SERVO_COUNT; i++) {
|
||||||
|
if (servo_pins_[i] != -1) {
|
||||||
|
current_positions[i] = servo_[i].GetPosition();
|
||||||
|
} else {
|
||||||
|
current_positions[i] = 90;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int left_position = 170;
|
||||||
|
int right_position = 10;
|
||||||
|
|
||||||
|
current_positions[LEFT_HAND] = left_position;
|
||||||
|
current_positions[RIGHT_HAND] = right_position;
|
||||||
|
MoveServos(300, current_positions);
|
||||||
|
|
||||||
|
// 左右摆动5次
|
||||||
|
for (int i = 0; i < 5; i++) {
|
||||||
|
// 波浪向左
|
||||||
|
current_positions[LEFT_HAND] = left_position - 30;
|
||||||
|
current_positions[RIGHT_HAND] = right_position + 30;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
|
||||||
|
// 波浪向右
|
||||||
|
current_positions[LEFT_HAND] = left_position + 30;
|
||||||
|
current_positions[RIGHT_HAND] = right_position - 30;
|
||||||
|
MoveServos(period / 10, current_positions);
|
||||||
|
}
|
||||||
|
|
||||||
|
current_positions[LEFT_HAND] = HAND_HOME_POSITION;
|
||||||
|
current_positions[RIGHT_HAND] = 180 - HAND_HOME_POSITION;
|
||||||
|
MoveServos(300, current_positions);
|
||||||
|
}
|
||||||
|
|
||||||
|
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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
105
main/boards/otto-robot/otto_movements.h
Normal file
105
main/boards/otto-robot/otto_movements.h
Normal file
@ -0,0 +1,105 @@
|
|||||||
|
#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);
|
||||||
|
|
||||||
|
//-- 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 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 HandsUp(int period = 1000, int dir = 0); // 双手举起
|
||||||
|
void HandsDown(int period = 1000, int dir = 0); // 双手放下
|
||||||
|
void HandWave(int period = 1000, int dir = LEFT); // 挥手
|
||||||
|
void HandWaveBoth(int period = 1000); // 双手同时挥手
|
||||||
|
|
||||||
|
// -- 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__
|
||||||
138
main/boards/otto-robot/otto_robot.cc
Normal file
138
main/boards/otto-robot/otto_robot.cc
Normal file
@ -0,0 +1,138 @@
|
|||||||
|
#include <driver/i2c_master.h>
|
||||||
|
#include <driver/spi_common.h>
|
||||||
|
#include <esp_lcd_panel_io.h>
|
||||||
|
#include <esp_lcd_panel_ops.h>
|
||||||
|
#include <esp_lcd_panel_vendor.h>
|
||||||
|
#include <esp_log.h>
|
||||||
|
#include <wifi_station.h>
|
||||||
|
|
||||||
|
#include "application.h"
|
||||||
|
#include "audio_codecs/no_audio_codec.h"
|
||||||
|
#include "button.h"
|
||||||
|
#include "config.h"
|
||||||
|
#include "display/lcd_display.h"
|
||||||
|
#include "iot/thing_manager.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"
|
||||||
|
|
||||||
|
#define TAG "OttoRobot"
|
||||||
|
|
||||||
|
LV_FONT_DECLARE(font_puhui_16_4);
|
||||||
|
LV_FONT_DECLARE(font_awesome_16_4);
|
||||||
|
|
||||||
|
extern void InitializeOttoController();
|
||||||
|
|
||||||
|
class OttoRobot : public WifiBoard {
|
||||||
|
private:
|
||||||
|
LcdDisplay* display_;
|
||||||
|
PowerManager* power_manager_;
|
||||||
|
Button boot_button_;
|
||||||
|
void InitializePowerManager() {
|
||||||
|
power_manager_ =
|
||||||
|
new PowerManager(POWER_CHARGE_DETECT_PIN, POWER_ADC_UNIT, POWER_ADC_CHANNEL);
|
||||||
|
}
|
||||||
|
|
||||||
|
void InitializeSpi() {
|
||||||
|
spi_bus_config_t buscfg = {};
|
||||||
|
buscfg.mosi_io_num = DISPLAY_MOSI_PIN;
|
||||||
|
buscfg.miso_io_num = GPIO_NUM_NC;
|
||||||
|
buscfg.sclk_io_num = 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_LOGD(TAG, "Install panel IO");
|
||||||
|
esp_lcd_panel_io_spi_config_t io_config = {};
|
||||||
|
io_config.cs_gpio_num = DISPLAY_CS_PIN;
|
||||||
|
io_config.dc_gpio_num = 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_LOGD(TAG, "Install LCD driver");
|
||||||
|
esp_lcd_panel_dev_config_t panel_config = {};
|
||||||
|
panel_config.reset_gpio_num = 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,
|
||||||
|
{
|
||||||
|
.text_font = &font_puhui_16_4,
|
||||||
|
.icon_font = &font_awesome_16_4,
|
||||||
|
.emoji_font = DISPLAY_HEIGHT >= 240 ? font_emoji_64_init() : font_emoji_32_init(),
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
void InitializeButtons() {
|
||||||
|
boot_button_.OnClick([this]() {
|
||||||
|
auto& app = Application::GetInstance();
|
||||||
|
if (app.GetDeviceState() == kDeviceStateStarting &&
|
||||||
|
!WifiStation::GetInstance().IsConnected()) {
|
||||||
|
ResetWifiConfiguration();
|
||||||
|
}
|
||||||
|
app.ToggleChatState();
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
void InitializeOttoController() {
|
||||||
|
ESP_LOGI(TAG, "初始化Otto机器人MCP控制器");
|
||||||
|
::InitializeOttoController();
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
OttoRobot() : boot_button_(BOOT_BUTTON_GPIO) {
|
||||||
|
InitializeSpi();
|
||||||
|
InitializeLcdDisplay();
|
||||||
|
InitializeButtons();
|
||||||
|
InitializePowerManager();
|
||||||
|
InitializeOttoController();
|
||||||
|
GetBacklight()->RestoreBrightness();
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual AudioCodec* GetAudioCodec() override {
|
||||||
|
static NoAudioCodecSimplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE,
|
||||||
|
AUDIO_I2S_SPK_GPIO_BCLK, AUDIO_I2S_SPK_GPIO_LRCK,
|
||||||
|
AUDIO_I2S_SPK_GPIO_DOUT, AUDIO_I2S_MIC_GPIO_SCK,
|
||||||
|
AUDIO_I2S_MIC_GPIO_WS, AUDIO_I2S_MIC_GPIO_DIN);
|
||||||
|
return &audio_codec;
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual Display* GetDisplay() override { return display_; }
|
||||||
|
|
||||||
|
virtual Backlight* GetBacklight() override {
|
||||||
|
static PwmBacklight backlight(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;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
DECLARE_BOARD(OttoRobot);
|
||||||
128
main/boards/otto-robot/power_manager.h
Normal file
128
main/boards/otto-robot/power_manager.h
Normal file
@ -0,0 +1,128 @@
|
|||||||
|
#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[] = {{2150, 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;
|
||||||
|
|
||||||
|
adc_oneshot_unit_handle_t adc_handle_;
|
||||||
|
|
||||||
|
void CheckBatteryStatus() {
|
||||||
|
is_charging_ = gpio_get_level(charging_pin_) == 0;
|
||||||
|
ReadBatteryAdcData();
|
||||||
|
}
|
||||||
|
|
||||||
|
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) {
|
||||||
|
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_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_,
|
||||||
|
.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_; }
|
||||||
|
};
|
||||||
|
#endif // __POWER_MANAGER_H__
|
||||||
@ -31,6 +31,7 @@ dependencies:
|
|||||||
espressif2022/image_player: ^1.1.0
|
espressif2022/image_player: ^1.1.0
|
||||||
espressif/adc_mic: ^0.2.0
|
espressif/adc_mic: ^0.2.0
|
||||||
espressif/esp_mmap_assets: ">=1.2"
|
espressif/esp_mmap_assets: ">=1.2"
|
||||||
|
txp666/otto-emoji-gif-component: ~1.0.2
|
||||||
|
|
||||||
tny-robotics/sh1106-esp-idf:
|
tny-robotics/sh1106-esp-idf:
|
||||||
version: ^1.0.0
|
version: ^1.0.0
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user