ESP IDF+K10小智GPIO+2个180度舵机=自然语音控制的机械臂 简单

#ifndef _BOARD_CONFIG_H_
#define _BOARD_CONFIG_H_

#include <driver/gpio.h>

#define AUDIO_INPUT_SAMPLE_RATE  24000
#define AUDIO_OUTPUT_SAMPLE_RATE 24000

#define AUDIO_INPUT_REFERENCE    true

#define AUDIO_I2S_GPIO_MCLK GPIO_NUM_3
#define AUDIO_I2S_GPIO_WS GPIO_NUM_38
#define AUDIO_I2S_GPIO_BCLK GPIO_NUM_0
#define AUDIO_I2S_GPIO_DIN  GPIO_NUM_39
#define AUDIO_I2S_GPIO_DOUT GPIO_NUM_45

#define AUDIO_CODEC_PA_PIN       GPIO_NUM_NC
#define AUDIO_CODEC_I2C_SDA_PIN  GPIO_NUM_47
#define AUDIO_CODEC_I2C_SCL_PIN  GPIO_NUM_48
#define AUDIO_CODEC_ES8311_ADDR  ES8311_CODEC_DEFAULT_ADDR
#define AUDIO_CODEC_ES7210_ADDR  0x23

#define BUILTIN_LED_GPIO        GPIO_NUM_46
#define BOOT_BUTTON_GPIO        GPIO_NUM_0
#define VOLUME_UP_BUTTON_GPIO   GPIO_NUM_NC
#define VOLUME_DOWN_BUTTON_GPIO GPIO_NUM_NC

/* Expander */
#define DRV_IO_EXP_INPUT_MASK  (IO_EXPANDER_PIN_NUM_2 | IO_EXPANDER_PIN_NUM_12)


#define DISPLAY_WIDTH   240
#define DISPLAY_HEIGHT  320
#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_NC
#define DISPLAY_BACKLIGHT_OUTPUT_INVERT false

/* DFRobot K10 Camera pins */
#define PWDN_GPIO_NUM       -1
#define RESET_GPIO_NUM      -1
#define XCLK_GPIO_NUM       7

#define VSYNC_GPIO_NUM      4
#define HREF_GPIO_NUM       5
#define PCLK_GPIO_NUM       17
#define SIOD_GPIO_NUM       20
#define SIOC_GPIO_NUM       19

/* Camera pins */
#define CAMERA_PIN_PWDN     PWDN_GPIO_NUM
#define CAMERA_PIN_RESET    RESET_GPIO_NUM
#define CAMERA_PIN_XCLK     XCLK_GPIO_NUM
#define CAMERA_PIN_SIOD     SIOD_GPIO_NUM
#define CAMERA_PIN_SIOC     SIOC_GPIO_NUM

#define CAMERA_PIN_D9       6
#define CAMERA_PIN_D8       15
#define CAMERA_PIN_D7       16
#define CAMERA_PIN_D6       18
#define CAMERA_PIN_D5       9
#define CAMERA_PIN_D4       11
#define CAMERA_PIN_D3       10
#define CAMERA_PIN_D2       8
#define CAMERA_PIN_VSYNC    VSYNC_GPIO_NUM
#define CAMERA_PIN_HREF     HREF_GPIO_NUM
#define CAMERA_PIN_PCLK     PCLK_GPIO_NUM

#define XCLK_FREQ_HZ 20000000

// ========== 通用舵机参数配置 ==========
// 这些是ServoController类需要的通用参数
#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_DEFAULT_ANGLE 90                // 默认中心位置
#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000  // 1MHz, 1us per tick
#define SERVO_TIMEBASE_PERIOD 20000           // 20000 ticks, 20ms (50Hz)
#define SERVO_MAX_SPEED_DEGREE_PER_SEC 180    // 最大转速限制

// ========== 机械臂专用配置 ==========
// 机械臂舵机控制引脚
#define ARM_SERVO_GPIO GPIO_NUM_1    // 机械臂升降
#define CLAW_SERVO_GPIO GPIO_NUM_2   // 机械爪开合

