大数跨境
0
0

一条语音指令搞定 5 路电机、11 种动作!这台开源小车有点东西

一条语音指令搞定 5 路电机、11 种动作!这台开源小车有点东西 DF创客社区
2025-10-10
1
导读:有兴趣的可以试试

【点击上方「蓝字」关注DF创客社区,一起成为技术颜控】

前不久,@云天在 DF创客社区论坛发了一个“能听会说的小车”雏形,但只用了行空板K10的四个引脚。

这次云天老师把金手指引出的“引脚”通过IO扩展板接入 3个L298N(DF-MD V1.3)电机驱动器,控制小车真正做到了“说一句,动一路”——前进、后退、左转、右转、机械臂 360° 旋转、抬起放下、机械爪张合,全部一条语音指令搞定。

整体架构

演示视频

视频来源:B站 @创客云天

电机接线对照表(TCA9555 → L298N)

功能段
P 引脚
8255 位
L298N 通道
备注
左轮 ENA
P8
BIT8
ENA
与 P12(IN1) 组合
右轮 ENB
P9
BIT9
ENB
与 P13(IN2) 组合
左轮 IN1
P5
BIT12
IN1

右轮 IN2
P4
BIT13
IN2

爪 ENA
P3
BIT14
ENA
接爪电机
爪 IN1
P12
BIT3
IN1
0张开,1闭合
臂水平
P2
BIT7
ENA
水平旋转电机
臂水平
P13
BIT4
IN2
0左旋转,1右旋转
臂俯仰
P11
BIT2
ENA
俯仰电机
臂俯仰 M
P1
——
IN1
直接 GPIO2 控制方向

软件关键片段

1. IO 扩展板一次初始化

uint16_t output_pins = (1 << 8) | (1 << 9) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 3) | (1 << 4) | (1 << 2) | (1 << 7);

    esp_io_expander_set_dir(io_expander, output_pins, IO_EXPANDER_OUTPUT);

    esp_io_expander_set_level(io_expander, output_pins, 0); // 全部归零,防止上电乱动

2. MCP “一句话注册”

mcp_server.AddTool("self.car.action",
    "可控制小车及车载机械臂、机械爪的运行动作,小车的动作是前进、后退、停止、左转、右转。机械臂的动作是左旋转、右旋转、抬起、放下。机械爪的动作是张开、闭合",
    PropertyList({Property("动作", kPropertyTypeString)}),
    [this, gpio_num_P1](const PropertyList& properties) -> ReturnValue {
        std::string action = properties["动作"].value<std::string>().c_str();

        if (action == "前进") {
            SetPnumLevel(81);
            SetPnumLevel(91);
            SetPnumLevel(121);
            SetPnumLevel(131);
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "停止") {
            SetPnumLevel(80);  // P8引脚
            SetPnumLevel(90);  // P9引脚
        } elseif (action == "后退") {
            SetPnumLevel(81);  // P8引脚
            SetPnumLevel(91);  // P9引脚
            SetPnumLevel(120); // P5引脚
            SetPnumLevel(130); // P4引脚
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "左转") {
            SetPnumLevel(81);
            SetPnumLevel(91);
            SetPnumLevel(120);
            SetPnumLevel(131);
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "右转") {
            SetPnumLevel(81);
            SetPnumLevel(91);
            SetPnumLevel(121);
            SetPnumLevel(130);
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "左旋转") {
            // 机械臂水平动作
            SetPnumLevel(71);  // P2引脚 E
            SetPnumLevel(40);  // P13引脚 M
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "右旋转") {
            // 机械臂水平动作
            SetPnumLevel(71);  // P2引脚 E
            SetPnumLevel(41);  // P13引脚 M
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "张开") {
            // 机械爪动作
            SetPnumLevel(141); // P3引脚 E
            SetPnumLevel(30);  // P12引脚 M
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1.6 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "闭合") {
            // 机械爪动作
            SetPnumLevel(141); // P3引脚 E
            SetPnumLevel(31);  // P12引脚 M
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1.6 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "抬起") {
            // 机械臂垂直运动
            SetPnumLevel(21); // P11引脚 E
            gpio_set_level(gpio_num_P1, 0); // M
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        } elseif (action == "放下") {
            // 机械臂垂直运动
            SetPnumLevel(21); // P11引脚 E
            gpio_set_level(gpio_num_P1, 1); // M
            if (stop_timer_) {
                xTimerStop(stop_timer_, 0);
                xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(0.8 * 1000), 0);
                xTimerStart(stop_timer_, 0);
            }
        }
        
        return ReturnValue(action);
    }
);

