ESP IDF+ESP32S3小智AI+4舵机=手搓桌面智能小狗狗代码优化（2）- Makelog(造物记)

10.9

【目标愿景】

这个帖子是ESP IDF+ESP32S3小智AI+4舵机=手搓一个桌面智能小狗狗- Makelog(造物记)的后续。

这个桌面小智AI+电子宠物，自己手搓出来的它，虽不完美，我是很喜欢的。

因为喜欢，就想让它有更多进步。比如上一帖子中的AI狗狗基于闪电大佬的舵机平滑运动代码完成的，运动是平滑的，这点很棒，可是却显得AI小狗不够活泼，小小的狗子却稳如老狗……并且动作不够丰富，所以在这个帖子中将记录代码的调整与优化，让更加它活泼灵动且动作丰富。

目标：

1.让动作更活泼，即取消平滑运动，采用直接设置角度，并减少动作间的延迟。

2. 增加动作种类。

材料清单

面包板小智AI X1
MG90 180度舵机 X4
3.7V锂电池 X1

步骤1 小智AI桌面宠物小狗的制作

ESP IDF+ESP32S3小智AI+4舵机=手搓一个桌面智能小狗狗- Makelog(造物记)

请注意舵机与腿的安装位置，舵机与ESP32S3开发板GPIO口对应，不同的组装方法、不同的舵机相对位置、腿的长度都会需要调整动作控制代码中舵机的角度以达到理想效果。比如左转、右转两个动作就花费了我大量时间来调试，即使现在也不是最优。

动作代码参考了@米士古大佬的代码。

步骤2 灵魂代码，不断优化

关键改进总结

取消平滑运动：动作更加快速直接

增加快速设置方法：避免不必要的延迟

丰富动作库：从6个动作扩展到10+个动作

活泼化设计：模仿真实小狗的活泼行为

保持兼容性：原有动作仍然可用

1、软件架构

本项目代码依然是在@闪电蘑菇大佬开源的小智源码上修改。

2、现有动作如下图所示：

小智AI小狗新版代码有以下6个文件组成：

1. config.h - 硬件配置定义

定义舵机引脚：SERVO_LF_GPIO、SERVO_RF_GPIO、SERVO_LB_GPIO、SERVO_RB_GPIO

舵机参数配置：脉宽、角度范围、默认位置等

代码

#ifndef _BOARD_CONFIG_H_
#define _BOARD_CONFIG_H_

#include <driver/gpio.h>

#define AUDIO_INPUT_SAMPLE_RATE  16000
#define AUDIO_OUTPUT_SAMPLE_RATE 24000

// 如果使用 Duplex I2S 模式，请注释下面一行
#define AUDIO_I2S_METHOD_SIMPLEX

#ifdef 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

#else

#define AUDIO_I2S_GPIO_WS GPIO_NUM_4
#define AUDIO_I2S_GPIO_BCLK GPIO_NUM_5
#define AUDIO_I2S_GPIO_DIN  GPIO_NUM_6
#define AUDIO_I2S_GPIO_DOUT GPIO_NUM_7

#endif


#define BUILTIN_LED_GPIO        GPIO_NUM_48
#define BOOT_BUTTON_GPIO        GPIO_NUM_0
#define TOUCH_BUTTON_GPIO       GPIO_NUM_47
#define VOLUME_UP_BUTTON_GPIO   GPIO_NUM_40
#define VOLUME_DOWN_BUTTON_GPIO GPIO_NUM_39

#define DISPLAY_SDA_PIN GPIO_NUM_41
#define DISPLAY_SCL_PIN GPIO_NUM_42
#define DISPLAY_WIDTH   128

#if CONFIG_OLED_SSD1306_128X32
#define DISPLAY_HEIGHT  32
#elif CONFIG_OLED_SSD1306_128X64
#define DISPLAY_HEIGHT  64
#elif CONFIG_OLED_SH1106_128X64
#define DISPLAY_HEIGHT  64
#define SH1106
#else
#error "未选择 OLED 屏幕类型"
#endif

#define DISPLAY_MIRROR_X true
#define DISPLAY_MIRROR_Y true

// 四舵机机器狗引脚定义
#define SERVO_LF_GPIO GPIO_NUM_9    // 左前腿
#define SERVO_RF_GPIO GPIO_NUM_10   // 右前腿  
#define SERVO_LB_GPIO GPIO_NUM_11   // 左后退
#define SERVO_RB_GPIO GPIO_NUM_12   // 右后退

// 舵机参数配置
#define SERVO_MIN_PULSEWIDTH_US 500           // 最小脉宽（微秒）对应0度
#define SERVO_MAX_PULSEWIDTH_US 2500          // 最大脉宽（微秒）对应180度
#define SERVO_MIN_DEGREE 0                    // 最小角度
#define SERVO_MAX_DEGREE 180                  // 最大角度
#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000  // 1MHz, 1us per tick
#define SERVO_TIMEBASE_PERIOD 20000           // 20000 ticks, 20ms (50Hz)

// 舵机默认位置和限制
#define SERVO_DEFAULT_ANGLE 90                // 默认中心位置
#define SERVO_MAX_SPEED_DEGREE_PER_SEC 180    // 最大转速限制

// 板子版本信息
#define BREAD_COMPACT_WIFI_WITH_SERVO_VERSION "1.0.0"

#endif // _BOARD_CONFIG_H_

2. servo_controller.h - 单舵机控制器头文件

定义 ServoController 类

舵机基本控制方法声明

MCP工具接口

代码

#ifndef __SERVO_CONTROLLER_H__
#define __SERVO_CONTROLLER_H__

#include <driver/ledc.h>
#include <driver/gpio.h>
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <functional>
#include "config.h"
#include "mcp_server.h"