// 机械臂升降舵机参数
#define ARM_MIN_ANGLE 120             // 最低位置
#define ARM_MAX_ANGLE 30            // 最高位置
#define ARM_DEFAULT_ANGLE 75         // 默认中间位置

// 机械爪舵机参数  
#define CLAW_MIN_ANGLE 75            // 完全张开
#define CLAW_MAX_ANGLE 140           // 完全闭合
#define CLAW_DEFAULT_ANGLE 75        // 默认张开状态

// 运动速度参数
#define ARM_MOVE_SPEED_MS 800        // 机械臂移动速度(毫秒)
#define CLAW_MOVE_SPEED_MS 500       // 机械爪移动速度(毫秒)


#endif // _BOARD_CONFIG_H_

#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 {
private:
    // 静态实例计数器
    static int instance_count_;
    int instance_index_;
    
    // 硬件相关
    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);
    void SmoothMoveTo(int target_angle, int speed_ms = 500);

public:
    ServoController(gpio_num_t servo_pin);
    ~ServoController();

    // 基本控制方法
    bool Initialize();
    void InitializeTools();  // 初始化MCP工具
    void SetAngle(int angle);
    int GetCurrentAngle() const { return current_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(); // 回到中心位置（90度）
    
    // 状态查询
    bool IsMoving() const { return is_moving_; }
    bool IsSweeping() const { return is_sweeping_; }
    
    // 设置回调函数（当运动完成时调用）
    void SetOnMoveCompleteCallback(std::function<void()> callback) {
        on_move_complete_callback_ = callback;
    }
};

#endif // __SERVO_CONTROLLER_H__

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

#define TAG "ServoController"

// 初始化静态成员变量
int ServoController::instance_count_ = 0;