把 12 条动作全部写进一个 Lambda,LLM 只需一次函数调用即可精准执行。

3. 停车回调函数

static void StopMotorCallbackStatic(TimerHandle_t xTimer) {
    auto* self = static_cast<Df_K10Board*>(pvTimerGetTimerID(xTimer));
    self->StopMotorCallback();
}

void StopMotorCallback() {
    SetPnumLevel(80);
    SetPnumLevel(90);
    SetPnumLevel(120);
    SetPnumLevel(130);
    SetPnumLevel(140);
    SetPnumLevel(30);
    SetPnumLevel(20);
    SetPnumLevel(70);
    SetPnumLevel(40);
}

每条动作都会根据实际行程重置定时器(爪 1.6 s、臂 0.8~1 s、底盘 1 s),兼顾“到位”与“安全”。

一句话总结

“当语音助手不再只是‘聊天’,而是能直接操作 5 路电机、11 种动作、100% 开源可二次开发,你会发现——给 ESP32 装上‘大模型’,才是嵌入式玩家的新浪漫。

完整代码

"XIAOZHI-ESP32-MAIN/main/boards/df-k10/df_k10_board.cc"

#include "wifi_board.h"
#include "k10_audio_codec.h"
#include "display/lcd_display.h"
#include "esp_lcd_ili9341.h"
#include "led_control.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 <freertos/timers.h>
#include "esp_io_expander_tca95xx_16bit.h"
#include <mcp_server.h>

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

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

    CircularStrip* led_strip_;
    TimerHandle_t stop_timer_ = nullptr;
    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, 01);
        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));
        }
        // 设置多个引脚为输出模式
        uint16_t output_pins = (1 << 8) | (1 << 9) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 3) | (1 << 4) | (1 << 2) | (1 << 7);  // 引脚 P8,P9,P5,P4,P3,P12,P13,P11,P2
        ret = esp_io_expander_set_dir(io_expander, output_pins, 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, output_pins, 0);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Set level failed: %s", esp_err_to_name(ret));
        }
    }

    void SetPnumLevel(uint16_t Pnum, uint8_t level) {
        esp_err_t ret = esp_io_expander_set_level(io_expander, 1 << Pnum, level);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Set level for P 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(1sizeof(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(1sizeof(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(),
            });
    }

    // 物联网初始化,添加对 AI 可见设备
    void InitializeIot() {
        gpio_num_t gpio_num_P0 = GPIO_NUM_1;
        gpio_num_t gpio_num_P1 = GPIO_NUM_2;
        gpio_config_t cfg = {
            .pin_bit_mask = (1ULL << gpio_num_P0) | (1ULL << gpio_num_P1),
            .mode = GPIO_MODE_OUTPUT,
            .pull_up_en = GPIO_PULLUP_DISABLE,
            .pull_down_en = GPIO_PULLDOWN_DISABLE,
            .intr_type = GPIO_INTR_DISABLE,
        };
        ESP_ERROR_CHECK(gpio_config(&cfg));
        gpio_set_level(gpio_num_P0, 0);
        gpio_set_level(gpio_num_P1, 0);
        stop_timer_ = xTimerCreate("StopMotorTimer", pdMS_TO_TICKS(1000), pdFALSE, this, StopMotorCallbackStatic);

        auto& mcp_server = McpServer::GetInstance();
        mcp_server.AddTool("self.car.action",
            "可控制小车及车载机械臂、机械爪的运行动作,小车的动作是前进、后退、停止、左转、右转。机械臂的动作是左旋转、右旋转、抬起、放下。机械爪的动作是张开、闭合",
            PropertyList({Property("动作", kPropertyTypeString)}),
            [this, gpio_num_P1](const PropertyList& properties) -> ReturnValue {
                std::string action = properties["动作"].value<std::string>().c_str();
                if (action == "前进") {
                    SetPnumLevel(81);
                    SetPnumLevel(91);
                    SetPnumLevel(121);
                    SetPnumLevel(131);
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                } elseif (action == "停止") {
                    SetPnumLevel(80);  // P8引脚
                    SetPnumLevel(90);  // P9引脚
                } elseif (action == "后退") {
                    SetPnumLevel(81);  // P8引脚
                    SetPnumLevel(91);  // P9引脚
                    SetPnumLevel(120); // P5引脚
                    SetPnumLevel(130); // P4引脚
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                    // 夹子
                    // SetPnumLevel(14,0); //P3引脚 E
                    // SetPnumLevel(3,0); //P12引脚 M
                    // 上下
                    // SetPnumLevel(2,0); //P11引脚 E
                    // 水平
                    // SetPnumLevel(7,0); //P2引脚 E
                    // SetPnumLevel(4,0); //P13引脚 M
                    // SetPnumLevel(15,0); //LED灯
                    // SetPnumLevel(16,0); //无对应引脚
                    // SetPnumLevel(1,0); //无对应引脚
                    // SetPnumLevel(5,0); //无对应引脚
                    // SetPnumLevel(6,0); //无对应引脚
                    // SetPnumLevel(10,0); //无对应引脚
                    // SetPnumLevel(11,0); //无对应引脚
                } elseif (action == "左转") {
                    SetPnumLevel(81);
                    SetPnumLevel(91);
                    SetPnumLevel(120);
                    SetPnumLevel(131);
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                } elseif (action == "右转") {
                    SetPnumLevel(81);
                    SetPnumLevel(91);
                    SetPnumLevel(121);
                    SetPnumLevel(130);
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                } elseif (action == "左旋转") {
                    SetPnumLevel(71);  // P2引脚 E
                    SetPnumLevel(40);  // P13引脚 M
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                } elseif (action == "右旋转") {
                    SetPnumLevel(71);  // P2引脚 E
                    SetPnumLevel(41);  // P13引脚 M
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                } elseif (action == "张开") {
                    SetPnumLevel(141); // P3引脚 E
                    SetPnumLevel(30);  // P12引脚 M
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1.6 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                } elseif (action == "闭合") {
                    SetPnumLevel(141); // P3引脚 E
                    SetPnumLevel(31);  // P12引脚 M
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1.6 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                } elseif (action == "抬起") {
                    SetPnumLevel(21); // P11引脚 E
                    gpio_set_level(gpio_num_P1, 0); // M
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(1 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                } elseif (action == "放下") {
                    SetPnumLevel(21); // P11引脚 E
                    gpio_set_level(gpio_num_P1, 1); // M
                    if (stop_timer_) {
                        xTimerStop(stop_timer_, 0);
                        xTimerChangePeriod(stop_timer_, pdMS_TO_TICKS(0.8 * 1000), 0);
                        xTimerStart(stop_timer_, 0);
                    }
                }
                return ReturnValue(action);
            });
    }

    static void StopMotorCallbackStatic(TimerHandle_t xTimer) {
        auto* self = static_cast<Df_K10Board*>(pvTimerGetTimerID(xTimer));
        self->StopMotorCallback();
    }

    void StopMotorCallback() {
        SetPnumLevel(80);
        SetPnumLevel(90);
        SetPnumLevel(120);
        SetPnumLevel(130);
        SetPnumLevel(140);
        SetPnumLevel(30);
        SetPnumLevel(20);
        SetPnumLevel(70);
        SetPnumLevel(40);
    }

public:
    Df_K10Board() {
        InitializeI2c();
        InitializeIoExpander();
        InitializeSpi();
        InitializeIli9341Display();
        InitializeButtons();
        InitializeIot();
        //InitializeCamera();
    }

    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;


原项目地址:https://mc.dfrobot.com.cn/thread-398324-1-1.html

项目作者:云天

首发于DF创客社区

转载请注明来源信息



硬件军火库


DFRobot硬件军火库


点击了解详情👆


DFRobot官方品牌店 https://dfrobot.taobao.com/

DFRobot官方旗舰店 https://dfrobot.jd.com/


文中涉及的文件,可以点击阅读原文,到DF创客社区论坛下载,大家有什么想说的,欢迎在下方留言



往期项目回顾


【保姆级教程】用树莓派5从零开始打造一台功能完备的4盘位NAS


用FireBeetle ESP32-E和LVGL 自制动态温湿度仪计


不只是一台玩具车!开源燃料电池机器人HydroBot全揭秘


DIY一台ESP32驱动热敏打印机,让你的创意“落地成纸”!


让时间“滑”着走,这台机械钟我能玩一天


点击阅读👆

【声明】内容源于网络
0
0
DF创客社区
我们是专注于创新和开源硬件开发的公司——DFRobot成立的创客社区,无论你是资深创客还是小白,这里都有你的一席之地。一个人玩自己的项目,你只是寂寞宅;一群人看你玩项目,你就是技术牛!快来分享你的项目吧!
内容 1282
粉丝 0
DF创客社区 我们是专注于创新和开源硬件开发的公司——DFRobot成立的创客社区,无论你是资深创客还是小白,这里都有你的一席之地。一个人玩自己的项目,你只是寂寞宅;一群人看你玩项目,你就是技术牛!快来分享你的项目吧!
总阅读2.8k
粉丝0
内容1.3k