class ServoController {
public:
    // 两个构造函数都要声明
    ServoController(gpio_num_t servo_pin, ledc_channel_t ledc_channel = LEDC_CHANNEL_0, ledc_timer_t ledc_timer = LEDC_TIMER_0);
    ServoController(gpio_num_t servo_pin); // 单参数构造函数
    ~ServoController();

    // 基本控制方法
    bool Initialize();
    void InitializeTools();
    void SetAngle(int angle);
    int GetCurrentAngle() const { return current_angle_; }
    
    // 新增快速设置方法
    void SetAngleDirect(int angle);
    
    // 运动控制方法
    void RotateClockwise(int degrees);
    void RotateCounterclockwise(int degrees);
    void SweepBetween(int min_angle, int max_angle, int speed_ms = 1000);
    void Stop();
    void Reset();
    
    // 状态查询
    bool IsMoving() const { return is_moving_; }
    bool IsSweeping() const { return is_sweeping_; }
    
    // 设置回调函数
    void SetOnMoveCompleteCallback(std::function<void()> callback) {
        on_move_complete_callback_ = callback;
    }

private:
    // 硬件相关
    gpio_num_t servo_pin_;
    ledc_channel_t ledc_channel_;
    ledc_timer_t ledc_timer_;
    
    // 状态变量
    int current_angle_;
    int target_angle_;
    bool is_moving_;
    bool is_sweeping_;
    bool stop_requested_;
    
    // 任务和队列
    TaskHandle_t servo_task_handle_;
    QueueHandle_t command_queue_;
    
    // 回调函数
    std::function<void()> on_move_complete_callback_;
    
    // 命令类型
    enum CommandType {
        CMD_SET_ANGLE,
        CMD_ROTATE_CW,
        CMD_ROTATE_CCW,
        CMD_SWEEP,
        CMD_STOP,
        CMD_RESET
    };
    
    // 命令结构
    struct ServoCommand {
        CommandType type;
        int param1;
        int param2;
        int param3;
    };
    
    // 私有方法
    void WriteAngle(int angle);
    uint32_t AngleToCompare(int angle);
    bool IsValidAngle(int angle) const;
    int ConstrainAngle(int angle) const;
    
    // 任务函数
    static void ServoTask(void* parameter);
    void ProcessCommands();
    void ExecuteSetAngle(int angle);
    void ExecuteRotate(int degrees, bool clockwise);
    void ExecuteSweep(int min_angle, int max_angle, int speed_ms);
};

#endif // __SERVO_CONTROLLER_H__

3. servo_controller.cc - 单舵机控制器实现

ServoController 类的具体实现

PWM控制、角度转换等核心功能

MCP工具注册

代码

#include "servo_controller.h"
#include <esp_log.h>
#include <cmath>

#define TAG "ServoController"

// 三参数构造函数
ServoController::ServoController(gpio_num_t servo_pin, ledc_channel_t ledc_channel, ledc_timer_t ledc_timer)
    : servo_pin_(servo_pin)
    , ledc_channel_(ledc_channel)
    , ledc_timer_(ledc_timer)
    , current_angle_(SERVO_DEFAULT_ANGLE)
    , target_angle_(SERVO_DEFAULT_ANGLE)
    , is_moving_(false)
    , is_sweeping_(false)
    , stop_requested_(false)
    , servo_task_handle_(nullptr)
    , command_queue_(nullptr)
    , on_move_complete_callback_(nullptr) {
}

// 单参数构造函数（调用三参数构造函数）
ServoController::ServoController(gpio_num_t servo_pin)
    : ServoController(servo_pin, LEDC_CHANNEL_0, LEDC_TIMER_0) {
}

ServoController::~ServoController() {
    Stop();
    if (servo_task_handle_ != nullptr) {
        vTaskDelete(servo_task_handle_);
    }
    if (command_queue_ != nullptr) {
        vQueueDelete(command_queue_);
    }
}

bool ServoController::Initialize() {
    ESP_LOGI(TAG, "初始化SG90舵机控制器，引脚: %d", servo_pin_);
    
    // 配置LEDC定时器
    ledc_timer_config_t timer_config = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .duty_resolution = LEDC_TIMER_14_BIT,
        .timer_num = ledc_timer_,
        .freq_hz = 50,
        .clk_cfg = LEDC_AUTO_CLK
    };
    
    esp_err_t ret = ledc_timer_config(&timer_config);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "LEDC定时器配置失败: %s", esp_err_to_name(ret));
        return false;
    }
    
    // 配置LEDC通道
    ledc_channel_config_t channel_config = {
        .gpio_num = servo_pin_,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = ledc_channel_,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = ledc_timer_,
        .duty = 0,
        .hpoint = 0
    };
    
    ret = ledc_channel_config(&channel_config);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "LEDC通道配置失败: %s", esp_err_to_name(ret));
        return false;
    }
    
    // 创建命令队列
    command_queue_ = xQueueCreate(10, sizeof(ServoCommand));
    if (command_queue_ == nullptr) {
        ESP_LOGE(TAG, "创建命令队列失败");
        return false;
    }
    
    // 创建舵机控制任务
    BaseType_t task_ret = xTaskCreate(
        ServoTask,
        "servo_task",
        4096,
        this,
        5,
        &servo_task_handle_
    );
    
    if (task_ret != pdPASS) {
        ESP_LOGE(TAG, "创建舵机任务失败");
        return false;
    }
    
    // 设置初始位置
    WriteAngle(current_angle_);
    
    ESP_LOGI(TAG, "SG90舵机控制器初始化成功");
    return true;
}

