forked from xiaozhi/xiaozhi-esp32
7435c98609
* 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 * fix(ota): 修复 ottoRobot和electronBot OTA 升级崩溃问题 bug * 1.增加robot舵机初始位置校准 2.fix(mcp_sever) 超出范围异常捕获类型 bug
377 lines
17 KiB
C++
377 lines
17 KiB
C++
/*
|
||
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"
|
||
#include "settings.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, // 连续点头
|
||
|
||
// 系统动作 21
|
||
ACTION_HOME = 21 // 复位到初始位置
|
||
};
|
||
|
||
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);
|
||
} else if (params.action_type == ACTION_HOME) {
|
||
// 复位动作
|
||
controller->electron_bot_.Home(true);
|
||
}
|
||
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_);
|
||
}
|
||
}
|
||
|
||
void LoadTrimsFromNVS() {
|
||
Settings settings("electron_trims", false);
|
||
|
||
int right_pitch = settings.GetInt("right_pitch", 0);
|
||
int right_roll = settings.GetInt("right_roll", 0);
|
||
int left_pitch = settings.GetInt("left_pitch", 0);
|
||
int left_roll = settings.GetInt("left_roll", 0);
|
||
int body = settings.GetInt("body", 0);
|
||
int head = settings.GetInt("head", 0);
|
||
electron_bot_.SetTrims(right_pitch, right_roll, left_pitch, left_roll, body, head);
|
||
}
|
||
|
||
public:
|
||
ElectronBotController() {
|
||
electron_bot_.Init(Right_Pitch_Pin, Right_Roll_Pin, Left_Pitch_Pin, Left_Roll_Pin, Body_Pin,
|
||
Head_Pin);
|
||
|
||
LoadTrimsFromNVS();
|
||
action_queue_ = xQueueCreate(10, sizeof(ElectronBotActionParams));
|
||
|
||
QueueAction(ACTION_HOME, 1, 1000, 0, 0);
|
||
|
||
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;
|
||
QueueAction(ACTION_HOME, 1, 1000, 0, 0);
|
||
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.electron.set_trim",
|
||
"校准单个舵机位置。设置指定舵机的微调参数以调整ElectronBot的初始姿态，设置将永久保存。"
|
||
"servo_type: 舵机类型(right_pitch:右臂旋转, right_roll:右臂推拉, left_pitch:左臂旋转, "
|
||
"left_roll:左臂推拉, body:身体, head:头部); "
|
||
"trim_value: 微调值(-30到30度)",
|
||
PropertyList({Property("servo_type", kPropertyTypeString, "right_pitch"),
|
||
Property("trim_value", kPropertyTypeInteger, 0, -30, 30)}),
|
||
[this](const PropertyList& properties) -> ReturnValue {
|
||
std::string servo_type = properties["servo_type"].value<std::string>();
|
||
int trim_value = properties["trim_value"].value<int>();
|
||
|
||
ESP_LOGI(TAG, "设置舵机微调: %s = %d度", servo_type.c_str(), trim_value);
|
||
|
||
// 获取当前所有微调值
|
||
Settings settings("electron_trims", true);
|
||
int right_pitch = settings.GetInt("right_pitch", 0);
|
||
int right_roll = settings.GetInt("right_roll", 0);
|
||
int left_pitch = settings.GetInt("left_pitch", 0);
|
||
int left_roll = settings.GetInt("left_roll", 0);
|
||
int body = settings.GetInt("body", 0);
|
||
int head = settings.GetInt("head", 0);
|
||
|
||
// 更新指定舵机的微调值
|
||
if (servo_type == "right_pitch") {
|
||
right_pitch = trim_value;
|
||
settings.SetInt("right_pitch", right_pitch);
|
||
} else if (servo_type == "right_roll") {
|
||
right_roll = trim_value;
|
||
settings.SetInt("right_roll", right_roll);
|
||
} else if (servo_type == "left_pitch") {
|
||
left_pitch = trim_value;
|
||
settings.SetInt("left_pitch", left_pitch);
|
||
} else if (servo_type == "left_roll") {
|
||
left_roll = trim_value;
|
||
settings.SetInt("left_roll", left_roll);
|
||
} else if (servo_type == "body") {
|
||
body = trim_value;
|
||
settings.SetInt("body", body);
|
||
} else if (servo_type == "head") {
|
||
head = trim_value;
|
||
settings.SetInt("head", head);
|
||
} else {
|
||
return "错误：无效的舵机类型，请使用: right_pitch, right_roll, left_pitch, "
|
||
"left_roll, body, head";
|
||
}
|
||
|
||
electron_bot_.SetTrims(right_pitch, right_roll, left_pitch, left_roll, body, head);
|
||
|
||
QueueAction(ACTION_HOME, 1, 500, 0, 0);
|
||
|
||
return "舵机 " + servo_type + " 微调设置为 " + std::to_string(trim_value) +
|
||
" 度，已永久保存";
|
||
});
|
||
|
||
mcp_server.AddTool("self.electron.get_trims", "获取当前的舵机微调设置", PropertyList(),
|
||
[this](const PropertyList& properties) -> ReturnValue {
|
||
Settings settings("electron_trims", false);
|
||
|
||
int right_pitch = settings.GetInt("right_pitch", 0);
|
||
int right_roll = settings.GetInt("right_roll", 0);
|
||
int left_pitch = settings.GetInt("left_pitch", 0);
|
||
int left_roll = settings.GetInt("left_roll", 0);
|
||
int body = settings.GetInt("body", 0);
|
||
int head = settings.GetInt("head", 0);
|
||
|
||
std::string result =
|
||
"{\"right_pitch\":" + std::to_string(right_pitch) +
|
||
",\"right_roll\":" + std::to_string(right_roll) +
|
||
",\"left_pitch\":" + std::to_string(left_pitch) +
|
||
",\"left_roll\":" + std::to_string(left_roll) +
|
||
",\"body\":" + std::to_string(body) +
|
||
",\"head\":" + std::to_string(head) + "}";
|
||
|
||
ESP_LOGI(TAG, "获取微调设置: %s", result.c_str());
|
||
return result;
|
||
});
|
||
|
||
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工具");
|
||
}
|
||
}
|