轮子操控

.clangd

@@ -0,0 +1,2 @@
+CompileFlags:
+    Remove: [-f*, -m*]

.devcontainer/Dockerfile

@@ -0,0 +1,13 @@
+ARG DOCKER_TAG=latest
+FROM espressif/idf:${DOCKER_TAG}
+
+ENV LC_ALL=C.UTF-8
+ENV LANG=C.UTF-8
+
+RUN apt-get update -y && apt-get install udev -y
+
+RUN echo "source /opt/esp/idf/export.sh > /dev/null 2>&1" >> ~/.bashrc
+
+ENTRYPOINT [ "/opt/esp/entrypoint.sh" ]
+
+CMD ["/bin/bash", "-c"]

.devcontainer/devcontainer.json

@@ -0,0 +1,21 @@
+{
+	"name": "ESP-IDF QEMU",
+	"build": {
+		"dockerfile": "Dockerfile"
+	},
+	"customizations": {
+		"vscode": {
+			"settings": {
+				"terminal.integrated.defaultProfile.linux": "bash",
+				"idf.espIdfPath": "/opt/esp/idf",
+				"idf.toolsPath": "/opt/esp",
+				"idf.gitPath": "/usr/bin/git"
+			},
+			"extensions": [
+				"espressif.esp-idf-extension",
+				"espressif.esp-idf-web"
+			]
+		}
+	},
+	"runArgs": ["--privileged"]
+}

.vscode/c_cpp_properties.json

@@ -0,0 +1,25 @@
+{
+  "configurations": [
+    {
+      "name": "ESP-IDF",
+      "compilerPath": "${config:idf.toolsPath}/tools/xtensa-esp-elf/esp-14.2.0_20241119/xtensa-esp-elf/bin/xtensa-esp32-elf-gcc",
+      "compileCommands": "${config:idf.buildPath}/compile_commands.json",
+      "includePath": [
+        "${config:idf.espIdfPath}/components/**",
+        
+        "/home/ahxbxa/esp5.4.2/esp-idf/components/**",
+        "${config:idf.espIdfPathWin}/components/**",
+        "${workspaceFolder}/**"
+      ],
+      "browse": {
+        "path": [
+          "${config:idf.espIdfPath}/components",
+          "${config:idf.espIdfPathWin}/components",
+          "${workspaceFolder}"
+        ],
+        "limitSymbolsToIncludedHeaders": true
+      }
+    }
+  ],
+  "version": 4
+}

.vscode/launch.json

@@ -0,0 +1,15 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "type": "gdbtarget",
+      "request": "attach",
+      "name": "Eclipse CDT GDB Adapter"
+    },
+    {
+      "type": "espidf",
+      "name": "Launch",
+      "request": "launch"
+    }
+  ]
+}

.vscode/settings.json

@@ -0,0 +1,77 @@
+{
+  "C_Cpp.intelliSenseEngine": "default",
+  "idf.port": "/dev/ttyUSB0",
+  "idf.flashType": "UART",
+  "files.associations": {
+    "array": "cpp",
+    "atomic": "cpp",
+    "bit": "cpp",
+    "bitset": "cpp",
+    "cctype": "cpp",
+    "charconv": "cpp",
+    "chrono": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "compare": "cpp",
+    "complex": "cpp",
+    "concepts": "cpp",
+    "condition_variable": "cpp",
+    "cstdarg": "cpp",
+    "cstddef": "cpp",
+    "cstdint": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "ctime": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "deque": "cpp",
+    "list": "cpp",
+    "map": "cpp",
+    "set": "cpp",
+    "string": "cpp",
+    "unordered_map": "cpp",
+    "vector": "cpp",
+    "exception": "cpp",
+    "algorithm": "cpp",
+    "functional": "cpp",
+    "iterator": "cpp",
+    "memory": "cpp",
+    "memory_resource": "cpp",
+    "netfwd": "cpp",
+    "numeric": "cpp",
+    "optional": "cpp",
+    "random": "cpp",
+    "ratio": "cpp",
+    "regex": "cpp",
+    "string_view": "cpp",
+    "system_error": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "utility": "cpp",
+    "format": "cpp",
+    "fstream": "cpp",
+    "future": "cpp",
+    "initializer_list": "cpp",
+    "iomanip": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "limits": "cpp",
+    "mutex": "cpp",
+    "new": "cpp",
+    "numbers": "cpp",
+    "ostream": "cpp",
+    "semaphore": "cpp",
+    "span": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "stop_token": "cpp",
+    "streambuf": "cpp",
+    "text_encoding": "cpp",
+    "thread": "cpp",
+    "cinttypes": "cpp",
+    "typeinfo": "cpp",
+    "variant": "cpp"
+  }
+}

CMakeLists.txt

@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.16)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(Mars_Car)

README.md

