From b39507bc308ce55d216f76151eb79913f0a41dee Mon Sep 17 00:00:00 2001 From: Chen Jichang Date: Thu, 19 Jun 2025 18:42:35 +0800 Subject: [PATCH] feat(twai): add cybergear example --- docs/en/api-reference/peripherals/twai.rst | 1 + docs/zh_CN/api-reference/peripherals/twai.rst | 1 + examples/peripherals/.build-test-rules.yml | 6 + .../peripherals/twai/cybergear/CMakeLists.txt | 8 + examples/peripherals/twai/cybergear/README.md | 174 ++++++++ .../twai/cybergear/main/CMakeLists.txt | 4 + .../twai/cybergear/main/Kconfig.projbuild | 59 +++ .../twai/cybergear/main/cybergear.c | 386 ++++++++++++++++++ .../twai/cybergear/main/cybergear.h | 222 ++++++++++ .../twai/cybergear/main/cybergear_console.c | 298 ++++++++++++++ .../twai/cybergear/main/cybergear_console.h | 22 + .../cybergear/main/cybergear_example_main.c | 69 ++++ .../twai/cybergear/main/cybergear_type.h | 95 +++++ .../cybergear/pytest_cybergear_example.py | 12 + 14 files changed, 1357 insertions(+) create mode 100644 examples/peripherals/twai/cybergear/CMakeLists.txt create mode 100644 examples/peripherals/twai/cybergear/README.md create mode 100644 examples/peripherals/twai/cybergear/main/CMakeLists.txt create mode 100644 examples/peripherals/twai/cybergear/main/Kconfig.projbuild create mode 100644 examples/peripherals/twai/cybergear/main/cybergear.c create mode 100644 examples/peripherals/twai/cybergear/main/cybergear.h create mode 100644 examples/peripherals/twai/cybergear/main/cybergear_console.c create mode 100644 examples/peripherals/twai/cybergear/main/cybergear_console.h create mode 100644 examples/peripherals/twai/cybergear/main/cybergear_example_main.c create mode 100644 examples/peripherals/twai/cybergear/main/cybergear_type.h create mode 100644 examples/peripherals/twai/cybergear/pytest_cybergear_example.py diff --git a/docs/en/api-reference/peripherals/twai.rst b/docs/en/api-reference/peripherals/twai.rst index 28d94a1d47..e4df75dd9d 100644 --- a/docs/en/api-reference/peripherals/twai.rst +++ b/docs/en/api-reference/peripherals/twai.rst @@ -375,6 +375,7 @@ Application Examples - :example:`peripherals/twai/twai_utils` demonstrates how to use the TWAI (Two-Wire Automotive Interface) APIs to create a command-line interface for TWAI bus communication, supporting frame transmission/reception, filtering, monitoring, and both classic and FD formats for testing and debugging TWAI networks. - :example:`peripherals/twai/twai_error_recovery` demonstrates how to recover nodes from the bus-off state and resume communication, as well as bus error reporting, node state changes, and other event information. - :example:`peripherals/twai/twai_network` using 2 nodes with different roles: transmitting and listening, demonstrates how to use the driver for single and bulk data transmission, as well as configure filters to receive these data. + - :example:`peripherals/twai/cybergear` demonstrates how to control XiaoMi CyberGear motors via TWAI interface. API Reference ------------- diff --git a/docs/zh_CN/api-reference/peripherals/twai.rst b/docs/zh_CN/api-reference/peripherals/twai.rst index 2342b27dba..b9a822b72d 100644 --- a/docs/zh_CN/api-reference/peripherals/twai.rst +++ b/docs/zh_CN/api-reference/peripherals/twai.rst @@ -375,6 +375,7 @@ TWAI控制器能够检测由于总线干扰产生的/损坏的不符合帧格式 - :example:`peripherals/twai/twai_utils` 演示了如何使用 TWAI（Two-Wire Automotive Interface，双线汽车接口）API 创建一个命令行工具，用于 TWAI 总线通信，支持帧的发送/接收、过滤、监控，以及经典和 FD 格式，以便测试和调试 TWAI 网络。 - :example:`peripherals/twai/twai_error_recovery` 演示了总线错误上报，节点状态变化等事件信息，以及如何从离线状态恢复节点并重新进行通信。 - :example:`peripherals/twai/twai_network` 通过发送、监听， 2 个不同角色的节点，演示了如何使用驱动程序进行单次的和大量的数据发送，以及配置过滤器以接收这些数据。 + - :example:`peripherals/twai/cybergear` 演示了如何通过 TWAI 接口控制 XiaoMi CyberGear 电机。 API 参考 -------- diff --git a/examples/peripherals/.build-test-rules.yml b/examples/peripherals/.build-test-rules.yml index 108219a239..6e09e6971e 100644 --- a/examples/peripherals/.build-test-rules.yml +++ b/examples/peripherals/.build-test-rules.yml @@ -503,6 +503,12 @@ examples/peripherals/touch_sensor/touch_sens_sleep: depends_components: - esp_driver_touch_sens +examples/peripherals/twai/cybergear: + disable: + - if: SOC_TWAI_SUPPORTED != 1 + depends_components: + - esp_driver_twai + examples/peripherals/twai/twai_error_recovery: disable: - if: SOC_TWAI_SUPPORTED != 1 diff --git a/examples/peripherals/twai/cybergear/CMakeLists.txt b/examples/peripherals/twai/cybergear/CMakeLists.txt new file mode 100644 index 0000000000..8de02161ed --- /dev/null +++ b/examples/peripherals/twai/cybergear/CMakeLists.txt @@ -0,0 +1,8 @@ +# 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) +# "Trim" the build. Include the minimal set of components, main, and anything it depends on. +idf_build_set_property(MINIMAL_BUILD ON) +project(cybergear_example) diff --git a/examples/peripherals/twai/cybergear/README.md b/examples/peripherals/twai/cybergear/README.md new file mode 100644 index 0000000000..193e249a25 --- /dev/null +++ b/examples/peripherals/twai/cybergear/README.md @@ -0,0 +1,174 @@ +| Supported Targets | ESP32 | ESP32-C3 | ESP32-C5 | ESP32-C6 | ESP32-H2 | ESP32-H21 | ESP32-P4 | ESP32-S2 | ESP32-S3 | +| ----------------- | ----- | -------- | -------- | -------- | -------- | --------- | -------- | -------- | -------- | + +# CyberGear Motor Control Example + +This example demonstrates how to control [XiaoMi CyberGear motors](https://www.mi.com/cyber-gear) using ESP32's TWAI interface. It provides a complete console interface for motor control with support for enable/disable, mode setting, speed and position control, and real-time status monitoring. + +> [!NOTE] +> For simplicity, this console example can only control one motor. + +## How to use example + +### Hardware Required + +* A development board with any supported Espressif SOC chip (see `Supported Targets` table above) +* XiaoMi CyberGear motor +* CAN transceiver (e.g., SN65HVD230) +* Connecting wires + +### Hardware Connection + +The CAN bus connection requires a CAN transceiver to convert between the ESP32's TWAI signals and the CAN bus differential signals. + +``` +ESP32 Pin CAN Transceiver CyberGear Motor +--------- --------------- --------------- +GPIO TX ---> CTX +GPIO RX <--- CRX +GND ---> GND GND <--- Separate power supply +VCC(3.3V) ---> VCC(3.3V) VCC (24V) <--- Separate power supply + CAN_H <--> CAN_H + CAN_L <--> CAN_L +--------- --------------- --------------- +``` + +### Configure the project + +Before project configuration and build, be sure to set the correct chip target using `idf.py set-target `. + +```bash +idf.py set-target esp32 +idf.py menuconfig +``` + +In the `Example Configuration` menu: + +* Set `TWAI TX GPIO number` - GPIO pin for TWAI TX +* Set `TWAI RX GPIO number` - GPIO pin for TWAI RX +* Set `ESP Node ID` - TWAI node identifier +* Set `Motor ID` - Motor identifier +* Set `Motor bitrate` - Motor bitrate + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view serial output: + +```bash +idf.py build +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +(To exit the serial monitor, type ``Ctrl-]``.) + +See the Getting Started Guide for full steps to configure and use ESP-IDF to build projects. + +## Example Output + +``` +I (330) Cybergear: CyberGear node started, TX: GPIO0, RX: GPIO2 +I (340) Cybergear: CyberGear console started + +Type 'help' to get the list of commands. +Use UP/DOWN arrows to navigate through command history. +Press TAB when typing command name to auto-complete. +I (350) main_task: Returned from app_main() +cybergear> +``` + +## Console Commands + +The example provides an interactive console with the following commands: + +### Control Commands + +```bash +# Enable motor +enable + +# Disable motor and clear fault +disable + +# Set control mode (0:Motion, 1:Location, 2:Speed, 3:Current) +mode + +# Motion control +motion -t -l -s -p -d + +# Set motor speed (rad/s) +speed -s + +# Set motor location (rad) +loc -s + +# Set motor current (A) +current -s + +# Set current location as zero +zero + +# Set motor ID +setid + +# Get motor ID +getid + +# Show motor status +status + +``` + +### Usage Examples + +```bash +cybergear> enable # Enable motor +cybergear> mode 1 # Set location control mode +cybergear> loc -s 1.57 # Set location to π/2 rad +cybergear> status # Check motor status +cybergear> mode 2 # Set speed control mode +cybergear> speed -s 5.0 # Set speed to 5 rad/s +cybergear> disable # Disable motor +``` + +### Note + +Command **setid**: The motor ID can not be changed when the motor is running, please disable the motor and set motor id. +Command **getid**: If you forget the motor ID you set previously, you can run getid cmd first. And then power off the motor and power it on again. Then it will broadcast its motor ID. + +### Technical Specifications + +- **TWAI Bitrate**: 125 Kbps, 250 Kbps, 500 Kbps or 1 Mbps (default 1 Mbps) +- **Position Range**: ±12.5 rad +- **Speed Range**: ±30.0 rad/s +- **Torque Range**: ±12.0 Nm +- **Current Range**: 0-23.0 A + + +### Motor Status Display + +The `status` command shows detailed motor information: + +``` +===== Motor Status ===== +Motor ID: 127 +Fault Status: 0 +State: 2 +Location: 0.125000 +Speed: 2.000000 +Torque: 0.300000 +Temperature: 36.500000 +======================== +``` + +## Troubleshooting + +**If Motor not responding** + + - Check CAN transceiver connections + - Verify motor ID settings are correct + - Confirm CAN bus bitrate matches (1Mbps) + - Check motor power supply + +For any technical queries, please open an [issue](https://github.com/espressif/esp-idf/issues) on GitHub. We will get back to you soon. diff --git a/examples/peripherals/twai/cybergear/main/CMakeLists.txt b/examples/peripherals/twai/cybergear/main/CMakeLists.txt new file mode 100644 index 0000000000..f25060a15b --- /dev/null +++ b/examples/peripherals/twai/cybergear/main/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register(SRCS "cybergear_example_main.c" "cybergear.c" "cybergear_console.c" + REQUIRES console esp_driver_twai + INCLUDE_DIRS "." + ) diff --git a/examples/peripherals/twai/cybergear/main/Kconfig.projbuild b/examples/peripherals/twai/cybergear/main/Kconfig.projbuild new file mode 100644 index 0000000000..71568105c0 --- /dev/null +++ b/examples/peripherals/twai/cybergear/main/Kconfig.projbuild @@ -0,0 +1,59 @@ +menu "Example Configuration" + + orsource "$IDF_PATH/examples/common_components/env_caps/$IDF_TARGET/Kconfig.env_caps" + + config EXAMPLE_TWAI_TX_GPIO_NUM + int "TX GPIO number" + range ENV_GPIO_RANGE_MIN ENV_GPIO_OUT_RANGE_MAX + default 0 + help + This option selects the GPIO pin used for the TWAI TX signal. Connect the + TWAI TX signal to your transceiver. + + config EXAMPLE_TWAI_RX_GPIO_NUM + int "RX GPIO number" + range ENV_GPIO_RANGE_MIN ENV_GPIO_IN_RANGE_MAX + default 2 + help + This option selects the GPIO pin used for the TWAI RX signal. Connect the + TWAI RX signal to your transceiver. + + config EXAMPLE_TWAI_ESP_NODE_ID + int "TWAI ESP Node ID" + range 0 256 + default 254 + help + This option selects the ESP Node ID of the ESP Controller. + + config EXAMPLE_TWAI_MOTOR_ID + int "TWAI CyberGear motor ID" + range 0 256 + default 127 + help + This option selects the motor ID of the CyberGear. 127 is the factory value. + Check the README if you don't know how to get the motor ID. + + choice EXAMPLE_TWAI_MOTOR_BITRATE_CHOICE + prompt "TWAI CyberGear motor bitrate" + default EXAMPLE_TWAI_MOTOR_BITRATE_1M + help + This option selects the bitrate of the CyberGear. + + config EXAMPLE_TWAI_MOTOR_BITRATE_1M + bool "1M" + config EXAMPLE_TWAI_MOTOR_BITRATE_500K + bool "500K" + config EXAMPLE_TWAI_MOTOR_BITRATE_250K + bool "250K" + config EXAMPLE_TWAI_MOTOR_BITRATE_125K + bool "125K" + endchoice + + config EXAMPLE_TWAI_MOTOR_BITRATE + int + default 1000000 if EXAMPLE_TWAI_MOTOR_BITRATE_1M + default 500000 if EXAMPLE_TWAI_MOTOR_BITRATE_500K + default 250000 if EXAMPLE_TWAI_MOTOR_BITRATE_250K + default 125000 if EXAMPLE_TWAI_MOTOR_BITRATE_125K + +endmenu diff --git a/examples/peripherals/twai/cybergear/main/cybergear.c b/examples/peripherals/twai/cybergear/main/cybergear.c new file mode 100644 index 0000000000..45cc81e656 --- /dev/null +++ b/examples/peripherals/twai/cybergear/main/cybergear.c @@ -0,0 +1,386 @@ +/* + * SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "esp_log.h" +#include "esp_check.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/semphr.h" +#include "esp_twai.h" +#include "esp_twai_onchip.h" +#include "cybergear.h" + +#define TAG "Cybergear" + +typedef struct { + union { + uint8_t byte[8]; + uint16_t half_word[4]; + uint32_t word[2]; + float float_data[2]; + }; +} __attribute__((aligned(4))) cybergear_data_t; + +typedef struct cybergear_t { + twai_node_handle_t node_hdl; + uint8_t motor_id; + uint8_t esp_node_id; + uint8_t mode; + uint16_t raw_location; + uint16_t raw_speed; + uint16_t raw_torque; + uint16_t raw_temperature; + union { + struct { + uint8_t under_voltage: 1; + uint8_t over_current: 1; + uint8_t over_heating: 1; + uint8_t magnetic_coding_fault: 1; + uint8_t hall_coding_fault: 1; + uint8_t uncalibrated: 1; + uint8_t state: 2; + } bit; + uint8_t raw; + } status; + SemaphoreHandle_t receive_sem; + SemaphoreHandle_t broadcast_sem; +} cybergear_t; + +static inline uint32_t map_float_to_uint(float x, float x_min, float x_max, uint32_t bits) +{ + if (x_max == x_min) { + return 0; + } + x = x < x_min ? x_min : x; + x = x > x_max ? x_max : x; + float span = x_max - x_min; + float normalized = (x - x_min) / span; + uint32_t max_val = (bits >= 32) ? UINT32_MAX : ((1UL << bits) - 1); + return (uint32_t)(normalized * max_val + 0.5f); +} + +static inline float map_uint_to_float(uint32_t x, float x_min, float x_max, uint32_t bits) +{ + if (x_max == x_min) { + return x_min; + } + if (bits == 0 || bits > 32) { + return x_min; + } + uint32_t in_max = (1U << bits) - 1; + return ((float)x / (float)in_max) * (x_max - x_min) + x_min; +} + +static IRAM_ATTR bool cybergear_rx_done_cb(twai_node_handle_t handle, const twai_rx_done_event_data_t *edata, void *user_ctx) +{ + cybergear_t *motor = (cybergear_t *)user_ctx; + uint8_t recv_buff[TWAI_FRAME_MAX_LEN]; + twai_frame_t rx_frame = { + .buffer = recv_buff, + .buffer_len = sizeof(recv_buff), + }; + BaseType_t high_task_woken = pdFALSE; + + twai_node_receive_from_isr(handle, &rx_frame); + + ESP_EARLY_LOGV(TAG, "rx_frame.header.id: 0x%x, rx_frame.buffer: %02x %02x %02x %02x %02x %02x %02x %02x", rx_frame.header.id, + rx_frame.buffer[0], rx_frame.buffer[1], rx_frame.buffer[2], rx_frame.buffer[3], + rx_frame.buffer[4], rx_frame.buffer[5], rx_frame.buffer[6], rx_frame.buffer[7]); + + if (rx_frame.header.id >> 24 == CYBERGEAR_CMD_FEEDBACK_STATUS) { + motor->status.raw = (rx_frame.header.id >> 16) & 0xFF; + motor->raw_location = rx_frame.buffer[0] << 8 | rx_frame.buffer[1]; + motor->raw_speed = rx_frame.buffer[2] << 8 | rx_frame.buffer[3]; + motor->raw_torque = rx_frame.buffer[4] << 8 | rx_frame.buffer[5]; + motor->raw_temperature = rx_frame.buffer[6] << 8 | rx_frame.buffer[7]; + xSemaphoreGiveFromISR(motor->receive_sem, &high_task_woken); + } else if (rx_frame.header.id >> 24 == CYBERGEAR_CMD_BROADCAST) { + motor->motor_id = rx_frame.header.id >> 8 & 0xFF; + xSemaphoreGiveFromISR(motor->broadcast_sem, &high_task_woken); + } + + return high_task_woken; +} + +static esp_err_t cybergear_send_twai_package(cybergear_t *motor, uint32_t message_id, cybergear_data_t *message_data) +{ + // Clear the semaphore + xSemaphoreTake(motor->receive_sem, 0); + twai_frame_t frame = { + .header = { + .id = message_id, + .dlc = sizeof(cybergear_data_t), + .ide = 1, + }, + .buffer = (uint8_t *)message_data, + .buffer_len = sizeof(cybergear_data_t), + }; + + ESP_RETURN_ON_ERROR(twai_node_transmit(motor->node_hdl, &frame, pdMS_TO_TICKS(CYBERGEAR_MOTOR_TIMEOUT)), TAG, "failed to send twai package"); + // Wait for the status update + if (xSemaphoreTake(motor->receive_sem, pdMS_TO_TICKS(CYBERGEAR_MOTOR_TIMEOUT)) != pdTRUE) { + ESP_LOGW(TAG, "timeout waiting for response"); + return ESP_ERR_TIMEOUT; + } + + return ESP_OK; +} + +esp_err_t cybergear_enable(cybergear_t *motor) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_ENABLE << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_disable(cybergear_t *motor, bool clear_fault) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_DISABLE << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.byte[0] = clear_fault; + + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_control_motion(cybergear_t *motor, const cybergear_motion_control_t *motion_control) +{ + ESP_RETURN_ON_FALSE(motor && motion_control, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint16_t torque = map_float_to_uint(motion_control->torque, CYBERGEAR_TORQUE_MIN, CYBERGEAR_TORQUE_MAX, 16); + uint16_t location = map_float_to_uint(motion_control->location, CYBERGEAR_LOCATION_MIN, CYBERGEAR_LOCATION_MAX, 16); + uint16_t speed = map_float_to_uint(motion_control->speed, CYBERGEAR_SPEED_MIN, CYBERGEAR_SPEED_MAX, 16); + uint16_t kp = map_float_to_uint(motion_control->kp, CYBERGEAR_KP_MIN, CYBERGEAR_KP_MAX, 16); + uint16_t kd = map_float_to_uint(motion_control->kd, CYBERGEAR_KD_MIN, CYBERGEAR_KD_MAX, 16); + + uint32_t message_id = CYBERGEAR_CMD_MOTION_CONTROL << 24 | torque << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + + // cybergear use big-endian here + message_data.byte[0] = location >> 8; + message_data.byte[1] = location & 0xFF; + message_data.byte[2] = speed >> 8; + message_data.byte[3] = speed & 0xFF; + message_data.byte[4] = kp >> 8; + message_data.byte[5] = kp & 0xFF; + message_data.byte[6] = kd >> 8; + message_data.byte[7] = kd & 0xFF; + + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_set_mode(cybergear_t *motor, cybergear_mode_t mode) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + ESP_RETURN_ON_FALSE(mode < MODE_MAX, ESP_ERR_INVALID_ARG, TAG, "mode must be 0-3, 0:Motion, 1:Location, 2:Speed, 3:Current"); + + uint32_t message_id = CYBERGEAR_CMD_SET_PARA << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.half_word[0] = CYBERGEAR_ADDR_RUN_MODE; + message_data.byte[4] = mode; + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_set_location(cybergear_t *motor, float location) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_SET_PARA << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.half_word[0] = CYBERGEAR_ADDR_LOCATION_REF; + message_data.float_data[1] = location; + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_set_speed(cybergear_t *motor, float speed) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_SET_PARA << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.half_word[0] = CYBERGEAR_ADDR_SPEED_REF; + message_data.float_data[1] = speed; + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_set_speed_limit(cybergear_t *motor, float speed_limit) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_SET_PARA << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.half_word[0] = CYBERGEAR_ADDR_LIMIT_SPEED; + message_data.float_data[1] = speed_limit; + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_set_current(cybergear_t *motor, float current) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_SET_PARA << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.half_word[0] = CYBERGEAR_ADDR_CURRENT_REF; + message_data.float_data[1] = current; + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_set_current_limit(cybergear_t *motor, float current_limit) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_SET_PARA << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.half_word[0] = CYBERGEAR_ADDR_LIMIT_CURRENT; + message_data.float_data[1] = current_limit; + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_reset_zero_location(cybergear_t *motor) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_SET_LOCATION_TO_ZERO << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.byte[0] = 1; + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_set_motor_id(cybergear_t *motor, uint8_t new_motor_id) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_SET_MOTOR_ID << 24 | new_motor_id << 16 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + esp_err_t ret = ESP_OK; + + // re-configure the filter + ESP_RETURN_ON_ERROR(twai_node_disable(motor->node_hdl), TAG, "failed to disable node"); + twai_mask_filter_config_t mask_filter = { + .id = (new_motor_id << 8 | motor->esp_node_id), + .mask = 0xFF, + .is_ext = 1, + }; + ESP_RETURN_ON_ERROR(twai_node_config_mask_filter(motor->node_hdl, 0, &mask_filter), TAG, "failed to configure filter"); + ESP_RETURN_ON_ERROR(twai_node_enable(motor->node_hdl), TAG, "failed to enable node"); + ESP_GOTO_ON_ERROR(cybergear_send_twai_package(motor, message_id, &message_data), err, TAG, "failed to send twai package"); + motor->motor_id = new_motor_id; + return ret; +err: + // resume the origin filter + twai_node_disable(motor->node_hdl); + mask_filter.id = motor->motor_id << 8 | motor->esp_node_id; + twai_node_config_mask_filter(motor->node_hdl, 0, &mask_filter); + twai_node_enable(motor->node_hdl); + return ret; +} + +esp_err_t cybergear_get_motor_id(cybergear_t *motor, uint8_t *ret_motor_id) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + + // Clear the semaphore + xSemaphoreTake(motor->broadcast_sem, 0); + ESP_LOGI(TAG, "Please power off the motor and power it on again, then it will broadcast its motor ID"); + // Wait for the status update, first broadcast is default motor ID, and the second broadcast is the real motor ID set by user + xSemaphoreTake(motor->broadcast_sem, portMAX_DELAY); + xSemaphoreTake(motor->broadcast_sem, portMAX_DELAY); + + if (ret_motor_id) { + *ret_motor_id = motor->motor_id; + } + return ESP_OK; +} + +esp_err_t cybergear_get_parameter(cybergear_t *motor, uint16_t para_index) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + uint32_t message_id = CYBERGEAR_CMD_READ_PARA << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + message_data.half_word[0] = para_index; + return cybergear_send_twai_package(motor, message_id, &message_data); +} + +esp_err_t cybergear_get_status(cybergear_t *motor, cybergear_status_t *status) +{ + ESP_RETURN_ON_FALSE(motor && status, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + + uint32_t message_id = CYBERGEAR_CMD_FEEDBACK_STATUS << 24 | motor->esp_node_id << 8 | motor->motor_id; + cybergear_data_t message_data = {0}; + ESP_RETURN_ON_ERROR(cybergear_send_twai_package(motor, message_id, &message_data), TAG, "failed to send twai package"); + + status->motor_id = motor->motor_id; + status->fault = motor->status.raw & 0x3F; + status->state = motor->status.raw >> 6; + status->location = map_uint_to_float(motor->raw_location, CYBERGEAR_LOCATION_MIN, CYBERGEAR_LOCATION_MAX, 16); + status->speed = map_uint_to_float(motor->raw_speed, CYBERGEAR_SPEED_MIN, CYBERGEAR_SPEED_MAX, 16); + status->torque = map_uint_to_float(motor->raw_torque, CYBERGEAR_TORQUE_MIN, CYBERGEAR_TORQUE_MAX, 16); + status->temperature = motor->raw_temperature / 10.0f; + return ESP_OK; +} + +esp_err_t cybergear_del(cybergear_t *motor) +{ + ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument"); + if (motor->receive_sem) { + vSemaphoreDelete(motor->receive_sem); + } + if (motor->broadcast_sem) { + vSemaphoreDelete(motor->broadcast_sem); + } + if (motor->node_hdl) { + ESP_RETURN_ON_ERROR(twai_node_disable(motor->node_hdl), TAG, "failed to disable node"); + ESP_RETURN_ON_ERROR(twai_node_delete(motor->node_hdl), TAG, "failed to delete node"); + } + free(motor); + return ESP_OK; +} + +esp_err_t cybergear_new_twai_motor(twai_node_handle_t twai_node, const cybergear_config_t *motor_config, cybergear_handle_t *ret_motor) +{ + esp_err_t ret = ESP_OK; + cybergear_t *motor = NULL; + ESP_GOTO_ON_FALSE(twai_node && motor_config && ret_motor, ESP_ERR_INVALID_ARG, err, TAG, "invalid argument"); + motor = calloc(1, sizeof(cybergear_t)); + ESP_GOTO_ON_FALSE(motor, ESP_ERR_NO_MEM, err, TAG, "no memory for motor"); + + motor->receive_sem = xSemaphoreCreateBinary(); + ESP_GOTO_ON_FALSE(motor->receive_sem, ESP_ERR_NO_MEM, err, TAG, "no memory for status update semaphore"); + motor->broadcast_sem = xSemaphoreCreateBinary(); + ESP_GOTO_ON_FALSE(motor->broadcast_sem, ESP_ERR_NO_MEM, err, TAG, "no memory for broadcast semaphore"); + motor->node_hdl = twai_node; + // Register RX done callbacks + twai_event_callbacks_t cbs = { + .on_rx_done = cybergear_rx_done_cb, + }; + ESP_GOTO_ON_ERROR(twai_node_register_event_callbacks(motor->node_hdl, &cbs, motor), err, TAG, "failed to register callbacks"); + + // configure the filter, cybergear only support ID-extended frame + twai_mask_filter_config_t mask_filter = { + .id = (motor_config->motor_id << 8 | motor_config->esp_node_id), + .mask = 0xFF, + .is_ext = 1, + }; + ESP_GOTO_ON_ERROR(twai_node_config_mask_filter(motor->node_hdl, 0, &mask_filter), err, TAG, "failed to configure filter"); + + ESP_GOTO_ON_ERROR(twai_node_enable(motor->node_hdl), err, TAG, "failed to enable node"); + + motor->motor_id = motor_config->motor_id; + motor->esp_node_id = motor_config->esp_node_id; + + *ret_motor = motor; + return ESP_OK; + +err: + if (motor) { + if (motor->node_hdl) { + twai_node_delete(motor->node_hdl); + } + if (motor->receive_sem) { + vSemaphoreDelete(motor->receive_sem); + } + if (motor->broadcast_sem) { + vSemaphoreDelete(motor->broadcast_sem); + } + free(motor); + } + + return ret; +} diff --git a/examples/peripherals/twai/cybergear/main/cybergear.h b/examples/peripherals/twai/cybergear/main/cybergear.h new file mode 100644 index 0000000000..aa8e513ba8 --- /dev/null +++ b/examples/peripherals/twai/cybergear/main/cybergear.h @@ -0,0 +1,222 @@ +/* + * SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ +#pragma once + +#include +#include +#include "esp_twai_onchip.h" +#include "cybergear_type.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief CyberGear motor config + */ +typedef struct { + uint32_t motor_id; /*!< Motor ID */ + uint32_t esp_node_id; /*!< ESP Node ID */ +} cybergear_config_t; + +/** + * @brief Create a CyberGear motor + * + * @param[in] twai_node The TWAI node handle + * @param[in] motor_config The motor config + * @param[out] ret_motor The motor handle + * @return + * - ESP_OK: Create CyberGear motor successfully + * - ESP_ERR_INVALID_ARG: Create CyberGear motor failed because of invalid argument + * - ESP_FAIL: Create CyberGear motor failed because of other error + */ +esp_err_t cybergear_new_twai_motor(twai_node_handle_t twai_node, const cybergear_config_t *motor_config, cybergear_handle_t *ret_motor); + +/** + * @brief Delete a CyberGear motor + * + * @param[in] motor The motor handle + * @return + * - ESP_OK: Delete CyberGear motor successfully + * - ESP_ERR_INVALID_ARG: Delete CyberGear motor failed because of invalid argument + * - ESP_FAIL: Delete CyberGear motor failed because of other error + */ +esp_err_t cybergear_del(cybergear_handle_t motor); + +/** + * @brief Enable the CyberGear motor + * + * @param[in] motor The motor handle + * @return + * - ESP_OK: Enable CyberGear motor successfully + * - ESP_ERR_INVALID_ARG: Enable CyberGear motor failed because of invalid argument + * - ESP_FAIL: Enable CyberGear motor failed because of other error + */ +esp_err_t cybergear_enable(cybergear_handle_t motor); + +/** + * @brief Disable the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] clear_fault Whether to clear the fault + * @return + * - ESP_OK: Disable CyberGear motor successfully + * - ESP_ERR_INVALID_ARG: Disable CyberGear motor failed because of invalid argument + * - ESP_FAIL: Disable CyberGear motor failed because of other error + */ +esp_err_t cybergear_disable(cybergear_handle_t motor, bool clear_fault); + +/** + * @brief Set the mode of the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] mode The mode to set + * @return + * - ESP_OK: Set mode successfully + * - ESP_ERR_INVALID_ARG: Set mode failed because of invalid argument + * - ESP_FAIL: Set mode failed because of other error + */ +esp_err_t cybergear_set_mode(cybergear_handle_t motor, cybergear_mode_t mode); + +/** + * @brief Set the location of the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] location The location to set + * @return + * - ESP_OK: Set location successfully + * - ESP_ERR_INVALID_ARG: Set location failed because of invalid argument + * - ESP_FAIL: Set location failed because of other error + */ +esp_err_t cybergear_set_location(cybergear_handle_t motor, float location); + +/** + * @brief Set the speed of the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] speed The speed to set + * @return + * - ESP_OK: Set speed successfully + * - ESP_ERR_INVALID_ARG: Set speed failed because of invalid argument + * - ESP_FAIL: Set speed failed because of other error + */ +esp_err_t cybergear_set_speed(cybergear_handle_t motor, float speed); + +/** + * @brief Set the speed limit of the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] speed_limit The speed limit to set + * @return + * - ESP_OK: Set speed limit successfully + * - ESP_ERR_INVALID_ARG: Set speed limit failed because of invalid argument + * - ESP_FAIL: Set speed limit failed because of other error + */ +esp_err_t cybergear_set_speed_limit(cybergear_handle_t motor, float speed_limit); + +/** + * @brief Set the current of the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] current The current to set + * @return + * - ESP_OK: Set current successfully + * - ESP_ERR_INVALID_ARG: Set current failed because of invalid argument + * - ESP_FAIL: Set current failed because of other error + */ +esp_err_t cybergear_set_current(cybergear_handle_t motor, float current); + +/** + * @brief Set the current limit of the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] current The current to set + * @return + * - ESP_OK: Set current successfully + * - ESP_ERR_INVALID_ARG: Set current failed because of invalid argument + * - ESP_FAIL: Set current failed because of other error + */ +esp_err_t cybergear_set_current_limit(cybergear_handle_t motor, float current_limit); + +/** + * @brief Set the zero location of the CyberGear motor + * + * @param[in] motor The motor handle + * @return + * - ESP_OK: Set zero location successfully + * - ESP_ERR_INVALID_ARG: Set zero location failed because of invalid argument + * - ESP_FAIL: Set zero location failed because of other error + */ +esp_err_t cybergear_reset_zero_location(cybergear_handle_t motor); + +/** + * @brief Set the motor ID of the CyberGear motor + * + * @note The motor ID can not be changed when the motor is running, please disable the motor and set motor id. + * + * @param[in] motor The motor handle + * @param[in] new_motor_id The new motor ID to set + * @return + * - ESP_OK: Set motor ID successfully + * - ESP_ERR_INVALID_ARG: Set motor ID failed because of invalid argument + * - ESP_FAIL: Set motor ID failed because of other error + */ +esp_err_t cybergear_set_motor_id(cybergear_handle_t motor, uint8_t new_motor_id); + +/** + * @brief Get the motor ID of the CyberGear motor + * + * @note After calling this function, you need to power off the motor and power it on again. Then it will broadcast its motor ID. + * This API will block until the motor ID is broadcasted. + * + * @param[in] motor The motor handle + * @param[out] ret_motor_id The motor ID to get + * @return + * - ESP_OK: Get motor ID successfully + * - ESP_ERR_INVALID_ARG: Get motor ID failed because of invalid argument + * - ESP_FAIL: Get motor ID failed because of other error + */ +esp_err_t cybergear_get_motor_id(cybergear_handle_t motor, uint8_t *ret_motor_id); + +/** + * @brief Get the parameter of the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] para_index The parameter index to get + * @return + * - ESP_OK: Get parameter successfully + * - ESP_ERR_INVALID_ARG: Get parameter failed because of invalid argument + * - ESP_FAIL: Get parameter failed because of other error + */ +esp_err_t cybergear_get_parameter(cybergear_handle_t motor, uint16_t para_index); + +/** + * @brief Motion control the CyberGear motor + * + * @param[in] motor The motor handle + * @param[in] motion_control The motion control to set + * @return + * - ESP_OK: Motion control successfully + * - ESP_ERR_INVALID_ARG: Motion control failed because of invalid argument + * - ESP_FAIL: Motion control failed because of other error + */ +esp_err_t cybergear_control_motion(cybergear_handle_t motor, const cybergear_motion_control_t *motion_control); + +/** + * @brief Get the status of the CyberGear motor + * + * @param[in] motor The motor handle + * @param[out] status The status to get + * @return + * - ESP_OK: Get status successfully + * - ESP_ERR_INVALID_ARG: Get status failed because of invalid argument + * - ESP_FAIL: Get status failed because of other error + */ +esp_err_t cybergear_get_status(cybergear_handle_t motor, cybergear_status_t *status); + +#ifdef __cplusplus +} +#endif diff --git a/examples/peripherals/twai/cybergear/main/cybergear_console.c b/examples/peripherals/twai/cybergear/main/cybergear_console.c new file mode 100644 index 0000000000..05c9596456 --- /dev/null +++ b/examples/peripherals/twai/cybergear/main/cybergear_console.c @@ -0,0 +1,298 @@ +/* + * SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: CC0-1.0 + */ + +#include "esp_console.h" +#include "esp_log.h" +#include "esp_check.h" +#include "argtable3/argtable3.h" +#include "cybergear.h" +#include "cybergear_console.h" + +// static motor handle +cybergear_handle_t s_motor; + +// Console commands +static int cmd_enable(int argc, char **argv) +{ + cybergear_set_speed_limit(s_motor, CYBERGEAR_SPEED_MAX); + cybergear_set_current_limit(s_motor, CYBERGEAR_CURRENT_MAX); + cybergear_enable(s_motor); + return 0; +} + +static int cmd_disable(int argc, char **argv) +{ + cybergear_disable(s_motor, true); + return 0; +} + +static struct { + struct arg_int *mode; + struct arg_end *end; +} set_mode_args; + +static int cmd_set_mode(int argc, char **argv) +{ + int nerrors = arg_parse(argc, argv, (void **)&set_mode_args); + if (nerrors != 0) { + arg_print_errors(stderr, set_mode_args.end, argv[0]); + return 1; + } + cybergear_mode_t mode = set_mode_args.mode->ival[0]; + cybergear_set_mode(s_motor, mode); + return 0; +} + +static struct { + struct arg_dbl *torque; + struct arg_dbl *location; + struct arg_dbl *speed; + struct arg_dbl *kp; + struct arg_dbl *kd; + struct arg_end *end; +} motion_control_args; + +static int cmd_motion_control(int argc, char **argv) +{ + int nerrors = arg_parse(argc, argv, (void **)&motion_control_args); + if (nerrors != 0) { + arg_print_errors(stderr, motion_control_args.end, argv[0]); + return 1; + } + cybergear_motion_control_t motion_control = { + .torque = motion_control_args.torque->dval[0], + .location = motion_control_args.location->dval[0], + .speed = motion_control_args.speed->dval[0], + .kp = motion_control_args.kp->dval[0], + .kd = motion_control_args.kd->dval[0], + }; + cybergear_control_motion(s_motor, &motion_control); + return 0; +} + +static struct { + struct arg_dbl *speed; + struct arg_end *end; +} set_speed_args; + +static int cmd_set_speed(int argc, char **argv) +{ + int nerrors = arg_parse(argc, argv, (void **)&set_speed_args); + if (nerrors != 0) { + arg_print_errors(stderr, set_speed_args.end, argv[0]); + return 1; + } + float speed_ref = set_speed_args.speed->dval[0]; + cybergear_set_speed(s_motor, speed_ref); + return 0; +} + +static struct { + struct arg_dbl *location; + struct arg_end *end; +} set_location_args; + +static int cmd_set_location(int argc, char **argv) +{ + int nerrors = arg_parse(argc, argv, (void **)&set_location_args); + if (nerrors != 0) { + arg_print_errors(stderr, set_location_args.end, argv[0]); + return 1; + } + float location = set_location_args.location->dval[0]; + cybergear_set_location(s_motor, location); + return 0; +} + +static struct { + struct arg_dbl *current; + struct arg_end *end; +} set_current_args; + +static int cmd_set_current(int argc, char **argv) +{ + int nerrors = arg_parse(argc, argv, (void **)&set_current_args); + if (nerrors != 0) { + arg_print_errors(stderr, set_current_args.end, argv[0]); + return 1; + } + float current = set_current_args.current->dval[0]; + cybergear_set_current(s_motor, current); + return 0; +} + +static struct { + struct arg_int *motor_id; + struct arg_end *end; +} set_motor_id_args; + +static int cmd_set_motor_id(int argc, char **argv) +{ + int nerrors = arg_parse(argc, argv, (void **)&set_motor_id_args); + if (nerrors != 0) { + arg_print_errors(stderr, set_motor_id_args.end, argv[0]); + return 1; + } + uint8_t motor_id = set_motor_id_args.motor_id->ival[0]; + cybergear_set_motor_id(s_motor, motor_id); + return 0; +} + +static int cmd_get_motor_id(int argc, char **argv) +{ + uint8_t motor_id; + cybergear_get_motor_id(s_motor, &motor_id); + printf("Motor ID: %d\n", motor_id); + return 0; +} + +static int cmd_zero_location(int argc, char **argv) +{ + cybergear_reset_zero_location(s_motor); + return 0; +} + +static int cmd_status(int argc, char **argv) +{ + cybergear_status_t status; + cybergear_get_status(s_motor, &status); + printf("===== Motor Status =====\n"); + printf("Motor ID: %d\n", status.motor_id); + printf("Fault Status: %d\n", status.fault); + printf("State: %d\n", status.state); + printf("Location: %f\n", status.location); + printf("Speed: %f\n", status.speed); + printf("Torque: %f\n", status.torque); + printf("Temperature: %f\n", status.temperature); + printf("========================\n"); + return 0; +} + +// Register CyberGear Console commands +void register_cybergear_commands(cybergear_handle_t motor_handle) +{ + s_motor = motor_handle; + const esp_console_cmd_t enable_cmd = { + .command = "enable", + .help = "Enable motor", + .hint = NULL, + .func = &cmd_enable, + .argtable = NULL + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&enable_cmd)); + + const esp_console_cmd_t disable_cmd = { + .command = "disable", + .help = "Disable motor and clear fault", + .hint = NULL, + .func = &cmd_disable, + .argtable = NULL + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&disable_cmd)); + + set_mode_args.mode = arg_int1(NULL, NULL, "", "Control mode (0:Motion, 1:Location, 2:Speed, 3:Current)"); + set_mode_args.end = arg_end(1); + + const esp_console_cmd_t set_mode_cmd = { + .command = "mode", + .help = "Set control mode", + .hint = NULL, + .func = &cmd_set_mode, + .argtable = &set_mode_args + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&set_mode_cmd)); + + motion_control_args.torque = arg_dbl1("t", "torque", "", "Torque (Nm)"); + motion_control_args.location = arg_dbl1("l", "loc", "", "Target location (rad)"); + motion_control_args.speed = arg_dbl1("s", "speed", "", "Target speed (rad/s)"); + motion_control_args.kp = arg_dbl1("p", "kp", "", "Proportional gain"); + motion_control_args.kd = arg_dbl1("d", "kd", "", "Derivative gain"); + motion_control_args.end = arg_end(5); + + const esp_console_cmd_t motion_control_cmd = { + .command = "motion", + .help = "Motion control command", + .hint = NULL, + .func = &cmd_motion_control, + .argtable = &motion_control_args + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&motion_control_cmd)); + + set_speed_args.speed = arg_dbl1("s", "set", "", "Target speed (rad/s)"); + set_speed_args.end = arg_end(1); + + const esp_console_cmd_t set_speed_cmd = { + .command = "speed", + .help = "Set motor speed", + .hint = NULL, + .func = &cmd_set_speed, + .argtable = &set_speed_args + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&set_speed_cmd)); + + set_location_args.location = arg_dbl1("s", "set", "", "Target location (rad)"); + set_location_args.end = arg_end(1); + + const esp_console_cmd_t set_location_cmd = { + .command = "loc", + .help = "Set motor location", + .hint = NULL, + .func = &cmd_set_location, + .argtable = &set_location_args + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&set_location_cmd)); + + set_current_args.current = arg_dbl1("s", "set", "", "Target current (A)"); + set_current_args.end = arg_end(1); + + const esp_console_cmd_t set_current_cmd = { + .command = "current", + .help = "Set motor current", + .hint = NULL, + .func = &cmd_set_current, + .argtable = &set_current_args + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&set_current_cmd)); + + const esp_console_cmd_t zero_location_cmd = { + .command = "zero", + .help = "Set current location to zero", + .hint = NULL, + .func = &cmd_zero_location, + .argtable = NULL + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&zero_location_cmd)); + + set_motor_id_args.motor_id = arg_int1(NULL, NULL, "", "Motor ID"); + set_motor_id_args.end = arg_end(1); + + const esp_console_cmd_t set_motor_id_cmd = { + .command = "setid", + .help = "Set motor ID", + .hint = NULL, + .func = &cmd_set_motor_id, + .argtable = &set_motor_id_args + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&set_motor_id_cmd)); + + const esp_console_cmd_t get_motor_id_cmd = { + .command = "getid", + .help = "Get motor ID", + .hint = NULL, + .func = &cmd_get_motor_id, + .argtable = NULL + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&get_motor_id_cmd)); + + const esp_console_cmd_t status_cmd = { + .command = "status", + .help = "Show motor status", + .hint = NULL, + .func = &cmd_status, + .argtable = NULL + }; + ESP_ERROR_CHECK(esp_console_cmd_register(&status_cmd)); +} diff --git a/examples/peripherals/twai/cybergear/main/cybergear_console.h b/examples/peripherals/twai/cybergear/main/cybergear_console.h new file mode 100644 index 0000000000..ab71ec19de --- /dev/null +++ b/examples/peripherals/twai/cybergear/main/cybergear_console.h @@ -0,0 +1,22 @@ +/* + * SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +#include "cybergear_type.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Register the CyberGear commands + */ +void register_cybergear_commands(cybergear_handle_t motor); + +#ifdef __cplusplus +} +#endif diff --git a/examples/peripherals/twai/cybergear/main/cybergear_example_main.c b/examples/peripherals/twai/cybergear/main/cybergear_example_main.c new file mode 100644 index 0000000000..76faade57a --- /dev/null +++ b/examples/peripherals/twai/cybergear/main/cybergear_example_main.c @@ -0,0 +1,69 @@ +/* + * SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: CC0-1.0 + */ + +#include +#include +#include "esp_system.h" +#include "esp_console.h" +#include "esp_log.h" +#include "cybergear.h" +#include "cybergear_console.h" + +static const char* TAG = "Cybergear"; + +#define TWAI_TX_GPIO CONFIG_EXAMPLE_TWAI_TX_GPIO_NUM +#define TWAI_RX_GPIO CONFIG_EXAMPLE_TWAI_RX_GPIO_NUM +#define ESP_NODE_ID CONFIG_EXAMPLE_TWAI_ESP_NODE_ID +#define MOTOR_ID CONFIG_EXAMPLE_TWAI_MOTOR_ID +#define MOTOR_BITRATE CONFIG_EXAMPLE_TWAI_MOTOR_BITRATE +#define TWAI_QUEUE_DEPTH 1 + +void app_main(void) +{ + // Initialize Console + esp_console_repl_t *repl = NULL; + esp_console_repl_config_t repl_config = ESP_CONSOLE_REPL_CONFIG_DEFAULT(); + + repl_config.prompt = "cybergear>"; + + // install console REPL environment +#if CONFIG_ESP_CONSOLE_UART + esp_console_dev_uart_config_t uart_config = ESP_CONSOLE_DEV_UART_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_console_new_repl_uart(&uart_config, &repl_config, &repl)); +#elif CONFIG_ESP_CONSOLE_USB_CDC + esp_console_dev_usb_cdc_config_t cdc_config = ESP_CONSOLE_DEV_CDC_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_console_new_repl_usb_cdc(&cdc_config, &repl_config, &repl)); +#elif CONFIG_ESP_CONSOLE_USB_SERIAL_JTAG + esp_console_dev_usb_serial_jtag_config_t usbjtag_config = ESP_CONSOLE_DEV_USB_SERIAL_JTAG_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_console_new_repl_usb_serial_jtag(&usbjtag_config, &repl_config, &repl)); +#endif + + // Initialize CyberGear + twai_onchip_node_config_t node_config = { + .io_cfg.tx = TWAI_TX_GPIO, + .io_cfg.rx = TWAI_RX_GPIO, + .io_cfg.quanta_clk_out = GPIO_NUM_NC, + .io_cfg.bus_off_indicator = GPIO_NUM_NC, + .bit_timing.bitrate = MOTOR_BITRATE, + .tx_queue_depth = TWAI_QUEUE_DEPTH, + }; + twai_node_handle_t twai_node; + ESP_ERROR_CHECK(twai_new_node_onchip(&node_config, &twai_node)); + cybergear_config_t cybergear_config = { + .motor_id = MOTOR_ID, + .esp_node_id = ESP_NODE_ID, + }; + cybergear_handle_t motor; + ESP_ERROR_CHECK(cybergear_new_twai_motor(twai_node, &cybergear_config, &motor)); + ESP_LOGI(TAG, "CyberGear node started, TX: GPIO%d, RX: GPIO%d", TWAI_TX_GPIO, TWAI_RX_GPIO); + + register_cybergear_commands(motor); + + ESP_LOGI(TAG, "CyberGear console started"); + + // Start console REPL + ESP_ERROR_CHECK(esp_console_start_repl(repl)); +} diff --git a/examples/peripherals/twai/cybergear/main/cybergear_type.h b/examples/peripherals/twai/cybergear/main/cybergear_type.h new file mode 100644 index 0000000000..42abae5d1e --- /dev/null +++ b/examples/peripherals/twai/cybergear/main/cybergear_type.h @@ -0,0 +1,95 @@ +/* + * SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ +#pragma once + +#include + +#define CYBERGEAR_CMD_BROADCAST 0x00 +#define CYBERGEAR_CMD_MOTION_CONTROL 0x01 +#define CYBERGEAR_CMD_FEEDBACK_STATUS 0x02 +#define CYBERGEAR_CMD_ENABLE 0x03 +#define CYBERGEAR_CMD_DISABLE 0x04 +#define CYBERGEAR_CMD_SET_LOCATION_TO_ZERO 0x06 +#define CYBERGEAR_CMD_SET_MOTOR_ID 0x07 +#define CYBERGEAR_CMD_READ_PARA 0x11 +#define CYBERGEAR_CMD_SET_PARA 0x12 +#define CYBERGEAR_CMD_FEEDBACK_FAULT 0x15 + +#define CYBERGEAR_ADDR_RUN_MODE 0x7005 +#define CYBERGEAR_ADDR_CURRENT_REF 0x7006 +#define CYBERGEAR_ADDR_SPEED_REF 0x700A +#define CYBERGEAR_ADDR_LIMIT_TORQUE 0x700B +#define CYBERGEAR_ADDR_CURRENT_KP 0x7010 +#define CYBERGEAR_ADDR_CURRENT_KI 0x7011 +#define CYBERGEAR_ADDR_CURRENT_FILTER_GAIN 0x7014 +#define CYBERGEAR_ADDR_LOCATION_REF 0x7016 +#define CYBERGEAR_ADDR_LIMIT_SPEED 0x7017 +#define CYBERGEAR_ADDR_LIMIT_CURRENT 0x7018 + +#define CYBERGEAR_LOCATION_MIN -12.5f +#define CYBERGEAR_LOCATION_MAX 12.5f +#define CYBERGEAR_SPEED_MIN -30.0f +#define CYBERGEAR_SPEED_MAX 30.0f +#define CYBERGEAR_KP_MIN 0.0f +#define CYBERGEAR_KP_MAX 500.0f +#define CYBERGEAR_KD_MIN 0.0f +#define CYBERGEAR_KD_MAX 5.0f +#define CYBERGEAR_TORQUE_MIN -12.0f +#define CYBERGEAR_TORQUE_MAX 12.0f +#define CYBERGEAR_CURRENT_MIN -23.0f +#define CYBERGEAR_CURRENT_MAX 23.0f +#define CYBERGEAR_CURRENT_FILTER_GAIN_MIN 0.0f +#define CYBERGEAR_CURRENT_FILTER_GAIN_MAX 1.0f + +#define CYBERGEAR_MOTOR_TIMEOUT 100 + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief CyberGear running mode + */ +typedef enum { + MODE_MOTION, /*!< Motion mode */ + MODE_LOCATION, /*!< Location mode */ + MODE_SPEED, /*!< Speed mode */ + MODE_CURRENT, /*!< Current mode */ + MODE_MAX, /*!< Mode count maximum */ +} cybergear_mode_t; + +/** + * @brief CyberGear motion control parameters + */ +typedef struct { + float torque; /*!< Motion torque */ + float location; /*!< Motion location */ + float speed; /*!< Motion speed */ + float kp; /*!< Motion proportional gain */ + float kd; /*!< Motion derivative gain */ +} cybergear_motion_control_t; + +/** + * @brief CyberGear status + */ +typedef struct { + uint8_t motor_id; /*!< Motor ID */ + uint8_t fault; /*!< Fault status */ + uint8_t state; /*!< Motor state */ + float location; /*!< Location */ + float speed; /*!< Feedback speed */ + float torque; /*!< Feedback torque */ + float temperature; /*!< Feedback temperature */ +} cybergear_status_t; + +/** + * @brief CyberGear handle + */ +typedef struct cybergear_t *cybergear_handle_t; /*!< CyberGear handle */ + +#ifdef __cplusplus +} +#endif diff --git a/examples/peripherals/twai/cybergear/pytest_cybergear_example.py b/examples/peripherals/twai/cybergear/pytest_cybergear_example.py new file mode 100644 index 0000000000..c4665ee72c --- /dev/null +++ b/examples/peripherals/twai/cybergear/pytest_cybergear_example.py @@ -0,0 +1,12 @@ +# SPDX-FileCopyrightText: 2025 Espressif Systems (Shanghai) CO LTD +# SPDX-License-Identifier: CC0-1.0 +import pytest +from pytest_embedded import Dut +from pytest_embedded_idf.utils import idf_parametrize +from pytest_embedded_idf.utils import soc_filtered_targets + + +@pytest.mark.generic +@idf_parametrize('target', soc_filtered_targets('SOC_TWAI_SUPPORTED == 1'), indirect=['target']) +def test_cybergear_example(dut: Dut) -> None: + dut.expect_exact('CyberGear console started')