void ServoController::InitializeTools() {
    auto& mcp_server = McpServer::GetInstance();
    ESP_LOGI(TAG, "开始注册舵机MCP工具...");

    // 设置舵机角度
    mcp_server.AddTool("self.servo.set_angle",
                       "设置SG90舵机到指定角度。angle: 目标角度(0-180度)",
                       PropertyList({Property("angle", kPropertyTypeInteger, 90, 0, 180)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int angle = properties["angle"].value<int>();
                           SetAngle(angle);
                           return "舵机设置到 " + std::to_string(angle) + " 度";
                       });

    // 顺时针旋转
    mcp_server.AddTool("self.servo.rotate_clockwise",
                       "顺时针旋转SG90舵机指定角度。degrees: 旋转角度(1-180度)",
                       PropertyList({Property("degrees", kPropertyTypeInteger, 30, 1, 180)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int degrees = properties["degrees"].value<int>();
                           RotateClockwise(degrees);
                           return "舵机顺时针旋转 " + std::to_string(degrees) + " 度";
                       });

    // 逆时针旋转
    mcp_server.AddTool("self.servo.rotate_counterclockwise",
                       "逆时针旋转SG90舵机指定角度。degrees: 旋转角度(1-180度)",
                       PropertyList({Property("degrees", kPropertyTypeInteger, 30, 1, 180)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int degrees = properties["degrees"].value<int>();
                           RotateCounterclockwise(degrees);
                           return "舵机逆时针旋转 " + std::to_string(degrees) + " 度";
                       });

    // 获取当前位置
    mcp_server.AddTool("self.servo.get_position",
                       "获取SG90舵机当前角度位置",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int angle = GetCurrentAngle();
                           return "当前舵机角度: " + std::to_string(angle) + " 度";
                       });

    // 扫描模式
    mcp_server.AddTool("self.servo.sweep",
                       "SG90舵机扫描模式，在指定角度范围内来回摆动。"
                       "min_angle: 最小角度(0-179度); max_angle: 最大角度(1-180度); "
                       "speed: 摆动速度，毫秒(100-5000ms)",
                       PropertyList({Property("min_angle", kPropertyTypeInteger, 0, 0, 179),
                                     Property("max_angle", kPropertyTypeInteger, 180, 1, 180),
                                     Property("speed", kPropertyTypeInteger, 1000, 100, 5000)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int min_angle = properties["min_angle"].value<int>();
                           int max_angle = properties["max_angle"].value<int>();
                           int speed = properties["speed"].value<int>();
                           SweepBetween(min_angle, max_angle, speed);
                           return "开始扫描模式: " + std::to_string(min_angle) + "° - " +
                                  std::to_string(max_angle) + "°";
                       });

    // 停止舵机
    mcp_server.AddTool("self.servo.stop",
                       "立即停止SG90舵机运动",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Stop();
                           return "舵机已停止";
                       });

    // 复位到中心位置
    mcp_server.AddTool("self.servo.reset",
                       "将SG90舵机复位到中心位置(90度)",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           Reset();
                           return "舵机已复位到中心位置(90度)";
                       });

    // 获取舵机状态
    mcp_server.AddTool("self.servo.get_status",
                       "获取SG90舵机当前状态",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int angle = GetCurrentAngle();
                           bool moving = IsMoving();
                           bool sweeping = IsSweeping();

                           std::string status = "{\"angle\":" + std::to_string(angle) +
                                              ",\"moving\":" + (moving ? "true" : "false") +
                                              ",\"sweeping\":" + (sweeping ? "true" : "false") + "}";
                           return status;
                       });

    ESP_LOGI(TAG, "舵机MCP工具注册完成");
}