@@ -0,0 +1,53 @@
+| Supported Targets | ESP32 | ESP32-C2 | ESP32-C3 | ESP32-C5 | ESP32-C6 | ESP32-C61 | ESP32-H2 | ESP32-P4 | ESP32-S2 | ESP32-S3 | Linux |
+| ----------------- | ----- | -------- | -------- | -------- | -------- | --------- | -------- | -------- | -------- | -------- | ----- |
+
+# Hello World Example
+
+Starts a FreeRTOS task to print "Hello World".
+
+(See the README.md file in the upper level 'examples' directory for more information about examples.)
+
+## How to use example
+
+Follow detailed instructions provided specifically for this example.
+
+Select the instructions depending on Espressif chip installed on your development board:
+
+- [ESP32 Getting Started Guide](https://docs.espressif.com/projects/esp-idf/en/stable/get-started/index.html)
+- [ESP32-S2 Getting Started Guide](https://docs.espressif.com/projects/esp-idf/en/latest/esp32s2/get-started/index.html)
+
+
+## Example folder contents
+
+The project **hello_world** contains one source file in C language [hello_world_main.c](main/hello_world_main.c). The file is located in folder [main](main).
+
+ESP-IDF projects are built using CMake. The project build configuration is contained in `CMakeLists.txt` files that provide set of directives and instructions describing the project's source files and targets (executable, library, or both).
+
+Below is short explanation of remaining files in the project folder.
+
+```
+├── CMakeLists.txt
+├── pytest_hello_world.py      Python script used for automated testing
+├── main
+│   ├── CMakeLists.txt
+│   └── hello_world_main.c
+└── README.md                  This is the file you are currently reading
+```
+
+For more information on structure and contents of ESP-IDF projects, please refer to Section [Build System](https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-guides/build-system.html) of the ESP-IDF Programming Guide.
+
+## Troubleshooting
+
+* Program upload failure
+
+    * Hardware connection is not correct: run `idf.py -p PORT monitor`, and reboot your board to see if there are any output logs.
+    * The baud rate for downloading is too high: lower your baud rate in the `menuconfig` menu, and try again.
+
+## Technical support and feedback
+
+Please use the following feedback channels:
+
+* For technical queries, go to the [esp32.com](https://esp32.com/) forum
+* For a feature request or bug report, create a [GitHub issue](https://github.com/espressif/esp-idf/issues)
+
+We will get back to you as soon as possible.

dependencies.lock

@@ -0,0 +1,21 @@
+dependencies:
+  espressif/servo:
+    component_hash: 309c787e48224255fad458cfd9ab86ea53f0fdad1c5e4f6f0c50309990b17108
+    dependencies:
+    - name: idf
+      require: private
+      version: '>=4.4'
+    source:
+      registry_url: https://components.espressif.com/
+      type: service
+    version: 0.1.0
+  idf:
+    source:
+      type: idf
+    version: 5.4.2
+direct_dependencies:
+- espressif/servo
+- idf
+manifest_hash: 3dad4570361cb98ff907c92e0dada8605c352b82ceaed7b23de18cc0b41ec601
+target: esp32
+version: 2.0.0

main/CMakeLists.txt

@@ -0,0 +1,5 @@
+idf_component_register(SRCS "main.cpp"
+    "src/ps2_controller.cpp"
+    "src/moveInfo.cpp"
+    "src/uartTool.cpp"
+    INCLUDE_DIRS "include")

main/idf_component.yml

@@ -0,0 +1,17 @@
+## IDF Component Manager Manifest File
+dependencies:
+  ## Required IDF version
+  idf:
+    version: '>=4.1.0'
+  # # Put list of dependencies here
+  # # For components maintained by Espressif:
+  # component: "~1.0.0"
+  # # For 3rd party components:
+  # username/component: ">=1.0.0,<2.0.0"
+  # username2/component2:
+  #   version: "~1.0.0"
+  #   # For transient dependencies `public` flag can be set.
+  #   # `public` flag doesn't have an effect dependencies of the `main` component.
+  #   # All dependencies of `main` are public by default.
+  #   public: true
+  espressif/servo: '*'

main/include/moveInfo.h

@@ -0,0 +1,57 @@
+#ifndef MOVEINFO_H
+#define MOVEINFO_H
+#include <string>
+class MoveInfo
+{
+public:
+    MoveInfo() = default;
+    MoveInfo(std::string i, int p, int t) : index(i), pwm(t), t(t) {};
+    ~MoveInfo() = default;
+
+    std::string getInfo();
+    void setInfo(std::string i,int p,int t);
+private:
+    std::string index;
+    int pwm;
+    int t;
+};
+class WheelMoveInfo : public MoveInfo
+{
+public:
+    WheelMoveInfo() = default;
+    WheelMoveInfo(std::string fl, std::string fr, std::string bl, std::string br, int lx, int ly);
+    WheelMoveInfo(std::string fl, int pwm_fl, int t_fl, std::string fr, int pwm_fr, int t_fr,
+                  std::string bl, int pwm_bl, int t_bl, std::string br, int pwm_br, int t_br)
+        : front_left(fl, pwm_fl, t_fl), front_right(fr, pwm_fr, t_fr),
+          back_left(bl, pwm_bl, t_bl), back_right(fl, pwm_br, t_br) {};
+    ~WheelMoveInfo() = default;
+
+    std::string getWheelInfo();
+private:
+    MoveInfo front_left;
+    MoveInfo front_right;
+    MoveInfo back_left;
+    MoveInfo back_right;
+};
+
+class SteerMoveInfo : public MoveInfo
+{
+public:
+    SteerMoveInfo() = default;
+    SteerMoveInfo(std::string fl, std::string fr, std::string bl, std::string br, int rx, int ry);
+    SteerMoveInfo(std::string fl, int pwm_fl, int t_fl, std::string fr, int pwm_fr, int t_fr,
+                  std::string bl, int pwm_bl, int t_bl, std::string br, int pwm_br, int t_br)
+        : front_left(fl, pwm_fl, t_fl), front_right(fr, pwm_fr, t_fr),
+          back_left(bl, pwm_bl, t_bl), back_right(fl, pwm_br, t_br) {};
+    ~SteerMoveInfo() = default;
+
+    std::string getSteerInfo();
+
+private:
+    MoveInfo front_left;
+    MoveInfo front_right;
+    MoveInfo back_left;
+    MoveInfo back_right;
+};
+
+#endif // MOVEINFO_H

main/include/parameter.h

@@ -0,0 +1,14 @@
+#ifndef PARAMETER_H
+#define PARAMETER_H
+#include <string>
+static const int MAX_WHEEL_SPEED = 1000;
+static const std::string WHEEL_FL="001";
+static const std::string WHEEL_FR="002";
+static const std::string WHEEL_BL="003";
+static const std::string WHEEL_BR="004";
+
+
+static const int FRAME_TIME= 10;
+
+
+#endif //PARAMETER_H

main/include/ps2_controller.h

@@ -0,0 +1,75 @@
+#ifndef PS2_CONTROLLER_H
+#define PS2_CONTROLLER_H
+
+// 常量定义
+#define READ_DELAY_MS 10
+#define SHORT_DELAY_MS 1
+// 按钮常量定义
+#define BUTTON_SELECT 0x0001
+#define BUTTON_L3 0x0002
+#define BUTTON_R3 0x0004
+#define BUTTON_START 0x0008
+#define BUTTON_PAD_UP 0x0010
+#define BUTTON_PAD_RIGHT 0x0020
+#define BUTTON_PAD_DOWN 0x0040
+#define BUTTON_PAD_LEFT 0x0080
+#define BUTTON_L2 0x0100
+#define BUTTON_R2 0x0200
+#define BUTTON_L1 0x0400
+#define BUTTON_R1 0x0800
+#define BUTTON_TRIANGLE 0x1000
+#define BUTTON_CIRCLE 0x2000
+#define BUTTON_CROSS 0x4000
+#define BUTTON_SQUARE 0x8000
+
+#define LX 5
+#define LY 6
+#define RX 7
+#define RY 8
+
+#include <stdio.h>
+#include "driver/gpio.h"
+class PS2Controller
+{
+public:
+    PS2Controller() = delete;
+    PS2Controller(gpio_num_t dat_pin, gpio_num_t cmd_pin, gpio_num_t sel_pin, gpio_num_t clk_pin);
+    ~PS2Controller() = default;
+
+    int getLx(){return data[LX];}
+    int getLy(){return data[LY];}
+    int getRx(){return data[RX];}
+    int getRy(){return data[RY];}
+    void launch();
+    
+private:
+
+    uint8_t sendAndReceive(uint8_t const &byte) const;
+    bool readState(bool motor1, uint8_t motor2);
+    int config(bool pressures, bool rumble);
+    void reconfig();
+    void sendCommandString(uint8_t *cmd, int length) const;
+    bool Button(uint16_t button) const;
+    bool NewButtonState(uint16_t button) const;
+    bool ButtonPressed(uint16_t button) const;
+    bool ButtonReleased(uint16_t button) const;
+
+    void ps2Task();
+    static void ps2StaticTask(void *pvParameters);
+
+    gpio_num_t DAT;
+    gpio_num_t CMD;
+    gpio_num_t SEL;
+    gpio_num_t CLK;
+
+    uint8_t data[21];
+    uint16_t last_buttons;
+    uint16_t buttons;
+    int64_t last_read;
+    uint8_t read_delay;
+    uint8_t controller_type;
+    bool en_Rumble;
+    bool en_Pressures;
+};
+
+#endif // PS2_CONTROLLER_H

main/include/uartTool.h

@@ -0,0 +1,48 @@
+#ifndef UART_H
+#define UART_H
+
+#include "driver/gpio.h"
+#include <string>
+#include <driver/uart.h>
+#include <stdio.h>
+
+static const int RX_BUF_SIZE = 1024;
+class UartTool
+{
+public:
+    UartTool(uart_port_t UART_NUM, gpio_num_t TXD_PIN, gpio_num_t RXD_PIN,
+             int b_rate = 115200,
+             uart_word_length_t d_bits = UART_DATA_8_BITS,
+             uart_parity_t p = UART_PARITY_DISABLE,
+             uart_stop_bits_t s_bits = UART_STOP_BITS_1,
+             uart_hw_flowcontrol_t f_ctrl = UART_HW_FLOWCTRL_DISABLE,
+             uint8_t rx = 0,
+             uart_sclk_t s_clk = UART_SCLK_APB)
+        : UART_NUM(UART_NUM),TXD_PIN(TXD_PIN), RXD_PIN(RXD_PIN)
+    {
+        config = {
+            .baud_rate = b_rate,
+            .data_bits = d_bits,
+            .parity = p,
+            .stop_bits = s_bits,
+            .flow_ctrl = f_ctrl,
+            .rx_flow_ctrl_thresh = rx,
+            .source_clk = s_clk,
+        };
+        // 安装串口驱动 串口编号、接收buff、发送buff、事件队列、分配中断的标志
+        uart_driver_install(UART_NUM, RX_BUF_SIZE * 2, 0, 0, NULL, 0);
+        // 串口参数配置 串口号、串口配置参数
+        uart_param_config(UART_NUM, &config);
+        // 设置串口引脚号 串口编号、tx引脚、rx引脚、rts引脚、cts引脚
+        uart_set_pin(UART_NUM, TXD_PIN, RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+        printf("串口通信已创建！\n");
+    }
+    int sendData(const std::string logName, const std::string data);
+    
+private:
+    uart_port_t UART_NUM;
+    gpio_num_t TXD_PIN;
+    gpio_num_t RXD_PIN;
+    uart_config_t config;
+};
+#endif // UART_H

main/main.cpp

@@ -0,0 +1,22 @@
+#include "ps2_controller.h"
+#include "moveInfo.h"
+#include "parameter.h"
+#include "driver/gpio.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_timer.h"
+#include "uartTool.h"
+extern "C" void app_main()
+{
+    PS2Controller ps2(GPIO_NUM_19,GPIO_NUM_18,GPIO_NUM_15,GPIO_NUM_23);
+    UartTool uartTool(UART_NUM_2,GPIO_NUM_17,GPIO_NUM_16);
+    ps2.launch();
+    
+    while(1)
+    {
+        WheelMoveInfo wheel(WHEEL_FL,WHEEL_FR,WHEEL_BL,WHEEL_BR,ps2.getLx(),ps2.getLy());
+        std::string cmd= wheel.getWheelInfo();
+        uartTool.sendData("WheelMove",cmd);       
+        vTaskDelay(1 / portTICK_PERIOD_MS);                  //延
+    }
+}

main/src/moveInfo.cpp

@@ -0,0 +1,78 @@
+#include "moveInfo.h"
+#include "parameter.h"
+#include <iostream>
+#include <string>
+std::string MoveInfo::getInfo()
+{
+    char *Pwm = new char[5];
+    char *T = new char[5];
+    sprintf(Pwm, "%04d", pwm);
+    sprintf(T, "%04d", t);
+    std::string result = "#" + std::string(index) + "P" + std::string(Pwm) + "T" + std::string(T) + "!";
+    delete[] Pwm;
+    delete[] T;
+    return result;
+}
+void MoveInfo::setInfo(std::string index, int pwm, int t)
+{
+    this->index = index;
+    this->pwm = pwm;
+    this->t = t;
+}
+
+//================轮子==============//
+WheelMoveInfo::WheelMoveInfo(std::string fl, std::string fr, std::string bl, std::string br, int lx, int ly)
+{
+    auto abs = [](auto x)
+    { return x > 0 ? x : -x; };
+    auto max = [](auto x, auto y)
+    { return x > y ? x : y; };
+
+    double x = lx * 1.0 - 255.0 / 2;
+    double y = ly * 1.0 - 255.0 / 2;
+    double leftRatio, rightRatio;
+    if (x < 0 && y > 0)
+    {
+        leftRatio = x + y;
+        rightRatio = max(-x, y);
+    }
+    else if (x < 0 && y < 0)
+    {
+        leftRatio = -abs(x - y);
+        if (x < y)
+            rightRatio = -x + 2 * y;
+        else
+            rightRatio = y;
+    }
+    else if (x > 0 && y > 0)
+    {
+        leftRatio = max(x, y);
+        rightRatio = y - x;
+    }
+    else
+    {
+        rightRatio = -abs(x + y);
+        if (x + y < 0)
+            leftRatio = y;
+        else
+            leftRatio = x + 2 * y;
+    }
+
+    front_left.setInfo(fl, 1500 + leftRatio * MAX_WHEEL_SPEED, FRAME_TIME);
+    back_left.setInfo(fl, 1500 + leftRatio * MAX_WHEEL_SPEED, FRAME_TIME);
+
+    front_right.setInfo(fr, 1500 + rightRatio * MAX_WHEEL_SPEED, FRAME_TIME);
+    back_right.setInfo(br, 1500 + rightRatio * MAX_WHEEL_SPEED, FRAME_TIME);
+}
+std::string WheelMoveInfo::getWheelInfo()
+{
+    std::string result = front_left.getInfo() + front_right.getInfo() + back_left.getInfo() + back_right.getInfo();
+    return result;
+}
+
+//==================底盘舵机============//
+std::string SteerMoveInfo::getSteerInfo()
+{
+    std::string result = front_left.getInfo() + front_right.getInfo() + back_left.getInfo() + back_right.getInfo();
+    return result;
+}

main/src/ps2_controller.cpp

@@ -0,0 +1,307 @@
+#include "ps2_controller.h"
+#include "freertos/FreeRTOS.h"
+#include "esp_timer.h"
+#include <string.h>
+PS2Controller::PS2Controller(gpio_num_t dat_pin, gpio_num_t cmd_pin,
+                             gpio_num_t sel_pin, gpio_num_t clk_pin)
+{
+    DAT = dat_pin;
+    CMD = cmd_pin;
+    SEL = sel_pin;
+    CLK = clk_pin;
+
+    // 配置GPIO
+    gpio_config_t io_conf = {
+        .pin_bit_mask = (1ULL << dat_pin),
+        .mode = GPIO_MODE_INPUT,
+        .pull_up_en = GPIO_PULLUP_ENABLE,
+        .pull_down_en = GPIO_PULLDOWN_DISABLE,
+        .intr_type = GPIO_INTR_DISABLE,
+    };
+    gpio_config(&io_conf);
+
+    io_conf.pin_bit_mask = (1ULL << cmd_pin) | (1ULL << sel_pin) | (1ULL << clk_pin);
+    io_conf.mode = GPIO_MODE_OUTPUT;
+    gpio_config(&io_conf);
+
+    // 初始化引脚状态
+    gpio_set_level(CMD, 1);
+    gpio_set_level(CLK, 1);
+    gpio_set_level(SEL, 1);
+
+    // 初始化内部状态
+    memset(data, 0, sizeof(data));
+    last_buttons = 0;
+    buttons = 0;
+    last_read = 0;
+    read_delay = 1; // 1ms读取一次
+    controller_type = 0;
+    en_Rumble = false;
+    en_Pressures = false;
+}
+uint8_t PS2Controller::sendAndReceive(uint8_t const &byte) const
+{
+    uint8_t tmp = 0;
+    for (int i = 0; i < 8; i++)
+    {
+        // 设置命令引脚的状态
+        gpio_set_level(CMD, (byte >> i) & 0x01);
+
+        // 拉低时钟信号开始传输
+        gpio_set_level(CLK, 0);
+        vTaskDelay(SHORT_DELAY_MS / portTICK_PERIOD_MS);
+
+        // 读取数据引脚的状态
+        if (gpio_get_level(DAT))
+            tmp |= (1 << i);
+
+        // 拉高时钟信号结束当前位的传输
+        gpio_set_level(CLK, 1);
+        vTaskDelay(SHORT_DELAY_MS / portTICK_PERIOD_MS);
+    }
+
+    // 释放命令引脚
+    gpio_set_level(CMD, 1);
+    vTaskDelay(SHORT_DELAY_MS / portTICK_PERIOD_MS);
+
+    return tmp;
+}
+
+bool PS2Controller::readState(bool motor1, uint8_t motor2)
+{
+    int64_t now = esp_timer_get_time() / 1000; // 转换为毫秒
+    int64_t elapsed = now - last_read;
+
+    // 如果超过1.5秒没有读取，重新配置手柄
+    if (elapsed > 1500)
+    {
+        reconfig();
+        printf("重新配置完成");
+    }
+    // 等待下一次读取时间
+    if (elapsed < read_delay)
+        vTaskDelay((read_delay - elapsed) / portTICK_PERIOD_MS);
+
+    // 准备发送数据
+    uint8_t data_to_send[9] = {0x01, 0x42, 0,
+                               (uint8_t)(motor1 ? 0x01 : 0x00),
+                               (uint8_t)((motor2 * 0xFF) / 255),
+                               0, 0, 0, 0};
+    uint8_t extra_data[12] = {0};
+    bool success = false;
+
+    // 尝试最多5次读取
+    for (int attempt = 0; attempt < 5; attempt++)
+    {
+        gpio_set_level(CMD, 1);
+        gpio_set_level(CLK, 1);
+
+        // 选择手柄开始通信
+        gpio_set_level(SEL, 0);
+        vTaskDelay(SHORT_DELAY_MS / portTICK_PERIOD_MS);
+
+        // 发送和接收9个字节
+        for (int i = 0; i < 9; i++)
+            data[i] = sendAndReceive(data_to_send[i]);
+
+        // 如果收到0x79，继续读取额外数据
+        if (data[1] == 0x79)
+            for (int i = 0; i < 12; i++)
+                data[i + 9] = sendAndReceive(extra_data[i]);
+
+        // 结束通信
+        gpio_set_level(SEL, 1);
+
+        // 检查响应是否有效
+        if ((data[1] & 0xF0) == 0x70)
+        {
+            success = true;
+            break;
+        }
+
+        // 重新配置手柄
+        reconfig();
+        vTaskDelay(read_delay / portTICK_PERIOD_MS);
+    }
+    // 更新状态
+    if (!success)
+        read_delay = (read_delay < 10) ? read_delay + 1 : 10;
+
+    last_buttons = buttons;
+    buttons = ((uint16_t)data[4] << 8) | data[3];
+    last_read = esp_timer_get_time() / 1000;
+    return success;
+}
+void PS2Controller::sendCommandString(uint8_t *cmd, int length) const
+{
+    for (int i = 0; i < length; i++)
+        sendAndReceive(cmd[i]);
+}
+void PS2Controller::reconfig()
+{
+    config(en_Pressures, en_Rumble);
+}
+int PS2Controller::config(bool pressures, bool rumble)
+{
+    int error = 1;
+    for (int i = 0; i < 10; i++)
+    {
+        gpio_set_level(CMD, 1);
+        gpio_set_level(CLK, 1);
+
+        // 选择手柄
+        gpio_set_level(SEL, 0);
+        vTaskDelay(SHORT_DELAY_MS / portTICK_PERIOD_MS);
+        // 发送和接收配置命令
+        uint8_t temp[9];
+        uint8_t cmd[9] = {0x01, 0x45, 0x00, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A};
+        for (int j = 0; j < 9; j++)
+            temp[j] = sendAndReceive(cmd[j]);
+        // 结束通信
+        gpio_set_level(SEL, 1);
+        controller_type = temp[3];
+
+        // TODO: 发送更多配置命令(待测试)
+        uint8_t config_cmd2[9] = {0x01, 0x44, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x00};
+        sendCommandString(config_cmd2, 9);
+
+        if (rumble)
+        {
+            uint8_t rumble_cmd[5] = {0x01, 0x4D, 0x00, 0x00, 0x01};
+            sendCommandString(rumble_cmd, 5);
+            en_Rumble = true;
+        }
+
+        if (pressures)
+        {
+            uint8_t pressure_cmd[9] = {0x01, 0x4F, 0x00, 0xFF, 0xFF, 0x03, 0x00, 0x00, 0x00};
+            sendCommandString(pressure_cmd, 9);
+            en_Pressures = true;
+        }
+        uint8_t config_cmd3[9] = {0x01, 0x43, 0x00, 0x00, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A};
+        sendCommandString(config_cmd3, 9);
+
+        readState(false, 0);
+
+        // 检查配置是否成功
+        if (pressures && data[1] == 0x79)
+        {
+            error = 0;
+            break;
+        }
+        if (data[1] == 0x73)
+        {
+            error = 0;
+            break;
+        }
+    }
+    // 检查最终配置状态
+    if (data[1] == 0x41 || data[1] == 0x42 ||
+        data[1] == 0x73 || data[1] == 0x79)
+    {
+        error = 0;
+    }
+    return error;
+}
+
+// 按钮状态检测
+bool PS2Controller::Button(uint16_t button) const
+{
+    return !(buttons & button);
+}
+
+bool PS2Controller::NewButtonState(uint16_t button) const
+{
+    return (last_buttons ^ buttons) & button;
+}
+
+bool PS2Controller::ButtonPressed(uint16_t button) const
+{
+    return NewButtonState(button) && Button(button);
+}
+
+bool PS2Controller::ButtonReleased(uint16_t button) const
+{
+    return NewButtonState(button) && !(last_buttons & button);
+}
+// 手柄主循环
+void PS2Controller::ps2Task()
+{
+    int error = config(true, true);
+    if (error) {
+        printf("Error configuring controller\n");
+        while (1) {
+            vTaskDelay(1000 / portTICK_PERIOD_MS);
+        }
+    }
+    
+    printf("Found Controller, configured successful\n");
+    
+    // 震动测试
+    if (en_Rumble) {
+        printf("Vibrating controller for 1 second...\n");
+        readState(true, 255);
+        vTaskDelay(1000 / portTICK_PERIOD_MS);
+        //read_gamepad(&ps2, false, 0);
+    }
+    
+    printf("Controller is ready. Press START + SELECT together to exit.\n");
+    while (1) {
+        readState( true, 255);
+        // 读取手柄数据
+        if (!readState(false, 0)) {
+            vTaskDelay(READ_DELAY_MS / portTICK_PERIOD_MS);
+            continue;
+        }
+        
+        // 检查是否同时按下START和SELECT键
+        if (ButtonPressed(BUTTON_START) && ButtonPressed(BUTTON_SELECT)) {
+            printf("START and SELECT pressed together. Exiting...\n");
+            break;
+        }
+        
+        // 打印按钮状态
+        if (Button(BUTTON_SELECT)) printf("SELECT pressed\n");
+        if (Button(BUTTON_START)) printf("START pressed\n");
+        if (Button(BUTTON_PAD_UP)) printf("UP pressed\n");
+        if (Button(BUTTON_PAD_DOWN)) printf("DOWN pressed\n");
+        if (Button(BUTTON_PAD_LEFT)) printf("LEFT pressed\n");
+        if (Button(BUTTON_PAD_RIGHT)) printf("RIGHT pressed\n");
+        if (Button(BUTTON_TRIANGLE)) printf("TRIANGLE pressed\n");
+        if (Button(BUTTON_CIRCLE)) printf("CIRCLE pressed\n");
+        if (Button(BUTTON_CROSS)) printf("CROSS pressed\n");
+        if (Button(BUTTON_SQUARE)) printf("SQUARE pressed\n");
+        if (Button(BUTTON_L1)) printf("L1 pressed\n");
+        if (Button(BUTTON_R1)) printf("R1 pressed\n");
+        if (Button(BUTTON_L2)) printf("L2 pressed\n");
+        if (Button(BUTTON_R2)) printf("R2 pressed\n");
+        
+        // 模拟摇杆值
+        if (en_Pressures) {
+            printf("LX: %d, LY: %d, RX: %d, RY: %d\n",
+                   data[LX], data[LY], 
+                   data[RX], data[RY]);
+        }
+        
+        vTaskDelay(READ_DELAY_MS / portTICK_PERIOD_MS);
+    }
+    
+    vTaskDelete(NULL);
+}
+
+void PS2Controller::ps2StaticTask(void *pvParameters)
+{
+    PS2Controller *instance = static_cast<PS2Controller *>(pvParameters);
+    instance->ps2Task();
+
+}
+void PS2Controller::launch()
+{
+    xTaskCreate(
+        ps2StaticTask, // 静态任务函数
+        "ps2Task",     // 任务名称
+        4096,          // 堆栈大小
+        this,          // 传递 this 指针
+        5,             // 优先级
+        NULL);
+}

main/src/uartTool.cpp

@@ -0,0 +1,8 @@
+#include "uartTool.h"
+#include "esp_log.h"
+int UartTool::sendData(const std::string logName, const std::string data)
+{
+    const int txBytes = uart_write_bytes(UART_NUM, data.c_str(), data.length());
+    ESP_LOGI(logName.c_str(), "Wrote %d bytes", txBytes); // log打印
+    return txBytes;
+}

managed_components/espressif__servo/.component_hash

@@ -0,0 +1 @@
+309c787e48224255fad458cfd9ab86ea53f0fdad1c5e4f6f0c50309990b17108

managed_components/espressif__servo/CHANGELOG.md

@@ -0,0 +1,7 @@
+# ChangeLog
+
+## v0.1.0 - 2024-11-27
+
+### Enhancements:
+
+* Initial version

managed_components/espressif__servo/CHECKSUMS.json

@@ -0,0 +1 @@
+{"version": "1.0", "algorithm": "sha256", "created_at": "2025-05-21T16:10:59.902457+00:00", "files": [{"path": "CMakeLists.txt", "size": 128, "hash": "38ac23e2264a2ad7d2e05f23bc8fd708a4f9c0822f74ed7bb9773d87ce88002b"}, {"path": "CHANGELOG.md", "size": 74, "hash": "30fcfae2b6b4710c4bd376d93f34e0938928e4ce4cebba0beb52cd45e48d61b7"}, {"path": "idf_component.yml", "size": 509, "hash": "464ab7ae0f510b33451d8696108917a02e5e8c06ff4009d1f5cad47aea80ba5b"}, {"path": "iot_servo.c", "size": 5182, "hash": "9ea3b88c9f6b13aaa5b63aca56717638cbe8732f53b12ada21254ae73902863d"}, {"path": "README.md", "size": 1626, "hash": "4d7932b9fad39a34668444a67f7084a624129034b2f91224f4c6f9a89d8db1ac"}, {"path": "license.txt", "size": 11358, "hash": "cfc7749b96f63bd31c3c42b5c471bf756814053e847c10f3eb003417bc523d30"}, {"path": "include/iot_servo.h", "size": 2682, "hash": "379919c1bdf68fc8a4470d2069be77d337791dd29f1a19abc3d241d704c11c23"}, {"path": "test_apps/CMakeLists.txt", "size": 348, "hash": "3277286f031a64222535386f94cdcbe16369aa293ea9faaee8064ac2b8e70331"}, {"path": "test_apps/pytest_servo.py", "size": 687, "hash": "3220824b927695ca3f3b402bebddb2393db95e4204786b029e699314f66e95d6"}, {"path": "test_apps/sdkconfig.defaults", "size": 107, "hash": "bcf12e50789a2e963e16c1b551704db736999ee1d2a125f64b59f78f2336917d"}, {"path": "examples/servo_control/CMakeLists.txt", "size": 243, "hash": "ee01f6c5ae720d98dc5f0ef759ca7f936cda79b6ccfc3a816d220bb35a6e226c"}, {"path": "examples/servo_control/README.md", "size": 722, "hash": "3967ab3ac875a8dcb610614b2ce40d9c9aa663012cdb2f2e0646a0fa33298755"}, {"path": "examples/servo_control/main/CMakeLists.txt", "size": 84, "hash": "6147975f403401f14ab08a987c311eff81bbb7e895bcc72ea605caa48eca6b8e"}, {"path": "examples/servo_control/main/idf_component.yml", "size": 106, "hash": "c0fb3be289b005d56a52a4c16bdd1201ab34b35158c4bfe612802a2f67fedc83"}, {"path": "examples/servo_control/main/servo_control.c", "size": 1866, "hash": "c120f6fe00bfc1b253c330460adcc651befbbb1fe2881e6d98f04c703891aa64"}, {"path": "test_apps/main/test_servo.c", "size": 2617, "hash": "9de189593fa89ea3ddf588f020edc04386690bd33f41f22a7d130ba5581faec9"}, {"path": "test_apps/main/CMakeLists.txt", "size": 132, "hash": "ae750188237b821242fcf599eaf4f315e04d1b5f328a80d4a9e14ab43607e97d"}]}

managed_components/espressif__servo/CMakeLists.txt

@@ -0,0 +1,3 @@
+idf_component_register(SRCS "iot_servo.c"
+                        INCLUDE_DIRS include
+                        REQUIRES driver)

managed_components/espressif__servo/README.md

@@ -0,0 +1,57 @@
+# Servo Motor Component
+
+This component provides an easy-to-use interface for controlling servo motors with the ESP-IDF framework. Servo motors are commonly used in robotics, automation, and various mechanical applications due to their precise control of angular position.
+
+The library uses PWM (Pulse Width Modulation) to control servo motor rotation and supports customizable configurations for angle limits, pulse width, frequency, and more.
+
+## Features
+
+- Easy control of servo motors using PWM signals
+- Support for custom angle range and pulse width
+- Flexible configuration of PWM channels and timers
+- Compatible with the ESP-IDF framework
+
+## Getting Started
+
+### Prerequisites
+
+- ESP-IDF installed and configured on your development environment
+- A servo motor compatible with PWM signals
+- Proper connections between the ESP32 and the servo motor (signal, VCC, GND)
+
+## How to use
+
+Initialize the sensor with the configuration:
+
+```c
+    servo_config_t servo_cfg = {
+        .max_angle = 180,
+        .min_width_us = 500,
+        .max_width_us = 2500,
+        .freq = 50,
+        .timer_number = LEDC_TIMER_0,
+        .channels = {
+            .servo_pin = {
+                SERVO_GPIO,
+            },
+            .ch = {
+                LEDC_CHANNEL_0,
+            },
+        },
+        .channel_number = 1,
+    };
+
+    // Initialize the servo
+    iot_servo_init(LEDC_LOW_SPEED_MODE, &servo_cfg);
+```
+
+Set the angle:
+
+```c
+uint16_t angle = 0;
+iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, angle);
+```
+
+## Reference
+
+[Documentation](https://docs.espressif.com/projects/esp-iot-solution/en/latest/motor/servo.html)

managed_components/espressif__servo/examples/servo_control/CMakeLists.txt

@@ -0,0 +1,6 @@
+# The following five lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.16)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(servo_control)

managed_components/espressif__servo/examples/servo_control/README.md

@@ -0,0 +1,36 @@
+# Example for the servo component
+
+This example shows how to use the servo component.
+
+## How to use
+
+Initialize the sensor with the configuration:
+
+```c
+    servo_config_t servo_cfg = {
+        .max_angle = 180,
+        .min_width_us = 500,
+        .max_width_us = 2500,
+        .freq = 50,
+        .timer_number = LEDC_TIMER_0,
+        .channels = {
+            .servo_pin = {
+                SERVO_GPIO,
+            },
+            .ch = {
+                LEDC_CHANNEL_0,
+            },
+        },
+        .channel_number = 1,
+    };
+
+    // Initialize the servo
+    iot_servo_init(LEDC_LOW_SPEED_MODE, &servo_cfg);
+```
+
+Set the angle:
+
+```c
+uint16_t angle = 0;
+iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, angle);
+```

managed_components/espressif__servo/examples/servo_control/main/CMakeLists.txt

@@ -0,0 +1,2 @@
+idf_component_register(SRCS "servo_control.c"
+                    INCLUDE_DIRS ".")

managed_components/espressif__servo/examples/servo_control/main/idf_component.yml

@@ -0,0 +1,4 @@
+dependencies:
+  espressif/servo:
+    version: "*"
+    override_path: "../../../../components/motor/servo"

managed_components/espressif__servo/examples/servo_control/main/servo_control.c

@@ -0,0 +1,73 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_log.h"
+#include "esp_system.h"
+#include "sdkconfig.h"
+#include "iot_servo.h"
+
+#define SERVO_GPIO          (2)  // Servo GPIO
+
+static const char *TAG = "Servo Control";
+
+static uint16_t calibration_value_0 = 30;    // Real 0 degree angle
+static uint16_t calibration_value_180 = 195; // Real 0 degree angle
+
+// Task to test the servo
+static void servo_test_task(void *arg)
+{
+    ESP_LOGI(TAG, "Servo Test Task");
+    while (1) {
+        // Set the angle of the servo
+        for (int i = calibration_value_0; i <= calibration_value_180; i += 1) {
+            iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, i);
+            vTaskDelay(20 / portTICK_PERIOD_MS);
+        }
+        // Return to the initial position
+        iot_servo_write_angle(LEDC_LOW_SPEED_MODE, 0, calibration_value_0);
+        vTaskDelay(1000 / portTICK_PERIOD_MS);
+    }
+    vTaskDelete(NULL);
+}
+
+static void servo_init(void)
+{
+    ESP_LOGI(TAG, "Servo Control");
+
+    // Configure the servo
+    servo_config_t servo_cfg = {
+        .max_angle = 180,
+        .min_width_us = 500,
+        .max_width_us = 2500,
+        .freq = 50,
+        .timer_number = LEDC_TIMER_0,
+        .channels = {
+            .servo_pin = {
+                SERVO_GPIO,
+            },
+            .ch = {
+                LEDC_CHANNEL_0,
+            },
+        },
+        .channel_number = 1,
+    };
+
+    // Initialize the servo
+    iot_servo_init(LEDC_LOW_SPEED_MODE, &servo_cfg);
+}
+
+void app_main(void)
+{
+    ESP_LOGI(TAG, "Servo Control");
+    // Initialize the servo
+    servo_init();
+    // Create the servo test task
+    xTaskCreate(servo_test_task, "servo_test_task", 2048, NULL, 5, NULL);
+}

managed_components/espressif__servo/idf_component.yml

@@ -0,0 +1,12 @@
+dependencies:
+  idf:
+    version: '>=4.4'
+description: Espressif's Servo Motor Component
+documentation: https://docs.espressif.com/projects/esp-iot-solution/en/latest/motor/servo.html
+issues: https://github.com/espressif/esp-iot-solution/issues
+repository: git://github.com/espressif/esp-iot-solution.git
+repository_info:
+  commit_sha: f5ca5b553873e8ff3c044d5bae56d0eae1d0673b
+  path: components/motor/servo
+url: https://github.com/espressif/esp-iot-solution/tree/master/components/motor/servo
+version: 0.1.0

managed_components/espressif__servo/include/iot_servo.h

@@ -0,0 +1,96 @@
+/*
+ * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef _IOT_SERVO_H_
+#define _IOT_SERVO_H_
+
+#include "esp_err.h"
+#include "driver/ledc.h"
+#include "driver/gpio.h"
+
+/**
+ * @brief Configuration of servo motor channel
+ *
+ */
+typedef struct {
+    gpio_num_t servo_pin[LEDC_CHANNEL_MAX];     /**< Pin number of pwm output */
+    ledc_channel_t ch[LEDC_CHANNEL_MAX];    /**< The ledc channel which used */
+} servo_channel_t;
+
+/**
+ * @brief Configuration of servo motor
+ *
+ */
+typedef struct {
+    uint16_t max_angle;        /**< Servo max angle */
+    uint16_t min_width_us;     /**< Pulse width corresponding to minimum angle, which is usually 500us */
+    uint16_t max_width_us;     /**< Pulse width corresponding to maximum angle, which is usually 2500us */
+    uint32_t freq;             /**< PWM frequency */
+    ledc_timer_t timer_number; /**< Timer number of ledc */
+    servo_channel_t channels;  /**< Channels to use */
+    uint8_t channel_number;    /**< Total channel number */
+} servo_config_t;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Initialize ledc to control the servo
+ *
+ * @param speed_mode Select the LEDC channel group with specified speed mode. Note that not all targets support high speed mode.
+ * @param config Pointer of servo configure struct
+ *
+ * @return
+ *     - ESP_OK Success
+ *     - ESP_ERR_INVALID_ARG Parameter error
+ *     - ESP_FAIL Configure ledc failed
+ */
+esp_err_t iot_servo_init(ledc_mode_t speed_mode, const servo_config_t *config);
+
+/**
+ * @brief Deinitialize ledc for servo
+ *
+ * @param speed_mode Select the LEDC channel group with specified speed mode.
+ *
+ * @return
+ *     - ESP_OK Success
+ */
+esp_err_t iot_servo_deinit(ledc_mode_t speed_mode);
+
+/**
+ * @brief Set the servo motor to a certain angle
+ *
+ * @note This API is not thread-safe
+ *
+ * @param speed_mode Select the LEDC channel group with specified speed mode.
+ * @param channel LEDC channel, select from ledc_channel_t
+ * @param angle The angle to go
+ *
+ * @return
+ *     - ESP_OK Success
+ *     - ESP_ERR_INVALID_ARG Parameter error
+ */
+esp_err_t iot_servo_write_angle(ledc_mode_t speed_mode, uint8_t channel, float angle);
+
+/**
+ * @brief Read current angle of one channel
+ *
+ * @param speed_mode Select the LEDC channel group with specified speed mode.
+ * @param channel LEDC channel, select from ledc_channel_t
+ * @param angle Current angle of the channel
+ *
+ * @return
+ *     - ESP_OK Success
+ *     - ESP_ERR_INVALID_ARG Parameter error
+ */
+esp_err_t iot_servo_read_angle(ledc_mode_t speed_mode, uint8_t channel, float *angle);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _IOT_SERVO_H_ */

managed_components/espressif__servo/iot_servo.c

@@ -0,0 +1,122 @@
+/*
+ * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "esp_log.h"
+#include "esp_err.h"
+#include "driver/ledc.h"
+#include "iot_servo.h"
+
+static const char *TAG = "servo";
+
+#define SERVO_CHECK(a, str, ret_val) \
+    if (!(a)) { \
+        ESP_LOGE(TAG,"%s(%d): %s", __FUNCTION__, __LINE__, str); \
+        return (ret_val); \
+    }
+
+#define SERVO_LEDC_INIT_BITS LEDC_TIMER_10_BIT
+#define SERVO_FREQ_MIN       50
+#define SERVO_FREQ_MAX       400
+
+static uint32_t g_full_duty = 0;
+static servo_config_t g_cfg[LEDC_SPEED_MODE_MAX] = {0};
+
+static uint32_t calculate_duty(ledc_mode_t speed_mode, float angle)
+{
+    float angle_us = angle / g_cfg[speed_mode].max_angle * (g_cfg[speed_mode].max_width_us - g_cfg[speed_mode].min_width_us) + g_cfg[speed_mode].min_width_us;
+    ESP_LOGD(TAG, "angle us: %f", angle_us);
+    uint32_t duty = (uint32_t)((float)g_full_duty * (angle_us) * g_cfg[speed_mode].freq / (1000000.0f));
+    return duty;
+}
+
+static float calculate_angle(ledc_mode_t speed_mode, uint32_t duty)
+{
+    float angle_us = (float)duty * 1000000.0f / (float)g_full_duty / (float)g_cfg[speed_mode].freq;
+    angle_us -= g_cfg[speed_mode].min_width_us;
+    angle_us = angle_us < 0.0f ? 0.0f : angle_us;
+    float angle = angle_us * g_cfg[speed_mode].max_angle / (g_cfg[speed_mode].max_width_us - g_cfg[speed_mode].min_width_us);
+    return angle;
+}
+
+esp_err_t iot_servo_init(ledc_mode_t speed_mode, const servo_config_t *config)
+{
+    esp_err_t ret;
+    SERVO_CHECK(NULL != config, "Pointer of config is invalid", ESP_ERR_INVALID_ARG);
+    SERVO_CHECK(config->channel_number > 0 && config->channel_number <= LEDC_CHANNEL_MAX, "Servo channel number out the range", ESP_ERR_INVALID_ARG);
+    SERVO_CHECK(config->freq <= SERVO_FREQ_MAX && config->freq >= SERVO_FREQ_MIN, "Servo pwm frequency out the range", ESP_ERR_INVALID_ARG);
+    uint64_t pin_mask = 0;
+    uint32_t ch_mask = 0;
+    for (size_t i = 0; i < config->channel_number; i++) {
+        uint64_t _pin_mask = 1ULL << config->channels.servo_pin[i];
+        uint32_t _ch_mask = 1UL << config->channels.ch[i];
+        SERVO_CHECK(!(pin_mask & _pin_mask), "servo gpio has a duplicate", ESP_ERR_INVALID_ARG);
+        SERVO_CHECK(!(ch_mask & _ch_mask), "servo channel has a duplicate", ESP_ERR_INVALID_ARG);
+        SERVO_CHECK(GPIO_IS_VALID_OUTPUT_GPIO(config->channels.servo_pin[i]), "servo gpio invalid", ESP_ERR_INVALID_ARG);
+        pin_mask |= _pin_mask;
+        ch_mask |= _ch_mask;
+    }
+
+    ledc_timer_config_t ledc_timer = {
+        .clk_cfg = LEDC_AUTO_CLK,
+        .duty_resolution = SERVO_LEDC_INIT_BITS,     // resolution of PWM duty
+        .freq_hz = config->freq,                     // frequency of PWM signal
+        .speed_mode = speed_mode,            // timer mode
+        .timer_num = config->timer_number            // timer index
+    };
+    ret = ledc_timer_config(&ledc_timer);
+    SERVO_CHECK(ESP_OK == ret, "ledc timer configuration failed", ESP_FAIL);
+    for (size_t i = 0; i < config->channel_number; i++) {
+        ledc_channel_config_t ledc_ch = {
+            .intr_type  = LEDC_INTR_DISABLE,
+            .channel    = config->channels.ch[i],
+            .duty       = calculate_duty(speed_mode, 0),
+            .gpio_num   = config->channels.servo_pin[i],
+            .speed_mode = speed_mode,
+            .timer_sel  = config->timer_number,
+            .hpoint     = 0
+        };
+        ret = ledc_channel_config(&ledc_ch);
+        SERVO_CHECK(ESP_OK == ret, "ledc channel configuration failed", ESP_FAIL);
+    }
+    g_full_duty = (1 << SERVO_LEDC_INIT_BITS) - 1;
+    g_cfg[speed_mode] = *config;
+
+    return ESP_OK;
+}
+
+esp_err_t iot_servo_deinit(ledc_mode_t speed_mode)
+{
+    SERVO_CHECK(speed_mode < LEDC_SPEED_MODE_MAX, "LEDC speed mode invalid", ESP_ERR_INVALID_ARG);
+    for (size_t i = 0; i < g_cfg[speed_mode].channel_number; i++) {
+        ledc_stop(speed_mode, g_cfg[speed_mode].channels.ch[i], 0);
+    }
+    ledc_timer_rst(speed_mode, g_cfg[speed_mode].timer_number);
+    g_full_duty = 0;
+    return ESP_OK;
+}
+
+esp_err_t iot_servo_write_angle(ledc_mode_t speed_mode, uint8_t channel, float angle)
+{
+    SERVO_CHECK(speed_mode < LEDC_SPEED_MODE_MAX, "LEDC speed mode invalid", ESP_ERR_INVALID_ARG);
+    SERVO_CHECK(channel < LEDC_CHANNEL_MAX, "LEDC channel number too large", ESP_ERR_INVALID_ARG);
+    SERVO_CHECK(angle >= 0.0f, "Angle can't to be negative", ESP_ERR_INVALID_ARG);
+    esp_err_t ret;
+    uint32_t duty = calculate_duty(speed_mode, angle);
+    ret = ledc_set_duty(speed_mode, (ledc_channel_t)channel, duty);
+    ret |= ledc_update_duty(speed_mode, (ledc_channel_t)channel);
+    SERVO_CHECK(ESP_OK == ret, "write servo angle failed", ESP_FAIL);
+    return ESP_OK;
+}
+
+esp_err_t iot_servo_read_angle(ledc_mode_t speed_mode, uint8_t channel, float *angle)
+{
+    SERVO_CHECK(speed_mode < LEDC_SPEED_MODE_MAX, "LEDC speed mode invalid", ESP_ERR_INVALID_ARG);
+    SERVO_CHECK(channel < LEDC_CHANNEL_MAX, "LEDC channel number too large", ESP_ERR_INVALID_ARG);
+    uint32_t duty = ledc_get_duty(speed_mode, channel);
+    float a = calculate_angle(speed_mode, duty);
+    *angle = a;
+    return ESP_OK;
+}

managed_components/espressif__servo/license.txt

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

managed_components/espressif__servo/test_apps/CMakeLists.txt

@@ -0,0 +1,9 @@
+# The following lines of boilerplate have to be in your project's CMakeLists
+# in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.5)
+
+set(EXTRA_COMPONENT_DIRS "$ENV{IDF_PATH}/tools/unit-test-app/components"
+                         "../../servo")
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(servo_test)

managed_components/espressif__servo/test_apps/main/CMakeLists.txt

@@ -0,0 +1,3 @@
+idf_component_register(SRC_DIRS "."
+                       PRIV_INCLUDE_DIRS "."
+                       PRIV_REQUIRES unity  servo)

managed_components/espressif__servo/test_apps/main/test_servo.c

@@ -0,0 +1,98 @@
+/*
+ * SPDX-FileCopyrightText: 2022-2024 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_log.h"
+#include "iot_servo.h"
+#include "unity.h"
+#include "unity_config.h"
+#include "sdkconfig.h"
+
+#define SERVO_CH0_PIN 1
+#define SERVO_CH1_PIN 2
+#define SERVO_CH2_PIN 3
+#define SERVO_CH3_PIN 4
+
+#define TEST_MEMORY_LEAK_THRESHOLD (-500)
+
+static size_t before_free_8bit;
+static size_t before_free_32bit;
+
+static void _set_angle(ledc_mode_t speed_mode, float angle)
+{
+    for (size_t i = 0; i < 4; i++) {
+        iot_servo_write_angle(speed_mode, i, angle);
+    }
+}
+
+TEST_CASE("Servo_motor test", "[servo][iot]")
+{
+    servo_config_t servo_cfg_ls = {
+        .max_angle = 180,
+        .min_width_us = 500,
+        .max_width_us = 2500,
+        .freq = 50,
+        .timer_number = LEDC_TIMER_0,
+        .channels = {
+            .servo_pin = {
+                SERVO_CH0_PIN,
+                SERVO_CH1_PIN,
+                SERVO_CH2_PIN,
+                SERVO_CH3_PIN,
+            },
+            .ch = {
+                LEDC_CHANNEL_0,
+                LEDC_CHANNEL_1,
+                LEDC_CHANNEL_2,
+                LEDC_CHANNEL_3,
+            },
+        },
+        .channel_number = 4,
+    } ;
+    TEST_ASSERT(ESP_OK == iot_servo_init(LEDC_LOW_SPEED_MODE, &servo_cfg_ls));
+
+    size_t i;
+    float angle_ls, angle_hs;
+    for (i = 0; i <= 180; i++) {
+        _set_angle(LEDC_LOW_SPEED_MODE, i);
+        vTaskDelay(50 / portTICK_PERIOD_MS);
+        iot_servo_read_angle(LEDC_LOW_SPEED_MODE, 0, &angle_ls);
+        ESP_LOGI("servo", "[%d|%.2f]", i, angle_ls);
+        (void)angle_hs;
+    }
+
+    iot_servo_deinit(LEDC_LOW_SPEED_MODE);
+}
+
+static void check_leak(size_t before_free, size_t after_free, const char *type)
+{
+    ssize_t delta = after_free - before_free;
+    printf("MALLOC_CAP_%s: Before %u bytes free, After %u bytes free (delta %d)\n", type, before_free, after_free, delta);
+    TEST_ASSERT_MESSAGE(delta >= TEST_MEMORY_LEAK_THRESHOLD, "memory leak");
+}
+
+void setUp(void)
+{
+    before_free_8bit = heap_caps_get_free_size(MALLOC_CAP_8BIT);
+    before_free_32bit = heap_caps_get_free_size(MALLOC_CAP_32BIT);
+}
+
+void tearDown(void)
+{
+    size_t after_free_8bit = heap_caps_get_free_size(MALLOC_CAP_8BIT);
+    size_t after_free_32bit = heap_caps_get_free_size(MALLOC_CAP_32BIT);
+    check_leak(before_free_8bit, after_free_8bit, "8BIT");
+    check_leak(before_free_32bit, after_free_32bit, "32BIT");
+}
+
+void app_main(void)
+{
+    printf("SERVO TEST \n");
+    unity_run_menu();
+}

managed_components/espressif__servo/test_apps/pytest_servo.py

@@ -0,0 +1,28 @@
+# SPDX-FileCopyrightText: 2024Espressif Systems (Shanghai) CO LTD
+# SPDX-License-Identifier: Apache-2.0
+
+'''
+Steps to run these cases:
+- Build
+  - . ${IDF_PATH}/export.sh
+  - pip install idf_build_apps
+  - python tools/build_apps.py components/motor/servo/test_apps -t esp32
+- Test
+  - pip install -r tools/requirements/requirement.pytest.txt
+  - pytest components/motor/servo/test_apps --target esp32
+'''
+
+import pytest
+from pytest_embedded import Dut
+
+@pytest.mark.target('esp32')
+@pytest.mark.target('esp32s2')
+@pytest.mark.env('generic')
+@pytest.mark.parametrize(
+    'config',
+    [
+        'defaults',
+    ],
+)
+def test_servo(dut: Dut)-> None:
+    dut.run_all_single_board_cases()

managed_components/espressif__servo/test_apps/sdkconfig.defaults

@@ -0,0 +1,4 @@
+# For IDF 5.0
+CONFIG_FREERTOS_HZ=1000
+CONFIG_ESP_TASK_WDT_EN=n
+CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=4096