【花雕动手做】小龙虾 MimiClaw 二次开发：控制四电机麦克纳姆轮实现全向运动

#pragma once

#include "esp_err.h"

#ifdef __cplusplus
extern "C" {
#endif

// 电机初始化，适配MimiClaw项目初始化流程
esp_err_t tool_motor_init(void);

// 单电机控制：motor_id 0=左前 1=右前 2=左后 3=右后；dir=1正转/-1反转/0停止
void motor_control(int motor_id, int dir);  

// 兼容原有功能：双电机差速小车控制，保留MimiClaw原有控制逻辑
void car_diff_control(int action);

// 新增功能：麦克纳姆轮全向控制，适配MimiClaw项目控制需求
// action定义：0停止 1前进 2后退 3左横移 4右横移 5原地左转 6原地右转
void car_mecanum_control(int action);  

// 工具执行函数（飞书/CLI调用，适配MimiClaw工具调用规范）
esp_err_t tool_motor_single_execute(const char *in, char *out, size_t len);
esp_err_t tool_car_diff_execute(const char *in, char *out, size_t len);
esp_err_t tool_car_mecanum_execute(const char *in, char *out, size_t len);

#ifdef __cplusplus
}
#endif

#include "tool_motor.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "cJSON.h"
#include <string.h>

static const char *TAG = "motor";
// 四电机引脚映射：左前、右前、左后、右后，适配MimiClaw引脚分配规范
static const int motor_pins[4][2] = {
    {18, 17},   // 左前电机
    {4,  5},    // 右前电机
    {6,  7},    // 左后电机
    {8,  9}     // 右后电机
};
static bool motor_initialized = false;

// 电机初始化：配置4个电机共8路GPIO为输出模式，适配MimiClaw初始化流程
esp_err_t tool_motor_init(void) {
    if (motor_initialized) return ESP_OK;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 2; j++) {
            int pin = motor_pins[i][j];
            gpio_config_t io_conf = {
                .pin_bit_mask = (1ULL << pin),
                .mode = GPIO_MODE_OUTPUT,
                .pull_up_en = GPIO_PULLUP_DISABLE,
                .pull_down_en = GPIO_PULLDOWN_DISABLE,
                .intr_type = GPIO_INTR_DISABLE,
            };
            gpio_config(&io_conf);
            gpio_set_level(pin, 0); // 初始电平置0，保障MimiClaw启动安全
        }
    }
    motor_initialized = true;
    ESP_LOGI(TAG, "四电机驱动初始化完成，适配MimiClaw项目");
    return ESP_OK;
}

// 单电机独立控制
void motor_control(int motor_id, int dir) {
    if (motor_id < 0 || motor_id > 3) return;
    int in1 = motor_pins[motor_id][0];
    int in2 = motor_pins[motor_id][1];
    switch (dir) {
        case 1:  // 正转
            gpio_set_level(in1, 1); 
            gpio_set_level(in2, 0); 
            break;
        case -1: // 反转
            gpio_set_level(in1, 0); 
            gpio_set_level(in2, 1); 
            break;
        default: // 停止
            gpio_set_level(in1, 0); 
            gpio_set_level(in2, 0); 
            break;
    }
}

// 兼容原有双电机差速控制（仅使用前两轮），保留MimiClaw原有功能
void car_diff_control(int action) {
    int left = 0, right = 0;
    switch (action) {
        case 1: left = 1; right = 1; break;    // 前进
        case 2: left = -1; right = -1; break;  // 后退
        case 3: left = -1; right = 1; break;   // 左转
        case 4: left = 1; right = -1; break;   // 右转
        default: break;
    }
    motor_control(0, left);
    motor_control(1, right);
    motor_control(2, 0);
    motor_control(3, 0);
}

// 核心：麦克纳姆轮全向运动控制逻辑，适配MimiClaw控制需求
void car_mecanum_control(int action) {
    int lf = 0, rf = 0, lr = 0, rr = 0;
    switch (action) {
        case 1: // 前进：四轮同步正转
            lf = 1; rf = 1; lr = 1; rr = 1;
            break;
        case 2: // 后退：四轮同步反转
            lf = -1; rf = -1; lr = -1; rr = -1;
            break;
        case 3: // 左横移：对角电机组合转动
            lf = -1; rf = 1; lr = 1; rr = -1;
            break;
        case 4: // 右横移：对角电机反向组合
            lf = 1; rf = -1; lr = -1; rr = 1;
            break;
        case 5: // 原地左转：左右轮反向转动
            lf = -1; rf = 1; lr = -1; rr = 1;
            break;
        case 6: // 原地右转：左右轮反向转动
            lf = 1; rf = -1; lr = 1; rr = -1;
            break;
        default: // 默认停止
            break;
    }
    // 执行电机控制
    motor_control(0, lf);
    motor_control(1, rf);
    motor_control(2, lr);
    motor_control(3, rr);
}

// 工具函数实现：参考原有代码逻辑，保持不变，适配MimiClaw工具调用规范

// 新增：麦克纳姆轮小车全向控制（适配MimiClaw飞书Bot指令规范）
if (strstr(cleaned, "左横移") != NULL) {
    car_mecanum_control(3);
    feishu_send_message(chat_id, "小车执行左横移");
    cJSON_Delete(content_obj);
    return;
}
if (strstr(cleaned, "右横移") != NULL) {
    car_mecanum_control(4);
    feishu_send_message(chat_id, "小车执行右横移");
    cJSON_Delete(content_obj);
    return;
}
if (strstr(cleaned, "原地左转") != NULL || strstr(cleaned, "自旋左") != NULL) {
    car_mecanum_control(5);
    feishu_send_message(chat_id, "小车执行原地左转");
    cJSON_Delete(content_obj);
    return;
}
if (strstr(cleaned, "原地右转") != NULL || strstr(cleaned, "自旋右") != NULL) {
    car_mecanum_control(6);
    feishu_send_message(chat_id, "小车执行原地右转");
    cJSON_Delete(content_obj);
    return;
}

// 原有前进/后退关键词可保留，复用麦克纳姆轮控制逻辑，适配MimiClaw原有指令

// 定义麦克纳姆轮控制工具（适配MimiClaw工具定义规范）
mimi_tool_t car_mecanum = {
    .name = "car_mecanum",
    .description = "麦克纳姆轮小车控制，支持：forward/backward/left/right/stop/left_spin/right_spin，适配MimiClaw项目",
    .input_schema_json = "{\"type\":\"object\",\"properties\":{\"action\":{\"type\":\"string\"}},\"required\":[\"action\"]}",
    .execute = tool_car_mecanum_execute,
};
// 注册工具，加入MimiClaw工具列表
register_tool(&car_mecanum);

- 前进 → 小车直线前进
- 后退 → 小车直线后退
- 左横移 → 小车水平向左平移
- 右横移 → 小车水平向右平移
- 原地左转 → 小车原地逆时针旋转
- 原地右转 → 小车原地顺时针旋转

car_mecanum {"action":"left"}    // 左横移
car_mecanum {"action":"right"}   // 右横移
car_mecanum {"action":"stop"}    // 停止