void ServoController::SetAngle(int angle) {
    if (!IsValidAngle(angle)) {
        ESP_LOGW(TAG, "无效角度: %d，将限制在有效范围内", angle);
        angle = ConstrainAngle(angle);
    }
    
    ServoCommand cmd = {CMD_SET_ANGLE, angle, 0, 0};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::SetAngleDirect(int angle) {
    if (!IsValidAngle(angle)) {
        ESP_LOGW(TAG, "无效角度: %d，将限制在有效范围内", angle);
        angle = ConstrainAngle(angle);
    }
    
    ESP_LOGI(TAG, "快速设置舵机角度: %d度", angle);
    WriteAngle(angle);
}

void ServoController::RotateClockwise(int degrees) {
    if (degrees <= 0) {
        ESP_LOGW(TAG, "旋转角度必须大于0");
        return;
    }
    
    ServoCommand cmd = {CMD_ROTATE_CW, degrees, 0, 0};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::RotateCounterclockwise(int degrees) {
    if (degrees <= 0) {
        ESP_LOGW(TAG, "旋转角度必须大于0");
        return;
    }
    
    ServoCommand cmd = {CMD_ROTATE_CCW, degrees, 0, 0};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::SweepBetween(int min_angle, int max_angle, int speed_ms) {
    if (!IsValidAngle(min_angle) || !IsValidAngle(max_angle)) {
        ESP_LOGW(TAG, "扫描角度范围无效: %d - %d", min_angle, max_angle);
        return;
    }
    
    if (min_angle >= max_angle) {
        ESP_LOGW(TAG, "最小角度必须小于最大角度");
        return;
    }
    
    ServoCommand cmd = {CMD_SWEEP, min_angle, max_angle, speed_ms};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::Stop() {
    stop_requested_ = true;
    ServoCommand cmd = {CMD_STOP, 0, 0, 0};
    xQueueSend(command_queue_, &cmd, 0);
}

void ServoController::Reset() {
    ServoCommand cmd = {CMD_RESET, SERVO_DEFAULT_ANGLE, 0, 0};
    xQueueSend(command_queue_, &cmd, portMAX_DELAY);
}

void ServoController::WriteAngle(int angle) {
    angle = ConstrainAngle(angle);
    uint32_t compare_value = AngleToCompare(angle);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, ledc_channel_, compare_value);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, ledc_channel_);
    current_angle_ = angle;
}

uint32_t ServoController::AngleToCompare(int angle) {
    float pulse_width_ms = 0.5f + (angle / 180.0f) * 2.0f;
    float duty_cycle = pulse_width_ms / 20.0f;
    uint32_t compare_value = (uint32_t)(duty_cycle * 16383);
    return compare_value;
}

bool ServoController::IsValidAngle(int angle) const {
    return angle >= SERVO_MIN_DEGREE && angle <= SERVO_MAX_DEGREE;
}

int ServoController::ConstrainAngle(int angle) const {
    if (angle < SERVO_MIN_DEGREE) return SERVO_MIN_DEGREE;
    if (angle > SERVO_MAX_DEGREE) return SERVO_MAX_DEGREE;
    return angle;
}

void ServoController::ServoTask(void* parameter) {
    ServoController* controller = static_cast<ServoController*>(parameter);
    controller->ProcessCommands();
}

void ServoController::ProcessCommands() {
    ServoCommand cmd;
    
    while (true) {
        if (xQueueReceive(command_queue_, &cmd, pdMS_TO_TICKS(100)) == pdTRUE) {
            if (stop_requested_ && cmd.type != CMD_STOP) {
                continue;
            }
            
            switch (cmd.type) {
                case CMD_SET_ANGLE:
                    ExecuteSetAngle(cmd.param1);
                    break;
                    
                case CMD_ROTATE_CW:
                    ExecuteRotate(cmd.param1, true);
                    break;
                    
                case CMD_ROTATE_CCW:
                    ExecuteRotate(cmd.param1, false);
                    break;
                    
                case CMD_SWEEP:
                    ExecuteSweep(cmd.param1, cmd.param2, cmd.param3);
                    break;
                    
                case CMD_STOP:
                    is_moving_ = false;
                    is_sweeping_ = false;
                    stop_requested_ = false;
                    ESP_LOGI(TAG, "舵机停止");
                    break;
                    
                case CMD_RESET:
                    ExecuteSetAngle(cmd.param1);
                    break;
            }
        }
    }
}

void ServoController::ExecuteSetAngle(int angle) {
    ESP_LOGI(TAG, "设置舵机角度: %d度", angle);
    is_moving_ = true;
    
    // 直接设置角度，不使用平滑运动
    SetAngleDirect(angle);
    is_moving_ = false;

    if (on_move_complete_callback_) {
        on_move_complete_callback_();
    }
}

void ServoController::ExecuteRotate(int degrees, bool clockwise) {
    int target = current_angle_ + (clockwise ? degrees : -degrees);
    target = ConstrainAngle(target);

    ESP_LOGI(TAG, "%s旋转 %d度，从 %d度 到 %d度",
             clockwise ? "顺时针" : "逆时针", degrees, current_angle_, target);

    is_moving_ = true;
    
    // 直接设置角度，不使用平滑运动
    SetAngleDirect(target);
    is_moving_ = false;

    if (on_move_complete_callback_) {
        on_move_complete_callback_();
    }
}

void ServoController::ExecuteSweep(int min_angle, int max_angle, int speed_ms) {
    ESP_LOGI(TAG, "开始扫描模式: %d度 - %d度，速度: %dms", min_angle, max_angle, speed_ms);

    is_sweeping_ = true;
    is_moving_ = true;

    bool direction = true;

    while (is_sweeping_ && !stop_requested_) {
        int target = direction ? max_angle : min_angle;
        
        // 直接设置角度，不使用平滑运动
        SetAngleDirect(target);
        
        // 使用延迟来控制扫描速度
        vTaskDelay(pdMS_TO_TICKS(speed_ms));

        if (stop_requested_) break;

        direction = !direction;
        vTaskDelay(pdMS_TO_TICKS(100));
    }

    is_sweeping_ = false;
    is_moving_ = false;

    ESP_LOGI(TAG, "扫描模式结束");

    if (on_move_complete_callback_) {
        on_move_complete_callback_();
    }
}

4. dog_controller.h - 机器狗控制器头文件

定义 DogController 类

动作枚举和腿类型定义

所有动作方法的声明

代码

#ifndef __DOG_CONTROLLER_H__
#define __DOG_CONTROLLER_H__

#include "servo_controller.h"
#include "config.h"
#include <vector>
#include <map>

// 腿的类型定义
enum LegType {
    LEG_LEFT_FRONT = 0,
    LEG_RIGHT_FRONT = 1,
    LEG_LEFT_BACK = 2,
    LEG_RIGHT_BACK = 3
};

// 动作类型定义
enum DogAction {
    ACTION_STAND = 0,
    ACTION_SIT,
    ACTION_LIE_DOWN,
    ACTION_STRETCH,
    ACTION_WAVE,
    ACTION_WALK_FORWARD,
    ACTION_WALK_BACKWARD,
    ACTION_TURN_LEFT,      // 新增：左转
    ACTION_TURN_RIGHT,     // 新增：右转
    ACTION_SHAKE_BODY,
    ACTION_DANCE,
    ACTION_BOW,
    ACTION_SHAKE_HEAD,
    ACTION_JUMP,           // 新增：跳跃
    ACTION_TAIL_WAG,       // 新增：摇尾巴
    ACTION_SCRATCH,        // 新增：挠痒痒
    ACTION_SPIN            // 新增：转圈
};

class DogController {
public:
    DogController();
    ~DogController();

    bool Initialize();
    void InitializeTools();
    
    // 基本动作控制（保持原有稳定版本）
    void Stand();
    void Sit();
    void LieDown();
    void Stretch();
    void Wave();
    void WalkForward(int steps = 1);
    void WalkBackward(int steps = 1);
    void ShakeBody();
    void Dance();
    void Bow();
    void ShakeHead();
    
    // 新增动作
    void TurnLeft(int steps = 1);    // 左转
    void TurnRight(int steps = 1);   // 右转
    void ExcitedJump();              // 兴奋跳跃
    void TailWag();                  // 摇尾巴
    void Scratch();                  // 挠痒痒
    void Spin();                     // 转圈
    
    // 停止所有动作
    void StopAll();
    
    // 状态查询
    bool IsMoving() const;
    DogAction GetCurrentAction() const { return current_action_; }

private:
    std::vector<ServoController*> servos_;
    DogAction current_action_;
    bool is_moving_;
    
    // 舵机校准参数
    struct LegCalibration {
        int stand_angle;
        int sit_angle;  
        int lie_angle;
        int forward_offset;
    };
    
    std::map<LegType, LegCalibration> leg_calibration_;
    
    void InitializeCalibration();
    
    // 原有稳定方法
    void SetLegAngle(LegType leg, int angle, int speed_ms = 500);
    void SetAllLegs(int lf_angle, int rf_angle, int lb_angle, int rb_angle, int speed_ms = 500);
    
    // 新增快速方法（不经过平滑运动）
    void QuickSetLegAngle(LegType leg, int angle);
    void QuickSetAllLegs(int lf_angle, int rf_angle, int lb_angle, int rb_angle);
    
    // 动作执行函数
    void ExecuteStand();
    void ExecuteSit();
    void ExecuteLieDown();
    void ExecuteStretch();
    void ExecuteWave();
    void ExecuteWalkForward(int steps);
    void ExecuteWalkBackward(int steps);
    void ExecuteShakeBody();
    void ExecuteDance();
    void ExecuteBow();
    void ExecuteShakeHead();
    
    // 新增动作执行函数
    void ExecuteTurnLeft(int steps);
    void ExecuteTurnRight(int steps);
    void ExecuteExcitedJump();
    void ExecuteTailWag();
    void ExecuteScratch();
    void ExecuteSpin();
    
    bool AllServosReady() const;
};

#endif // __DOG_CONTROLLER_H__

5. dog_controller.cc - 机器狗控制器实现

四舵机协同控制逻辑

站立、坐下、趴下、前进、后退、转向等动作实现

完整的MCP工具注册

#机器狗动作编排与调试的重点文件

#增加动作与动作的打磨都要在这个文件中进行

代码

#include "dog_controller.h"
#include "mcp_server.h"
#include <esp_log.h>

#define TAG "DogController"

DogController::DogController() 
    : current_action_(ACTION_STAND)
    , is_moving_(false) {
    
    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] = {90, 60, 20, -30};
    leg_calibration_[LEG_RIGHT_FRONT] = {90, 120, 160, 30};
    leg_calibration_[LEG_LEFT_BACK] = {90, 45, 30, -30};
    leg_calibration_[LEG_RIGHT_BACK] = {90, 135, 150, 30};
}

// ========== 快速设置方法 ==========
void DogController::QuickSetLegAngle(LegType leg, int angle) {
    if (leg < servos_.size() && servos_[leg] != nullptr) {
        servos_[leg]->SetAngleDirect(angle);  // 使用 SetAngleDirect 而不是 SetAngle
    }
}

void DogController::QuickSetAllLegs(int lf_angle, int rf_angle, int lb_angle, int rb_angle) {
    if (servos_[LEG_LEFT_FRONT] != nullptr) {
        servos_[LEG_LEFT_FRONT]->SetAngleDirect(lf_angle);
    }
    if (servos_[LEG_RIGHT_FRONT] != nullptr) {
        servos_[LEG_RIGHT_FRONT]->SetAngleDirect(rf_angle);
    }
    if (servos_[LEG_LEFT_BACK] != nullptr) {
        servos_[LEG_LEFT_BACK]->SetAngleDirect(lb_angle);
    }
    if (servos_[LEG_RIGHT_BACK] != nullptr) {
        servos_[LEG_RIGHT_BACK]->SetAngleDirect(rb_angle);
    }
}

// ========== 原有稳定动作（保持不变） ==========

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::TurnLeft(int steps) {
    if (is_moving_) return;
    ESP_LOGI(TAG, "执行左转动作，步数: %d", steps);
    current_action_ = ACTION_TURN_LEFT;
    is_moving_ = true;
    ExecuteTurnLeft(steps);
    is_moving_ = false;
}

void DogController::TurnRight(int steps) {
    if (is_moving_) return;
    ESP_LOGI(TAG, "执行右转动作，步数: %d", steps);
    current_action_ = ACTION_TURN_RIGHT;
    is_moving_ = true;
    ExecuteTurnRight(steps);
    is_moving_ = false;
}

void DogController::ExcitedJump() {
    if (is_moving_) return;
    ESP_LOGI(TAG, "执行兴奋跳跃动作");
    current_action_ = ACTION_JUMP;
    is_moving_ = true;
    ExecuteExcitedJump();
    is_moving_ = false;
}

void DogController::TailWag() {
    if (is_moving_) return;
    ESP_LOGI(TAG, "执行摇尾巴动作");
    current_action_ = ACTION_TAIL_WAG;
    is_moving_ = true;
    ExecuteTailWag();
    is_moving_ = false;
}

void DogController::Scratch() {
    if (is_moving_) return;
    ESP_LOGI(TAG, "执行挠痒痒动作");
    current_action_ = ACTION_SCRATCH;
    is_moving_ = true;
    ExecuteScratch();
    is_moving_ = false;
}

void DogController::Spin() {
    if (is_moving_) return;
    ESP_LOGI(TAG, "执行转圈动作");
    current_action_ = ACTION_SPIN;
    is_moving_ = true;
    ExecuteSpin();
    is_moving_ = false;
}

// ========== 动作执行实现 ==========

// 原有稳定动作实现（保持不变）站立
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();
    for (int i = 0; i < 3; i++) {
        SetLegAngle(LEG_RIGHT_FRONT, 
                   leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 90, 150);
        vTaskDelay(pdMS_TO_TICKS(100));
        SetLegAngle(LEG_RIGHT_FRONT, 
                   leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 60, 150);
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    ExecuteStand();
}

// 前进
void DogController::ExecuteWalkForward(int steps) {
    for (int step = 0; step < steps ; step++) {
        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));
        
        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));

        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));
        
        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++) {
        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));
        
        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));

        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));
        
        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() {
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].stand_angle + 45,
        leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 45,
        leg_calibration_[LEG_LEFT_BACK].stand_angle,
        leg_calibration_[LEG_RIGHT_BACK].stand_angle,
        100
    );
    vTaskDelay(pdMS_TO_TICKS(100));
    
    SetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].stand_angle,
        leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
        leg_calibration_[LEG_LEFT_BACK].stand_angle - 45,
        leg_calibration_[LEG_RIGHT_BACK].stand_angle - 45,
        100
    );
    vTaskDelay(pdMS_TO_TICKS(100));
    
    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,
            100
        );
        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,
            100
        );
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    for (int i = 0; i < 3; i++) {
        QuickSetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle - 20,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle + 20
        );
        vTaskDelay(pdMS_TO_TICKS(100));
        
        QuickSetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle + 20,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle - 20
        );
        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::ExecuteTurnLeft(int steps) {
    for (int step = 0; step < steps; step++) {
        QuickSetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 30,
            leg_calibration_[LEG_LEFT_BACK].stand_angle + 30,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle
        );
        vTaskDelay(pdMS_TO_TICKS(80));
        
        QuickSetAllLegs(
            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
        );
        vTaskDelay(pdMS_TO_TICKS(80));
        
        ExecuteStand();
        vTaskDelay(pdMS_TO_TICKS(80));
    }
}
// 右转
void DogController::ExecuteTurnRight(int steps) {
    // 明确注释为左转的镜像
    for (int step = 0; step < steps; step++) {
        QuickSetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle - 30 ,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle - 30
        );
        vTaskDelay(pdMS_TO_TICKS(80));
        QuickSetAllLegs(
            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
        );
        vTaskDelay(pdMS_TO_TICKS(80));
        
        ExecuteStand();
        vTaskDelay(pdMS_TO_TICKS(80));
    }
}

