首页 > 分享 > ESP IDF+ESP32S3小智AI+4舵机=手搓一个桌面智能小狗狗（1）

ESP IDF+ESP32S3小智AI+4舵机=手搓一个桌面智能小狗狗（1）

萌宠菠菠乐园
2026-02-19 09:06

#include "dog_controller.h"#include "mcp_server.h"#include <esp_log.h> #define TAG "DogController" DogController::DogController() : current_action_(ACTION_STAND) , is_moving_(false) { // 初始化四个舵机控制器，使用不同的LEDC通道 servos_.push_back(new ServoController(SERVO_LF_GPIO, LEDC_CHANNEL_0, LEDC_TIMER_0)); servos_.push_back(new ServoController(SERVO_RF_GPIO, LEDC_CHANNEL_1, LEDC_TIMER_0)); servos_.push_back(new ServoController(SERVO_LB_GPIO, LEDC_CHANNEL_2, LEDC_TIMER_0)); servos_.push_back(new ServoController(SERVO_RB_GPIO, LEDC_CHANNEL_3, LEDC_TIMER_0)); InitializeCalibration();} DogController::~DogController() { StopAll(); for (auto servo : servos_) { if (servo != nullptr) { delete servo; } } servos_.clear();} bool DogController::Initialize() { ESP_LOGI(TAG, "初始化四舵机机器狗控制器"); // 初始化所有舵机 for (int i = 0; i < servos_.size(); i++) { if (servos_[i] != nullptr && !servos_[i]->Initialize()) { ESP_LOGE(TAG, "舵机 %d 初始化失败", i); return false; } } // 设置初始站立位置 vTaskDelay(pdMS_TO_TICKS(1000)); // 等待所有舵机就绪 Stand(); ESP_LOGI(TAG, "机器狗控制器初始化成功"); return true;} void DogController::InitializeCalibration() { // 根据您的描述：左侧腿舵机小角度向前摆，右侧腿大角度向前摆 // 这里需要根据实际机械结构调整这些参数 // 左前腿校准 leg_calibration_[LEG_LEFT_FRONT] = { .stand_angle = 90, // 站立中间位置 .sit_angle = 60, // 坐下角度 .lie_angle = 20, // 趴下角度 .forward_offset = -30 // 向前摆动偏移（小角度） }; // 右前腿校准 leg_calibration_[LEG_RIGHT_FRONT] = { .stand_angle = 90, .sit_angle = 120, // 对称角度 .lie_angle = 160, // 对称角度 .forward_offset = 30 // 向前摆动偏移（大角度） }; // 左后腿校准 leg_calibration_[LEG_LEFT_BACK] = { .stand_angle = 90, .sit_angle = 45, // 对称角度 .lie_angle = 30, // 对称角度 .forward_offset = -30 // 向前摆动偏移（小角度） }; // 右后腿校准 leg_calibration_[LEG_RIGHT_BACK] = { .stand_angle = 90, .sit_angle = 135, // 对称角度 .lie_angle = 150, // 对称角度 .forward_offset = 30 // 向前摆动偏移（大角度） };} void DogController::Stand() { if (is_moving_) return; ESP_LOGI(TAG, "执行起立站立动作"); current_action_ = ACTION_STAND; is_moving_ = true; ExecuteStand(); is_moving_ = false;} void DogController::Sit() { if (is_moving_) return; ESP_LOGI(TAG, "执行坐下动作"); current_action_ = ACTION_SIT; is_moving_ = true; ExecuteSit(); is_moving_ = false;} void DogController::LieDown() { if (is_moving_) return; ESP_LOGI(TAG, "执行趴下动作"); current_action_ = ACTION_LIE_DOWN; is_moving_ = true; ExecuteLieDown(); is_moving_ = false;} void DogController::Stretch() { if (is_moving_) return; ESP_LOGI(TAG, "执行伸懒腰动作"); current_action_ = ACTION_STRETCH; is_moving_ = true; ExecuteStretch(); is_moving_ = false;} void DogController::Wave() { if (is_moving_) return; ESP_LOGI(TAG, "执行挥手招手打招呼动作"); current_action_ = ACTION_WAVE; is_moving_ = true; ExecuteWave(); is_moving_ = false;} void DogController::WalkForward(int steps) { if (is_moving_) return; ESP_LOGI(TAG, "执行前进动作，步数: %d", steps); current_action_ = ACTION_WALK_FORWARD; is_moving_ = true; ExecuteWalkForward(steps); is_moving_ = false;} void DogController::WalkBackward(int steps) { if (is_moving_) return; ESP_LOGI(TAG, "执行后退动作，步数: %d", steps); current_action_ = ACTION_WALK_BACKWARD; is_moving_ = true; ExecuteWalkBackward(steps); is_moving_ = false;} void DogController::ShakeBody() { if (is_moving_) return; ESP_LOGI(TAG, "执行摇摆身体动作"); current_action_ = ACTION_SHAKE_BODY; is_moving_ = true; ExecuteShakeBody(); is_moving_ = false;} void DogController::Dance() { if (is_moving_) return; ESP_LOGI(TAG, "执行跳舞动作"); current_action_ = ACTION_DANCE; is_moving_ = true; ExecuteDance(); is_moving_ = false;} void DogController::Bow() { if (is_moving_) return; ESP_LOGI(TAG, "执行鞠躬动作"); current_action_ = ACTION_BOW; is_moving_ = true; ExecuteBow(); is_moving_ = false;} void DogController::ShakeHead() { if (is_moving_) return; ESP_LOGI(TAG, "执行摇头动作"); current_action_ = ACTION_SHAKE_HEAD; is_moving_ = true; ExecuteShakeHead(); is_moving_ = false;} void DogController::StopAll() { ESP_LOGI(TAG, "停止所有动作"); for (auto servo : servos_) { if (servo != nullptr) { servo->Stop(); } } is_moving_ = false;} bool DogController::IsMoving() const { return is_moving_;} void DogController::SetLegAngle(LegType leg, int angle, int speed_ms) { if (leg < servos_.size() && servos_[leg] != nullptr) { servos_[leg]->SetAngle(angle); // 注意：这里需要等待动作完成，可以使用回调或延时 vTaskDelay(pdMS_TO_TICKS(speed_ms)); }} void DogController::SetAllLegs(int lf_angle, int rf_angle, int lb_angle, int rb_angle, int speed_ms) { if (servos_[LEG_LEFT_FRONT] != nullptr) { servos_[LEG_LEFT_FRONT]->SetAngle(lf_angle); } if (servos_[LEG_RIGHT_FRONT] != nullptr) { servos_[LEG_RIGHT_FRONT]->SetAngle(rf_angle); } if (servos_[LEG_LEFT_BACK] != nullptr) { servos_[LEG_LEFT_BACK]->SetAngle(lb_angle); } if (servos_[LEG_RIGHT_BACK] != nullptr) { servos_[LEG_RIGHT_BACK]->SetAngle(rb_angle); } vTaskDelay(pdMS_TO_TICKS(speed_ms));} bool DogController::AllServosReady() const { for (auto servo : servos_) { if (servo != nullptr && servo->IsMoving()) { return false; } } return true;} void DogController::ExecuteStand() { // 站立姿势：所有腿在中间位置 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle , leg_calibration_[LEG_RIGHT_FRONT].stand_angle , leg_calibration_[LEG_LEFT_BACK].stand_angle , leg_calibration_[LEG_RIGHT_BACK].stand_angle , 400 );} void DogController::ExecuteSit() { // 坐下姿势：前腿稍微弯曲，后腿大幅度弯曲 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle, leg_calibration_[LEG_RIGHT_FRONT].stand_angle, leg_calibration_[LEG_LEFT_BACK].sit_angle, leg_calibration_[LEG_RIGHT_BACK].sit_angle, 400 );} void DogController::ExecuteLieDown() { // 趴下姿势：所有腿都贴近身体 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].lie_angle, leg_calibration_[LEG_RIGHT_FRONT].lie_angle, leg_calibration_[LEG_LEFT_BACK].lie_angle, leg_calibration_[LEG_RIGHT_BACK].lie_angle, 600 );} void DogController::ExecuteStretch() { // 伸懒腰：前腿向前伸展，后腿向后伸展 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle - 60 , leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 60, leg_calibration_[LEG_LEFT_BACK].stand_angle + 60, leg_calibration_[LEG_RIGHT_BACK].stand_angle - 60, 400 ); vTaskDelay(pdMS_TO_TICKS(1000)); // 回到站立姿势 ExecuteStand();} void DogController::ExecuteWave() { // 回到坐姿 ExecuteSit(); // 挥手：右前腿 waving 动作 for (int i = 0; i < 3; i++) { SetLegAngle(LEG_RIGHT_FRONT, leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 90, 150); vTaskDelay(pdMS_TO_TICKS(150)); SetLegAngle(LEG_RIGHT_FRONT, leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 60, 150); vTaskDelay(pdMS_TO_TICKS(150)); } // 回到站立姿势 ExecuteStand(); } void DogController::ExecuteWalkForward(int steps) { for (int step = 0; step < steps ; step++) { // 步态1：对角线腿抬起 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle + leg_calibration_[LEG_LEFT_FRONT].forward_offset, leg_calibration_[LEG_RIGHT_FRONT].stand_angle , leg_calibration_[LEG_LEFT_BACK].stand_angle , leg_calibration_[LEG_RIGHT_BACK].stand_angle + leg_calibration_[LEG_RIGHT_BACK].forward_offset, 300 ); vTaskDelay(pdMS_TO_TICKS(100)); // 步态2：另一对角线腿抬起 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle , leg_calibration_[LEG_RIGHT_FRONT].stand_angle - leg_calibration_[LEG_RIGHT_FRONT].forward_offset, leg_calibration_[LEG_LEFT_BACK].stand_angle - leg_calibration_[LEG_LEFT_BACK].forward_offset, leg_calibration_[LEG_RIGHT_BACK].stand_angle , 300 ); vTaskDelay(pdMS_TO_TICKS(100)); // 步态3：对角线腿抬起 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle , leg_calibration_[LEG_RIGHT_FRONT].stand_angle + leg_calibration_[LEG_RIGHT_FRONT].forward_offset, leg_calibration_[LEG_LEFT_BACK].stand_angle + leg_calibration_[LEG_LEFT_BACK].forward_offset, leg_calibration_[LEG_RIGHT_BACK].stand_angle , 300 ); vTaskDelay(pdMS_TO_TICKS(100)); // 步态4：另一对角线腿抬起 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle - leg_calibration_[LEG_LEFT_FRONT].forward_offset, leg_calibration_[LEG_RIGHT_FRONT].stand_angle , leg_calibration_[LEG_LEFT_BACK].stand_angle , leg_calibration_[LEG_RIGHT_BACK].stand_angle - leg_calibration_[LEG_RIGHT_BACK].forward_offset, 300 ); vTaskDelay(pdMS_TO_TICKS(100)); } // 回到站立姿势 ExecuteStand();} void DogController::ExecuteWalkBackward(int steps) { for (int step = 0; step < steps ; step++) { // 后退步态1：与前进相反的对角线运动 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle - leg_calibration_[LEG_LEFT_FRONT].forward_offset, leg_calibration_[LEG_RIGHT_FRONT].stand_angle, leg_calibration_[LEG_LEFT_BACK].stand_angle, leg_calibration_[LEG_RIGHT_BACK].stand_angle - leg_calibration_[LEG_RIGHT_BACK].forward_offset, 300 ); vTaskDelay(pdMS_TO_TICKS(100)); // 后退步态2：另一对角线 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle, leg_calibration_[LEG_RIGHT_FRONT].stand_angle + leg_calibration_[LEG_RIGHT_FRONT].forward_offset, leg_calibration_[LEG_LEFT_BACK].stand_angle + leg_calibration_[LEG_LEFT_BACK].forward_offset, leg_calibration_[LEG_RIGHT_BACK].stand_angle, 300 ); vTaskDelay(pdMS_TO_TICKS(100)); // 后退步态3 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle, leg_calibration_[LEG_RIGHT_FRONT].stand_angle - leg_calibration_[LEG_RIGHT_FRONT].forward_offset, leg_calibration_[LEG_LEFT_BACK].stand_angle - leg_calibration_[LEG_LEFT_BACK].forward_offset, leg_calibration_[LEG_RIGHT_BACK].stand_angle, 300 ); vTaskDelay(pdMS_TO_TICKS(100)); // 后退步态4 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle + leg_calibration_[LEG_LEFT_FRONT].forward_offset, leg_calibration_[LEG_RIGHT_FRONT].stand_angle, leg_calibration_[LEG_LEFT_BACK].stand_angle, leg_calibration_[LEG_RIGHT_BACK].stand_angle + leg_calibration_[LEG_RIGHT_BACK].forward_offset, 300 ); vTaskDelay(pdMS_TO_TICKS(100)); } // 回到站立姿势 ExecuteStand();} void DogController::ExecuteShakeBody() { // 身体左右摇摆：左右腿交替倾斜 for (int i = 0; i < 4; i++) { // 向左倾斜 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle - 20, leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 20, leg_calibration_[LEG_LEFT_BACK].stand_angle - 20, leg_calibration_[LEG_RIGHT_BACK].stand_angle + 20, 200 ); vTaskDelay(pdMS_TO_TICKS(100)); // 向右倾斜 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle + 20, leg_calibration_[LEG_RIGHT_FRONT].stand_angle - 20, leg_calibration_[LEG_LEFT_BACK].stand_angle + 20, leg_calibration_[LEG_RIGHT_BACK].stand_angle - 20, 200 ); vTaskDelay(pdMS_TO_TICKS(100)); } // 回到站立姿势 ExecuteStand();} void DogController::ExecuteDance() { // 跳舞动作：一系列有趣的动作组合 // 1. 抬起前腿 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle + 60, leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 60, leg_calibration_[LEG_LEFT_BACK].stand_angle, leg_calibration_[LEG_RIGHT_BACK].stand_angle, 200 ); vTaskDelay(pdMS_TO_TICKS(100)); // 2. 放下前腿，抬起后腿 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle, leg_calibration_[LEG_RIGHT_FRONT].stand_angle, leg_calibration_[LEG_LEFT_BACK].stand_angle - 60, leg_calibration_[LEG_RIGHT_BACK].stand_angle - 60, 200 ); vTaskDelay(pdMS_TO_TICKS(100)); // 3. 快速左右摇摆 for (int i = 0; i < 3; i++) { SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle - 30, leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 30, leg_calibration_[LEG_LEFT_BACK].stand_angle - 30, leg_calibration_[LEG_RIGHT_BACK].stand_angle + 30, 200 ); vTaskDelay(pdMS_TO_TICKS(100)); SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle + 30, leg_calibration_[LEG_RIGHT_FRONT].stand_angle - 30, leg_calibration_[LEG_LEFT_BACK].stand_angle + 30, leg_calibration_[LEG_RIGHT_BACK].stand_angle - 30, 200 ); vTaskDelay(pdMS_TO_TICKS(100)); } // 回到站立姿势 ExecuteStand();} void DogController::ExecuteBow() { // 鞠躬动作：前腿弯曲，身体前倾 SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].sit_angle, leg_calibration_[LEG_RIGHT_FRONT].sit_angle, leg_calibration_[LEG_LEFT_BACK].stand_angle, leg_calibration_[LEG_RIGHT_BACK].stand_angle, 500 ); vTaskDelay(pdMS_TO_TICKS(1000)); // 回到站立姿势 ExecuteStand();} void DogController::ExecuteShakeHead() { // 摇头动作：通过前腿的交替运动模拟摇头 for (int i = 0; i < 3; i++) { SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle - 40, leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 40, leg_calibration_[LEG_LEFT_BACK].stand_angle, leg_calibration_[LEG_RIGHT_BACK].stand_angle, 200 ); vTaskDelay(pdMS_TO_TICKS(100)); SetAllLegs( leg_calibration_[LEG_LEFT_FRONT].stand_angle + 40, leg_calibration_[LEG_RIGHT_FRONT].stand_angle - 40, leg_calibration_[LEG_LEFT_BACK].stand_angle, leg_calibration_[LEG_RIGHT_BACK].stand_angle, 200 ); vTaskDelay(pdMS_TO_TICKS(100)); } // 回到站立姿势 ExecuteStand();} void DogController::InitializeTools() { auto& mcp_server = McpServer::GetInstance(); ESP_LOGI(TAG, "开始注册机器狗MCP工具..."); // 站立 mcp_server.AddTool("self.dog.stand", "机器狗站立姿势", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { Stand(); return "机器狗已站立"; }); // 坐下 mcp_server.AddTool("self.dog.sit", "机器狗坐下", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { Sit(); return "机器狗已坐下"; }); // 趴下 mcp_server.AddTool("self.dog.lie_down", "机器狗趴下", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { LieDown(); return "机器狗已趴下"; }); // 伸懒腰 mcp_server.AddTool("self.dog.stretch", "机器狗伸懒腰", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { Stretch(); return "机器狗在伸懒腰"; }); // 挥手打招呼 mcp_server.AddTool("self.dog.wave", "机器狗挥手打招呼", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { Wave(); return "机器狗在挥手打招呼"; }); // 前进 mcp_server.AddTool("self.dog.walk_forward", "机器狗前进。steps: 步数(1-10)", PropertyList({Property("steps", kPropertyTypeInteger, 8, 1, 10)}), [this](const PropertyList& properties) -> ReturnValue { int steps = properties["steps"].value<int>(); WalkForward(steps); return "机器狗前进 " + std::to_string(steps) + " 步"; }); // 后退 mcp_server.AddTool("self.dog.walk_backward", "机器狗后退。steps: 步数(1-10)", PropertyList({Property("steps", kPropertyTypeInteger, 8, 1, 10)}), [this](const PropertyList& properties) -> ReturnValue { int steps = properties["steps"].value<int>(); WalkBackward(steps); return "机器狗后退 " + std::to_string(steps) + " 步"; }); // 摇摆身体 mcp_server.AddTool("self.dog.shake_body", "机器狗摇摆身体", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { ShakeBody(); return "机器狗在摇摆身体"; }); // 跳舞 mcp_server.AddTool("self.dog.dance", "机器狗跳舞", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { Dance(); return "机器狗在跳舞"; }); // 鞠躬 mcp_server.AddTool("self.dog.bow", "机器狗鞠躬", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { Bow(); return "机器狗在鞠躬"; }); // 摇头 mcp_server.AddTool("self.dog.shake_head", "机器狗摇头", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { ShakeHead(); return "机器狗在摇头"; }); // 停止所有动作 mcp_server.AddTool("self.dog.stop", "停止机器狗所有动作", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { StopAll(); return "机器狗已停止"; }); // 获取状态 mcp_server.AddTool("self.dog.get_status", "获取机器狗当前状态", PropertyList(), [this](const PropertyList& properties) -> ReturnValue { std::string action_name; switch (GetCurrentAction()) { case ACTION_STAND: action_name = "站立"; break; case ACTION_SIT: action_name = "坐下"; break; case ACTION_LIE_DOWN: action_name = "趴下"; break; case ACTION_STRETCH: action_name = "伸懒腰"; break; case ACTION_WAVE: action_name = "挥手"; break; case ACTION_WALK_FORWARD: action_name = "前进"; break; case ACTION_WALK_BACKWARD: action_name = "后退"; break; case ACTION_SHAKE_BODY: action_name = "摇摆身体"; break; case ACTION_DANCE: action_name = "跳舞"; break; case ACTION_BOW: action_name = "鞠躬"; break; case ACTION_SHAKE_HEAD: action_name = "摇头"; break; default: action_name = "未知"; } return "当前状态: " + action_name + ", 是否运动中: " + (IsMoving() ? "是" : "否"); }); ESP_LOGI(TAG, "机器狗MCP工具注册完成");}