ServoController::ServoController(gpio_num_t servo_pin)
    : servo_pin_(servo_pin) {
    
    // 为每个实例分配唯一的LEDC通道和定时器
    instance_index_ = instance_count_++;
    
    if (instance_index_ == 0) {
        ledc_channel_ = LEDC_CHANNEL_0;
        ledc_timer_ = LEDC_TIMER_0;
    } else {
        ledc_channel_ = LEDC_CHANNEL_1;
        ledc_timer_ = LEDC_TIMER_1;
    }
    
    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() {
    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，实例: %d", servo_pin_, instance_index_);
    
    // 配置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, // 50Hz for servo
        .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舵机控制器初始化成功，使用通道: %d，定时器: %d", ledc_channel_, ledc_timer_);
    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::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) {
    angle = ConstrainAngle(angle);
    
    // SG90舵机：0.5ms (0°) ~ 2.5ms (180°)
    const float min_pulse_us = 500.0f;   // 0.5ms
    const float max_pulse_us = 2500.0f;  // 2.5ms  
    const float pulse_range_us = max_pulse_us - min_pulse_us;
    
    // 计算当前角度对应的脉冲宽度（微秒）
    float pulse_width_us = min_pulse_us + (angle / 180.0f) * pulse_range_us;
    
    // 转换为占空比（周期20ms = 20000us）
    float duty_cycle = pulse_width_us / 20000.0f;
    
    // 14位分辨率：2^14 - 1 = 16383
    uint32_t compare_value = (uint32_t)(duty_cycle * 16383.0f);
    
    ESP_LOGD(TAG, "角度 %d° -> 脉宽 %.1fus -> 占空比 %.4f -> 比较值 %lu", 
             angle, pulse_width_us, duty_cycle, compare_value);
    
    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;
    SmoothMoveTo(angle, 500);
    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;
    SmoothMoveTo(target, 500);
    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; // true = 向最大角度，false = 向最小角度

    while (is_sweeping_ && !stop_requested_) {
        int target = direction ? max_angle : min_angle;
        SmoothMoveTo(target, 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_();
    }
}

void ServoController::SmoothMoveTo(int target_angle, int speed_ms) {
    target_angle = ConstrainAngle(target_angle);

    if (target_angle == current_angle_) {
        return; // 已经在目标位置
    }

    int start_angle = current_angle_;
    int angle_diff = target_angle - start_angle;
    int steps = abs(angle_diff);

    if (steps == 0) return;

    int delay_per_step = speed_ms / steps;
    if (delay_per_step < 10) delay_per_step = 10; // 最小延迟

    for (int i = 1; i <= steps && !stop_requested_; i++) {
        int current_step_angle = start_angle + (angle_diff * i) / steps;
        WriteAngle(current_step_angle);
        vTaskDelay(pdMS_TO_TICKS(delay_per_step));
    }

    // 确保到达精确位置
    if (!stop_requested_) {
        WriteAngle(target_angle);
    }
}

#ifndef __ARM_CONTROLLER_H__
#define __ARM_CONTROLLER_H__

#include "servo_controller.h"
#include "mcp_server.h"
#include <functional>
#include <string>

class ArmController {
public:
    ArmController(gpio_num_t arm_pin, gpio_num_t claw_pin);
    ~ArmController();

    bool Initialize();
    void InitializeTools();

    // 简化的机械臂控制方法
    void SetArmHeight(int height);       // 设置机械臂高度(0-100%)
    void MoveArmUp(int degrees = 10);    // 机械臂升高指定度数
    void MoveArmDown(int degrees = 10);  // 机械臂降低指定度数
    
    // 简化的机械爪控制方法
    void SetGripStrength(int strength);  // 设置抓取力度(0-100%)
    void OpenClaw();                     // 完全张开
    void CloseClaw();                    // 完全闭合
    void TightenClaw(int percent = 10);  // 夹紧一点
    void LoosenClaw(int percent = 10);   // 松开一点
    
    // 状态查询
    int GetArmPosition() const { return arm_servo_->GetCurrentAngle(); }
    int GetClawPosition() const { return claw_servo_->GetCurrentAngle(); }
    bool IsMoving() const { return arm_servo_->IsMoving() || claw_servo_->IsMoving(); }
    std::string GetStatus();  // 添加这行声明

private:
    ServoController* arm_servo_;    // 机械臂升降舵机
    ServoController* claw_servo_;   // 机械爪舵机
    
    // 私有方法
    int ArmAngleToHeight(int angle) const;
    int HeightToArmAngle(int height) const;
    int ClawAngleToStrength(int angle) const;
    int StrengthToClawAngle(int strength) const;
};

#endif // __ARM_CONTROLLER_H__

#include "arm_controller.h"
#include "config.h"
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

#define TAG "ArmController"

ArmController::ArmController(gpio_num_t arm_pin, gpio_num_t claw_pin) {
    arm_servo_ = new ServoController(arm_pin);
    claw_servo_ = new ServoController(claw_pin);
}

ArmController::~ArmController() {
    if (arm_servo_) {
        arm_servo_->Stop();
        delete arm_servo_;
    }
    if (claw_servo_) {
        claw_servo_->Stop();
        delete claw_servo_;
    }
}

bool ArmController::Initialize() {
    ESP_LOGI(TAG, "初始化机械臂控制系统");
    
    // 分别初始化两个舵机，互不干扰
    bool arm_ok = arm_servo_->Initialize();
    bool claw_ok = claw_servo_->Initialize();
    
    if (!arm_ok || !claw_ok) {
        ESP_LOGE(TAG, "舵机初始化失败 - 机械臂: %s, 机械爪: %s", 
                 arm_ok ? "成功" : "失败", claw_ok ? "成功" : "失败");
        return false;
    }
    
    // 设置初始位置
    SetArmHeight(50);
    OpenClaw();
    
    ESP_LOGI(TAG, "机械臂控制系统初始化成功");
    return true;
}

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

    // 机械臂高度控制
    mcp_server.AddTool("self.arm.set_height",
                       "设置机械臂高度。height: 高度百分比(0-100)",
                       PropertyList({Property("height", kPropertyTypeInteger, 50, 0, 100)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int height = properties["height"].value<int>();
                           SetArmHeight(height);
                           return "机械臂设置到 " + std::to_string(height) + "% 高度";
                       });

    mcp_server.AddTool("self.arm.move_up",
                       "机械臂升高。degrees: 升高角度",
                       PropertyList({Property("degrees", kPropertyTypeInteger, 10, 1, 90)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int degrees = properties["degrees"].value<int>();
                           MoveArmUp(degrees);
                           return "机械臂升高 " + std::to_string(degrees) + " 度";
                       });

    mcp_server.AddTool("self.arm.move_down",
                       "机械臂降低。degrees: 降低角度",
                       PropertyList({Property("degrees", kPropertyTypeInteger, 10, 1, 90)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int degrees = properties["degrees"].value<int>();
                           MoveArmDown(degrees);
                           return "机械臂降低 " + std::to_string(degrees) + " 度";
                       });

    // 机械爪控制
    mcp_server.AddTool("self.arm.set_grip",
                       "设置机械爪抓取力度。strength: 力度百分比(0-100)",
                       PropertyList({Property("strength", kPropertyTypeInteger, 50, 0, 100)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int strength = properties["strength"].value<int>();
                           SetGripStrength(strength);
                           return "抓取力度设置为 " + std::to_string(strength) + "%";
                       });

    mcp_server.AddTool("self.arm.open_claw",
                       "完全打开机械爪",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           OpenClaw();
                           return "机械爪已打开";
                       });

    mcp_server.AddTool("self.arm.close_claw",
                       "完全关闭机械爪",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           CloseClaw();
                           return "机械爪已关闭";
                       });

    mcp_server.AddTool("self.arm.tighten",
                       "机械爪夹紧一点",
                       PropertyList({Property("percent", kPropertyTypeInteger, 10, 1, 50)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int percent = properties["percent"].value<int>();
                           TightenClaw(percent);
                           return "夹紧 " + std::to_string(percent) + "%";
                       });

    mcp_server.AddTool("self.arm.loosen",
                       "机械爪松开一点",
                       PropertyList({Property("percent", kPropertyTypeInteger, 10, 1, 50)}),
                       [this](const PropertyList& properties) -> ReturnValue {
                           int percent = properties["percent"].value<int>();
                           LoosenClaw(percent);
                           return "松开 " + std::to_string(percent) + "%";
                       });

    // 状态查询
    mcp_server.AddTool("self.arm.get_status",
                       "获取机械臂状态",
                       PropertyList(),
                       [this](const PropertyList& properties) -> ReturnValue {
                           return GetStatus();  // 现在这个函数已正确定义
                       });

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

// 简化的控制函数 - 直接调用舵机，不等待
void ArmController::SetArmHeight(int height) {
    int angle = HeightToArmAngle(height);
    ESP_LOGI(TAG, "设置机械臂高度: %d%% -> %d°", height, angle);
    arm_servo_->SetAngle(angle);  // 异步执行，立即返回
}

void ArmController::MoveArmUp(int degrees) {
    int current = arm_servo_->GetCurrentAngle();
    int target = current - degrees;
    target = (target < ARM_MAX_ANGLE) ? ARM_MAX_ANGLE : target;
    ESP_LOGI(TAG, "机械臂升高: %d° -> %d°", current, target);
    arm_servo_->SetAngle(target);
}

void ArmController::MoveArmDown(int degrees) {
    int current = arm_servo_->GetCurrentAngle();
    int target = current + degrees;
    target = (target > ARM_MIN_ANGLE) ? ARM_MIN_ANGLE : target;
    ESP_LOGI(TAG, "机械臂降低: %d° -> %d°", current, target);
    arm_servo_->SetAngle(target);
}

void ArmController::SetGripStrength(int strength) {
    int angle = StrengthToClawAngle(strength);
    ESP_LOGI(TAG, "设置抓取力度: %d%% -> %d°", strength, angle);
    claw_servo_->SetAngle(angle);  // 异步执行，立即返回
}

void ArmController::OpenClaw() {
    ESP_LOGI(TAG, "打开机械爪");
    claw_servo_->SetAngle(CLAW_MIN_ANGLE);
}

void ArmController::CloseClaw() {
    ESP_LOGI(TAG, "关闭机械爪");
    claw_servo_->SetAngle(CLAW_MAX_ANGLE);
}

void ArmController::TightenClaw(int percent) {
    int current = claw_servo_->GetCurrentAngle();
    int current_strength = ClawAngleToStrength(current);
    int new_strength = current_strength + percent;
    if (new_strength > 100) new_strength = 100;
    
    ESP_LOGI(TAG, "夹紧: %d%% -> %d%%", current_strength, new_strength);
    SetGripStrength(new_strength);
}

void ArmController::LoosenClaw(int percent) {
    int current = claw_servo_->GetCurrentAngle();
    int current_strength = ClawAngleToStrength(current);
    int new_strength = current_strength - percent;
    if (new_strength < 0) new_strength = 0;
    
    ESP_LOGI(TAG, "松开: %d%% -> %d%%", current_strength, new_strength);
    SetGripStrength(new_strength);
}

std::string ArmController::GetStatus() {
    int arm_angle = arm_servo_->GetCurrentAngle();
    int claw_angle = claw_servo_->GetCurrentAngle();
    bool arm_moving = arm_servo_->IsMoving();
    bool claw_moving = claw_servo_->IsMoving();
    
    std::string status = "{" +
        std::string("\"arm_angle\":") + std::to_string(arm_angle) +
        ",\"arm_height\":" + std::to_string(ArmAngleToHeight(arm_angle)) +
        ",\"claw_angle\":" + std::to_string(claw_angle) +
        ",\"grip_strength\":" + std::to_string(ClawAngleToStrength(claw_angle)) +
        ",\"arm_moving\":" + (arm_moving ? "true" : "false") +
        ",\"claw_moving\":" + (claw_moving ? "true" : "false") +
        "}";
    
    return status;
}

// 转换函数保持不变
int ArmController::ArmAngleToHeight(int angle) const {
    return (angle - ARM_MIN_ANGLE) * 100 / (ARM_MAX_ANGLE - ARM_MIN_ANGLE);
}

int ArmController::HeightToArmAngle(int height) const {
    return ARM_MIN_ANGLE + (height * (ARM_MAX_ANGLE - ARM_MIN_ANGLE) / 100);
}

int ArmController::ClawAngleToStrength(int angle) const {
    return (angle - CLAW_MIN_ANGLE) * 100 / (CLAW_MAX_ANGLE - CLAW_MIN_ANGLE);
}

int ArmController::StrengthToClawAngle(int strength) const {
    return CLAW_MIN_ANGLE + (strength * (CLAW_MAX_ANGLE - CLAW_MIN_ANGLE) / 100);
}

#include "wifi_board.h"
#include "k10_audio_codec.h"
#include "display/lcd_display.h"
#include "esp_lcd_ili9341.h"
#include "led_control.h"
#include "font_awesome_symbols.h"
#include "application.h"
#include "button.h"
#include "config.h"
#include "esp32_camera.h"

#include "led/circular_strip.h"
#include "assets/lang_config.h"

#include <esp_log.h>
#include <esp_lcd_panel_vendor.h>
#include <driver/i2c_master.h>
#include <driver/spi_common.h>
#include <wifi_station.h>

#include "esp_io_expander_tca95xx_16bit.h"
#include "arm_controller.h"  // arm控制类声明

#define TAG "DF-K10"

LV_FONT_DECLARE(font_puhui_20_4);
LV_FONT_DECLARE(font_awesome_20_4);

class Df_K10Board : public WifiBoard {
private:
    i2c_master_bus_handle_t i2c_bus_;
    esp_io_expander_handle_t io_expander;
    LcdDisplay *display_;
    button_handle_t btn_a;
    button_handle_t btn_b;
    Esp32Camera* camera_;
    ArmController* arm_controller_;  // 机械臂控制器

    button_driver_t* btn_a_driver_ = nullptr;
    button_driver_t* btn_b_driver_ = nullptr;

    CircularStrip* led_strip_;

    static Df_K10Board* instance_;

    void InitializeI2c() {
        // Initialize I2C peripheral
        i2c_master_bus_config_t i2c_bus_cfg = {
                .i2c_port = (i2c_port_t)1,
                .sda_io_num = AUDIO_CODEC_I2C_SDA_PIN,
                .scl_io_num = AUDIO_CODEC_I2C_SCL_PIN,
                .clk_source = I2C_CLK_SRC_DEFAULT,
                .glitch_ignore_cnt = 7,
                .intr_priority = 0,
                .trans_queue_depth = 0,
                .flags = {
                                .enable_internal_pullup = 1,
                        },
        };
        ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_cfg, &i2c_bus_));
    }

    void InitializeSpi() {
        spi_bus_config_t buscfg = {};
        buscfg.mosi_io_num = GPIO_NUM_21;
        buscfg.miso_io_num = GPIO_NUM_NC;
        buscfg.sclk_io_num = GPIO_NUM_12;
        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));
    }

    esp_err_t IoExpanderSetLevel(uint16_t pin_mask, uint8_t level) {
        return esp_io_expander_set_level(io_expander, pin_mask, level);
    }

    uint8_t IoExpanderGetLevel(uint16_t pin_mask) {
        uint32_t pin_val = 0;
        esp_io_expander_get_level(io_expander, DRV_IO_EXP_INPUT_MASK, &pin_val);
        pin_mask &= DRV_IO_EXP_INPUT_MASK;
        return (uint8_t)((pin_val & pin_mask) ? 1 : 0);
    }

    void InitializeIoExpander() {
        esp_io_expander_new_i2c_tca95xx_16bit(
                i2c_bus_, ESP_IO_EXPANDER_I2C_TCA9555_ADDRESS_000, &io_expander);

        esp_err_t ret;
        ret = esp_io_expander_print_state(io_expander);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Print state failed: %s", esp_err_to_name(ret));
        }
        ret = esp_io_expander_set_dir(io_expander, IO_EXPANDER_PIN_NUM_0,
                                                                    IO_EXPANDER_OUTPUT);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Set direction failed: %s", esp_err_to_name(ret));
        }
        ret = esp_io_expander_set_level(io_expander, 0, 1);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Set level failed: %s", esp_err_to_name(ret));
        }
        ret = esp_io_expander_set_dir(
                io_expander, DRV_IO_EXP_INPUT_MASK,
                IO_EXPANDER_INPUT);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Set direction failed: %s", esp_err_to_name(ret));
        }
    }

    void InitializeButtons() {
        instance_ = this;

        // Button A
        button_config_t btn_a_config = {
            .long_press_time = 1000,
            .short_press_time = 0
        };
        btn_a_driver_ = (button_driver_t*)calloc(1, sizeof(button_driver_t));
        btn_a_driver_->enable_power_save = false;
        btn_a_driver_->get_key_level = [](button_driver_t *button_driver) -> uint8_t {
            return !instance_->IoExpanderGetLevel(IO_EXPANDER_PIN_NUM_2);
        };
        ESP_ERROR_CHECK(iot_button_create(&btn_a_config, btn_a_driver_, &btn_a));
        iot_button_register_cb(btn_a, BUTTON_SINGLE_CLICK, nullptr, [](void* button_handle, void* usr_data) {
            auto self = static_cast<Df_K10Board*>(usr_data);
            auto& app = Application::GetInstance();
            if (app.GetDeviceState() == kDeviceStateStarting && !WifiStation::GetInstance().IsConnected()) {
                self->ResetWifiConfiguration();
            }
            app.ToggleChatState();
        }, this);
        iot_button_register_cb(btn_a, BUTTON_LONG_PRESS_START, nullptr, [](void* button_handle, void* usr_data) {
            auto self = static_cast<Df_K10Board*>(usr_data);
            auto codec = self->GetAudioCodec();
            auto volume = codec->output_volume() - 10;
            if (volume < 0) {
                volume = 0;
            }
            codec->SetOutputVolume(volume);
            self->GetDisplay()->ShowNotification(Lang::Strings::VOLUME + std::to_string(volume));
        }, this);

        // Button B
        button_config_t btn_b_config = {
            .long_press_time = 1000,
            .short_press_time = 0
        };
        btn_b_driver_ = (button_driver_t*)calloc(1, sizeof(button_driver_t));
        btn_b_driver_->enable_power_save = false;
        btn_b_driver_->get_key_level = [](button_driver_t *button_driver) -> uint8_t {
            return !instance_->IoExpanderGetLevel(IO_EXPANDER_PIN_NUM_12);
        };
        ESP_ERROR_CHECK(iot_button_create(&btn_b_config, btn_b_driver_, &btn_b));
        iot_button_register_cb(btn_b, BUTTON_SINGLE_CLICK, nullptr, [](void* button_handle, void* usr_data) {
            auto self = static_cast<Df_K10Board*>(usr_data);
            auto& app = Application::GetInstance();
            if (app.GetDeviceState() == kDeviceStateStarting && !WifiStation::GetInstance().IsConnected()) {
                self->ResetWifiConfiguration();
            }
            app.ToggleChatState();
        }, this);
        iot_button_register_cb(btn_b, BUTTON_LONG_PRESS_START, nullptr, [](void* button_handle, void* usr_data) {
            auto self = static_cast<Df_K10Board*>(usr_data);
            auto codec = self->GetAudioCodec();
            auto volume = codec->output_volume() + 10;
            if (volume > 100) {
                volume = 100;
            }
            codec->SetOutputVolume(volume);
            self->GetDisplay()->ShowNotification(Lang::Strings::VOLUME + std::to_string(volume));
        }, this);
    }

    void InitializeCamera() {
        camera_config_t config = {};
        config.ledc_channel = LEDC_CHANNEL_2;   // LEDC通道选择  用于生成XCLK时钟 但是S3不用
        config.ledc_timer = LEDC_TIMER_2;       // LEDC timer选择  用于生成XCLK时钟 但是S3不用
        config.pin_d0 = CAMERA_PIN_D2;
        config.pin_d1 = CAMERA_PIN_D3;
        config.pin_d2 = CAMERA_PIN_D4;
        config.pin_d3 = CAMERA_PIN_D5;
        config.pin_d4 = CAMERA_PIN_D6;
        config.pin_d5 = CAMERA_PIN_D7;
        config.pin_d6 = CAMERA_PIN_D8;
        config.pin_d7 = CAMERA_PIN_D9;
        config.pin_xclk = CAMERA_PIN_XCLK;
        config.pin_pclk = CAMERA_PIN_PCLK;
        config.pin_vsync = CAMERA_PIN_VSYNC;
        config.pin_href = CAMERA_PIN_HREF;
        config.pin_sccb_sda = -1;  // 这里如果写-1 表示使用已经初始化的I2C接口
        config.pin_sccb_scl = CAMERA_PIN_SIOC;
        config.sccb_i2c_port = 1;               //  这里如果写1 默认使用I2C1
        config.pin_pwdn = CAMERA_PIN_PWDN;
        config.pin_reset = CAMERA_PIN_RESET;
        config.xclk_freq_hz = XCLK_FREQ_HZ;
        config.pixel_format = PIXFORMAT_RGB565;
        config.frame_size = FRAMESIZE_VGA;
        config.jpeg_quality = 12;
        config.fb_count = 1;
        config.fb_location = CAMERA_FB_IN_PSRAM;
        config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;

        camera_ = new Esp32Camera(config);
    }

    void InitializeIli9341Display() {
        esp_lcd_panel_io_handle_t panel_io = nullptr;
        esp_lcd_panel_handle_t panel = nullptr;

        // 液晶屏控制IO初始化
        ESP_LOGD(TAG, "Install panel IO");
        esp_lcd_panel_io_spi_config_t io_config = {};
        io_config.cs_gpio_num = GPIO_NUM_14;
        io_config.dc_gpio_num = GPIO_NUM_13;
        io_config.spi_mode = 0;
        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 = GPIO_NUM_NC;
        panel_config.bits_per_pixel = 16;
        panel_config.color_space = ESP_LCD_COLOR_SPACE_BGR;

        ESP_ERROR_CHECK(esp_lcd_new_panel_ili9341(panel_io, &panel_config, &panel));
        ESP_ERROR_CHECK(esp_lcd_panel_reset(panel));
        ESP_ERROR_CHECK(esp_lcd_panel_init(panel));
        ESP_ERROR_CHECK(esp_lcd_panel_invert_color(panel, DISPLAY_BACKLIGHT_OUTPUT_INVERT));
        ESP_ERROR_CHECK(esp_lcd_panel_swap_xy(panel, DISPLAY_SWAP_XY));
        ESP_ERROR_CHECK(esp_lcd_panel_mirror(panel, DISPLAY_MIRROR_X, DISPLAY_MIRROR_Y));
        ESP_ERROR_CHECK(esp_lcd_panel_disp_on_off(panel, true));

        display_ = new SpiLcdDisplay(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_20_4,
                                        .icon_font = &font_awesome_20_4,
                                        .emoji_font = font_emoji_64_init(),
                                });
    }
    
    // 机械臂初始化函数
    void InitializeArmController() {
        ESP_LOGI(TAG, "初始化机械臂控制器");
    arm_controller_ = new ArmController(ARM_SERVO_GPIO, CLAW_SERVO_GPIO);
    
    if (!arm_controller_->Initialize()) {
        ESP_LOGE(TAG, "机械臂控制器初始化失败");
        delete arm_controller_;
        arm_controller_ = nullptr;
        return;
        }

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

    // 物联网初始化，添加对 AI 可见设备
    void InitializeIot() {
        led_strip_ = new CircularStrip(BUILTIN_LED_GPIO, 3);
        new LedStripControl(led_strip_);
        // 注册机械臂MCP工具
        if (arm_controller_ != nullptr) {
            arm_controller_->InitializeTools();
        }
    }

//构造函数
public:
    Df_K10Board() :
        arm_controller_(nullptr) {
        InitializeI2c();
        InitializeIoExpander();
        InitializeSpi();
        InitializeIli9341Display();
        InitializeButtons();
        InitializeArmController();  // 添加机械臂初始化
        InitializeIot();
        InitializeCamera();
    }
    
    // 析构函数
    virtual ~Df_K10Board() {
        if (arm_controller_) {
            delete arm_controller_;
        }        
    }

    virtual Led* GetLed() override {
        return led_strip_;
    }

    virtual AudioCodec *GetAudioCodec() override {
        static K10AudioCodec audio_codec(
                    i2c_bus_,
                    AUDIO_INPUT_SAMPLE_RATE,
                    AUDIO_OUTPUT_SAMPLE_RATE,
                    AUDIO_I2S_GPIO_MCLK,
                    AUDIO_I2S_GPIO_BCLK,
                    AUDIO_I2S_GPIO_WS,
                    AUDIO_I2S_GPIO_DOUT,
                    AUDIO_I2S_GPIO_DIN,
                    AUDIO_CODEC_PA_PIN,
                    AUDIO_CODEC_ES8311_ADDR,
                    AUDIO_CODEC_ES7210_ADDR,
                    AUDIO_INPUT_REFERENCE);
        return &audio_codec;
    }

    virtual Camera* GetCamera() override {
        return camera_;
    }

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

DECLARE_BOARD(Df_K10Board);

Df_K10Board* Df_K10Board::instance_ = nullptr;