// 兴奋跳跃
void DogController::ExecuteExcitedJump() {
    // 快速下蹲准备
    ExecuteSit();
    vTaskDelay(pdMS_TO_TICKS(100));
    
    // 跳跃动作
    QuickSetAllLegs(
        leg_calibration_[LEG_LEFT_FRONT].stand_angle - 45,
        leg_calibration_[LEG_RIGHT_FRONT].stand_angle + 45,
        leg_calibration_[LEG_LEFT_BACK].stand_angle - 45,
        leg_calibration_[LEG_RIGHT_BACK].stand_angle + 45
    );
    vTaskDelay(pdMS_TO_TICKS(80));
    
    ExecuteStand();

    for (int i = 0; i < 6; i++) {
        QuickSetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle - 30,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle + 30
        );
        vTaskDelay(pdMS_TO_TICKS(100));
        
        QuickSetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle + 30,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle - 30
        );
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    ExecuteStand();
}

// 摇尾巴
void DogController::ExecuteTailWag() {
    for (int i = 0; i < 6; i++) {
        QuickSetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle - 20,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle + 20
        );
        vTaskDelay(pdMS_TO_TICKS(200));
        
        QuickSetAllLegs(
            leg_calibration_[LEG_LEFT_FRONT].stand_angle,
            leg_calibration_[LEG_RIGHT_FRONT].stand_angle,
            leg_calibration_[LEG_LEFT_BACK].stand_angle + 20,
            leg_calibration_[LEG_RIGHT_BACK].stand_angle - 20
        );
        vTaskDelay(pdMS_TO_TICKS(200));
    }
    ExecuteStand();
}

// 挠痒痒
void DogController::ExecuteScratch() {
    for (int i = 0; i < 4; i++) {
        QuickSetAllLegs(
            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
        );
        vTaskDelay(pdMS_TO_TICKS(120));
        
        QuickSetAllLegs(
            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
        );
        vTaskDelay(pdMS_TO_TICKS(120));
    }
    ExecuteStand();
}

// 转圈
void DogController::ExecuteSpin() {
    for (int i = 0; i < 16; i++) {
        ExecuteTurnLeft(1);
        vTaskDelay(pdMS_TO_TICKS(50));
    }
}

// ========== 辅助方法 ==========

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));
}

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_;
}

bool DogController::AllServosReady() const {
    for (auto servo : servos_) {
        if (servo != nullptr && servo->IsMoving()) {
            return false;
        }
    }
    return true;
}

void DogController::InitializeTools() {
    auto& mcp_server = McpServer::GetInstance();
    ESP_LOGI(TAG, "开始注册机器狗MCP工具...");

    // 原有稳定动作
    mcp_server.AddTool("self.dog.stand", "机器狗站立姿势", PropertyList(), [this](...) { Stand(); return "机器狗已站立"; });
    mcp_server.AddTool("self.dog.sit", "机器狗坐下", PropertyList(), [this](...) { Sit(); return "机器狗已坐下"; });
    mcp_server.AddTool("self.dog.lie_down", "机器狗趴下", PropertyList(), [this](...) { LieDown(); return "机器狗已趴下"; });
    mcp_server.AddTool("self.dog.stretch", "机器狗伸懒腰", PropertyList(), [this](...) { Stretch(); return "机器狗在伸懒腰"; });
    mcp_server.AddTool("self.dog.wave", "机器狗挥手打招呼", PropertyList(), [this](...) { Wave(); return "机器狗在挥手打招呼"; });
    mcp_server.AddTool("self.dog.walk_forward", "机器狗前进", 
        PropertyList({Property("steps", kPropertyTypeInteger, 5, 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", "机器狗后退", 
        PropertyList({Property("steps", kPropertyTypeInteger, 5, 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](...) { ShakeBody(); return "机器狗在摇摆身体"; });
    mcp_server.AddTool("self.dog.dance", "机器狗跳舞", PropertyList(), [this](...) { Dance(); return "机器狗在跳舞"; });
    mcp_server.AddTool("self.dog.bow", "机器狗鞠躬", PropertyList(), [this](...) { Bow(); return "机器狗在鞠躬"; });
    mcp_server.AddTool("self.dog.shake_head", "机器狗摇头", PropertyList(), [this](...) { ShakeHead(); return "机器狗在摇头"; });

    // 新增动作
    mcp_server.AddTool("self.dog.turn_left", "机器狗左转", 
        PropertyList({Property("steps", kPropertyTypeInteger, 10, 1, 10)}),
        [this](const PropertyList& properties) -> ReturnValue {
            int steps = properties["steps"].value<int>();
            TurnLeft(steps);
            return "机器狗左转 " + std::to_string(steps) + " 步";
        });
    mcp_server.AddTool("self.dog.turn_right", "机器狗右转", 
        PropertyList({Property("steps", kPropertyTypeInteger, 10, 1, 10)}),
        [this](const PropertyList& properties) -> ReturnValue {
            int steps = properties["steps"].value<int>();
            TurnRight(steps);
            return "机器狗右转 " + std::to_string(steps) + " 步";
        });
    mcp_server.AddTool("self.dog.excited_jump", "机器狗兴奋跳跃", PropertyList(), [this](...) { ExcitedJump(); return "机器狗兴奋跳跃"; });
    mcp_server.AddTool("self.dog.tail_wag", "机器狗摇尾巴", PropertyList(), [this](...) { TailWag(); return "机器狗在摇尾巴"; });
    mcp_server.AddTool("self.dog.scratch", "机器狗挠痒痒", PropertyList(), [this](...) { Scratch(); return "机器狗在挠痒痒"; });
    mcp_server.AddTool("self.dog.spin", "机器狗转圈", PropertyList(), [this](...) { Spin(); return "机器狗在转圈"; });

    // 停止和状态查询
    mcp_server.AddTool("self.dog.stop", "停止机器狗所有动作", PropertyList(), [this](...) { 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_TURN_LEFT: action_name = "左转"; break;
                case ACTION_TURN_RIGHT: 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;
                case ACTION_JUMP: action_name = "跳跃"; break;
                case ACTION_TAIL_WAG: action_name = "摇尾巴"; break;
                case ACTION_SCRATCH: action_name = "挠痒痒"; break;
                case ACTION_SPIN: action_name = "转圈"; break;
                default: action_name = "未知";
            }
            return "当前状态: " + action_name + ", 是否运动中: " + (IsMoving() ? "是" : "否");
        });

    ESP_LOGI(TAG, "机器狗MCP工具注册完成");
}

6. bread_compact_wifi_with_servo.cc - 主程序文件

机器狗控制器的初始化和集成

系统启动和任务管理

代码

#include "wifi_board.h"
#include "codecs/no_audio_codec.h"
#include "display/oled_display.h"
#include "system_reset.h"
#include "application.h"
#include "button.h"
#include "config.h"
#include "mcp_server.h"
#include "servo_controller.h"
#include "led/single_led.h"
#include "assets/lang_config.h"

#include <wifi_station.h>
#include <esp_log.h>
#include <driver/i2c_master.h>
#include <esp_lcd_panel_ops.h>
#include <esp_lcd_panel_vendor.h>
#include "dog_controller.h"

#ifdef SH1106
#include <esp_lcd_panel_sh1106.h>
#endif

#define TAG "BreadCompactWifiWithServo"

LV_FONT_DECLARE(font_puhui_14_1);
LV_FONT_DECLARE(font_awesome_14_1);

class BreadCompactWifiWithServoBoard : public WifiBoard {
private:
    i2c_master_bus_handle_t display_i2c_bus_;
    esp_lcd_panel_io_handle_t panel_io_ = nullptr;
    esp_lcd_panel_handle_t panel_ = nullptr;
    Display *display_ = nullptr;
    Button boot_button_;
    Button touch_button_;
    Button volume_up_button_;
    Button volume_down_button_;
    DogController *dog_controller_;

    void InitializeDisplayI2c() {
        i2c_master_bus_config_t bus_config = {
            .i2c_port = (i2c_port_t)0,
            .sda_io_num = DISPLAY_SDA_PIN,
            .scl_io_num = DISPLAY_SCL_PIN,
            .clk_source = I2C_CLK_SRC_DEFAULT,
            .glitch_ignore_cnt = 7,
            .intr_priority = 0,
            .trans_queue_depth = 0,
            .flags = {
                .enable_internal_pullup = 1,
            },
        };
        ESP_ERROR_CHECK(i2c_new_master_bus(&bus_config, &display_i2c_bus_));
    }

    void InitializeSsd1306Display() {
        // SSD1306 config
        esp_lcd_panel_io_i2c_config_t io_config = {
            .dev_addr = 0x3C,
            .on_color_trans_done = nullptr,
            .user_ctx = nullptr,
            .control_phase_bytes = 1,
            .dc_bit_offset = 6,
            .lcd_cmd_bits = 8,
            .lcd_param_bits = 8,
            .flags = {
                .dc_low_on_data = 0,
                .disable_control_phase = 0,
            },
            .scl_speed_hz = 400 * 1000,
        };

        ESP_ERROR_CHECK(esp_lcd_new_panel_io_i2c_v2(display_i2c_bus_, &io_config, &panel_io_));

        ESP_LOGI(TAG, "Install SSD1306 driver");
        esp_lcd_panel_dev_config_t panel_config = {};
        panel_config.reset_gpio_num = -1;
        panel_config.bits_per_pixel = 1;

        esp_lcd_panel_ssd1306_config_t ssd1306_config = {
            .height = static_cast<uint8_t>(DISPLAY_HEIGHT),
        };
        panel_config.vendor_config = &ssd1306_config;

#ifdef SH1106
        ESP_ERROR_CHECK(esp_lcd_new_panel_sh1106(panel_io_, &panel_config, &panel_));
#else
        ESP_ERROR_CHECK(esp_lcd_new_panel_ssd1306(panel_io_, &panel_config, &panel_));
#endif
        ESP_LOGI(TAG, "SSD1306 driver installed");

        // Reset the display
        ESP_ERROR_CHECK(esp_lcd_panel_reset(panel_));
        if (esp_lcd_panel_init(panel_) != ESP_OK) {
            ESP_LOGE(TAG, "Failed to initialize display");
            display_ = new NoDisplay();
            return;
        }
        ESP_ERROR_CHECK(esp_lcd_panel_invert_color(panel_, false));

        // Set the display to on
        ESP_LOGI(TAG, "Turning display on");
        ESP_ERROR_CHECK(esp_lcd_panel_disp_on_off(panel_, true));

        display_ = new OledDisplay(panel_io_, panel_, DISPLAY_WIDTH, DISPLAY_HEIGHT, DISPLAY_MIRROR_X, DISPLAY_MIRROR_Y,
            {&font_puhui_14_1, &font_awesome_14_1});
    }

    void InitializeButtons() {
        boot_button_.OnClick([this]() {
            auto& app = Application::GetInstance();
            if (app.GetDeviceState() == kDeviceStateStarting && !WifiStation::GetInstance().IsConnected()) {
                ResetWifiConfiguration();
            }
            app.ToggleChatState();
        });
        touch_button_.OnPressDown([this]() {
            Application::GetInstance().StartListening();
        });
        touch_button_.OnPressUp([this]() {
            Application::GetInstance().StopListening();
        });

        volume_up_button_.OnClick([this]() {
            auto codec = GetAudioCodec();
            auto volume = codec->output_volume() + 10;
            if (volume > 100) {
                volume = 100;
            }
            codec->SetOutputVolume(volume);
            GetDisplay()->ShowNotification(Lang::Strings::VOLUME + std::to_string(volume));
        });

        volume_up_button_.OnLongPress([this]() {
            GetAudioCodec()->SetOutputVolume(100);
            GetDisplay()->ShowNotification(Lang::Strings::MAX_VOLUME);
        });

        volume_down_button_.OnClick([this]() {
            auto codec = GetAudioCodec();
            auto volume = codec->output_volume() - 10;
            if (volume < 0) {
                volume = 0;
            }
            codec->SetOutputVolume(volume);
            GetDisplay()->ShowNotification(Lang::Strings::VOLUME + std::to_string(volume));
        });

        volume_down_button_.OnLongPress([this]() {
            GetAudioCodec()->SetOutputVolume(0);
            GetDisplay()->ShowNotification(Lang::Strings::MUTED);
        });
    }

    void InitializeDogController() {
        ESP_LOGI(TAG, "初始化机器狗控制器");
        dog_controller_ = new DogController();

        if (!dog_controller_->Initialize()) {
            ESP_LOGE(TAG, "机器狗控制器初始化失败");
            delete dog_controller_;
            dog_controller_ = nullptr;
            return;
        }

        ESP_LOGI(TAG, "机器狗控制器初始化完成");
    }

    // 物联网初始化，逐步迁移到 MCP 协议
    void InitializeTools() {
        if (dog_controller_ != nullptr) {
            dog_controller_->InitializeTools();
        }
    }

public:
    BreadCompactWifiWithServoBoard() :
        boot_button_(BOOT_BUTTON_GPIO),
        touch_button_(TOUCH_BUTTON_GPIO),
        volume_up_button_(VOLUME_UP_BUTTON_GPIO),
        volume_down_button_(VOLUME_DOWN_BUTTON_GPIO),
        dog_controller_(nullptr) {
    
        InitializeDisplayI2c();
        InitializeSsd1306Display();
        InitializeButtons();
        InitializeDogController();  
        InitializeTools();
    }

    virtual ~BreadCompactWifiWithServoBoard() {
        if (dog_controller_) {
            delete dog_controller_;
        }
    }

    virtual Led* GetLed() override {
        static SingleLed led(BUILTIN_LED_GPIO);
        return &led;
    }

    virtual AudioCodec* GetAudioCodec() override {
#ifdef AUDIO_I2S_METHOD_SIMPLEX
        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);
#else
        static NoAudioCodecDuplex audio_codec(AUDIO_INPUT_SAMPLE_RATE, AUDIO_OUTPUT_SAMPLE_RATE,
            AUDIO_I2S_GPIO_BCLK, AUDIO_I2S_GPIO_WS, AUDIO_I2S_GPIO_DOUT, AUDIO_I2S_GPIO_DIN);
#endif
        return &audio_codec;
    }

    virtual Display* GetDisplay() override {
        return display_;
    }
};

DECLARE_BOARD(BreadCompactWifiWithServoBoard);

以上代码文件附在后面：

附件

bread-compact-wifi-with-servo2.0.zip

ESP IDF+ESP32S3小智AI+4舵机=手搓桌面智能小狗狗代码优化（2） 简单