diff --git a/.config b/.config index 7b9a22b..672bc9e 100644 --- a/.config +++ b/.config @@ -256,7 +256,7 @@ CONFIG_RT_USING_PWM=y # CONFIG_RT_USING_SDIO is not set CONFIG_RT_USING_SPI=y # CONFIG_RT_USING_SOFT_SPI is not set -# CONFIG_RT_USING_QSPI is not set +CONFIG_RT_USING_QSPI=y # CONFIG_RT_USING_SPI_MSD is not set # CONFIG_RT_USING_SFUD is not set # CONFIG_RT_USING_ENC28J60 is not set @@ -1531,7 +1531,7 @@ CONFIG_BSP_UART3_TX_BUFSIZE=512 CONFIG_BSP_USING_PWM=y CONFIG_BSP_USING_PWM1=y CONFIG_BSP_USING_PWM1_CH1=y -# CONFIG_BSP_USING_QSPI is not set +CONFIG_BSP_USING_QSPI=y CONFIG_BSP_USING_ADC=y CONFIG_BSP_USING_ADC1=y # CONFIG_BSP_USING_SPI is not set diff --git a/CMakeLists.txt b/CMakeLists.txt index 68a6512..4cbec69 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,12 +29,14 @@ PROJECT(rtthread C CXX ASM) INCLUDE_DIRECTORIES( . applications + applications/common applications/controller applications/erpc applications/erpc/service applications/erpc/client applications/erpc/common applications/erpc/proto/generated + applications/planner applications/servo_manager board board/CubeMX_Config/Inc @@ -103,25 +105,29 @@ ADD_DEFINITIONS( # Library source files SET(RT_APP_ERPC_SOURCES + applications/erpc/service/src/servo_service_impl.cpp + applications/erpc/proto/generated/servo_service_server.cpp applications/erpc/common/src/erpc_error_handler.cpp - applications/erpc/proto/generated/face_servo_interface.cpp - applications/erpc/service/src/head_service_impl.cpp - applications/erpc/proto/generated/face_servo_server.cpp + applications/erpc/proto/generated/servo_service_interface.cpp applications/erpc/service/src/erpc_server_main.cpp ) SET(RT_APPLICATIONS_SOURCES - applications/main.c + applications/main.cpp ) SET(RT_CHERRYUSB_SOURCES + rt-thread/components/drivers/usb/cherryusb/core/usbd_core.c rt-thread/components/drivers/usb/cherryusb/platform/rtthread/usb_msh.c - rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_dc_dwc2.c rt-thread/components/drivers/usb/cherryusb/osal/usb_osal_rtthread.c - rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_glue_st.c rt-thread/components/drivers/usb/cherryusb/class/cdc/usbd_cdc_acm.c rt-thread/components/drivers/usb/cherryusb/platform/rtthread/usb_check.c - rt-thread/components/drivers/usb/cherryusb/core/usbd_core.c + rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_glue_st.c + rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_dc_dwc2.c +) + +SET(RT_COMMON_SOURCES + applications/common/curve/src/s_curve.cpp ) SET(RT_COMPILER_SOURCES @@ -136,11 +142,13 @@ SET(RT_COMPILER_SOURCES SET(RT_CONTROLLER_SOURCES applications/controller/src/pid_controller.cpp + applications/planner/s_curve_planner/src/s_curve_position_planner.cpp + applications/planner/s_curve_planner/src/s_curve_velocity_planner.cpp ) SET(RT_CPP_SOURCES - rt-thread/components/libc/cplusplus/cxx_crt.cpp rt-thread/components/libc/cplusplus/cxx_crt_init.c + rt-thread/components/libc/cplusplus/cxx_crt.cpp ) SET(RT_DEVICEDRIVERS_SOURCES @@ -158,6 +166,7 @@ SET(RT_DEVICEDRIVERS_SOURCES rt-thread/components/drivers/misc/rt_drv_pwm.c rt-thread/components/drivers/pin/dev_pin.c rt-thread/components/drivers/serial/dev_serial.c + rt-thread/components/drivers/spi/dev_qspi_core.c rt-thread/components/drivers/spi/dev_spi.c rt-thread/components/drivers/spi/dev_spi_core.c ) @@ -171,6 +180,7 @@ SET(RT_DRIVERS_SOURCES libraries/HAL_Drivers/drivers/drv_adc.c libraries/HAL_Drivers/drivers/drv_gpio.c libraries/HAL_Drivers/drivers/drv_pwm.c + libraries/HAL_Drivers/drivers/drv_qspi.c libraries/HAL_Drivers/drivers/drv_spi.c libraries/HAL_Drivers/drivers/drv_tim.c libraries/HAL_Drivers/drivers/drv_usart.c @@ -178,29 +188,29 @@ SET(RT_DRIVERS_SOURCES ) SET(RT_ERPC_SOURCES - packages/erpc-1.14.0/erpc_c/infra/erpc_framed_transport.cpp - packages/erpc-1.14.0/erpc_c/infra/erpc_message_buffer.cpp - packages/erpc-1.14.0/erpc_c/infra/erpc_crc16.cpp - packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_dynamic.cpp packages/erpc-1.14.0/erpc_c/infra/erpc_message_loggers.cpp - packages/erpc-1.14.0/erpc_c/setup/erpc_server_setup.cpp - packages/erpc-1.14.0/erpc_c/infra/erpc_transport_arbitrator.cpp + packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_dynamic.cpp packages/erpc-1.14.0/erpc_c/port/erpc_threading_pthreads.cpp - packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_static.cpp - packages/erpc-1.14.0/erpc_c/infra/erpc_simple_server.cpp - packages/erpc-1.14.0/erpc_c/port/erpc_port_rtt.cpp packages/erpc-1.14.0/erpc_c/infra/erpc_server.cpp + packages/erpc-1.14.0/erpc_c/port/erpc_port_rtt.cpp + packages/erpc-1.14.0/erpc_c/setup/erpc_server_setup.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_simple_server.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_framed_transport.cpp + packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_static.cpp packages/erpc-1.14.0/erpc_c/transports/erpc_usb_cdc_transport.cpp - packages/erpc-1.14.0/erpc_c/infra/erpc_basic_codec.cpp - packages/erpc-1.14.0/erpc_c/infra/erpc_pre_post_action.cpp - packages/erpc-1.14.0/erpc_c/transports/erpc_inter_thread_buffer_transport.cpp packages/erpc-1.14.0/erpc_c/setup/erpc_setup_usb_cdc.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_transport_arbitrator.cpp + packages/erpc-1.14.0/erpc_c/transports/erpc_inter_thread_buffer_transport.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_crc16.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_basic_codec.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_message_buffer.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_pre_post_action.cpp ) SET(RT_FINSH_SOURCES + rt-thread/components/finsh/cmd.c rt-thread/components/finsh/shell.c rt-thread/components/finsh/msh_parse.c - rt-thread/components/finsh/cmd.c rt-thread/components/finsh/msh.c ) @@ -223,15 +233,15 @@ SET(RT_KERNEL_SOURCES ) SET(RT_KLIBC_SOURCES + rt-thread/src/klibc/rt_vsscanf.c + rt-thread/src/klibc/kstring.c rt-thread/src/klibc/kerrno.c rt-thread/src/klibc/kstdio.c - rt-thread/src/klibc/kstring.c - rt-thread/src/klibc/rt_vsscanf.c ) SET(RT_KTIME_SOURCES - rt-thread/components/drivers/ktime/src/hrtimer.c rt-thread/components/drivers/ktime/src/boottime.c + rt-thread/components/drivers/ktime/src/hrtimer.c rt-thread/components/drivers/ktime/src/cputimer.c ) @@ -245,35 +255,35 @@ SET(RT_LIBCPU_SOURCES ) SET(RT_LIBRARIES_SOURCES - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc_ex.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_comp.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_lptim.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_gpio.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr_ex.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc_ex.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_sram.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc_ex.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_qspi.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c - libraries/STM32H7xx_HAL/CMSIS/Device/ST/STM32H7xx/Source/Templates/system_stm32h7xx.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_usart.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cec.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc_ex.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_mdma.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_usart.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma_ex.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_lptim.c + libraries/STM32H7xx_HAL/CMSIS/Device/ST/STM32H7xx/Source/Templates/system_stm32h7xx.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rng.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_comp.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_mdma.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cec.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc.c ) SET(RT_POSIX_SOURCES @@ -290,8 +300,10 @@ SET(RT_POSIX_SOURCES rt-thread/components/libc/posix/tls/emutls.c ) -SET(RT_SERVO_MANAGER_SOURCES +SET(RT_SERVO_CONTROL_SOURCES + applications/servo_manager/src/servo_control.cpp applications/servo_manager/src/servo_manager.cpp + applications/servo_manager/src/servo_driver.cpp ) SET(RT_UTC_UTEST_SOURCES @@ -319,6 +331,7 @@ SET(RT_COMPILER_LIBS # Libraries ADD_LIBRARY(rtt_app_erpc OBJECT ${RT_APP_ERPC_SOURCES}) ADD_LIBRARY(rtt_CherryUSB OBJECT ${RT_CHERRYUSB_SOURCES}) +ADD_LIBRARY(rtt_common OBJECT ${RT_COMMON_SOURCES}) ADD_LIBRARY(rtt_Compiler OBJECT ${RT_COMPILER_SOURCES}) ADD_LIBRARY(rtt_controller OBJECT ${RT_CONTROLLER_SOURCES}) ADD_LIBRARY(rtt_CPP OBJECT ${RT_CPP_SOURCES}) @@ -332,7 +345,7 @@ ADD_LIBRARY(rtt_ktime OBJECT ${RT_KTIME_SOURCES}) ADD_LIBRARY(rtt_libcpu OBJECT ${RT_LIBCPU_SOURCES}) ADD_LIBRARY(rtt_Libraries OBJECT ${RT_LIBRARIES_SOURCES}) ADD_LIBRARY(rtt_POSIX OBJECT ${RT_POSIX_SOURCES}) -ADD_LIBRARY(rtt_servo_manager OBJECT ${RT_SERVO_MANAGER_SOURCES}) +ADD_LIBRARY(rtt_servo_control OBJECT ${RT_SERVO_CONTROL_SOURCES}) # Interface libraries ADD_LIBRARY(rtt_utc_UTest INTERFACE) @@ -349,6 +362,7 @@ ADD_EXECUTABLE(${CMAKE_PROJECT_NAME}.elf ${RT_APPLICATIONS_SOURCES}) TARGET_LINK_LIBRARIES(${CMAKE_PROJECT_NAME}.elf rtt_app_erpc rtt_CherryUSB + rtt_common rtt_Compiler rtt_controller rtt_CPP @@ -362,7 +376,7 @@ TARGET_LINK_LIBRARIES(${CMAKE_PROJECT_NAME}.elf rtt_libcpu rtt_Libraries rtt_POSIX - rtt_servo_manager + rtt_servo_control rtt_utc_UTest rtt_utestcases ) diff --git a/applications/SConscript b/applications/SConscript index 9bb9aba..1de3b0f 100644 --- a/applications/SConscript +++ b/applications/SConscript @@ -3,6 +3,7 @@ import os cwd = GetCurrentDir() src = Glob('*.c') +src += Glob('*.cpp') CPPPATH = [cwd] group = DefineGroup('Applications', src, depend = [''], CPPPATH = CPPPATH) diff --git a/applications/common/SConscript b/applications/common/SConscript new file mode 100644 index 0000000..882e644 --- /dev/null +++ b/applications/common/SConscript @@ -0,0 +1,10 @@ +from building import * +import os + +cwd = GetCurrentDir() +src = Glob('curve/src/*.cpp') +CPPPATH = [cwd] + +group = DefineGroup('common', src, depend = [''], CPPPATH = CPPPATH) + +Return('group') diff --git a/applications/common/curve/include/s_curve.h b/applications/common/curve/include/s_curve.h new file mode 100644 index 0000000..3c8a980 --- /dev/null +++ b/applications/common/curve/include/s_curve.h @@ -0,0 +1,167 @@ +// +// Created by lgv on 2026/3/2. +// +/** + * @file s_curve.h + * @brief 严格点到点位置 S 曲线轨迹生成器 + * + * 实现标准 7 段式位置 S 曲线速度剖面： + * 1. 加加速度上升段（加速度逐渐增大） + * 2. 匀加速段 + * 3. 加加速度下降段（加速度逐渐降到 0） + * 4. 匀速巡航段 + * 5. 加加速度下降段（减速度逐渐增大） + * 6. 匀减速段 + * 7. 加加速度上升段（减速度逐渐降到 0） + * + * 本类只负责严格点到点 profile 规划与采样，不负责实时目标跟踪。 + */ + +#pragma once + +#include +#include +#include + + +namespace cmvr +{ + +/** + * @brief 单次点到点运动的 S 曲线剖面参数 + */ +struct SCurveProfile +{ + // 7 个阶段的持续时间 + double t1; // 加加速度上升时间（加速阶段） + double t2; // 匀加速时间 + double t3; // 加加速度下降时间（加速阶段结束） + double t4; // 匀速时间（巡航） + double t5; // 加加速度下降时间（减速阶段） + double t6; // 匀减速时间 + double t7; // 加加速度上升时间（减速阶段结束） + + double total_time; + + // 运动约束 + double j_max; // 最大加加速度 (rad/s³) + double a_max; // 最大加速度 (rad/s²) + double v_max; // 最大速度 (rad/s) + + // 运动参数 + double distance; // 运动总距离 + double direction; // +1 或 -1 + double v_cruise; // 实际达到的巡航速度 + double a_limit; // 实际达到的加速度上限 + + // 初始条件 + double p0; // 初始位置 + double v0; // 初始速度 + double a0; // 初始加速度 + double vf; // 终止速度 + + SCurveProfile() + : t1(0), t2(0), t3(0), t4(0), t5(0), t6(0), t7(0) + , total_time(0) + , j_max(50.0), a_max(10.0), v_max(3.0) + , distance(0), direction(1.0), v_cruise(0), a_limit(0) + , p0(0), v0(0), a0(0), vf(0) + {} +}; + +/** + * @brief 严格点到点位置 S 曲线规划器 + */ +class SCurve +{ +public: + /** + * @brief 构造函数（带运动约束） + * @param max_velocity 最大速度 (rad/s) + * @param max_acceleration 最大加速度 (rad/s²) + * @param max_jerk 最大加加速度 (rad/s³) + */ + SCurve(double max_velocity = 3.0, + double max_acceleration = 10.0, + double max_jerk = 50.0); + + /** + * @brief 设置运动约束 + */ + void setConstraints(double max_velocity, double max_acceleration, double max_jerk); + + /** + * @brief 获取当前运动约束 + */ + void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const; + + /** + * @brief 计算点到点运动的完整 S 曲线剖面 + * @param start_position 起始位置 + * @param end_position 终止位置 + * @param start_velocity 起始速度（默认 0） + * @param end_velocity 终止速度（默认 0） + * @return 计算得到的剖面参数 + */ + SCurveProfile calculateProfile(double start_position, double end_position, + double start_velocity = 0.0, double end_velocity = 0.0); + + /** + * @brief 给定剖面下，获取时刻 t 的位置 + * @param profile S 曲线剖面 + * @param t 从起点开始的时间 + * @return 时刻 t 的位置 + */ + double getPositionAtTime(const SCurveProfile& profile, double t) const; + + /** + * @brief 给定剖面下，获取时刻 t 的速度 + * @param profile S 曲线剖面 + * @param t 从起点开始的时间 + * @return 时刻 t 的速度 + */ + double getVelocityAtTime(const SCurveProfile& profile, double t) const; + + /** + * @brief 给定剖面下，获取时刻 t 的加速度 + * @param profile S 曲线剖面 + * @param t 从起点开始的时间 + * @return 时刻 t 的加速度 + */ + double getAccelerationAtTime(const SCurveProfile& profile, double t) const; + + /** + * @brief 给定剖面下，获取时刻 t 的加加速度 + * @param profile S 曲线剖面 + * @param t 从起点开始的时间 + * @return 时刻 t 的加加速度 + */ + double getJerkAtTime(const SCurveProfile& profile, double t) const; + + /** + * @brief 按固定时间间隔生成轨迹点 + * @param profile S 曲线剖面 + * @param dt 轨迹点时间间隔 + * @param positions 输出：每个时间点的位置 + * @param velocities 输出：每个时间点的速度 + * @param accelerations 输出：每个时间点的加速度 + */ + void generateTrajectory(const SCurveProfile& profile, double dt, + std::vector& positions, + std::vector& velocities, + std::vector& accelerations) const; + +private: + double max_velocity_; + double max_acceleration_; + double max_jerk_; + + static constexpr double EPSILON = 1e-9; + static constexpr double VELOCITY_THRESHOLD = 1e-6; + + double computeSegmentJerk(const SCurveProfile& profile, double t) const; + void calculateShortProfile(SCurveProfile& profile) const; + void calculateLongProfile(SCurveProfile& profile) const; +}; + +} // namespace cmvr diff --git a/applications/common/curve/src/s_curve.cpp b/applications/common/curve/src/s_curve.cpp new file mode 100644 index 0000000..e5025cc --- /dev/null +++ b/applications/common/curve/src/s_curve.cpp @@ -0,0 +1,542 @@ +// +// Created by lgv on 2026/3/2. +// + + +/** + * @file s_curve_generator.cpp + * @brief S 曲线轨迹生成器实现 + */ + +#include "common/curve/include/s_curve.h" +#include +#include +#include + +namespace cmvr +{ +SCurve::SCurve(double max_velocity, double max_acceleration, double max_jerk) + : max_velocity_(std::abs(max_velocity)) + , max_acceleration_(std::abs(max_acceleration)) + , max_jerk_(std::abs(max_jerk)) +{} + +void SCurve::setConstraints(double max_velocity, double max_acceleration, double max_jerk) +{ + max_velocity_ = std::abs(max_velocity); + max_acceleration_ = std::abs(max_acceleration); + max_jerk_ = std::abs(max_jerk); +} + +void SCurve::getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const +{ + max_velocity = max_velocity_; + max_acceleration = max_acceleration_; + max_jerk = max_jerk_; +} + +SCurveProfile SCurve::calculateProfile(double start_position, double end_position, + double start_velocity, double end_velocity) +{ + SCurveProfile profile; + profile.j_max = max_jerk_; + profile.a_max = max_acceleration_; + profile.v_max = max_velocity_; + profile.p0 = start_position; + profile.v0 = start_velocity; + profile.a0 = 0.0; // Assume starting from zero acceleration + profile.vf = end_velocity; + + double displacement = end_position - start_position; + profile.distance = std::abs(displacement); + profile.direction = (displacement >= 0) ? 1.0 : -1.0; + + if (profile.distance < EPSILON) { + // 无需运动 + profile.total_time = 0.0; + return profile; + } + + const double start_speed = profile.direction * start_velocity; + const double end_speed = profile.direction * end_velocity; + if (start_speed < -VELOCITY_THRESHOLD || end_speed < -VELOCITY_THRESHOLD) { + profile.total_time = 0.0; + return profile; + } + + if (std::abs(start_speed) > VELOCITY_THRESHOLD || + std::abs(end_speed) > VELOCITY_THRESHOLD) { + auto calc_transition = [&](double v_from, double v_to, + double& t_j, double& t_a, + double& duration, double& distance) { + t_j = 0.0; + t_a = 0.0; + duration = 0.0; + distance = 0.0; + + const double dv = std::abs(v_to - v_from); + if (dv <= 1e-12 || profile.a_max <= 1e-12 || profile.j_max <= 1e-12) { + return; + } + + const double dv_switch = profile.a_max * profile.a_max / profile.j_max; + if (dv < dv_switch) { + t_j = std::sqrt(dv / profile.j_max); + t_a = 0.0; + } else { + t_j = profile.a_max / profile.j_max; + t_a = dv / profile.a_max - t_j; + } + + duration = 2.0 * t_j + t_a; + distance = 0.5 * (v_from + v_to) * duration; + }; + + const double v_cap = std::max({profile.v_max, start_speed, end_speed}); + const double v_peak_min = std::max(start_speed, end_speed); + + double acc_tj = 0.0, acc_ta = 0.0, acc_duration = 0.0, acc_distance = 0.0; + double dec_tj = 0.0, dec_ta = 0.0, dec_duration = 0.0, dec_distance = 0.0; + calc_transition(start_speed, v_peak_min, + acc_tj, acc_ta, acc_duration, acc_distance); + calc_transition(v_peak_min, end_speed, + dec_tj, dec_ta, dec_duration, dec_distance); + const double d_min = acc_distance + dec_distance; + if (profile.distance + EPSILON < d_min) { + profile.total_time = 0.0; + return profile; + } + + calc_transition(start_speed, v_cap, + acc_tj, acc_ta, acc_duration, acc_distance); + calc_transition(v_cap, end_speed, + dec_tj, dec_ta, dec_duration, dec_distance); + const double d_cap = acc_distance + dec_distance; + + double v_peak = v_cap; + double t4 = 0.0; + + if (profile.distance > d_cap + EPSILON) { + t4 = (profile.distance - d_cap) / std::max(v_peak, EPSILON); + } else { + double lo = v_peak_min; + double hi = v_cap; + for (int iter = 0; iter < 80; ++iter) { + const double mid = 0.5 * (lo + hi); + double mid_acc_tj = 0.0, mid_acc_ta = 0.0, mid_acc_duration = 0.0, mid_acc_distance = 0.0; + double mid_dec_tj = 0.0, mid_dec_ta = 0.0, mid_dec_duration = 0.0, mid_dec_distance = 0.0; + calc_transition(start_speed, mid, + mid_acc_tj, mid_acc_ta, mid_acc_duration, mid_acc_distance); + calc_transition(mid, end_speed, + mid_dec_tj, mid_dec_ta, mid_dec_duration, mid_dec_distance); + const double d_mid = mid_acc_distance + mid_dec_distance; + if (d_mid < profile.distance) { + lo = mid; + } else { + hi = mid; + } + } + v_peak = hi; + calc_transition(start_speed, v_peak, + acc_tj, acc_ta, acc_duration, acc_distance); + calc_transition(v_peak, end_speed, + dec_tj, dec_ta, dec_duration, dec_distance); + } + + profile.t1 = acc_tj; + profile.t2 = acc_ta; + profile.t3 = acc_tj; + profile.t4 = t4; + profile.t5 = dec_tj; + profile.t6 = dec_ta; + profile.t7 = dec_tj; + profile.v_cruise = v_peak; + profile.a_limit = std::max(profile.j_max * profile.t1, profile.j_max * profile.t5); + profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 + + profile.t5 + profile.t6 + profile.t7; + return profile; + } + + const double j = profile.j_max; + const double a = profile.a_max; + const double v = profile.v_max; + const double t_j_a = a / j; + + if (v < a * t_j_a) { + const double t_j_v = std::sqrt(v / j); + const double s_min = 2.0 * j * t_j_v * t_j_v * t_j_v; + if (profile.distance > s_min + EPSILON) { + calculateLongProfile(profile); + } else { + calculateShortProfile(profile); + } + } else { + const double t_a_v = v / a - t_j_a; + const double s_half_v = + a * (t_j_a * t_j_a + 1.5 * t_j_a * t_a_v + 0.5 * t_a_v * t_a_v); + const double s_min = 2.0 * s_half_v; + if (profile.distance > s_min + EPSILON) { + calculateLongProfile(profile); + } else { + calculateShortProfile(profile); + } + } + + return profile; +} + +void SCurve::calculateShortProfile(SCurveProfile& profile) const +{ + // 短距离：无巡航段（t4 = 0），与 moveL_SCurveLocal 中的 SCurveProfile1D 保持一致。 + const double j = profile.j_max; + const double a = profile.a_max; + const double v = profile.v_max; + const double d = profile.distance; + const double t_j_a = a / j; + + if (v < a * t_j_a) { + // 达不到 a_max：极短距离时只有 4 个 jerk 段。 + const double t1 = std::cbrt(d / (2.0 * j)); + profile.t1 = t1; + profile.t2 = 0.0; + profile.t3 = t1; + profile.t4 = 0.0; + profile.t5 = t1; + profile.t6 = 0.0; + profile.t7 = t1; + profile.v_cruise = j * t1 * t1; + profile.a_limit = j * t1; + } else { + // 能达到 a_max，但达不到 v_max：求无巡航的匀加速持续时间。 + const double D = t_j_a * t_j_a + 4.0 * (d / a); + double t2 = (-3.0 * t_j_a + std::sqrt(std::max(0.0, D))) * 0.5; + + if (t2 < 0.0) { + const double t1 = std::cbrt(d / (2.0 * j)); + profile.t1 = t1; + profile.t2 = 0.0; + profile.t3 = t1; + profile.t4 = 0.0; + profile.t5 = t1; + profile.t6 = 0.0; + profile.t7 = t1; + profile.v_cruise = j * t1 * t1; + profile.a_limit = j * t1; + } else { + profile.t1 = t_j_a; + profile.t2 = t2; + profile.t3 = t_j_a; + profile.t4 = 0.0; + profile.t5 = t_j_a; + profile.t6 = t2; + profile.t7 = t_j_a; + profile.v_cruise = a * (t2 + t_j_a); + profile.a_limit = a; + } + } + + profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 + + profile.t5 + profile.t6 + profile.t7; +} + +void SCurve::calculateLongProfile(SCurveProfile& profile) const +{ + // 含巡航段的完整 7 段式 S 曲线；与 SCurveProfile1D 的 full-profile 公式一致。 + const double j = profile.j_max; + const double a = profile.a_max; + const double v = profile.v_max; + const double d = profile.distance; + const double t_j_a = a / j; + + if (v < a * t_j_a) { + // 达不到 a_max，但可以达到 v_max 并进入巡航。 + const double t_j_v = std::sqrt(v / j); + const double s_min = 2.0 * j * t_j_v * t_j_v * t_j_v; + profile.t1 = t_j_v; + profile.t2 = 0.0; + profile.t3 = t_j_v; + profile.t4 = std::max(0.0, (d - s_min) / v); + profile.t5 = t_j_v; + profile.t6 = 0.0; + profile.t7 = t_j_v; + profile.v_cruise = v; + profile.a_limit = j * t_j_v; + } else { + // 能达到 a_max 和 v_max。 + const double t_a = v / a - t_j_a; + const double s_half = + a * (t_j_a * t_j_a + 1.5 * t_j_a * t_a + 0.5 * t_a * t_a); + const double s_min = 2.0 * s_half; + + profile.t1 = t_j_a; + profile.t2 = t_a; + profile.t3 = t_j_a; + profile.t4 = std::max(0.0, (d - s_min) / v); + profile.t5 = t_j_a; + profile.t6 = t_a; + profile.t7 = t_j_a; + profile.v_cruise = v; + profile.a_limit = a; + } + + profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 + + profile.t5 + profile.t6 + profile.t7; +} + +double SCurve::computeSegmentJerk(const SCurveProfile& profile, double t) const +{ + double j = profile.j_max * profile.direction; + + // 判断所处阶段 + double t_end1 = profile.t1; + double t_end2 = t_end1 + profile.t2; + double t_end3 = t_end2 + profile.t3; + double t_end4 = t_end3 + profile.t4; + double t_end5 = t_end4 + profile.t5; + double t_end6 = t_end5 + profile.t6; + // double t_end7 = t_end6 + profile.t7; // = total_time + + if (t < t_end1) { + return j; // 段 1：正加加速度（加速） + } else if (t < t_end2) { + return 0.0; // 段 2：加加速度为 0（匀加速） + } else if (t < t_end3) { + return -j; // 段 3：负加加速度（减小加速度） + } else if (t < t_end4) { + return 0.0; // 段 4：加加速度为 0（巡航） + } else if (t < t_end5) { + return -j; // 段 5：负加加速度（开始减速） + } else if (t < t_end6) { + return 0.0; // 段 6：加加速度为 0（匀减速） + } else { + return j; // 段 7：正加加速度（减速结束） + } +} + +double SCurve::getPositionAtTime(const SCurveProfile& profile, double t) const +{ + if (t <= 0) return profile.p0; + if (t >= profile.total_time) return profile.p0 + profile.distance * profile.direction; + + double j = profile.j_max * profile.direction; + double p = profile.p0; + double v = profile.v0; + double a = profile.a0; + + // 时间边界 + double t_end1 = profile.t1; + double t_end2 = t_end1 + profile.t2; + double t_end3 = t_end2 + profile.t3; + double t_end4 = t_end3 + profile.t4; + double t_end5 = t_end4 + profile.t5; + double t_end6 = t_end5 + profile.t6; + + // 依次处理各阶段 + auto processSegment = [&](double dt, double jerk) { + p += v * dt + 0.5 * a * dt * dt + (1.0/6.0) * jerk * dt * dt * dt; + v += a * dt + 0.5 * jerk * dt * dt; + a += jerk * dt; + }; + + // 段 1 + if (t <= t_end1) { + processSegment(t, j); + return p; + } + processSegment(profile.t1, j); + + // 段 2 + if (t <= t_end2) { + processSegment(t - t_end1, 0.0); + return p; + } + processSegment(profile.t2, 0.0); + + // 段 3 + if (t <= t_end3) { + processSegment(t - t_end2, -j); + return p; + } + processSegment(profile.t3, -j); + + // 段 4（巡航） + if (t <= t_end4) { + processSegment(t - t_end3, 0.0); + return p; + } + processSegment(profile.t4, 0.0); + + // 段 5 + if (t <= t_end5) { + processSegment(t - t_end4, -j); + return p; + } + processSegment(profile.t5, -j); + + // 段 6 + if (t <= t_end6) { + processSegment(t - t_end5, 0.0); + return p; + } + processSegment(profile.t6, 0.0); + + // 段 7 + processSegment(t - t_end6, j); + return p; +} + +double SCurve::getVelocityAtTime(const SCurveProfile& profile, double t) const +{ + if (t <= 0) return profile.v0; + if (t >= profile.total_time) return profile.vf; + + double j = profile.j_max * profile.direction; + double v = profile.v0; + double a = profile.a0; + + // 时间边界 + double t_end1 = profile.t1; + double t_end2 = t_end1 + profile.t2; + double t_end3 = t_end2 + profile.t3; + double t_end4 = t_end3 + profile.t4; + double t_end5 = t_end4 + profile.t5; + double t_end6 = t_end5 + profile.t6; + + auto processSegment = [&](double dt, double jerk) { + v += a * dt + 0.5 * jerk * dt * dt; + a += jerk * dt; + }; + + if (t <= t_end1) { + v += a * t + 0.5 * j * t * t; + return v; + } + processSegment(profile.t1, j); + + if (t <= t_end2) { + v += a * (t - t_end1); + return v; + } + processSegment(profile.t2, 0.0); + + if (t <= t_end3) { + double dt = t - t_end2; + v += a * dt + 0.5 * (-j) * dt * dt; + return v; + } + processSegment(profile.t3, -j); + + if (t <= t_end4) { + v += a * (t - t_end3); + return v; + } + processSegment(profile.t4, 0.0); + + if (t <= t_end5) { + double dt = t - t_end4; + v += a * dt + 0.5 * (-j) * dt * dt; + return v; + } + processSegment(profile.t5, -j); + + if (t <= t_end6) { + v += a * (t - t_end5); + return v; + } + processSegment(profile.t6, 0.0); + + double dt = t - t_end6; + v += a * dt + 0.5 * j * dt * dt; + return v; +} + +double SCurve::getAccelerationAtTime(const SCurveProfile& profile, double t) const +{ + if (t <= 0 || t >= profile.total_time) return 0.0; + + double j = profile.j_max * profile.direction; + double a = profile.a0; + + // 时间边界 + double t_end1 = profile.t1; + double t_end2 = t_end1 + profile.t2; + double t_end3 = t_end2 + profile.t3; + double t_end4 = t_end3 + profile.t4; + double t_end5 = t_end4 + profile.t5; + double t_end6 = t_end5 + profile.t6; + + if (t <= t_end1) { + return a + j * t; + } + a += j * profile.t1; + + if (t <= t_end2) { + return a; + } + + if (t <= t_end3) { + return a + (-j) * (t - t_end2); + } + a += (-j) * profile.t3; + + if (t <= t_end4) { + return a; // Should be ~0 + } + + if (t <= t_end5) { + return a + (-j) * (t - t_end4); + } + a += (-j) * profile.t5; + + if (t <= t_end6) { + return a; + } + + return a + j * (t - t_end6); +} + +double SCurve::getJerkAtTime(const SCurveProfile& profile, double t) const +{ + if (t <= 0.0 || t >= profile.total_time) { + return 0.0; + } + return computeSegmentJerk(profile, t); +} + +void SCurve::generateTrajectory(const SCurveProfile& profile, double dt, + std::vector& positions, + std::vector& velocities, + std::vector& accelerations) const +{ + positions.clear(); + velocities.clear(); + accelerations.clear(); + + if (profile.total_time <= 0 || dt <= 0) { + positions.push_back(profile.p0); + velocities.push_back(profile.v0); + accelerations.push_back(profile.a0); + return; + } + + int num_points = static_cast(std::ceil(profile.total_time / dt)) + 1; + positions.reserve(num_points); + velocities.reserve(num_points); + accelerations.reserve(num_points); + + for (double t = 0; t <= profile.total_time; t += dt) { + positions.push_back(getPositionAtTime(profile, t)); + velocities.push_back(getVelocityAtTime(profile, t)); + accelerations.push_back(getAccelerationAtTime(profile, t)); + } + + // 确保包含最终点 + if (positions.empty() || + std::abs(positions.back() - (profile.p0 + profile.distance * profile.direction)) > EPSILON) { + positions.push_back(profile.p0 + profile.distance * profile.direction); + velocities.push_back(profile.vf); + accelerations.push_back(0.0); + } +} + +} diff --git a/applications/common/type/common_type.h b/applications/common/type/common_type.h new file mode 100644 index 0000000..06196be --- /dev/null +++ b/applications/common/type/common_type.h @@ -0,0 +1,9 @@ +// +// Created by Administrator on 2026/4/22. +// + +#ifndef RTTHREAD_COMMON_TYPE_H +#define RTTHREAD_COMMON_TYPE_H +#define PI (3.14159265358979323846) + +#endif //RTTHREAD_COMMON_TYPE_H diff --git a/applications/common/type/servo_types.h b/applications/common/type/servo_types.h new file mode 100644 index 0000000..236fd59 --- /dev/null +++ b/applications/common/type/servo_types.h @@ -0,0 +1,13 @@ +// +// Created by Administrator on 2026/4/22. +// + +#ifndef RTTHREAD_SERVO_TYPES_H +#define RTTHREAD_SERVO_TYPES_H +enum class MotionMode +{ + Immediate, // 直接到达 + SCurve // S 曲线 +}; + +#endif //RTTHREAD_SERVO_TYPES_H diff --git a/applications/erpc/SConscript b/applications/erpc/SConscript index c927521..a96b5b8 100644 --- a/applications/erpc/SConscript +++ b/applications/erpc/SConscript @@ -22,13 +22,13 @@ if GetDepend(['ERPC_MODE_CLIENT']): gen_dir = p('proto', 'generated') # interface 建议始终编译（两端都可能需要） -src += [p('proto', 'generated', 'face_servo_interface.cpp')] +src += [p('proto', 'generated', 'servo_service_interface.cpp')] src += [p('common', 'src', 'erpc_error_handler.cpp')] if GetDepend(['ERPC_MODE_SERVER']): - src += [p('proto', 'generated', 'face_servo_server.cpp')] + src += [p('proto', 'generated', 'servo_service_server.cpp')] if GetDepend(['ERPC_MODE_CLIENT']): - src += [p('proto', 'generated', 'face_servo_client.cpp')] + src += [p('proto', 'generated', 'servo_service_client.cpp')] CPPPATH = [ cwd, diff --git a/applications/erpc/proto/face_servo.erpc b/applications/erpc/proto/face_servo.erpc deleted file mode 100644 index b6f538d..0000000 --- a/applications/erpc/proto/face_servo.erpc +++ /dev/null @@ -1,30 +0,0 @@ -// face_servo.erpc -// 生成命令 .\erpcgen.exe -o .\generated\ -g c .\face_servo.erpc -// .\erpcgen.exe -g py .\face_servo.erpc - -program face_servo - -// 舵机ID字符串最大长度 -const int32 kServoIdMaxLen = 32 - -// 最多支持的舵机数量 -const int32 kMaxServoCount = 48 - -// 按 ID 设置角度的命令 -struct ServoCmd { - string id @max_length(kServoIdMaxLen) - float angle_deg -} - -interface FaceServo -{ - // 接口1：一次性设置所有舵机角度（可变长度） - // angles_deg 的顺序由固件映射表顺序定义 - setAllAngles(list angles_deg @max_length(kMaxServoCount)) -> int32 - - // 接口2：按字符串ID批量设置角度（可变长度） - setAngles(list cmds @max_length(kMaxServoCount)) -> int32 - - // 单个：按字符串ID设置角度 - setAngle(string id @max_length(kServoIdMaxLen), float angle_deg) -> int32 -} diff --git a/applications/erpc/proto/generated/c_face_servo_client.cpp b/applications/erpc/proto/generated/c_face_servo_client.cpp deleted file mode 100644 index 8619239..0000000 --- a/applications/erpc/proto/generated/c_face_servo_client.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. - * - * AUTOGENERATED - DO NOT EDIT - */ - - -#include "c_face_servo_client.h" -#include "face_servo_client.hpp" -#include "erpc_manually_constructed.hpp" - -using namespace erpc; -using namespace std; -using namespace erpcShim; - - -#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC -FaceServo_client *s_FaceServo_client = nullptr; -#else -ERPC_MANUALLY_CONSTRUCTED_STATIC(FaceServo_client, s_FaceServo_client); -#endif - -int32_t setAllAngles(const list_float_1_t * angles_deg) -{ - int32_t result; - result = s_FaceServo_client->setAllAngles(angles_deg); - - return result; -} - -int32_t setAngles(const list_ServoCmd_1_t * cmds) -{ - int32_t result; - result = s_FaceServo_client->setAngles(cmds); - - return result; -} - -int32_t setAngle(const char * id, float angle_deg) -{ - int32_t result; - result = s_FaceServo_client->setAngle(id, angle_deg); - - return result; -} - -void initFaceServo_client(erpc_client_t client) -{ -#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC - erpc_assert(s_FaceServo_client == nullptr); - s_FaceServo_client = new FaceServo_client(reinterpret_cast(client)); -#else - erpc_assert(!s_FaceServo_client.isUsed()); - s_FaceServo_client.construct(reinterpret_cast(client)); -#endif -} - -void deinitFaceServo_client(void) -{ -#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC - if (s_FaceServo_client != nullptr) - { - delete s_FaceServo_client; - s_FaceServo_client = nullptr; - } -#else - s_FaceServo_client.destroy(); -#endif -} diff --git a/applications/erpc/proto/generated/c_face_servo_client.h b/applications/erpc/proto/generated/c_face_servo_client.h deleted file mode 100644 index f553433..0000000 --- a/applications/erpc/proto/generated/c_face_servo_client.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. - * - * AUTOGENERATED - DO NOT EDIT - */ - - -#if !defined(_c_face_servo_client_h_) -#define _c_face_servo_client_h_ - -#include "face_servo_common.h" -#include "erpc_client_manager.h" - -#if defined(__cplusplus) -extern "C" -{ -#endif - -#if !defined(ERPC_FUNCTIONS_DEFINITIONS) -#define ERPC_FUNCTIONS_DEFINITIONS - - -/*! @brief FaceServo identifiers */ -enum _FaceServo_ids -{ - kFaceServo_service_id = 1, - kFaceServo_setAllAngles_id = 1, - kFaceServo_setAngles_id = 2, - kFaceServo_setAngle_id = 3, -}; - -//! @name FaceServo -//@{ -int32_t setAllAngles(const list_float_1_t * angles_deg); - -int32_t setAngles(const list_ServoCmd_1_t * cmds); - -int32_t setAngle(const char * id, float angle_deg); -//@} - -#endif // ERPC_FUNCTIONS_DEFINITIONS - -void initFaceServo_client(erpc_client_t client); - -void deinitFaceServo_client(void); - -#if defined(__cplusplus) -} -#endif - -#endif // _c_face_servo_client_h_ diff --git a/applications/erpc/proto/generated/c_face_servo_server.cpp b/applications/erpc/proto/generated/c_face_servo_server.cpp deleted file mode 100644 index 9566306..0000000 --- a/applications/erpc/proto/generated/c_face_servo_server.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. - * - * AUTOGENERATED - DO NOT EDIT - */ - - -#include -#include "c_face_servo_server.h" -#include "face_servo_server.hpp" -#include "erpc_manually_constructed.hpp" - -using namespace erpc; -using namespace std; -using namespace erpcShim; - - -class FaceServo_server: public FaceServo_interface -{ - public: - virtual ~FaceServo_server() {}; - - - int32_t setAllAngles(const list_float_1_t * angles_deg) - { - int32_t result; - result = ::setAllAngles(angles_deg); - - return result; - } - - int32_t setAngles(const list_ServoCmd_1_t * cmds) - { - int32_t result; - result = ::setAngles(cmds); - - return result; - } - - int32_t setAngle(const char * id, float angle_deg) - { - int32_t result; - result = ::setAngle(id, angle_deg); - - return result; - } -}; - -ERPC_MANUALLY_CONSTRUCTED_STATIC(FaceServo_service, s_FaceServo_service); -ERPC_MANUALLY_CONSTRUCTED_STATIC(FaceServo_server, s_FaceServo_server); - -erpc_service_t create_FaceServo_service(void) -{ - erpc_service_t service; - -#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC - service = new (nothrow) FaceServo_service(new (nothrow)FaceServo_server()); -#else - if (s_FaceServo_service.isUsed()) - { - service = NULL; - } - else - { - s_FaceServo_server.construct(); - s_FaceServo_service.construct(s_FaceServo_server.get()); - service = s_FaceServo_service.get(); - } -#endif - - return service; -} - -void destroy_FaceServo_service(erpc_service_t service) -{ -#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC - if (service) - { - delete (FaceServo_server *)(((FaceServo_service *)service)->getHandler()); - delete (FaceServo_service *)service; - } -#else - (void)service; - erpc_assert(service == s_FaceServo_service.get()); - s_FaceServo_service.destroy(); - s_FaceServo_server.destroy(); -#endif -} - diff --git a/applications/erpc/proto/generated/c_face_servo_server.h b/applications/erpc/proto/generated/c_face_servo_server.h deleted file mode 100644 index e81ad53..0000000 --- a/applications/erpc/proto/generated/c_face_servo_server.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. - * - * AUTOGENERATED - DO NOT EDIT - */ - - -#if !defined(_c_face_servo_server_h_) -#define _c_face_servo_server_h_ - -#include "face_servo_common.h" - -#if defined(__cplusplus) -extern "C" -{ -#endif - -typedef void * erpc_service_t; - -#if !defined(ERPC_FUNCTIONS_DEFINITIONS) -#define ERPC_FUNCTIONS_DEFINITIONS - - -/*! @brief FaceServo identifiers */ -enum _FaceServo_ids -{ - kFaceServo_service_id = 1, - kFaceServo_setAllAngles_id = 1, - kFaceServo_setAngles_id = 2, - kFaceServo_setAngle_id = 3, -}; - -//! @name FaceServo -//@{ -int32_t setAllAngles(const list_float_1_t * angles_deg); - -int32_t setAngles(const list_ServoCmd_1_t * cmds); - -int32_t setAngle(const char * id, float angle_deg); -//@} - - -#endif // ERPC_FUNCTIONS_DEFINITIONS - -/*! @brief Return FaceServo_service service object. */ -erpc_service_t create_FaceServo_service(void); - -/*! @brief Destroy FaceServo_service service object. */ -void destroy_FaceServo_service(erpc_service_t service); - - -#if defined(__cplusplus) -} -#endif - -#endif // _c_face_servo_server_h_ diff --git a/applications/erpc/proto/generated/c_servo_service_client.cpp b/applications/erpc/proto/generated/c_servo_service_client.cpp new file mode 100644 index 0000000..42bf2fc --- /dev/null +++ b/applications/erpc/proto/generated/c_servo_service_client.cpp @@ -0,0 +1,85 @@ +/* + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * + * AUTOGENERATED - DO NOT EDIT + */ + + +#include "c_servo_service_client.h" +#include "servo_service_client.hpp" +#include "erpc_manually_constructed.hpp" + +using namespace erpc; +using namespace std; +using namespace erpcShim; + + +#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC +servo_service_client *s_servo_service_client = nullptr; +#else +ERPC_MANUALLY_CONSTRUCTED_STATIC(servo_service_client, s_servo_service_client); +#endif + +bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad) +{ + bool result; + result = s_servo_service_client->setConstraints(id, max_velocity_rad, max_acceleration_rad, max_jerk_rad); + + return result; +} + +bool setMode(RpcMotionMode mode) +{ + bool result; + result = s_servo_service_client->setMode(mode); + + return result; +} + +bool setUpdatePeriodMs(uint32_t ms) +{ + bool result; + result = s_servo_service_client->setUpdatePeriodMs(ms); + + return result; +} + +int32_t move(const list_ServoCmd_1_t * cmds) +{ + int32_t result; + result = s_servo_service_client->move(cmds); + + return result; +} + +int32_t moveJ(const list_float_1_t * angles_rad) +{ + int32_t result; + result = s_servo_service_client->moveJ(angles_rad); + + return result; +} + +void initservo_service_client(erpc_client_t client) +{ +#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC + erpc_assert(s_servo_service_client == nullptr); + s_servo_service_client = new servo_service_client(reinterpret_cast(client)); +#else + erpc_assert(!s_servo_service_client.isUsed()); + s_servo_service_client.construct(reinterpret_cast(client)); +#endif +} + +void deinitservo_service_client(void) +{ +#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC + if (s_servo_service_client != nullptr) + { + delete s_servo_service_client; + s_servo_service_client = nullptr; + } +#else + s_servo_service_client.destroy(); +#endif +} diff --git a/applications/erpc/proto/generated/c_servo_service_client.h b/applications/erpc/proto/generated/c_servo_service_client.h new file mode 100644 index 0000000..bea4765 --- /dev/null +++ b/applications/erpc/proto/generated/c_servo_service_client.h @@ -0,0 +1,57 @@ +/* + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * + * AUTOGENERATED - DO NOT EDIT + */ + + +#if !defined(_c_servo_service_client_h_) +#define _c_servo_service_client_h_ + +#include "servo_service_common.h" +#include "erpc_client_manager.h" + +#if defined(__cplusplus) +extern "C" +{ +#endif + +#if !defined(ERPC_FUNCTIONS_DEFINITIONS) +#define ERPC_FUNCTIONS_DEFINITIONS + + +/*! @brief servo_service identifiers */ +enum _servo_service_ids +{ + kservo_service_service_id = 1, + kservo_service_setConstraints_id = 1, + kservo_service_setMode_id = 2, + kservo_service_setUpdatePeriodMs_id = 3, + kservo_service_move_id = 4, + kservo_service_moveJ_id = 5, +}; + +//! @name servo_service +//@{ +bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad); + +bool setMode(RpcMotionMode mode); + +bool setUpdatePeriodMs(uint32_t ms); + +int32_t move(const list_ServoCmd_1_t * cmds); + +int32_t moveJ(const list_float_1_t * angles_rad); +//@} + +#endif // ERPC_FUNCTIONS_DEFINITIONS + +void initservo_service_client(erpc_client_t client); + +void deinitservo_service_client(void); + +#if defined(__cplusplus) +} +#endif + +#endif // _c_servo_service_client_h_ diff --git a/applications/erpc/proto/generated/c_servo_service_server.cpp b/applications/erpc/proto/generated/c_servo_service_server.cpp new file mode 100644 index 0000000..bc8fadd --- /dev/null +++ b/applications/erpc/proto/generated/c_servo_service_server.cpp @@ -0,0 +1,105 @@ +/* + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * + * AUTOGENERATED - DO NOT EDIT + */ + + +#include +#include "c_servo_service_server.h" +#include "servo_service_server.hpp" +#include "erpc_manually_constructed.hpp" + +using namespace erpc; +using namespace std; +using namespace erpcShim; + + +class servo_service_server: public servo_service_interface +{ + public: + virtual ~servo_service_server() {}; + + + bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad) + { + bool result; + result = ::setConstraints(id, max_velocity_rad, max_acceleration_rad, max_jerk_rad); + + return result; + } + + bool setMode(RpcMotionMode mode) + { + bool result; + result = ::setMode(mode); + + return result; + } + + bool setUpdatePeriodMs(uint32_t ms) + { + bool result; + result = ::setUpdatePeriodMs(ms); + + return result; + } + + int32_t move(const list_ServoCmd_1_t * cmds) + { + int32_t result; + result = ::move(cmds); + + return result; + } + + int32_t moveJ(const list_float_1_t * angles_rad) + { + int32_t result; + result = ::moveJ(angles_rad); + + return result; + } +}; + +ERPC_MANUALLY_CONSTRUCTED_STATIC(servo_service_service, s_servo_service_service); +ERPC_MANUALLY_CONSTRUCTED_STATIC(servo_service_server, s_servo_service_server); + +erpc_service_t create_servo_service_service(void) +{ + erpc_service_t service; + +#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC + service = new (nothrow) servo_service_service(new (nothrow)servo_service_server()); +#else + if (s_servo_service_service.isUsed()) + { + service = NULL; + } + else + { + s_servo_service_server.construct(); + s_servo_service_service.construct(s_servo_service_server.get()); + service = s_servo_service_service.get(); + } +#endif + + return service; +} + +void destroy_servo_service_service(erpc_service_t service) +{ +#if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC + if (service) + { + delete (servo_service_server *)(((servo_service_service *)service)->getHandler()); + delete (servo_service_service *)service; + } +#else + (void)service; + erpc_assert(service == s_servo_service_service.get()); + s_servo_service_service.destroy(); + s_servo_service_server.destroy(); +#endif +} + diff --git a/applications/erpc/proto/generated/c_servo_service_server.h b/applications/erpc/proto/generated/c_servo_service_server.h new file mode 100644 index 0000000..3ff8cf8 --- /dev/null +++ b/applications/erpc/proto/generated/c_servo_service_server.h @@ -0,0 +1,62 @@ +/* + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * + * AUTOGENERATED - DO NOT EDIT + */ + + +#if !defined(_c_servo_service_server_h_) +#define _c_servo_service_server_h_ + +#include "servo_service_common.h" + +#if defined(__cplusplus) +extern "C" +{ +#endif + +typedef void * erpc_service_t; + +#if !defined(ERPC_FUNCTIONS_DEFINITIONS) +#define ERPC_FUNCTIONS_DEFINITIONS + + +/*! @brief servo_service identifiers */ +enum _servo_service_ids +{ + kservo_service_service_id = 1, + kservo_service_setConstraints_id = 1, + kservo_service_setMode_id = 2, + kservo_service_setUpdatePeriodMs_id = 3, + kservo_service_move_id = 4, + kservo_service_moveJ_id = 5, +}; + +//! @name servo_service +//@{ +bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad); + +bool setMode(RpcMotionMode mode); + +bool setUpdatePeriodMs(uint32_t ms); + +int32_t move(const list_ServoCmd_1_t * cmds); + +int32_t moveJ(const list_float_1_t * angles_rad); +//@} + + +#endif // ERPC_FUNCTIONS_DEFINITIONS + +/*! @brief Return servo_service_service service object. */ +erpc_service_t create_servo_service_service(void); + +/*! @brief Destroy servo_service_service service object. */ +void destroy_servo_service_service(erpc_service_t service); + + +#if defined(__cplusplus) +} +#endif + +#endif // _c_servo_service_server_h_ diff --git a/applications/erpc/proto/generated/face_servo_client.hpp b/applications/erpc/proto/generated/face_servo_client.hpp deleted file mode 100644 index ac87566..0000000 --- a/applications/erpc/proto/generated/face_servo_client.hpp +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. - * - * AUTOGENERATED - DO NOT EDIT - */ - - -#if !defined(_face_servo_client_hpp_) -#define _face_servo_client_hpp_ - -#include "face_servo_interface.hpp" - -#include "erpc_client_manager.h" - -namespace erpcShim -{ - -class FaceServo_client: public FaceServo_interface -{ - public: - FaceServo_client(erpc::ClientManager *manager); - - virtual ~FaceServo_client(); - - virtual int32_t setAllAngles(const list_float_1_t * angles_deg); - - virtual int32_t setAngles(const list_ServoCmd_1_t * cmds); - - virtual int32_t setAngle(const char * id, float angle_deg); - - protected: - erpc::ClientManager *m_clientManager; -}; - -} // erpcShim - - -#endif // _face_servo_client_hpp_ diff --git a/applications/erpc/proto/generated/face_servo_interface.hpp b/applications/erpc/proto/generated/face_servo_interface.hpp deleted file mode 100644 index 34d74e1..0000000 --- a/applications/erpc/proto/generated/face_servo_interface.hpp +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. - * - * AUTOGENERATED - DO NOT EDIT - */ - - -#if !defined(_face_servo_interface_hpp_) -#define _face_servo_interface_hpp_ - -#include "face_servo_common.hpp" - -namespace erpcShim -{ - - -// Abstract base class for FaceServo -class FaceServo_interface -{ - public: - static const uint8_t m_serviceId = 1; - static const uint8_t m_setAllAnglesId = 1; - static const uint8_t m_setAnglesId = 2; - static const uint8_t m_setAngleId = 3; - - virtual ~FaceServo_interface(void); - - virtual int32_t setAllAngles(const list_float_1_t * angles_deg) = 0; - - virtual int32_t setAngles(const list_ServoCmd_1_t * cmds) = 0; - - virtual int32_t setAngle(const char * id, float angle_deg) = 0; -private: -}; -} // erpcShim - - -#endif // _face_servo_interface_hpp_ diff --git a/applications/erpc/proto/generated/face_servo_server.hpp b/applications/erpc/proto/generated/face_servo_server.hpp deleted file mode 100644 index e7fc6e1..0000000 --- a/applications/erpc/proto/generated/face_servo_server.hpp +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. - * - * AUTOGENERATED - DO NOT EDIT - */ - - -#if !defined(_face_servo_server_hpp_) -#define _face_servo_server_hpp_ - -#include "face_servo_interface.hpp" - -#include "erpc_server.hpp" -#include "erpc_codec.hpp" - -#if 11400 != ERPC_VERSION_NUMBER -#error "The generated shim code version is different to the rest of eRPC code." -#endif - - -namespace erpcShim -{ - -/*! - * @brief Service subclass for FaceServo. - */ -class FaceServo_service : public erpc::Service -{ -public: - FaceServo_service(FaceServo_interface *_FaceServo_interface); - - virtual ~FaceServo_service(); - - /*! @brief return service interface handler. */ - FaceServo_interface* getHandler(void); - - /*! @brief Call the correct server shim based on method unique ID. */ - virtual erpc_status_t handleInvocation(uint32_t methodId, uint32_t sequence, erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport); - -private: - FaceServo_interface *m_handler; - /*! @brief Server shim for setAllAngles of FaceServo interface. */ - erpc_status_t setAllAngles_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence); - - /*! @brief Server shim for setAngles of FaceServo interface. */ - erpc_status_t setAngles_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence); - - /*! @brief Server shim for setAngle of FaceServo interface. */ - erpc_status_t setAngle_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence); -}; - -} // erpcShim - - -#endif // _face_servo_server_hpp_ diff --git a/applications/erpc/proto/generated/face_servo_client.cpp b/applications/erpc/proto/generated/servo_service_client.cpp similarity index 57% rename from applications/erpc/proto/generated/face_servo_client.cpp rename to applications/erpc/proto/generated/servo_service_client.cpp index 835e20d..276af2f 100644 --- a/applications/erpc/proto/generated/face_servo_client.cpp +++ b/applications/erpc/proto/generated/servo_service_client.cpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -9,7 +9,7 @@ #include "erpc_port.h" #endif #include "erpc_codec.hpp" -#include "face_servo_client.hpp" +#include "servo_service_client.hpp" #include "erpc_manually_constructed.hpp" #if 11400 != ERPC_VERSION_NUMBER @@ -20,45 +20,15 @@ using namespace erpc; using namespace std; using namespace erpcShim; - -// Constant variable definitions -#if defined(__MINGW32__) -__declspec( selectany ) -#else -#pragma weak kServoIdMaxLen -#endif -extern const int32_t kServoIdMaxLen = 32; -#if defined(__MINGW32__) -__declspec( selectany ) -#else -#pragma weak kMaxServoCount -#endif -extern const int32_t kMaxServoCount = 48; - -//! @brief Function to write struct list_float_1_t -static void write_list_float_1_t_struct(erpc::Codec * codec, const list_float_1_t * data); - //! @brief Function to write struct ServoCmd static void write_ServoCmd_struct(erpc::Codec * codec, const ServoCmd * data); //! @brief Function to write struct list_ServoCmd_1_t static void write_list_ServoCmd_1_t_struct(erpc::Codec * codec, const list_ServoCmd_1_t * data); +//! @brief Function to write struct list_float_1_t +static void write_list_float_1_t_struct(erpc::Codec * codec, const list_float_1_t * data); -// Write struct list_float_1_t function implementation -static void write_list_float_1_t_struct(erpc::Codec * codec, const list_float_1_t * data) -{ - if(NULL == data) - { - return; - } - - codec->startWriteList(data->elementsCount); - for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount) - { - codec->write(data->elements[listCount]); - } -} // Write struct ServoCmd function implementation static void write_ServoCmd_struct(erpc::Codec * codec, const ServoCmd * data) @@ -74,7 +44,7 @@ static void write_ServoCmd_struct(erpc::Codec * codec, const ServoCmd * data) codec->writeString(id_len, (const char*)data->id); } - codec->write(data->angle_deg); + codec->write(data->angle_rad); } // Write struct list_ServoCmd_1_t function implementation @@ -92,24 +62,39 @@ static void write_list_ServoCmd_1_t_struct(erpc::Codec * codec, const list_Servo } } +// Write struct list_float_1_t function implementation +static void write_list_float_1_t_struct(erpc::Codec * codec, const list_float_1_t * data) +{ + if(NULL == data) + { + return; + } + + codec->startWriteList(data->elementsCount); + for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount) + { + codec->write(data->elements[listCount]); + } +} -FaceServo_client::FaceServo_client(ClientManager *manager) + +servo_service_client::servo_service_client(ClientManager *manager) :m_clientManager(manager) { } -FaceServo_client::~FaceServo_client() +servo_service_client::~servo_service_client() { } -// FaceServo interface setAllAngles function client shim. -int32_t FaceServo_client::setAllAngles(const list_float_1_t * angles_deg) +// servo_service interface setConstraints function client shim. +bool servo_service_client::setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad) { erpc_status_t err = kErpcStatus_Success; - int32_t result; + bool result; #if ERPC_PRE_POST_ACTION pre_post_action_cb preCB = m_clientManager->getPreCB(); @@ -131,9 +116,19 @@ int32_t FaceServo_client::setAllAngles(const list_float_1_t * angles_deg) } else { - codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setAllAnglesId, request.getSequence()); + codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setConstraintsId, request.getSequence()); - write_list_float_1_t_struct(codec, angles_deg); + { + uint32_t id_len = strlen((const char*)id); + + codec->writeString(id_len, (const char*)id); + } + + codec->write(max_velocity_rad); + + codec->write(max_acceleration_rad); + + codec->write(max_jerk_rad); // Send message to server // Codec status is checked inside this function. @@ -148,7 +143,7 @@ int32_t FaceServo_client::setAllAngles(const list_float_1_t * angles_deg) m_clientManager->releaseRequest(request); // Invoke error handler callback function - m_clientManager->callErrorHandler(err, m_setAllAnglesId); + m_clientManager->callErrorHandler(err, m_setConstraintsId); #if ERPC_PRE_POST_ACTION pre_post_action_cb postCB = m_clientManager->getPostCB(); @@ -161,14 +156,140 @@ int32_t FaceServo_client::setAllAngles(const list_float_1_t * angles_deg) if (err != kErpcStatus_Success) { - result = -1; + result = false; } return result; } -// FaceServo interface setAngles function client shim. -int32_t FaceServo_client::setAngles(const list_ServoCmd_1_t * cmds) +// servo_service interface setMode function client shim. +bool servo_service_client::setMode(RpcMotionMode mode) +{ + erpc_status_t err = kErpcStatus_Success; + + bool result; + +#if ERPC_PRE_POST_ACTION + pre_post_action_cb preCB = m_clientManager->getPreCB(); + if (preCB) + { + preCB(); + } +#endif + + // Get a new request. + RequestContext request = m_clientManager->createRequest(false); + + // Encode the request. + Codec * codec = request.getCodec(); + + if (codec == NULL) + { + err = kErpcStatus_MemoryError; + } + else + { + codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setModeId, request.getSequence()); + + codec->write(static_cast(mode)); + + // Send message to server + // Codec status is checked inside this function. + m_clientManager->performRequest(request); + + codec->read(result); + + err = codec->getStatus(); + } + + // Dispose of the request. + m_clientManager->releaseRequest(request); + + // Invoke error handler callback function + m_clientManager->callErrorHandler(err, m_setModeId); + +#if ERPC_PRE_POST_ACTION + pre_post_action_cb postCB = m_clientManager->getPostCB(); + if (postCB) + { + postCB(); + } +#endif + + + if (err != kErpcStatus_Success) + { + result = false; + } + + return result; +} + +// servo_service interface setUpdatePeriodMs function client shim. +bool servo_service_client::setUpdatePeriodMs(uint32_t ms) +{ + erpc_status_t err = kErpcStatus_Success; + + bool result; + +#if ERPC_PRE_POST_ACTION + pre_post_action_cb preCB = m_clientManager->getPreCB(); + if (preCB) + { + preCB(); + } +#endif + + // Get a new request. + RequestContext request = m_clientManager->createRequest(false); + + // Encode the request. + Codec * codec = request.getCodec(); + + if (codec == NULL) + { + err = kErpcStatus_MemoryError; + } + else + { + codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setUpdatePeriodMsId, request.getSequence()); + + codec->write(ms); + + // Send message to server + // Codec status is checked inside this function. + m_clientManager->performRequest(request); + + codec->read(result); + + err = codec->getStatus(); + } + + // Dispose of the request. + m_clientManager->releaseRequest(request); + + // Invoke error handler callback function + m_clientManager->callErrorHandler(err, m_setUpdatePeriodMsId); + +#if ERPC_PRE_POST_ACTION + pre_post_action_cb postCB = m_clientManager->getPostCB(); + if (postCB) + { + postCB(); + } +#endif + + + if (err != kErpcStatus_Success) + { + result = false; + } + + return result; +} + +// servo_service interface move function client shim. +int32_t servo_service_client::move(const list_ServoCmd_1_t * cmds) { erpc_status_t err = kErpcStatus_Success; @@ -194,7 +315,7 @@ int32_t FaceServo_client::setAngles(const list_ServoCmd_1_t * cmds) } else { - codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setAnglesId, request.getSequence()); + codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_moveId, request.getSequence()); write_list_ServoCmd_1_t_struct(codec, cmds); @@ -211,7 +332,7 @@ int32_t FaceServo_client::setAngles(const list_ServoCmd_1_t * cmds) m_clientManager->releaseRequest(request); // Invoke error handler callback function - m_clientManager->callErrorHandler(err, m_setAnglesId); + m_clientManager->callErrorHandler(err, m_moveId); #if ERPC_PRE_POST_ACTION pre_post_action_cb postCB = m_clientManager->getPostCB(); @@ -230,8 +351,8 @@ int32_t FaceServo_client::setAngles(const list_ServoCmd_1_t * cmds) return result; } -// FaceServo interface setAngle function client shim. -int32_t FaceServo_client::setAngle(const char * id, float angle_deg) +// servo_service interface moveJ function client shim. +int32_t servo_service_client::moveJ(const list_float_1_t * angles_rad) { erpc_status_t err = kErpcStatus_Success; @@ -257,18 +378,9 @@ int32_t FaceServo_client::setAngle(const char * id, float angle_deg) } else { - codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_setAngleId, request.getSequence()); + codec->startWriteMessage(message_type_t::kInvocationMessage, m_serviceId, m_moveJId, request.getSequence()); - { - uint32_t id_len = strlen((const char*)id); - - erpc_assert(kServoIdMaxLen >= 0); - erpc_assert(id_len <= static_cast(kServoIdMaxLen)); - - codec->writeString(id_len, (const char*)id); - } - - codec->write(angle_deg); + write_list_float_1_t_struct(codec, angles_rad); // Send message to server // Codec status is checked inside this function. @@ -283,7 +395,7 @@ int32_t FaceServo_client::setAngle(const char * id, float angle_deg) m_clientManager->releaseRequest(request); // Invoke error handler callback function - m_clientManager->callErrorHandler(err, m_setAngleId); + m_clientManager->callErrorHandler(err, m_moveJId); #if ERPC_PRE_POST_ACTION pre_post_action_cb postCB = m_clientManager->getPostCB(); diff --git a/applications/erpc/proto/generated/servo_service_client.hpp b/applications/erpc/proto/generated/servo_service_client.hpp new file mode 100644 index 0000000..9923a99 --- /dev/null +++ b/applications/erpc/proto/generated/servo_service_client.hpp @@ -0,0 +1,42 @@ +/* + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * + * AUTOGENERATED - DO NOT EDIT + */ + + +#if !defined(_servo_service_client_hpp_) +#define _servo_service_client_hpp_ + +#include "servo_service_interface.hpp" + +#include "erpc_client_manager.h" + +namespace erpcShim +{ + +class servo_service_client: public servo_service_interface +{ + public: + servo_service_client(erpc::ClientManager *manager); + + virtual ~servo_service_client(); + + virtual bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad); + + virtual bool setMode(RpcMotionMode mode); + + virtual bool setUpdatePeriodMs(uint32_t ms); + + virtual int32_t move(const list_ServoCmd_1_t * cmds); + + virtual int32_t moveJ(const list_float_1_t * angles_rad); + + protected: + erpc::ClientManager *m_clientManager; +}; + +} // erpcShim + + +#endif // _servo_service_client_hpp_ diff --git a/applications/erpc/proto/generated/face_servo_common.h b/applications/erpc/proto/generated/servo_service_common.h similarity index 61% rename from applications/erpc/proto/generated/face_servo_common.h rename to applications/erpc/proto/generated/servo_service_common.h index fc132fc..368145a 100644 --- a/applications/erpc/proto/generated/face_servo_common.h +++ b/applications/erpc/proto/generated/servo_service_common.h @@ -1,12 +1,12 @@ /* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. * * AUTOGENERATED - DO NOT EDIT */ -#if !defined(_face_servo_common_h_) -#define _face_servo_common_h_ +#if !defined(_servo_service_common_h_) +#define _servo_service_common_h_ #if defined(__cplusplus) @@ -24,8 +24,15 @@ extern "C" #endif -#if !defined(ERPC_TYPE_DEFINITIONS_FACE_SERVO) -#define ERPC_TYPE_DEFINITIONS_FACE_SERVO +#if !defined(ERPC_TYPE_DEFINITIONS_SERVO_SERVICE) +#define ERPC_TYPE_DEFINITIONS_SERVO_SERVICE + +// Enumerators data types declarations +typedef enum RpcMotionMode +{ + RpcMotionModeImmediate = 0, + RpcMotionModeSCurve = 1 +} RpcMotionMode; // Aliases data types declarations typedef struct list_float_1_t list_float_1_t; @@ -42,7 +49,7 @@ struct list_float_1_t struct ServoCmd { char * id; - float angle_deg; + float angle_rad; }; struct list_ServoCmd_1_t @@ -52,15 +59,10 @@ struct list_ServoCmd_1_t }; -// Constant variable declarations -extern const int32_t kServoIdMaxLen; - -extern const int32_t kMaxServoCount; - -#endif // ERPC_TYPE_DEFINITIONS_FACE_SERVO +#endif // ERPC_TYPE_DEFINITIONS_SERVO_SERVICE #if defined(__cplusplus) } #endif -#endif // _face_servo_common_h_ +#endif // _servo_service_common_h_ diff --git a/applications/erpc/proto/generated/face_servo_common.hpp b/applications/erpc/proto/generated/servo_service_common.hpp similarity index 57% rename from applications/erpc/proto/generated/face_servo_common.hpp rename to applications/erpc/proto/generated/servo_service_common.hpp index a0f465d..7cf4024 100644 --- a/applications/erpc/proto/generated/face_servo_common.hpp +++ b/applications/erpc/proto/generated/servo_service_common.hpp @@ -1,12 +1,12 @@ /* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. * * AUTOGENERATED - DO NOT EDIT */ -#if !defined(_face_servo_common_hpp_) -#define _face_servo_common_hpp_ +#if !defined(_servo_service_common_hpp_) +#define _servo_service_common_hpp_ #include @@ -19,8 +19,15 @@ #endif -#if !defined(ERPC_TYPE_DEFINITIONS_FACE_SERVO) -#define ERPC_TYPE_DEFINITIONS_FACE_SERVO +#if !defined(ERPC_TYPE_DEFINITIONS_SERVO_SERVICE) +#define ERPC_TYPE_DEFINITIONS_SERVO_SERVICE + +// Enumerators data types declarations +typedef enum RpcMotionMode +{ + RpcMotionModeImmediate = 0, + RpcMotionModeSCurve = 1 +} RpcMotionMode; // Aliases data types declarations typedef struct list_float_1_t list_float_1_t; @@ -37,7 +44,7 @@ struct list_float_1_t struct ServoCmd { char * id; - float angle_deg; + float angle_rad; }; struct list_ServoCmd_1_t @@ -47,12 +54,7 @@ struct list_ServoCmd_1_t }; -// Constant variable declarations -extern const int32_t kServoIdMaxLen; - -extern const int32_t kMaxServoCount; - -#endif // ERPC_TYPE_DEFINITIONS_FACE_SERVO +#endif // ERPC_TYPE_DEFINITIONS_SERVO_SERVICE -#endif // _face_servo_common_hpp_ +#endif // _servo_service_common_hpp_ diff --git a/applications/erpc/proto/generated/face_servo_interface.cpp b/applications/erpc/proto/generated/servo_service_interface.cpp similarity index 58% rename from applications/erpc/proto/generated/face_servo_interface.cpp rename to applications/erpc/proto/generated/servo_service_interface.cpp index eee31a3..54cfb64 100644 --- a/applications/erpc/proto/generated/face_servo_interface.cpp +++ b/applications/erpc/proto/generated/servo_service_interface.cpp @@ -1,11 +1,11 @@ /* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. * * AUTOGENERATED - DO NOT EDIT */ -#include "face_servo_interface.hpp" +#include "servo_service_interface.hpp" #if 11400 != ERPC_VERSION_NUMBER #error "The generated shim code version is different to the rest of eRPC code." @@ -15,6 +15,6 @@ using namespace std; using namespace erpcShim; -FaceServo_interface::~FaceServo_interface(void) +servo_service_interface::~servo_service_interface(void) { } diff --git a/applications/erpc/proto/generated/servo_service_interface.hpp b/applications/erpc/proto/generated/servo_service_interface.hpp new file mode 100644 index 0000000..8dc58e8 --- /dev/null +++ b/applications/erpc/proto/generated/servo_service_interface.hpp @@ -0,0 +1,44 @@ +/* + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * + * AUTOGENERATED - DO NOT EDIT + */ + + +#if !defined(_servo_service_interface_hpp_) +#define _servo_service_interface_hpp_ + +#include "servo_service_common.hpp" + +namespace erpcShim +{ + + +// Abstract base class for servo_service +class servo_service_interface +{ + public: + static const uint8_t m_serviceId = 1; + static const uint8_t m_setConstraintsId = 1; + static const uint8_t m_setModeId = 2; + static const uint8_t m_setUpdatePeriodMsId = 3; + static const uint8_t m_moveId = 4; + static const uint8_t m_moveJId = 5; + + virtual ~servo_service_interface(void); + + virtual bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad) = 0; + + virtual bool setMode(RpcMotionMode mode) = 0; + + virtual bool setUpdatePeriodMs(uint32_t ms) = 0; + + virtual int32_t move(const list_ServoCmd_1_t * cmds) = 0; + + virtual int32_t moveJ(const list_float_1_t * angles_rad) = 0; +private: +}; +} // erpcShim + + +#endif // _servo_service_interface_hpp_ diff --git a/applications/erpc/proto/generated/face_servo_server.cpp b/applications/erpc/proto/generated/servo_service_server.cpp similarity index 57% rename from applications/erpc/proto/generated/face_servo_server.cpp rename to applications/erpc/proto/generated/servo_service_server.cpp index 8ebb5e6..1ce45d6 100644 --- a/applications/erpc/proto/generated/face_servo_server.cpp +++ b/applications/erpc/proto/generated/servo_service_server.cpp @@ -1,11 +1,11 @@ /* - * Generated by erpcgen 1.14.0 on Wed Jan 14 16:06:40 2026. + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. * * AUTOGENERATED - DO NOT EDIT */ -#include "face_servo_server.hpp" +#include "servo_service_server.hpp" #if ERPC_ALLOCATION_POLICY == ERPC_ALLOCATION_POLICY_DYNAMIC #include #include "erpc_port.h" @@ -25,50 +25,15 @@ extern bool nestingDetection; #endif - -// Constant variable definitions -#if defined(__MINGW32__) -__declspec( selectany ) -#else -#pragma weak kServoIdMaxLen -#endif -extern const int32_t kServoIdMaxLen = 32; -#if defined(__MINGW32__) -__declspec( selectany ) -#else -#pragma weak kMaxServoCount -#endif -extern const int32_t kMaxServoCount = 48; - -//! @brief Function to read struct list_float_1_t -static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data); - //! @brief Function to read struct ServoCmd static void read_ServoCmd_struct(erpc::Codec * codec, ServoCmd * data); //! @brief Function to read struct list_ServoCmd_1_t static void read_list_ServoCmd_1_t_struct(erpc::Codec * codec, list_ServoCmd_1_t * data); +//! @brief Function to read struct list_float_1_t +static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data); -// Read struct list_float_1_t function implementation -static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data) -{ - if(NULL == data) - { - return; - } - - codec->startReadList(data->elementsCount); - data->elements = (float *) erpc_malloc(data->elementsCount * sizeof(float)); - if ((data->elements == NULL) && (data->elementsCount > 0)) - { - codec->updateStatus(kErpcStatus_MemoryError); - } - for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount) - { - codec->read(data->elements[listCount]); - } -} // Read struct ServoCmd function implementation static void read_ServoCmd_struct(erpc::Codec * codec, ServoCmd * data) @@ -94,7 +59,7 @@ static void read_ServoCmd_struct(erpc::Codec * codec, ServoCmd * data) } } - codec->read(data->angle_deg); + codec->read(data->angle_rad); } // Read struct list_ServoCmd_1_t function implementation @@ -117,9 +82,26 @@ static void read_list_ServoCmd_1_t_struct(erpc::Codec * codec, list_ServoCmd_1_t } } +// Read struct list_float_1_t function implementation +static void read_list_float_1_t_struct(erpc::Codec * codec, list_float_1_t * data) +{ + if(NULL == data) + { + return; + } + + codec->startReadList(data->elementsCount); + data->elements = (float *) erpc_malloc(data->elementsCount * sizeof(float)); + if ((data->elements == NULL) && (data->elementsCount > 0)) + { + codec->updateStatus(kErpcStatus_MemoryError); + } + for (uint32_t listCount = 0U; listCount < data->elementsCount; ++listCount) + { + codec->read(data->elements[listCount]); + } +} -//! @brief Function to free space allocated inside struct list_float_1_t -static void free_list_float_1_t_struct(list_float_1_t * data); //! @brief Function to free space allocated inside struct ServoCmd static void free_ServoCmd_struct(ServoCmd * data); @@ -127,12 +109,9 @@ static void free_ServoCmd_struct(ServoCmd * data); //! @brief Function to free space allocated inside struct list_ServoCmd_1_t static void free_list_ServoCmd_1_t_struct(list_ServoCmd_1_t * data); +//! @brief Function to free space allocated inside struct list_float_1_t +static void free_list_float_1_t_struct(list_float_1_t * data); -// Free space allocated inside struct list_float_1_t function implementation -static void free_list_float_1_t_struct(list_float_1_t * data) -{ - erpc_free(data->elements); -} // Free space allocated inside struct ServoCmd function implementation static void free_ServoCmd_struct(ServoCmd * data) @@ -151,45 +130,63 @@ static void free_list_ServoCmd_1_t_struct(list_ServoCmd_1_t * data) erpc_free(data->elements); } +// Free space allocated inside struct list_float_1_t function implementation +static void free_list_float_1_t_struct(list_float_1_t * data) +{ + erpc_free(data->elements); +} -FaceServo_service::FaceServo_service(FaceServo_interface *_FaceServo_interface) - : erpc::Service(FaceServo_interface::m_serviceId) - , m_handler(_FaceServo_interface) + +servo_service_service::servo_service_service(servo_service_interface *_servo_service_interface) + : erpc::Service(servo_service_interface::m_serviceId) + , m_handler(_servo_service_interface) { } -FaceServo_service::~FaceServo_service() +servo_service_service::~servo_service_service() { } // return service interface handler. -FaceServo_interface* FaceServo_service::getHandler(void) +servo_service_interface* servo_service_service::getHandler(void) { return m_handler; } // Call the correct server shim based on method unique ID. -erpc_status_t FaceServo_service::handleInvocation(uint32_t methodId, uint32_t sequence, Codec * codec, MessageBufferFactory *messageFactory, Transport * transport) +erpc_status_t servo_service_service::handleInvocation(uint32_t methodId, uint32_t sequence, Codec * codec, MessageBufferFactory *messageFactory, Transport * transport) { erpc_status_t erpcStatus; switch (methodId) { - case FaceServo_interface::m_setAllAnglesId: + case servo_service_interface::m_setConstraintsId: { - erpcStatus = setAllAngles_shim(codec, messageFactory, transport, sequence); + erpcStatus = setConstraints_shim(codec, messageFactory, transport, sequence); break; } - case FaceServo_interface::m_setAnglesId: + case servo_service_interface::m_setModeId: { - erpcStatus = setAngles_shim(codec, messageFactory, transport, sequence); + erpcStatus = setMode_shim(codec, messageFactory, transport, sequence); break; } - case FaceServo_interface::m_setAngleId: + case servo_service_interface::m_setUpdatePeriodMsId: { - erpcStatus = setAngle_shim(codec, messageFactory, transport, sequence); + erpcStatus = setUpdatePeriodMs_shim(codec, messageFactory, transport, sequence); + break; + } + + case servo_service_interface::m_moveId: + { + erpcStatus = move_shim(codec, messageFactory, transport, sequence); + break; + } + + case servo_service_interface::m_moveJId: + { + erpcStatus = moveJ_shim(codec, messageFactory, transport, sequence); break; } @@ -203,22 +200,40 @@ erpc_status_t FaceServo_service::handleInvocation(uint32_t methodId, uint32_t se return erpcStatus; } -// Server shim for setAllAngles of FaceServo interface. -erpc_status_t FaceServo_service::setAllAngles_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) +// Server shim for setConstraints of servo_service interface. +erpc_status_t servo_service_service::setConstraints_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) { erpc_status_t err = kErpcStatus_Success; - list_float_1_t *angles_deg = NULL; - angles_deg = (list_float_1_t *) erpc_malloc(sizeof(list_float_1_t)); - if (angles_deg == NULL) - { - codec->updateStatus(kErpcStatus_MemoryError); - } - int32_t result; + char * id = NULL; + float max_velocity_rad; + float max_acceleration_rad; + float max_jerk_rad; + bool result; // startReadMessage() was already called before this shim was invoked. - read_list_float_1_t_struct(codec, angles_deg); + { + uint32_t id_len; + char * id_local; + codec->readString(id_len, &id_local); + id = (char*) erpc_malloc((id_len + 1) * sizeof(char)); + if ((id == NULL) || (id_local == NULL)) + { + codec->updateStatus(kErpcStatus_MemoryError); + } + else + { + memcpy(id, id_local, id_len); + (id)[id_len] = 0; + } + } + + codec->read(max_velocity_rad); + + codec->read(max_acceleration_rad); + + codec->read(max_jerk_rad); err = codec->getStatus(); if (err == kErpcStatus_Success) @@ -227,7 +242,7 @@ erpc_status_t FaceServo_service::setAllAngles_shim(Codec * codec, MessageBufferF #if ERPC_NESTED_CALLS_DETECTION nestingDetection = true; #endif - result = m_handler->setAllAngles(angles_deg); + result = m_handler->setConstraints(id, max_velocity_rad, max_acceleration_rad, max_jerk_rad); #if ERPC_NESTED_CALLS_DETECTION nestingDetection = false; #endif @@ -242,24 +257,110 @@ erpc_status_t FaceServo_service::setAllAngles_shim(Codec * codec, MessageBufferF codec->reset(transport->reserveHeaderSize()); // Build response message. - codec->startWriteMessage(message_type_t::kReplyMessage, FaceServo_interface::m_serviceId, FaceServo_interface::m_setAllAnglesId, sequence); + codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setConstraintsId, sequence); codec->write(result); err = codec->getStatus(); } - if (angles_deg) - { - free_list_float_1_t_struct(angles_deg); - } - erpc_free(angles_deg); + erpc_free(id); return err; } -// Server shim for setAngles of FaceServo interface. -erpc_status_t FaceServo_service::setAngles_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) +// Server shim for setMode of servo_service interface. +erpc_status_t servo_service_service::setMode_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) +{ + erpc_status_t err = kErpcStatus_Success; + + RpcMotionMode mode; + int32_t _tmp_local_i32; + bool result; + + // startReadMessage() was already called before this shim was invoked. + + codec->read(_tmp_local_i32); + mode = static_cast(_tmp_local_i32); + + err = codec->getStatus(); + if (err == kErpcStatus_Success) + { + // Invoke the actual served function. +#if ERPC_NESTED_CALLS_DETECTION + nestingDetection = true; +#endif + result = m_handler->setMode(mode); +#if ERPC_NESTED_CALLS_DETECTION + nestingDetection = false; +#endif + + // preparing MessageBuffer for serializing data + err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize()); + } + + if (err == kErpcStatus_Success) + { + // preparing codec for serializing data + codec->reset(transport->reserveHeaderSize()); + + // Build response message. + codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setModeId, sequence); + + codec->write(result); + + err = codec->getStatus(); + } + + return err; +} + +// Server shim for setUpdatePeriodMs of servo_service interface. +erpc_status_t servo_service_service::setUpdatePeriodMs_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) +{ + erpc_status_t err = kErpcStatus_Success; + + uint32_t ms; + bool result; + + // startReadMessage() was already called before this shim was invoked. + + codec->read(ms); + + err = codec->getStatus(); + if (err == kErpcStatus_Success) + { + // Invoke the actual served function. +#if ERPC_NESTED_CALLS_DETECTION + nestingDetection = true; +#endif + result = m_handler->setUpdatePeriodMs(ms); +#if ERPC_NESTED_CALLS_DETECTION + nestingDetection = false; +#endif + + // preparing MessageBuffer for serializing data + err = messageFactory->prepareServerBufferForSend(codec->getBufferRef(), transport->reserveHeaderSize()); + } + + if (err == kErpcStatus_Success) + { + // preparing codec for serializing data + codec->reset(transport->reserveHeaderSize()); + + // Build response message. + codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_setUpdatePeriodMsId, sequence); + + codec->write(result); + + err = codec->getStatus(); + } + + return err; +} + +// Server shim for move of servo_service interface. +erpc_status_t servo_service_service::move_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) { erpc_status_t err = kErpcStatus_Success; @@ -282,7 +383,7 @@ erpc_status_t FaceServo_service::setAngles_shim(Codec * codec, MessageBufferFact #if ERPC_NESTED_CALLS_DETECTION nestingDetection = true; #endif - result = m_handler->setAngles(cmds); + result = m_handler->move(cmds); #if ERPC_NESTED_CALLS_DETECTION nestingDetection = false; #endif @@ -297,7 +398,7 @@ erpc_status_t FaceServo_service::setAngles_shim(Codec * codec, MessageBufferFact codec->reset(transport->reserveHeaderSize()); // Build response message. - codec->startWriteMessage(message_type_t::kReplyMessage, FaceServo_interface::m_serviceId, FaceServo_interface::m_setAnglesId, sequence); + codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_moveId, sequence); codec->write(result); @@ -313,41 +414,22 @@ erpc_status_t FaceServo_service::setAngles_shim(Codec * codec, MessageBufferFact return err; } -// Server shim for setAngle of FaceServo interface. -erpc_status_t FaceServo_service::setAngle_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) +// Server shim for moveJ of servo_service interface. +erpc_status_t servo_service_service::moveJ_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) { erpc_status_t err = kErpcStatus_Success; - char * id = NULL; - float angle_deg; + list_float_1_t *angles_rad = NULL; + angles_rad = (list_float_1_t *) erpc_malloc(sizeof(list_float_1_t)); + if (angles_rad == NULL) + { + codec->updateStatus(kErpcStatus_MemoryError); + } int32_t result; // startReadMessage() was already called before this shim was invoked. - { - uint32_t id_len; - char * id_local; - codec->readString(id_len, &id_local); - if ((kServoIdMaxLen >= 0) && (id_len <= static_cast(kServoIdMaxLen))) - { - id = (char*) erpc_malloc((kServoIdMaxLen + 1) * sizeof(char)); - if ((id == NULL) || (id_local == NULL)) - { - codec->updateStatus(kErpcStatus_MemoryError); - } - else - { - memcpy(id, id_local, id_len); - (id)[id_len] = 0; - } - } - else - { - codec->updateStatus(kErpcStatus_InvalidArgument); - } - } - - codec->read(angle_deg); + read_list_float_1_t_struct(codec, angles_rad); err = codec->getStatus(); if (err == kErpcStatus_Success) @@ -356,7 +438,7 @@ erpc_status_t FaceServo_service::setAngle_shim(Codec * codec, MessageBufferFacto #if ERPC_NESTED_CALLS_DETECTION nestingDetection = true; #endif - result = m_handler->setAngle(id, angle_deg); + result = m_handler->moveJ(angles_rad); #if ERPC_NESTED_CALLS_DETECTION nestingDetection = false; #endif @@ -371,14 +453,18 @@ erpc_status_t FaceServo_service::setAngle_shim(Codec * codec, MessageBufferFacto codec->reset(transport->reserveHeaderSize()); // Build response message. - codec->startWriteMessage(message_type_t::kReplyMessage, FaceServo_interface::m_serviceId, FaceServo_interface::m_setAngleId, sequence); + codec->startWriteMessage(message_type_t::kReplyMessage, servo_service_interface::m_serviceId, servo_service_interface::m_moveJId, sequence); codec->write(result); err = codec->getStatus(); } - erpc_free(id); + if (angles_rad) + { + free_list_float_1_t_struct(angles_rad); + } + erpc_free(angles_rad); return err; } diff --git a/applications/erpc/proto/generated/servo_service_server.hpp b/applications/erpc/proto/generated/servo_service_server.hpp new file mode 100644 index 0000000..a3114aa --- /dev/null +++ b/applications/erpc/proto/generated/servo_service_server.hpp @@ -0,0 +1,61 @@ +/* + * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * + * AUTOGENERATED - DO NOT EDIT + */ + + +#if !defined(_servo_service_server_hpp_) +#define _servo_service_server_hpp_ + +#include "servo_service_interface.hpp" + +#include "erpc_server.hpp" +#include "erpc_codec.hpp" + +#if 11400 != ERPC_VERSION_NUMBER +#error "The generated shim code version is different to the rest of eRPC code." +#endif + + +namespace erpcShim +{ + +/*! + * @brief Service subclass for servo_service. + */ +class servo_service_service : public erpc::Service +{ +public: + servo_service_service(servo_service_interface *_servo_service_interface); + + virtual ~servo_service_service(); + + /*! @brief return service interface handler. */ + servo_service_interface* getHandler(void); + + /*! @brief Call the correct server shim based on method unique ID. */ + virtual erpc_status_t handleInvocation(uint32_t methodId, uint32_t sequence, erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport); + +private: + servo_service_interface *m_handler; + /*! @brief Server shim for setConstraints of servo_service interface. */ + erpc_status_t setConstraints_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence); + + /*! @brief Server shim for setMode of servo_service interface. */ + erpc_status_t setMode_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence); + + /*! @brief Server shim for setUpdatePeriodMs of servo_service interface. */ + erpc_status_t setUpdatePeriodMs_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence); + + /*! @brief Server shim for move of servo_service interface. */ + erpc_status_t move_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence); + + /*! @brief Server shim for moveJ of servo_service interface. */ + erpc_status_t moveJ_shim(erpc::Codec * codec, erpc::MessageBufferFactory *messageFactory, erpc::Transport * transport, uint32_t sequence); +}; + +} // erpcShim + + +#endif // _servo_service_server_hpp_ diff --git a/applications/erpc/proto/servo_service.erpc b/applications/erpc/proto/servo_service.erpc new file mode 100644 index 0000000..103136a --- /dev/null +++ b/applications/erpc/proto/servo_service.erpc @@ -0,0 +1,30 @@ +// servo_service.erpc +// Generator examples: +// .\erpcgen.exe -o .\generated\ -g c .\servo_service.erpc +// .\erpcgen.exe -g py .\servo_service.erpc + +program servo_service + +enum RpcMotionMode { + RpcMotionModeImmediate = 0, + RpcMotionModeSCurve = 1 +} +struct ServoCmd { + string id + float angle_rad +} +interface servo_service +{ + setConstraints(string id, + float max_velocity_rad, + float max_acceleration_rad, + float max_jerk_rad) -> bool + + setMode(RpcMotionMode mode)->bool + setUpdatePeriodMs(uint32 ms) -> bool + move(list cmds) -> int32 + + moveJ(list angles_rad) -> int32 + + +} diff --git a/applications/erpc/service/include/head_service_impl.h b/applications/erpc/service/include/head_service_impl.h deleted file mode 100644 index fc2b2e7..0000000 --- a/applications/erpc/service/include/head_service_impl.h +++ /dev/null @@ -1,35 +0,0 @@ -// -// Created by Administrator on 2025/12/24. -// - -#ifndef RTTHREAD_HEAD_SERVICE_IMPL_H -#define RTTHREAD_HEAD_SERVICE_IMPL_H - - -#include -#include "erpc/proto/generated/face_servo_server.hpp" -#include "servo_manager/include/servo_manager.h" - -extern "C" { -#include -} - -class FaceServoServiceImpl : public erpcShim::FaceServo_interface -{ -public: - FaceServoServiceImpl(); - virtual ~FaceServoServiceImpl() = default; - - - int32_t setAllAngles(const list_float_1_t* angles_deg) override; - int32_t setAngle(const char* id, float angle_deg) override; - int32_t setAngles(const list_ServoCmd_1_t* cmds) override; -private: - ServoManager _servos; - rt_err_t _init_err; - static const std::vector kServoCfg; - -}; - - -#endif //RTTHREAD_HEAD_SERVICE_IMPL_H diff --git a/applications/erpc/service/include/servo_service_impl.h b/applications/erpc/service/include/servo_service_impl.h new file mode 100644 index 0000000..bd582a7 --- /dev/null +++ b/applications/erpc/service/include/servo_service_impl.h @@ -0,0 +1,42 @@ +// +// Created by Administrator on 2025/12/24. +// + +#ifndef RTTHREAD_HEAD_SERVICE_IMPL_H +#define RTTHREAD_HEAD_SERVICE_IMPL_H + +#include +#include + +#include "erpc/proto/generated/servo_service_server.hpp" +#include "servo_manager/include/servo_manager.h" + +extern "C" { +#include +} + +class ServoServiceImpl : public erpcShim::servo_service_interface +{ +public: + ServoServiceImpl(); + ~ServoServiceImpl() override; + + bool setConstraints(const char* id, + float max_velocity_rad, + float max_acceleration_rad, + float max_jerk_rad) override; + bool setMode(RpcMotionMode mode) override; + bool setUpdatePeriodMs(uint32_t ms) override; + int32_t move(const list_ServoCmd_1_t* cmds) override; + int32_t moveJ(const list_float_1_t* angles_rad) override; + +private: + ServoManager manager_; + rt_err_t init_err_; + MotionMode mode_; + rt_mutex_t mutex_; + + static const std::vector kServoCfg; +}; + +#endif // RTTHREAD_HEAD_SERVICE_IMPL_H diff --git a/applications/erpc/service/src/erpc_server_main.cpp b/applications/erpc/service/src/erpc_server_main.cpp index f88eaac..8a50702 100644 --- a/applications/erpc/service/src/erpc_server_main.cpp +++ b/applications/erpc/service/src/erpc_server_main.cpp @@ -13,10 +13,9 @@ #include "erpc_usb_cdc_transport.hpp" -#include "erpc/proto/generated/face_servo_server.hpp" -#include "erpc/proto/generated/face_servo_interface.hpp" - -#include "erpc/service/include/head_service_impl.h" +#include "erpc/proto/generated/servo_service_server.hpp" +#include "erpc/proto/generated/servo_service_interface.hpp" +#include "erpc/service/include/servo_service_impl.h" #include "erpc/common/include/erpc_error_handler.h" @@ -68,8 +67,8 @@ static void erpc_server_entry(void *parameter) } // 4) register service - static FaceServoServiceImpl impl; // 必须继承 erpcShim::demo_interface - static erpcShim::FaceServo_service service(&impl); // 生成的 service wrapper + static ServoServiceImpl impl; // 必须继承 erpcShim::demo_interface + static erpcShim::servo_service_service service(&impl); // 生成的 service wrapper erpc_add_service_to_server(server, &service); diff --git a/applications/erpc/service/src/head_service_impl.cpp b/applications/erpc/service/src/head_service_impl.cpp deleted file mode 100644 index fa8616a..0000000 --- a/applications/erpc/service/src/head_service_impl.cpp +++ /dev/null @@ -1,129 +0,0 @@ -// -// Created by Administrator on 2025/12/24. -// - -#include "erpc/service/include/head_service_impl.h" -#include - - -const std::vector FaceServoServiceImpl::kServoCfg = { - {"eye_l_up", "pwm1", 1, 20000000, 500000, 2500000, -90.f, 90.f, 0.f}, - // {"eye_r_up", "pwm1", 2, 20000000, 500000, 2500000, 0.f, 180.f, 90.f}, - // ... -}; - -FaceServoServiceImpl::FaceServoServiceImpl() -: _servos(kServoCfg), _init_err(RT_EOK) -{ - _init_err = _servos.init(true, true); // enable=true, go_home=true - if (_init_err != RT_EOK) - { - rt_kprintf("[FaceServo][E] init FAILED, err=%d\n", (int)_init_err); - } -} - -int32_t FaceServoServiceImpl::setAllAngles(const list_float_1_t* angles_deg) -{ - // rt_kprintf("-"); - if (_init_err != RT_EOK) - { - rt_kprintf("[FaceServo][E] setAllAngles: service not ready, init_err=%d\n", (int)_init_err); - return _init_err; - } - - if (!angles_deg) - { - rt_kprintf("[FaceServo][E] setAllAngles: angles_deg is null\n"); - return -RT_EINVAL; - } - - if (!angles_deg->elements || angles_deg->elementsCount == 0) - { - rt_kprintf("[FaceServo][E] setAllAngles: empty list (elements=%p, count=%d)\n", - angles_deg->elements, (int)angles_deg->elementsCount); - return -RT_EINVAL; - } - - const size_t need = _servos.count(); - const size_t got = (size_t)angles_deg->elementsCount; - - // 实际使用数量：谁小用谁 - const size_t n = (got < need) ? got : need; - rt_err_t err = _servos.setAllAngles(angles_deg->elements, n); - if (err != RT_EOK) - { - rt_kprintf("[FaceServo][E] setAllAngles: ServoManager err=%d\n", (int)err); - return err; - } - - return RT_EOK; -} - -int32_t FaceServoServiceImpl::setAngle(const char* id, float angle_deg) -{ - if (_init_err != RT_EOK) - { - rt_kprintf("[FaceServo][E] setAngle: service not ready, init_err=%d\n", (int)_init_err); - return _init_err; - } - - if (!id) - { - rt_kprintf("[FaceServo][E] setAngle: id is null\n"); - return -RT_EINVAL; - } - - rt_err_t err = _servos.setAngle(id, angle_deg); - if (err != RT_EOK) - { - rt_kprintf("[FaceServo][E] setAngle: id=%s angle=%.2f err=%d\n", - id, (double)angle_deg, (int)err); - return err; - } - - return RT_EOK; -} - -int32_t FaceServoServiceImpl::setAngles(const list_ServoCmd_1_t* cmds) -{ - if (_init_err != RT_EOK) - { - rt_kprintf("[FaceServo][E] setAngles: service not ready, init_err=%d\n", (int)_init_err); - return _init_err; - } - - if (!cmds) - { - rt_kprintf("[FaceServo][E] setAngles: cmds is null\n"); - return -RT_EINVAL; - } - - if (!cmds->elements || cmds->elementsCount == 0) - { - rt_kprintf("[FaceServo][E] setAngles: empty list (elements=%p, count=%d)\n", - cmds->elements, (int)cmds->elementsCount); - return -RT_EINVAL; - } - - for (uint32_t i = 0; i < cmds->elementsCount; ++i) - { - const char* id = cmds->elements[i].id; - const float angle = cmds->elements[i].angle_deg; - - if (!id) - { - rt_kprintf("[FaceServo][E] setAngles: cmds[%d].id is null\n", (int)i); - return -RT_EINVAL; - } - - rt_err_t err = _servos.setAngle(id, angle); - if (err != RT_EOK) - { - rt_kprintf("[FaceServo][E] setAngles: fail i=%d id=%s angle=%.2f err=%d\n", - (int)i, id, (double)angle, (int)err); - return err; - } - } - - return RT_EOK; -} diff --git a/applications/erpc/service/src/servo_service_impl.cpp b/applications/erpc/service/src/servo_service_impl.cpp new file mode 100644 index 0000000..2c95c6a --- /dev/null +++ b/applications/erpc/service/src/servo_service_impl.cpp @@ -0,0 +1,277 @@ +// +// Created by Administrator on 2025/12/24. +// + +#include "erpc/service/include/servo_service_impl.h" +#include +#include "common/type/common_type.h" + +const std::vector ServoServiceImpl::kServoCfg = { + {"eye_l_up", "pwm1", 1, 20000000, 500000, 2500000, -0.5f * PI, 0.5f * PI, 0.0f}, + // {"eye_r_up", "pwm1", 2, 20000000, 500000, 2500000, 0.0f, kPi, 0.5f * kPi}, + // ... +}; + +ServoServiceImpl::ServoServiceImpl() + : manager_(kServoCfg), + init_err_(RT_EOK), + mode_(MotionMode::Immediate), + mutex_(RT_NULL) +{ + mutex_ = rt_mutex_create("sv_svc", RT_IPC_FLAG_PRIO); + if (!mutex_) { + init_err_ = -RT_ENOMEM; + rt_kprintf("[ServoService][E] mutex create FAILED, err=%d\n", (int)init_err_); + return; + } + + init_err_ = manager_.init(true, true); + if (init_err_ != RT_EOK) { + rt_kprintf("[ServoService][E] init FAILED, err=%d\n", (int)init_err_); + return; + } + + manager_.setUpdatePeriodMs(1); + + for (const auto& cfg : kServoCfg) { + ServoControl* servo = manager_.get(cfg.servo_id); + if (!servo) { + init_err_ = -RT_ENOSYS; + rt_kprintf("[ServoService][E] missing servo control, id=%s\n", cfg.servo_id.c_str()); + return; + } + + servo->setConstraints( + 3.14, + 31.4, + 31.4); + } + + init_err_ = manager_.start(); + if (init_err_ != RT_EOK) { + rt_kprintf("[ServoService][E] manager start FAILED, err=%d\n", (int)init_err_); + } +} + +ServoServiceImpl::~ServoServiceImpl() +{ + if (mutex_) { + rt_mutex_delete(mutex_); + mutex_ = RT_NULL; + } +} + +bool ServoServiceImpl::setConstraints(const char* id, + float max_velocity_rad, + float max_acceleration_rad, + float max_jerk_rad) +{ + if (init_err_ != RT_EOK) { + rt_kprintf("[ServoService][E] setConstraints: service not ready, init_err=%d\n", + (int)init_err_); + return false; + } + + if (!id) { + rt_kprintf("[ServoService][E] setConstraints: id is null\n"); + return false; + } + + if (max_velocity_rad <= 0.0f || + max_acceleration_rad <= 0.0f || + max_jerk_rad <= 0.0f) { + rt_kprintf("[ServoService][E] setConstraints: invalid constraints, id=%s v=%.4f a=%.4f j=%.4f\n", + id, + (double)max_velocity_rad, + (double)max_acceleration_rad, + (double)max_jerk_rad); + return false; + } + + ServoControl* servo = manager_.get(std::string(id)); + if (!servo) { + rt_kprintf("[ServoService][E] setConstraints: servo not found, id=%s\n", id); + return false; + } + + servo->setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad); + rt_kprintf("[ServoService] setConstraints: id=%s v=%.4f a=%.4f j=%.4f\n", + id, + (double)max_velocity_rad, + (double)max_acceleration_rad, + (double)max_jerk_rad); + return true; +} + +bool ServoServiceImpl::setMode(RpcMotionMode mode) +{ + if (init_err_ != RT_EOK) { + rt_kprintf("[ServoService][E] setMode: service not ready, init_err=%d\n", + (int)init_err_); + return false; + } + + if (!mutex_) { + rt_kprintf("[ServoService][E] setMode: mutex is null\n"); + return false; + } + + MotionMode motion_mode; + switch (mode) { + case RpcMotionModeImmediate: + motion_mode = MotionMode::Immediate; + break; + case RpcMotionModeSCurve: + motion_mode = MotionMode::SCurve; + break; + default: + rt_kprintf("[ServoService][E] setMode: invalid mode=%d\n", (int)mode); + return false; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + mode_ = motion_mode; + rt_mutex_release(mutex_); + + rt_kprintf("[ServoService] setMode: mode=%d\n", (int)mode); + return true; +} + +bool ServoServiceImpl::setUpdatePeriodMs(uint32_t ms) +{ + if (init_err_ != RT_EOK) { + rt_kprintf("[ServoService][E] setUpdatePeriodMs: service not ready, init_err=%d\n", + (int)init_err_); + return false; + } + + manager_.setUpdatePeriodMs(ms); + manager_.wake(); + + rt_kprintf("[ServoService] setUpdatePeriodMs: ms=%u\n", (unsigned)ms); + return true; +} + +int32_t ServoServiceImpl::move(const list_ServoCmd_1_t* cmds) +{ + if (init_err_ != RT_EOK) { + rt_kprintf("[ServoService][E] move: service not ready, init_err=%d\n", + (int)init_err_); + return init_err_; + } + + if (!cmds) { + rt_kprintf("[ServoService][E] move: cmds is null\n"); + return -RT_EINVAL; + } + + if (!cmds->elements || cmds->elementsCount == 0) { + rt_kprintf("[ServoService][E] move: empty list (elements=%p, count=%d)\n", + cmds->elements, + (int)cmds->elementsCount); + return -RT_EINVAL; + } + + if (!mutex_) { + rt_kprintf("[ServoService][E] move: mutex is null\n"); + return -RT_ENOMEM; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const MotionMode motion_mode = mode_; + rt_mutex_release(mutex_); + + for (uint32_t i = 0; i < cmds->elementsCount; ++i) { + const char* id = cmds->elements[i].id; + if (!id) { + rt_kprintf("[ServoService][E] move: cmds[%d].id is null\n", (int)i); + return -RT_EINVAL; + } + + ServoControl* servo = manager_.get(std::string(id)); + if (!servo) { + rt_kprintf("[ServoService][E] move: servo not found, id=%s\n", id); + return -RT_ENOSYS; + } + + const rt_err_t err = servo->move(cmds->elements[i].angle_rad, motion_mode); + if (err != RT_EOK) { + rt_kprintf("[ServoService][E] move: move failed, id=%s angle=%.4f mode=%d err=%d\n", + id, + (double)cmds->elements[i].angle_rad, + (int)motion_mode, + (int)err); + return err; + } + } + + if (motion_mode == MotionMode::SCurve) { + manager_.wake(); + } + + return RT_EOK; +} + +int32_t ServoServiceImpl::moveJ(const list_float_1_t* angles_rad) +{ + if (init_err_ != RT_EOK) { + rt_kprintf("[ServoService][E] moveJ: service not ready, init_err=%d\n", + (int)init_err_); + return init_err_; + } + + if (!angles_rad) { + rt_kprintf("[ServoService][E] moveJ: angles_rad is null\n"); + return -RT_EINVAL; + } + + if (!angles_rad->elements || angles_rad->elementsCount == 0) { + rt_kprintf("[ServoService][E] moveJ: empty list (elements=%p, count=%d)\n", + angles_rad->elements, + (int)angles_rad->elementsCount); + return -RT_EINVAL; + } + + const size_t need = kServoCfg.size(); + const size_t got = (size_t)angles_rad->elementsCount; + if (got != need) { + rt_kprintf("[ServoService][E] moveJ: invalid angles count, need=%d got=%d\n", + (int)need, + (int)got); + return -RT_EINVAL; + } + + if (!mutex_) { + rt_kprintf("[ServoService][E] moveJ: mutex is null\n"); + return -RT_ENOMEM; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const MotionMode motion_mode = mode_; + rt_mutex_release(mutex_); + + for (size_t i = 0; i < need; ++i) { + ServoControl* servo = manager_.get(kServoCfg[i].servo_id); + if (!servo) { + rt_kprintf("[ServoService][E] moveJ: servo not found, id=%s\n", + kServoCfg[i].servo_id.c_str()); + return -RT_ENOSYS; + } + + const rt_err_t err = servo->move(angles_rad->elements[i], motion_mode); + if (err != RT_EOK) { + rt_kprintf("[ServoService][E] moveJ: move failed, id=%s angle=%.4f mode=%d err=%d\n", + kServoCfg[i].servo_id.c_str(), + (double)angles_rad->elements[i], + (int)motion_mode, + (int)err); + return err; + } + } + + if (motion_mode == MotionMode::SCurve) { + manager_.wake(); + } + + return RT_EOK; +} diff --git a/applications/main.c b/applications/main.c deleted file mode 100644 index 7b2924b..0000000 --- a/applications/main.c +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (c) 2006-2018, RT-Thread Development Team - * - * SPDX-License-Identifier: Apache-2.0 - * - * Change Logs: - * Date Author Notes - * 2019-03-05 whj4674672 first version - */ - -#include -#include -#include - -/* defined the LED0 pin: PB1 */ -#define LED0_PIN GET_PIN(C, 13) - - -int main(void) -{ - int count = 1; - /* set LED0 pin mode to output */ - rt_pin_mode(LED0_PIN, PIN_MODE_OUTPUT); - - - while (count++) - { - rt_pin_write(LED0_PIN, PIN_HIGH); - rt_thread_mdelay(100); - rt_pin_write(LED0_PIN, PIN_LOW); - rt_thread_mdelay(100); - // rt_kprintf("Hello RT-Thread!\n"); - } - - return RT_EOK; -} - - - - diff --git a/applications/main.cpp b/applications/main.cpp new file mode 100644 index 0000000..0721cd6 --- /dev/null +++ b/applications/main.cpp @@ -0,0 +1,119 @@ +/* + * Copyright (c) 2006-2018, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2019-03-05 whj4674672 first version + */ + +#include +#include +#include + +#include "planner/s_curve_planner/include/s_curve_position_planner.h" + +/* defined the LED0 pin: PC13 */ +#define LED0_PIN GET_PIN(C, 13) + +namespace +{ + +constexpr rt_int32_t kWarmupIterations = 10; +constexpr rt_int32_t kBenchmarkIterations = 10000; +constexpr double kBenchmarkDtSec = 0.01; +constexpr double kMaxVelocityRad = 3.14; +constexpr double kMaxAccelerationRad = 31.4; +constexpr double kMaxJerkRad = 31.4; +constexpr double kPositionGain =2.0; +constexpr double kTargetPosA = 1.35; +constexpr double kTargetPosB = 1.35; + +volatile double g_planner_sink = 0.0; + +void run_position_planner_benchmark(const char* label, rt_int32_t target_switch_period) +{ + cmvr::SCurvePositionPlanner1D planner( + kMaxVelocityRad, + kMaxAccelerationRad, + kMaxJerkRad); + + planner.setPositionGain(kPositionGain); + planner.initialize(0.0, 0.0, 0.0); + planner.setTarget(kTargetPosA); + + for (rt_int32_t i = 0; i < kWarmupIterations; ++i) + { + if (target_switch_period > 0 && (i % target_switch_period) == 0) + { + const bool use_target_a = ((i / target_switch_period) & 1) == 0; + planner.setTarget(use_target_a ? kTargetPosA : kTargetPosB); + } + g_planner_sink = planner.update(kBenchmarkDtSec); + } + + planner.initialize(0.0, 0.0, 0.0); + planner.setTarget(kTargetPosA); + + const rt_tick_t start_tick = rt_tick_get(); + + for (rt_int32_t i = 0; i < kBenchmarkIterations; ++i) + { + if (target_switch_period > 0 && (i % target_switch_period) == 0) + { + const bool use_target_a = ((i / target_switch_period) & 1) == 0; + planner.setTarget(use_target_a ? kTargetPosA : kTargetPosB); + } + g_planner_sink = planner.update(kBenchmarkDtSec); + } + + const rt_tick_t end_tick = rt_tick_get(); + const rt_uint64_t elapsed_ticks = static_cast(end_tick - start_tick); + const rt_uint64_t elapsed_us = + (elapsed_ticks * 1000000ULL) / RT_TICK_PER_SECOND; + const rt_uint64_t avg_ns = + (elapsed_ticks * 1000000000ULL) / + (RT_TICK_PER_SECOND * static_cast(kBenchmarkIterations)); + + rt_kprintf( + "[PlannerBench] %s iter=%d dt=%.3fms switch=%d elapsed=%llu us avg=%llu ns pos=%.6f vel=%.6f moving=%d\n", + label, + kBenchmarkIterations, + kBenchmarkDtSec * 1000.0, + target_switch_period, + elapsed_us, + avg_ns, + planner.getPosition(), + planner.getVelocity(), + planner.isMoving() ? 1 : 0); +} + +void run_planner_benchmarks() +{ + rt_kprintf("[PlannerBench] start RT_TICK_PER_SECOND=%d\n", RT_TICK_PER_SECOND); + run_position_planner_benchmark("hold_target", 0); + run_position_planner_benchmark("retarget_20", 20); + run_position_planner_benchmark("retarget_5", 5); + rt_kprintf("[PlannerBench] done sink=%.6f\n", g_planner_sink); +} + +} // namespace + +int main(void) +{ + int count = 1; + + rt_pin_mode(LED0_PIN, PIN_MODE_OUTPUT); + run_planner_benchmarks(); + + while (count++) + { + rt_pin_write(LED0_PIN, PIN_HIGH); + rt_thread_mdelay(100); + rt_pin_write(LED0_PIN, PIN_LOW); + rt_thread_mdelay(100); + } + + return RT_EOK; +} diff --git a/applications/planner/SConscript b/applications/planner/SConscript new file mode 100644 index 0000000..5c70e65 --- /dev/null +++ b/applications/planner/SConscript @@ -0,0 +1,10 @@ +from building import * +import os + +cwd = GetCurrentDir() +src = Glob('s_curve_planner/src/*.cpp') +CPPPATH = [cwd] + +group = DefineGroup('controller', src, depend = [''], CPPPATH = CPPPATH) + +Return('group') diff --git a/applications/planner/s_curve_planner/include/s_curve_position_planner.h b/applications/planner/s_curve_planner/include/s_curve_position_planner.h new file mode 100644 index 0000000..551abaa --- /dev/null +++ b/applications/planner/s_curve_planner/include/s_curve_position_planner.h @@ -0,0 +1,100 @@ +// +// Created by lgv on 2026/3/10. +// + +#pragma once + +#include "s_curve_velocity_planner.h" + +#include +#include + +namespace cmvr +{ + +struct SCurvePositionPlannerState +{ + double position; + double velocity; + double acceleration; + double jerk; + double target_position; + bool is_moving; + + SCurvePositionPlannerState() + : position(0.0) + , velocity(0.0) + , acceleration(0.0) + , jerk(0.0) + , target_position(0.0) + , is_moving(false) + {} +}; + +/** + * @brief 单轴位置跟踪规划器 + * + * 结构： + * - 外环：position_error -> desired_velocity + * - 内环：SCurveVelocityPlanner1D 负责 jerk-limited 速度规划 + * + * 这个类用于高频变化目标的位置跟踪，不承担严格点到点 profile 语义。 + */ +class SCurvePositionPlanner1D +{ +public: + SCurvePositionPlanner1D(double max_velocity = 3.0, + double max_acceleration = 10.0, + double max_jerk = 50.0); + + void setConstraints(double max_velocity, double max_acceleration, double max_jerk); + void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const; + + void setPositionGain(double position_gain); + double getPositionGain() const { return position_gain_; } + + void initialize(double position, double velocity = 0.0, double acceleration = 0.0); + void reset(); + + void setTarget(double target_position); + double update(double dt); + + const SCurvePositionPlannerState& getState() const { return state_; } + double getPosition() const { return state_.position; } + double getVelocity() const { return state_.velocity; } + double getAcceleration() const { return state_.acceleration; } + double getJerk() const { return state_.jerk; } + double getTargetPosition() const { return state_.target_position; } + bool isMoving() const { return state_.is_moving; } + +private: + double max_velocity_; + double max_acceleration_; + double max_jerk_; + + SCurvePositionPlannerState state_; + SCurveVelocityPlanner1D velocity_planner_; + double position_gain_; + + static constexpr double EPSILON = 1e-9; + static constexpr double VELOCITY_THRESHOLD = 1e-6; + static constexpr double ACCELERATION_THRESHOLD = 1e-6; + static constexpr double POSITION_THRESHOLD = 1e-7; + + double computeDesiredVelocity(double position_error) const; + double computePlannerStoppingDistance(double velocity, double acceleration) const; + + static double clamp(double value, double min_val, double max_val) + { + return std::max(min_val, std::min(max_val, value)); + } + + static double sign(double value) + { + if (value > EPSILON) return 1.0; + if (value < -EPSILON) return -1.0; + return 0.0; + } +}; + +} // namespace cmvr diff --git a/applications/planner/s_curve_planner/include/s_curve_velocity_planner.h b/applications/planner/s_curve_planner/include/s_curve_velocity_planner.h new file mode 100644 index 0000000..f9e5977 --- /dev/null +++ b/applications/planner/s_curve_planner/include/s_curve_velocity_planner.h @@ -0,0 +1,240 @@ +// +// Created by lgv on 2026/3/9. +// + +#pragma once + +#include +#include +#include + +namespace cmvr +{ + +/** + * @brief 单次速度切换的 S 曲线剖面参数 + * + * 本类表示从任意 (v0, a0) 连续规划到 (vf, 0) 的 3 段 jerk-limited profile： + * + * seg1: jerk = j1, duration = t1 + * seg2: jerk = j2, duration = t2 (当前实现 j2 = 0) + * seg3: jerk = j3, duration = t3 + * + * 其中 j1/j3 为 ±j_max，j2 为 0。 + */ +struct SCurveVelocityProfile +{ + double t1; + double t2; + double t3; + + double total_time; + + double j1; + double j2; + double j3; + + double v0; + double a0; + double vf; + + bool valid; + + SCurveVelocityProfile() + : t1(0.0), t2(0.0), t3(0.0) + , total_time(0.0) + , j1(0.0), j2(0.0), j3(0.0) + , v0(0.0), a0(0.0), vf(0.0) + , valid(false) + {} +}; + +/** + * @brief S 曲线速度规划器状态 + */ +struct SCurveVelocityPlannerState +{ + double velocity; + double acceleration; + double jerk; + + double target_velocity; + + double elapsed_time; + bool has_active_profile; + bool is_moving; + + SCurveVelocityPlannerState() + : velocity(0.0) + , acceleration(0.0) + , jerk(0.0) + , target_velocity(0.0) + , elapsed_time(0.0) + , has_active_profile(false) + , is_moving(false) + {} +}; + +/** + * @brief 单轴速度型 S 曲线规划器 + * + * 语义： + * - setTargetVelocity(v): 设置新目标并重规划 + * - update(dt): 只采样当前 active profile + * - overwriteState(v,a): 仅覆盖当前状态并清掉旧 profile + * - synchronizeAndReplan(v,a): 用测量状态同步，并按当前目标重规划 + * + * 数学目标： + * - 从任意 (v0, a0) 连续规划到 (vf, 0) + * - 满足 |v| <= v_max, |a| <= a_max, |j| <= j_max + */ +class SCurveVelocityPlanner1D +{ +public: + SCurveVelocityPlanner1D(double max_velocity = 1.0, + double max_acceleration = 1.0, + double max_jerk = 5.0); + + void setConstraints(double max_velocity, double max_acceleration, double max_jerk); + void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const; + + /** + * @brief 设置目标变化阈值 + * + * 目标变化量小于该阈值时，不触发重规划。 + */ + void setTargetReplanThreshold(double threshold); + + /** + * @brief 设置同步反馈时的重规划阈值 + * + * 测量速度与当前 profile 采样状态的偏差小于该阈值时， + * synchronizeAndReplan() 只对齐当前状态，不打断现有 profile。 + */ + void setSynchronizeReplanThreshold(double threshold); + + void initialize(double velocity = 0.0, double acceleration = 0.0); + void reset(); + + /** + * @brief 仅覆盖当前状态，并清掉旧 profile + * + * 注意： + * - 这个接口不会自动重规划 + * - 用于初始化、调试、或显式中断旧轨迹 + */ + void overwriteState(double velocity, + double acceleration = 0.0, + bool keep_target_velocity = true); + + /** + * @brief 用测量状态同步，并按当前目标重新规划 + * + * 这是闭环场景真正该用的接口。 + */ + void synchronizeAndReplan(double velocity, + double acceleration = 0.0); + + /** + * @brief 设置目标速度，并立即重规划 + */ + void setTargetVelocity(double target_velocity); + + /** + * @brief 平滑停下 + */ + void stop() { setTargetVelocity(0.0); } + + /** + * @brief 更新一步（只采样） + * @param dt 时间步长 + * @return 当前采样后的速度 + */ + double update(double dt); + + /** + * @brief 计算单次速度切换剖面 + */ + SCurveVelocityProfile calculateProfile(double start_velocity, + double end_velocity, + double start_acceleration = 0.0) const; + + /** + * @brief 给定剖面下，获取时刻 t 的速度 + */ + double getVelocityAtTime(const SCurveVelocityProfile& profile, double t) const; + + /** + * @brief 给定剖面下，获取时刻 t 的加速度 + */ + double getAccelerationAtTime(const SCurveVelocityProfile& profile, double t) const; + + /** + * @brief 给定剖面下，获取时刻 t 的 jerk + */ + double getJerkAtTime(const SCurveVelocityProfile& profile, double t) const; + + /** + * @brief 按固定时间间隔生成轨迹点 + */ + void generateTrajectory(const SCurveVelocityProfile& profile, double dt, + std::vector& velocities, + std::vector& accelerations, + std::vector& jerks) const; + + const SCurveVelocityPlannerState& getState() const { return state_; } + const SCurveVelocityProfile& getActiveProfile() const { return active_profile_; } + + double getVelocity() const { return state_.velocity; } + double getAcceleration() const { return state_.acceleration; } + double getJerk() const { return state_.jerk; } + double getTargetVelocity() const { return state_.target_velocity; } + bool hasActiveProfile() const { return state_.has_active_profile; } + bool isMoving() const { return state_.is_moving; } + +private: + double max_velocity_; + double max_acceleration_; + double max_jerk_; + + double target_replan_threshold_; + double synchronize_replan_threshold_; + + SCurveVelocityPlannerState state_; + SCurveVelocityProfile active_profile_; + + static constexpr double EPSILON = 1e-9; + static constexpr double VELOCITY_THRESHOLD = 1e-6; + static constexpr double ACCELERATION_THRESHOLD = 1e-6; + static constexpr double DEFAULT_TARGET_REPLAN_THRESHOLD = 1e-9; + static constexpr double DEFAULT_SYNCHRONIZE_REPLAN_THRESHOLD = 5e-2; + +private: + void updateIsMovingFlag(); + + /** + * @brief 判断 profile 是否满足速度上限 + */ + bool isVelocityLimitSatisfied(const SCurveVelocityProfile& profile) const; + + /** + * @brief 计算完整剖面终点状态 + */ + void integrateWholeProfile(const SCurveVelocityProfile& profile, + double& v_end, + double& a_end) const; + + static double clamp(double value, double min_val, double max_val) + { + return std::max(min_val, std::min(max_val, value)); + } + + static double sign(double value) + { + if (value > EPSILON) return 1.0; + if (value < -EPSILON) return -1.0; + return 0.0; + } +}; + +} // namespace cmvr diff --git a/applications/planner/s_curve_planner/src/s_curve_position_planner.cpp b/applications/planner/s_curve_planner/src/s_curve_position_planner.cpp new file mode 100644 index 0000000..70b5e82 --- /dev/null +++ b/applications/planner/s_curve_planner/src/s_curve_position_planner.cpp @@ -0,0 +1,193 @@ +// +// Created by lgv on 2026/3/10. +// + +#include "planner/s_curve_planner/include/s_curve_position_planner.h" + +#include +#include + +namespace cmvr +{ + +SCurvePositionPlanner1D::SCurvePositionPlanner1D(double max_velocity, + double max_acceleration, + double max_jerk) + : max_velocity_(std::abs(max_velocity)) + , max_acceleration_(std::abs(max_acceleration)) + , max_jerk_(std::abs(max_jerk)) + , velocity_planner_(max_velocity_, max_acceleration_, max_jerk_) + , position_gain_(1.0) +{ + velocity_planner_.setTargetReplanThreshold(std::max(1e-3, 0.02 * max_velocity_)); + reset(); +} + +void SCurvePositionPlanner1D::setConstraints(double max_velocity, + double max_acceleration, + double max_jerk) +{ + max_velocity_ = std::abs(max_velocity); + max_acceleration_ = std::abs(max_acceleration); + max_jerk_ = std::abs(max_jerk); + velocity_planner_.setConstraints(max_velocity_, max_acceleration_, max_jerk_); + velocity_planner_.setTargetReplanThreshold(std::max(1e-3, 0.02 * max_velocity_)); +} + +void SCurvePositionPlanner1D::getConstraints(double& max_velocity, + double& max_acceleration, + double& max_jerk) const +{ + max_velocity = max_velocity_; + max_acceleration = max_acceleration_; + max_jerk = max_jerk_; +} + +void SCurvePositionPlanner1D::setPositionGain(double position_gain) +{ + position_gain_ = std::max(0.0, position_gain); +} + +void SCurvePositionPlanner1D::initialize(double position, double velocity, double acceleration) +{ + state_.position = position; + velocity_planner_.initialize(velocity, acceleration); + state_.velocity = velocity_planner_.getVelocity(); + state_.acceleration = velocity_planner_.getAcceleration(); + state_.jerk = 0.0; + state_.target_position = position; + state_.is_moving = false; +} + +void SCurvePositionPlanner1D::reset() +{ + state_ = SCurvePositionPlannerState(); + velocity_planner_.reset(); +} + +void SCurvePositionPlanner1D::setTarget(double target_position) +{ + state_.target_position = target_position; +} + +double SCurvePositionPlanner1D::update(double dt) +{ + if (dt <= 0.0 || dt > 0.1) { + return state_.position; + } + + const double position_error = state_.target_position - state_.position; + if (std::abs(position_error) < POSITION_THRESHOLD && + std::abs(velocity_planner_.getVelocity()) < VELOCITY_THRESHOLD && + std::abs(velocity_planner_.getAcceleration()) < ACCELERATION_THRESHOLD) { + velocity_planner_.overwriteState(0.0, 0.0, false); + state_.position = state_.target_position; + state_.velocity = 0.0; + state_.acceleration = 0.0; + state_.jerk = 0.0; + state_.is_moving = false; + return state_.position; + } + + velocity_planner_.setTargetVelocity(computeDesiredVelocity(position_error)); + + const double old_velocity = velocity_planner_.getVelocity(); + const double new_velocity = velocity_planner_.update(dt); + + state_.velocity = new_velocity; + state_.acceleration = velocity_planner_.getAcceleration(); + state_.jerk = velocity_planner_.getJerk(); + state_.position += 0.5 * (old_velocity + new_velocity) * dt; + + const double updated_error = state_.target_position - state_.position; + if (std::abs(updated_error) < POSITION_THRESHOLD && + std::abs(state_.velocity) < VELOCITY_THRESHOLD && + std::abs(state_.acceleration) < ACCELERATION_THRESHOLD) { + velocity_planner_.overwriteState(0.0, 0.0, false); + state_.position = state_.target_position; + state_.velocity = 0.0; + state_.acceleration = 0.0; + state_.jerk = 0.0; + state_.is_moving = false; + return state_.position; + } + + state_.is_moving = velocity_planner_.isMoving() || + std::abs(updated_error) > POSITION_THRESHOLD; + return state_.position; +} + +double SCurvePositionPlanner1D::computeDesiredVelocity(double position_error) const +{ + if (std::abs(position_error) <= POSITION_THRESHOLD) { + return 0.0; + } + + const double proportional_velocity = + clamp(position_gain_ * position_error, -max_velocity_, max_velocity_); + + return proportional_velocity; + const double current_velocity = velocity_planner_.getVelocity(); + const double current_acceleration = velocity_planner_.getAcceleration(); + const double stopping_distance = + computePlannerStoppingDistance(current_velocity, current_acceleration); + + if (std::abs(current_velocity) > VELOCITY_THRESHOLD && + sign(current_velocity) != sign(position_error)) { + return 0.0; + } + + if (std::abs(current_velocity) > VELOCITY_THRESHOLD && + sign(current_velocity) == sign(position_error) && + std::abs(position_error) <= std::abs(stopping_distance) * 1.05) { + return 0.0; + } + + double low = 0.0; + double high = max_velocity_; + for (int i = 0; i < 40; ++i) { + const double mid = 0.5 * (low + high); + const double distance = + std::abs(computePlannerStoppingDistance(sign(position_error) * mid, 0.0)); + if (distance <= std::abs(position_error)) { + low = mid; + } else { + high = mid; + } + } + + return sign(position_error) * + std::min(std::abs(proportional_velocity), low); +} + +double SCurvePositionPlanner1D::computePlannerStoppingDistance(double velocity, + double acceleration) const +{ + if (std::abs(velocity) <= VELOCITY_THRESHOLD && + std::abs(acceleration) <= ACCELERATION_THRESHOLD) { + return 0.0; + } + + const auto profile = velocity_planner_.calculateProfile(velocity, 0.0, acceleration); + if (!profile.valid || profile.total_time <= EPSILON) { + return 0.0; + } + + const int steps = std::max(16, static_cast(std::ceil(profile.total_time / 0.002))); + const double dt = profile.total_time / static_cast(steps); + + double distance = 0.0; + double previous_time = 0.0; + double previous_velocity = velocity_planner_.getVelocityAtTime(profile, 0.0); + for (int i = 1; i <= steps; ++i) { + const double t = (i == steps) ? profile.total_time : i * dt; + const double current_velocity = velocity_planner_.getVelocityAtTime(profile, t); + distance += 0.5 * (previous_velocity + current_velocity) * (t - previous_time); + previous_time = t; + previous_velocity = current_velocity; + } + + return distance; +} + +} // namespace cmvr diff --git a/applications/planner/s_curve_planner/src/s_curve_velocity_planner.cpp b/applications/planner/s_curve_planner/src/s_curve_velocity_planner.cpp new file mode 100644 index 0000000..b674819 --- /dev/null +++ b/applications/planner/s_curve_planner/src/s_curve_velocity_planner.cpp @@ -0,0 +1,562 @@ +// +// Created by lgv on 2026/3/9. +// + +#include "../include/s_curve_velocity_planner.h" + +#include +#include + +namespace cmvr +{ + +SCurveVelocityPlanner1D::SCurveVelocityPlanner1D(double max_velocity, + double max_acceleration, + double max_jerk) + : max_velocity_(std::abs(max_velocity)) + , max_acceleration_(std::abs(max_acceleration)) + , max_jerk_(std::abs(max_jerk)) + , target_replan_threshold_(DEFAULT_TARGET_REPLAN_THRESHOLD) + , synchronize_replan_threshold_(DEFAULT_SYNCHRONIZE_REPLAN_THRESHOLD) +{ + reset(); +} + +void SCurveVelocityPlanner1D::setConstraints(double max_velocity, + double max_acceleration, + double max_jerk) +{ + max_velocity_ = std::abs(max_velocity); + max_acceleration_ = std::abs(max_acceleration); + max_jerk_ = std::abs(max_jerk); + + state_.velocity = clamp(state_.velocity, -max_velocity_, max_velocity_); + state_.acceleration = clamp(state_.acceleration, -max_acceleration_, max_acceleration_); + state_.target_velocity = clamp(state_.target_velocity, -max_velocity_, max_velocity_); + + if (state_.has_active_profile) { + active_profile_ = calculateProfile(state_.velocity, + state_.target_velocity, + state_.acceleration); + state_.elapsed_time = 0.0; + state_.has_active_profile = active_profile_.valid && active_profile_.total_time > EPSILON; + } + + updateIsMovingFlag(); +} + +void SCurveVelocityPlanner1D::getConstraints(double& max_velocity, + double& max_acceleration, + double& max_jerk) const +{ + max_velocity = max_velocity_; + max_acceleration = max_acceleration_; + max_jerk = max_jerk_; +} + +void SCurveVelocityPlanner1D::setTargetReplanThreshold(double threshold) +{ + target_replan_threshold_ = std::max(0.0, threshold); +} + +void SCurveVelocityPlanner1D::setSynchronizeReplanThreshold(double threshold) +{ + synchronize_replan_threshold_ = std::max(0.0, threshold); +} + +void SCurveVelocityPlanner1D::initialize(double velocity, double acceleration) +{ + state_ = SCurveVelocityPlannerState(); + state_.velocity = clamp(velocity, -max_velocity_, max_velocity_); + state_.acceleration = clamp(acceleration, -max_acceleration_, max_acceleration_); + state_.jerk = 0.0; + state_.target_velocity = state_.velocity; + state_.elapsed_time = 0.0; + state_.has_active_profile = false; + + active_profile_ = SCurveVelocityProfile(); + active_profile_.v0 = state_.velocity; + active_profile_.a0 = state_.acceleration; + active_profile_.vf = state_.velocity; + active_profile_.valid = false; + + updateIsMovingFlag(); +} + +void SCurveVelocityPlanner1D::reset() +{ + state_ = SCurveVelocityPlannerState(); + active_profile_ = SCurveVelocityProfile(); +} + +void SCurveVelocityPlanner1D::overwriteState(double velocity, + double acceleration, + bool keep_target_velocity) +{ + state_.velocity = clamp(velocity, -max_velocity_, max_velocity_); + state_.acceleration = clamp(acceleration, -max_acceleration_, max_acceleration_); + state_.jerk = 0.0; + state_.elapsed_time = 0.0; + state_.has_active_profile = false; + + if (!keep_target_velocity) { + state_.target_velocity = state_.velocity; + } else { + state_.target_velocity = clamp(state_.target_velocity, -max_velocity_, max_velocity_); + } + + active_profile_ = SCurveVelocityProfile(); + active_profile_.v0 = state_.velocity; + active_profile_.a0 = state_.acceleration; + active_profile_.vf = state_.target_velocity; + active_profile_.valid = false; + + updateIsMovingFlag(); +} + + void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity, + double acceleration) +{ + const double measured_velocity = clamp(velocity, -max_velocity_, max_velocity_); + const double measured_acceleration = + clamp(acceleration, -max_acceleration_, max_acceleration_); + + if (!state_.has_active_profile && + std::abs(measured_velocity - state_.target_velocity) <= VELOCITY_THRESHOLD) { + state_.velocity = state_.target_velocity; + state_.acceleration = 0.0; + state_.jerk = 0.0; + updateIsMovingFlag(); + return; + } + + // 如果测量值已经基本落在当前采样状态上，就继续沿现有 profile 走。 + // 否则每拍都从同一目标重规划，会把已经进入的 jerk phase 反复打断。 + if (state_.has_active_profile && + std::abs(measured_velocity - state_.velocity) <= synchronize_replan_threshold_) { + state_.velocity = measured_velocity; + state_.acceleration = measured_acceleration; + state_.jerk = getJerkAtTime(active_profile_, state_.elapsed_time); + updateIsMovingFlag(); + return; + } + + state_.velocity = measured_velocity; + state_.acceleration = measured_acceleration; + state_.jerk = 0.0; + + active_profile_ = calculateProfile(state_.velocity, + state_.target_velocity, + state_.acceleration); + + state_.elapsed_time = 0.0; + state_.has_active_profile = active_profile_.total_time > EPSILON; + + updateIsMovingFlag(); +} + +void SCurveVelocityPlanner1D::setTargetVelocity(double target_velocity) +{ + const double new_target = clamp(target_velocity, -max_velocity_, max_velocity_); + + const bool same_target = + std::abs(new_target - state_.target_velocity) <= target_replan_threshold_; + + if (same_target && + state_.has_active_profile) { + return; + } + + if (same_target && + !state_.has_active_profile && + std::abs(state_.velocity - new_target) <= VELOCITY_THRESHOLD && + std::abs(state_.acceleration) <= ACCELERATION_THRESHOLD) { + return; + } + + state_.target_velocity = new_target; + + active_profile_ = calculateProfile(state_.velocity, + state_.target_velocity, + state_.acceleration); + + state_.elapsed_time = 0.0; + state_.has_active_profile = active_profile_.valid && active_profile_.total_time > EPSILON; + + updateIsMovingFlag(); +} + +double SCurveVelocityPlanner1D::getVelocityAtTime(const SCurveVelocityProfile& profile, double t) const +{ + if (!profile.valid) { + return profile.v0; + } + + if (t <= 0.0) { + return profile.v0; + } + if (t >= profile.total_time) { + return profile.vf; + } + + double v = profile.v0; + double a = profile.a0; + + auto process_segment = [&](double dt, double jerk) { + v += a * dt + 0.5 * jerk * dt * dt; + a += jerk * dt; + }; + + // seg1 + if (t <= profile.t1) { + process_segment(t, profile.j1); + return v; + } + process_segment(profile.t1, profile.j1); + + // seg2 + const double t_after_1 = t - profile.t1; + if (t_after_1 <= profile.t2) { + process_segment(t_after_1, profile.j2); + return v; + } + process_segment(profile.t2, profile.j2); + + // seg3 + const double t_after_2 = t_after_1 - profile.t2; + process_segment(t_after_2, profile.j3); + return v; +} + +double SCurveVelocityPlanner1D::getAccelerationAtTime(const SCurveVelocityProfile& profile, double t) const +{ + if (!profile.valid) { + return profile.a0; + } + + if (t <= 0.0) { + return profile.a0; + } + if (t >= profile.total_time) { + return 0.0; + } + + double a = profile.a0; + + auto process_segment = [&](double dt, double jerk) { + a += jerk * dt; + }; + + // seg1 + if (t <= profile.t1) { + process_segment(t, profile.j1); + return a; + } + process_segment(profile.t1, profile.j1); + + // seg2 + const double t_after_1 = t - profile.t1; + if (t_after_1 <= profile.t2) { + process_segment(t_after_1, profile.j2); + return a; + } + process_segment(profile.t2, profile.j2); + + // seg3 + const double t_after_2 = t_after_1 - profile.t2; + process_segment(t_after_2, profile.j3); + return a; +} + +double SCurveVelocityPlanner1D::getJerkAtTime(const SCurveVelocityProfile& profile, double t) const +{ + if (!profile.valid) { + return 0.0; + } + + if (t <= 0.0 || t >= profile.total_time) { + return 0.0; + } + + if (t <= profile.t1) { + return profile.j1; + } + if (t <= profile.t1 + profile.t2) { + return profile.j2; + } + return profile.j3; +} + +void SCurveVelocityPlanner1D::integrateWholeProfile(const SCurveVelocityProfile& profile, + double& v_end, + double& a_end) const +{ + double v = profile.v0; + double a = profile.a0; + + auto process_segment = [&](double dt, double jerk) { + v += a * dt + 0.5 * jerk * dt * dt; + a += jerk * dt; + }; + + process_segment(profile.t1, profile.j1); + process_segment(profile.t2, profile.j2); + process_segment(profile.t3, profile.j3); + + v_end = v; + a_end = a; +} + +bool SCurveVelocityPlanner1D::isVelocityLimitSatisfied(const SCurveVelocityProfile& profile) const +{ + if (!profile.valid) { + return true; + } + + std::vector check_times; + check_times.reserve(8); + + check_times.push_back(0.0); + check_times.push_back(profile.t1); + check_times.push_back(profile.t1 + profile.t2); + check_times.push_back(profile.total_time); + + // seg1 内部若 acceleration 过零，则 velocity 可能在段内出现极值 + if (std::abs(profile.j1) > EPSILON && profile.t1 > EPSILON) { + const double tz = -profile.a0 / profile.j1; + if (tz > EPSILON && tz < profile.t1 - EPSILON) { + check_times.push_back(tz); + } + } + + for (double t : check_times) { + const double v = getVelocityAtTime(profile, t); + if (std::abs(v) > max_velocity_ + 1e-7) { + return false; + } + } + + return true; +} + +SCurveVelocityProfile SCurveVelocityPlanner1D::calculateProfile(double start_velocity, + double end_velocity, + double start_acceleration) const +{ + SCurveVelocityProfile best; + best.v0 = clamp(start_velocity, -max_velocity_, max_velocity_); + best.a0 = clamp(start_acceleration, -max_acceleration_, max_acceleration_); + best.vf = clamp(end_velocity, -max_velocity_, max_velocity_); + + if (max_jerk_ <= EPSILON || max_acceleration_ <= EPSILON) { + // 退化情况：这里不做特殊退化规划，直接返回 invalid + return best; + } + + const double v0 = best.v0; + const double a0 = best.a0; + const double vf = best.vf; + + // 目标方向坐标：尽量统一到“signed frame 下 Δv >= 0” + double s = sign(vf - v0); + if (std::abs(vf - v0) <= VELOCITY_THRESHOLD) { + s = 1.0; + } + + const double dv = s * (vf - v0); // >= 0 + const double a0s = s * a0; // signed frame 下的初始 acceleration + const double j = max_jerk_; + const double amax = max_acceleration_; + + auto normalize_nonnegative = [](double x) { + return (x < 0.0 && x > -1e-10) ? 0.0 : x; + }; + + auto try_candidate = [&](double t1, double t2, double t3, + double j1s, double j2s, double j3s) { + t1 = normalize_nonnegative(t1); + t2 = normalize_nonnegative(t2); + t3 = normalize_nonnegative(t3); + + if (t1 < 0.0 || t2 < 0.0 || t3 < 0.0) { + return; + } + + SCurveVelocityProfile cand; + cand.t1 = t1; + cand.t2 = t2; + cand.t3 = t3; + cand.total_time = t1 + t2 + t3; + + cand.j1 = s * j1s; + cand.j2 = s * j2s; + cand.j3 = s * j3s; + + cand.v0 = v0; + cand.a0 = a0; + cand.vf = vf; + cand.valid = true; + + double vend = 0.0; + double aend = 0.0; + integrateWholeProfile(cand, vend, aend); + + if (std::abs(vend - vf) > 1e-5) { + return; + } + if (std::abs(aend) > 1e-5) { + return; + } + if (!isVelocityLimitSatisfied(cand)) { + return; + } + + if (!best.valid || cand.total_time < best.total_time) { + best = cand; + } + }; + + // ---------------- UDU 三角形（无平台） ---------------- + { + // Δv = (2A^2 - a0^2)/(2j) + const double A2 = 0.5 * (2.0 * j * dv + a0s * a0s); + if (A2 >= -1e-10) { + const double A = std::sqrt(std::max(0.0, A2)); + if (A <= amax + 1e-10 && A >= a0s - 1e-10) { + const double t1 = (A - a0s) / j; + const double t2 = 0.0; + const double t3 = A / j; + try_candidate(t1, t2, t3, +j, 0.0, -j); + } + } + } + + // ---------------- UDU 梯形（有平台） ---------------- + { + const double A = amax; + if (A > EPSILON && A >= a0s - 1e-10) { + const double base = (2.0 * A * A - a0s * a0s) / (2.0 * j); + const double t1 = (A - a0s) / j; + const double t2 = (dv - base) / A; + const double t3 = A / j; + try_candidate(t1, t2, t3, +j, 0.0, -j); + } + } + + // ---------------- DUD 三角形（无平台） ---------------- + { + // Δv = (a0^2 - 2B^2)/(2j) + const double B2 = 0.5 * (a0s * a0s - 2.0 * j * dv); + if (B2 >= -1e-10) { + const double B = std::sqrt(std::max(0.0, B2)); + if (B <= amax + 1e-10 && B >= -a0s - 1e-10) { + const double t1 = (a0s + B) / j; + const double t2 = 0.0; + const double t3 = B / j; + try_candidate(t1, t2, t3, -j, 0.0, +j); + } + } + } + + // ---------------- DUD 梯形（有平台） ---------------- + { + const double B = amax; + if (B > EPSILON && B >= -a0s - 1e-10) { + const double base = (a0s * a0s - 2.0 * B * B) / (2.0 * j); + const double t1 = (a0s + B) / j; + const double t2 = (base - dv) / B; + const double t3 = B / j; + try_candidate(t1, t2, t3, -j, 0.0, +j); + } + } + + // 已到终点附近 + if (!best.valid && + std::abs(vf - v0) <= VELOCITY_THRESHOLD && + std::abs(a0) <= ACCELERATION_THRESHOLD) { + best.valid = false; + best.total_time = 0.0; + best.v0 = v0; + best.a0 = a0; + best.vf = vf; + } + + return best; +} + +void SCurveVelocityPlanner1D::generateTrajectory(const SCurveVelocityProfile& profile, double dt, + std::vector& velocities, + std::vector& accelerations, + std::vector& jerks) const +{ + velocities.clear(); + accelerations.clear(); + jerks.clear(); + + if (!profile.valid || profile.total_time <= 0.0 || dt <= 0.0) { + velocities.push_back(profile.v0); + accelerations.push_back(profile.a0); + jerks.push_back(0.0); + return; + } + + const int num_points = static_cast(std::ceil(profile.total_time / dt)) + 1; + velocities.reserve(num_points); + accelerations.reserve(num_points); + jerks.reserve(num_points); + + for (double t = 0.0; t <= profile.total_time; t += dt) { + velocities.push_back(getVelocityAtTime(profile, t)); + accelerations.push_back(getAccelerationAtTime(profile, t)); + jerks.push_back(getJerkAtTime(profile, t)); + } + + if (velocities.empty() || std::abs(velocities.back() - profile.vf) > VELOCITY_THRESHOLD) { + velocities.push_back(profile.vf); + accelerations.push_back(0.0); + jerks.push_back(0.0); + } +} + +double SCurveVelocityPlanner1D::update(double dt) +{ + if (dt <= 0.0) { + return state_.velocity; + } + + if (!state_.has_active_profile || !active_profile_.valid) { + state_.jerk = 0.0; + updateIsMovingFlag(); + return state_.velocity; + } + + state_.elapsed_time += dt; + + if (state_.elapsed_time >= active_profile_.total_time) { + state_.elapsed_time = active_profile_.total_time; + state_.velocity = active_profile_.vf; + state_.acceleration = 0.0; + state_.jerk = 0.0; + state_.has_active_profile = false; + updateIsMovingFlag(); + return state_.velocity; + } + + state_.velocity = getVelocityAtTime(active_profile_, state_.elapsed_time); + state_.acceleration = getAccelerationAtTime(active_profile_, state_.elapsed_time); + state_.jerk = getJerkAtTime(active_profile_, state_.elapsed_time); + + updateIsMovingFlag(); + return state_.velocity; +} + +void SCurveVelocityPlanner1D::updateIsMovingFlag() +{ + state_.is_moving = + state_.has_active_profile || + std::abs(state_.velocity) > VELOCITY_THRESHOLD || + std::abs(state_.target_velocity) > VELOCITY_THRESHOLD || + std::abs(state_.acceleration) > ACCELERATION_THRESHOLD; +} + +} // namespace cmvr diff --git a/applications/servo_manager/SConscript b/applications/servo_manager/SConscript index 0682504..cc30498 100644 --- a/applications/servo_manager/SConscript +++ b/applications/servo_manager/SConscript @@ -5,6 +5,6 @@ cwd = GetCurrentDir() src = Glob('src/*.cpp') CPPPATH = [cwd] -group = DefineGroup('servo_manager', src, depend = [''], CPPPATH = CPPPATH) +group = DefineGroup('servo_control', src, depend = [''], CPPPATH = CPPPATH) Return('group') diff --git a/applications/servo_manager/include/servo_control.h b/applications/servo_manager/include/servo_control.h new file mode 100644 index 0000000..6c2da72 --- /dev/null +++ b/applications/servo_manager/include/servo_control.h @@ -0,0 +1,67 @@ +// +// Created by Administrator on 2026/4/22. +// + +#ifndef SERVO_CONTROL_H +#define SERVO_CONTROL_H + +#include +#include + +#include "common/type/servo_types.h" +#include "common/curve/include/s_curve.h" +#include "servo_manager/include/servo_driver.h" +#include + + +class ServoControl +{ +public: + explicit ServoControl(const ServoDriver::Config& cfg); + ~ServoControl(); + + rt_err_t init(bool enable_after_init = true, bool go_home = true); + + rt_err_t move(float angle_rad, MotionMode mode); + rt_err_t stop(); + + void setConstraints(double max_velocity_rad, + double max_acceleration_rad, + double max_jerk_rad); + + void update(rt_tick_t now_tick); + + const std::string& id() const; + + float currentAngle() const; + float targetAngle() const; + float currentVelocity() const; + + bool isActive() const; + +private: + void sampleStateNoLock(rt_tick_t now_tick); + double ticksToSeconds(rt_tick_t delta_ticks) const; + +private: + // 角度变化阈值，rad + static constexpr float ESP = 1e-4f; + ServoDriver driver_; + cmvr::SCurve curve_; + + float current_angle_rad_; + float current_velocity_rad_per_sec_; + float target_angle_rad_; + + rt_tick_t start_tick_; + uint32_t generation_; + + bool active_; + bool dirty_; + + cmvr::SCurveProfile profile_; + + rt_mutex_t mutex_; +}; + +#endif // SERVO_CONTROL_H \ No newline at end of file diff --git a/applications/servo_manager/include/servo_driver.h b/applications/servo_manager/include/servo_driver.h new file mode 100644 index 0000000..0ed0d75 --- /dev/null +++ b/applications/servo_manager/include/servo_driver.h @@ -0,0 +1,61 @@ +// +// Created by Administrator on 2026/4/22. +// + +#ifndef RTTHREAD_SERVO_DRIVER_H +#define RTTHREAD_SERVO_DRIVER_H + + +#include +#include + +#include +#include + +class ServoDriver +{ +public: + struct Config + { + std::string servo_id; + std::string pwm_dev_name; + int pwm_channel; + + rt_uint32_t period_ns; + rt_uint32_t min_pulse_ns; + rt_uint32_t max_pulse_ns; + + float min_angle_rad; + float max_angle_rad; + float home_angle_rad; + }; + +public: + explicit ServoDriver(const Config& cfg); + ~ServoDriver(); + + rt_err_t init(bool enable_after_init = true, bool go_home = true); + + rt_err_t enable(bool on); + rt_err_t setAngle(float angle_rad); + + const std::string& id() const; + const Config& config() const; + +private: + rt_err_t enableNoLock(bool on); + rt_err_t setAngleNoLock(float angle_rad); + + static float clampf(float v, float lo, float hi); + static rt_uint32_t angleToPulseNs(const Config& cfg, float angle_rad); + +private: + Config cfg_; + rt_device_pwm* pwm_dev_; + rt_uint32_t last_pulse_; + bool inited_; + + rt_mutex_t mutex_; +}; + +#endif //RTTHREAD_SERVO_DRIVER_H diff --git a/applications/servo_manager/include/servo_manager.h b/applications/servo_manager/include/servo_manager.h index b2f8e31..9f8cacb 100644 --- a/applications/servo_manager/include/servo_manager.h +++ b/applications/servo_manager/include/servo_manager.h @@ -1,83 +1,54 @@ // -// Created by Administrator on 2026/1/7. +// Created by Administrator on 2026/4/22. // #ifndef SERVO_MANAGER_H #define SERVO_MANAGER_H -extern "C" { -#include -#include -} - -#include #include +#include +#include #include +#include "servo_manager/include/servo_control.h" +#include "servo_manager/include/servo_driver.h" + +#include + class ServoManager { public: - struct ServoConfig - { - const char* servo_id; // 唯一ID - const char* pwm_dev_name; - int pwm_channel; - - // ns - rt_uint32_t period_ns; - rt_uint32_t min_pulse_ns; - rt_uint32_t max_pulse_ns; - - float min_angle_deg; - float max_angle_deg; - float home_angle_deg; - }; - - struct ServoCmd - { - const char* id; - float angle_deg; - }; - -public: - - explicit ServoManager(const std::vector &cfg); + explicit ServoManager(const std::vector& cfg); ~ServoManager(); - size_t count() const { return _cfg.size(); } - rt_err_t init(bool enable_after_init = true, bool go_home = true); - rt_err_t enable(const char* servo_id, bool on); - rt_err_t setAngle(const char* servo_id, float angle_deg); + ServoControl* get(const std::string& servo_id); + const ServoControl* get(const std::string& servo_id) const; - // 全部设置：两种形态（vector / 指针） - rt_err_t setAllAngles(const std::vector& angles_deg); - rt_err_t setAllAngles(const float* angles_deg, size_t n); + size_t count() const; - // 批量设置：两种形态（vector / 指针） - rt_err_t setAngles(const std::vector& cmds); - rt_err_t setAngles(const ServoCmd* cmds, size_t n); + rt_err_t start(); + void stop(); - const char* idAt(size_t order) const; - int orderOf(const char* servo_id) const; + void setUpdatePeriodMs(rt_uint32_t update_period_ms); + void wake(); private: - rt_err_t enableNoLock(size_t order, bool on); - rt_err_t setAngleNoLock(size_t order, float angle_deg); + static void threadEntry(void* parameter); + void run(); - int findOrderByIdNoLock(const char* servo_id) const; - - static float clampf(float v, float lo, float hi); - static rt_uint32_t angleToPulseNs(const ServoConfig& c, float angle_deg); + int findIndexByIdNoLock(const std::string& servo_id) const; private: - std::vector _cfg; - std::vector _pwm_dev; - std::vector _last_pulse; + std::vector controls_; - rt_mutex_t _mutex; + rt_mutex_t mutex_; + rt_sem_t wake_sem_; + rt_thread_t thread_; + + rt_uint32_t update_period_ms_{1}; + volatile bool running_; }; #endif // SERVO_MANAGER_H - diff --git a/applications/servo_manager/src/servo_control.cpp b/applications/servo_manager/src/servo_control.cpp new file mode 100644 index 0000000..71f4163 --- /dev/null +++ b/applications/servo_manager/src/servo_control.cpp @@ -0,0 +1,283 @@ +// +// Created by Administrator on 2026/4/22. +// + +#include "servo_manager/include/servo_control.h" + +#include + + +ServoControl::ServoControl(const ServoDriver::Config& cfg) + : driver_(cfg), + curve_(), + current_angle_rad_(cfg.home_angle_rad), + current_velocity_rad_per_sec_(0.0f), + target_angle_rad_(cfg.home_angle_rad), + start_tick_(0), + generation_(0), + active_(false), + dirty_(false), + profile_(), + mutex_(RT_NULL) +{ + mutex_ = rt_mutex_create("sv_ctl", RT_IPC_FLAG_PRIO); +} + +ServoControl::~ServoControl() +{ + if (mutex_) { + rt_mutex_delete(mutex_); + mutex_ = RT_NULL; + } +} + +rt_err_t ServoControl::init(bool enable_after_init, bool go_home) +{ + const rt_err_t err = driver_.init(enable_after_init, go_home); + if (err != RT_EOK) { + return err; + } + + if (!mutex_) { + return -RT_ENOMEM; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + + const float init_angle = go_home ? driver_.config().home_angle_rad + : driver_.config().min_angle_rad; + + current_angle_rad_ = init_angle; + current_velocity_rad_per_sec_ = 0.0f; + target_angle_rad_ = init_angle; + start_tick_ = rt_tick_get(); + generation_ = 0; + active_ = false; + dirty_ = false; + + rt_mutex_release(mutex_); + return RT_EOK; +} + +rt_err_t ServoControl::move(float angle_rad, MotionMode mode) +{ + if (!mutex_) { + return -RT_ENOMEM; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + + const rt_tick_t now_tick = rt_tick_get(); + sampleStateNoLock(now_tick); + + ++generation_; + target_angle_rad_ = angle_rad; + + if (mode == MotionMode::Immediate) { + current_angle_rad_ = angle_rad; + current_velocity_rad_per_sec_ = 0.0f; + start_tick_ = now_tick; + active_ = false; + dirty_ = false; + + rt_mutex_release(mutex_); + return driver_.setAngle(angle_rad); + } + + if (mode != MotionMode::SCurve) { + rt_mutex_release(mutex_); + return -RT_EINVAL; + } + + const float start_angle_rad = current_angle_rad_; + const float start_velocity_rad_per_sec = current_velocity_rad_per_sec_; + profile_ = curve_.calculateProfile( + current_angle_rad_, angle_rad, current_velocity_rad_per_sec_, 0.0); + + if (profile_.total_time <= 0.0) { + profile_ = curve_.calculateProfile( + current_angle_rad_, angle_rad, 0.0, 0.0); + } + + start_tick_ = now_tick; + + if (profile_.total_time <= 0.0) { + if (std::fabs(angle_rad - start_angle_rad) > ESP) { + rt_kprintf("[ServoControl][W] scurve fallback to immediate, id=%s start=%.4f target=%.4f vel=%.4f\n", + id().c_str(), + (double)start_angle_rad, + (double)angle_rad, + (double)start_velocity_rad_per_sec); + } + current_angle_rad_ = angle_rad; + current_velocity_rad_per_sec_ = 0.0f; + active_ = false; + dirty_ = false; + + rt_mutex_release(mutex_); + return driver_.setAngle(angle_rad); + } + + const double tick_sec = 1.0 / RT_TICK_PER_SECOND; + if (profile_.total_time <= tick_sec) { + rt_kprintf("[ServoControl][W] scurve duration too short, id=%s duration=%.6f tick=%.6f\n", + id().c_str(), + profile_.total_time, + tick_sec); + } + + active_ = true; + dirty_ = false; + + rt_mutex_release(mutex_); + return RT_EOK; +} + +rt_err_t ServoControl::stop() +{ + if (!mutex_) { + return -RT_ENOMEM; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + + const rt_tick_t now_tick = rt_tick_get(); + sampleStateNoLock(now_tick); + + ++generation_; + target_angle_rad_ = current_angle_rad_; + current_velocity_rad_per_sec_ = 0.0f; + start_tick_ = now_tick; + active_ = false; + dirty_ = false; + + const float hold_angle = current_angle_rad_; + + rt_mutex_release(mutex_); + return driver_.setAngle(hold_angle); +} + +void ServoControl::setConstraints(double max_velocity_rad, + double max_acceleration_rad, + double max_jerk_rad) +{ + if (!mutex_) { + curve_.setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad); + return; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + curve_.setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad); + rt_mutex_release(mutex_); +} + +void ServoControl::update(rt_tick_t now_tick) +{ + if (!mutex_) { + return; + } + + float output_angle = 0.0f; + bool should_write = false; + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + + const float previous_angle = current_angle_rad_; + sampleStateNoLock(now_tick); + + if (dirty_ || std::fabs(current_angle_rad_ - previous_angle) > ESP) { + output_angle = current_angle_rad_; + dirty_ = false; + should_write = true; + } + + rt_mutex_release(mutex_); + + if (should_write) { + const rt_err_t err = driver_.setAngle(output_angle); + if (err != RT_EOK) { + rt_kprintf("[ServoControl][E] setAngle failed, id=%s angle=%.4f err=%d\n", + id().c_str(), + (double)output_angle, + (int)err); + } + } +} + +const std::string& ServoControl::id() const +{ + return driver_.id(); +} + +float ServoControl::currentAngle() const +{ + if (!mutex_) { + return current_angle_rad_; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const float v = current_angle_rad_; + rt_mutex_release(mutex_); + return v; +} + +float ServoControl::targetAngle() const +{ + if (!mutex_) { + return target_angle_rad_; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const float v = target_angle_rad_; + rt_mutex_release(mutex_); + return v; +} + +float ServoControl::currentVelocity() const +{ + if (!mutex_) { + return current_velocity_rad_per_sec_; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const float v = current_velocity_rad_per_sec_; + rt_mutex_release(mutex_); + return v; +} + +bool ServoControl::isActive() const +{ + if (!mutex_) { + return active_; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const bool v = active_; + rt_mutex_release(mutex_); + return v; +} + +void ServoControl::sampleStateNoLock(rt_tick_t now_tick) +{ + if (!active_) { + return; + } + + const double elapsed_sec = ticksToSeconds(now_tick - start_tick_); + if (elapsed_sec >= profile_.total_time) { + current_angle_rad_ = target_angle_rad_; + current_velocity_rad_per_sec_ = 0.0f; + active_ = false; + return; + } + + current_angle_rad_ = static_cast( + curve_.getPositionAtTime(profile_, elapsed_sec)); + current_velocity_rad_per_sec_ = static_cast( + curve_.getVelocityAtTime(profile_, elapsed_sec)); +} + +double ServoControl::ticksToSeconds(rt_tick_t delta_ticks) const +{ + return static_cast(delta_ticks) / RT_TICK_PER_SECOND; +} diff --git a/applications/servo_manager/src/servo_driver.cpp b/applications/servo_manager/src/servo_driver.cpp new file mode 100644 index 0000000..0a3ad35 --- /dev/null +++ b/applications/servo_manager/src/servo_driver.cpp @@ -0,0 +1,188 @@ +// +// Created by Administrator on 2026/4/22. +// + +#include "servo_manager/include/servo_driver.h" + +ServoDriver::ServoDriver(const Config& cfg) + : cfg_(cfg), + pwm_dev_(RT_NULL), + last_pulse_(0), + inited_(false), + mutex_(RT_NULL) +{ + mutex_ = rt_mutex_create("sv_drv", RT_IPC_FLAG_PRIO); +} + +ServoDriver::~ServoDriver() +{ + if (mutex_) { + rt_mutex_delete(mutex_); + mutex_ = RT_NULL; + } +} + +rt_err_t ServoDriver::init(bool enable_after_init, bool go_home) +{ + if (cfg_.servo_id.empty() || cfg_.pwm_dev_name.empty()) { + rt_kprintf("[ServoDriver][E] init: invalid cfg, id=%s pwm=%s\n", + cfg_.servo_id.c_str(), + cfg_.pwm_dev_name.c_str()); + return -RT_EINVAL; + } + + if (!mutex_) { + return -RT_ENOMEM; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + + pwm_dev_ = (rt_device_pwm*)rt_device_find(cfg_.pwm_dev_name.c_str()); + if (!pwm_dev_) { + rt_kprintf("[ServoDriver][E] init: rt_device_find failed, id=%s pwm=%s ch=%d\n", + cfg_.servo_id.c_str(), + cfg_.pwm_dev_name.c_str(), + cfg_.pwm_channel); + rt_mutex_release(mutex_); + return -RT_ENOSYS; + } + + const float init_angle = go_home ? cfg_.home_angle_rad : cfg_.min_angle_rad; + const rt_uint32_t pulse = angleToPulseNs(cfg_, init_angle); + + rt_err_t err = rt_pwm_set(pwm_dev_, cfg_.pwm_channel, cfg_.period_ns, pulse); + if (err != RT_EOK) { + rt_kprintf("[ServoDriver][E] init: rt_pwm_set failed, id=%s ch=%d pulse=%u err=%d\n", + cfg_.servo_id.c_str(), + cfg_.pwm_channel, + (unsigned)pulse, + (int)err); + rt_mutex_release(mutex_); + return err; + } + + last_pulse_ = pulse; + inited_ = true; + + if (enable_after_init) { + err = rt_pwm_enable(pwm_dev_, cfg_.pwm_channel); + if (err != RT_EOK) { + rt_kprintf("[ServoDriver][E] init: rt_pwm_enable failed, id=%s ch=%d err=%d\n", + cfg_.servo_id.c_str(), + cfg_.pwm_channel, + (int)err); + rt_mutex_release(mutex_); + return err; + } + } + + rt_mutex_release(mutex_); + + rt_kprintf("[ServoDriver] init OK: id=%s pwm=%s ch=%d enable=%d go_home=%d\n", + cfg_.servo_id.c_str(), + cfg_.pwm_dev_name.c_str(), + cfg_.pwm_channel, + (int)enable_after_init, + (int)go_home); + + return RT_EOK; +} + +rt_err_t ServoDriver::enable(bool on) +{ + if (!mutex_) { + return -RT_ENOMEM; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const rt_err_t err = enableNoLock(on); + rt_mutex_release(mutex_); + return err; +} + +rt_err_t ServoDriver::setAngle(float angle_rad) +{ + if (!mutex_) { + return -RT_ENOMEM; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const rt_err_t err = setAngleNoLock(angle_rad); + rt_mutex_release(mutex_); + return err; +} + +const std::string& ServoDriver::id() const +{ + return cfg_.servo_id; +} + +const ServoDriver::Config& ServoDriver::config() const +{ + return cfg_; +} + +rt_err_t ServoDriver::enableNoLock(bool on) +{ + if (!inited_ || !pwm_dev_) { + return -RT_ERROR; + } + + return on ? rt_pwm_enable(pwm_dev_, cfg_.pwm_channel) + : rt_pwm_disable(pwm_dev_, cfg_.pwm_channel); +} + +rt_err_t ServoDriver::setAngleNoLock(float angle_rad) +{ + if (!inited_ || !pwm_dev_) { + return -RT_ERROR; + } + + const rt_uint32_t pulse = angleToPulseNs(cfg_, angle_rad); + if (pulse == last_pulse_) { + return RT_EOK; + } + + const rt_err_t err = rt_pwm_set(pwm_dev_, cfg_.pwm_channel, cfg_.period_ns, pulse); + if (err != RT_EOK) { + return err; + } + + last_pulse_ = pulse; + return RT_EOK; +} + +float ServoDriver::clampf(float v, float lo, float hi) +{ + if (v < lo) { + return lo; + } + if (v > hi) { + return hi; + } + return v; +} + +rt_uint32_t ServoDriver::angleToPulseNs(const Config& cfg, float angle_rad) +{ + const float a = clampf(angle_rad, cfg.min_angle_rad, cfg.max_angle_rad); + + const float in_span = cfg.max_angle_rad - cfg.min_angle_rad; + const float out_span = (float)(cfg.max_pulse_ns - cfg.min_pulse_ns); + + float t = 0.0f; + if (in_span > 1e-6f) { + t = (a - cfg.min_angle_rad) / in_span; + } + + float pulse = (float)cfg.min_pulse_ns + t * out_span; + + if (pulse < (float)cfg.min_pulse_ns) { + pulse = (float)cfg.min_pulse_ns; + } + if (pulse > (float)cfg.max_pulse_ns) { + pulse = (float)cfg.max_pulse_ns; + } + + return (rt_uint32_t)(pulse + 0.5f); +} \ No newline at end of file diff --git a/applications/servo_manager/src/servo_manager.cpp b/applications/servo_manager/src/servo_manager.cpp index e5e8fed..8914dd8 100644 --- a/applications/servo_manager/src/servo_manager.cpp +++ b/applications/servo_manager/src/servo_manager.cpp @@ -1,283 +1,213 @@ // -// Created by Administrator on 2026/1/7. +// Created by Administrator on 2026/4/22. // #include "servo_manager/include/servo_manager.h" - - -ServoManager::ServoManager(const std::vector &cfg) -: _cfg(std::move(cfg)), - _pwm_dev(_cfg.size(), RT_NULL), - _last_pulse(_cfg.size(), 0), - _mutex(RT_NULL) +ServoManager::ServoManager(const std::vector& cfg) + : controls_(), + mutex_(RT_NULL), + wake_sem_(RT_NULL), + thread_(RT_NULL), + running_(false) { - _mutex = rt_mutex_create("sv_mtx", RT_IPC_FLAG_PRIO); + controls_.reserve(cfg.size()); + for (const auto& c : cfg) { + controls_.emplace_back(c); + } + + mutex_ = rt_mutex_create("sv_mgr", RT_IPC_FLAG_PRIO); + wake_sem_ = rt_sem_create("sv_wk", 0, RT_IPC_FLAG_PRIO); } ServoManager::~ServoManager() { - if (_mutex) - { - rt_mutex_delete(_mutex); - _mutex = RT_NULL; + stop(); + + if (mutex_) { + rt_mutex_delete(mutex_); + mutex_ = RT_NULL; + } + + if (wake_sem_) { + rt_sem_delete(wake_sem_); + wake_sem_ = RT_NULL; } } rt_err_t ServoManager::init(bool enable_after_init, bool go_home) { - if (_cfg.empty()) - { - rt_kprintf("[ServoManager][E] init: empty cfg\n"); - return -RT_EINVAL; - } - - if (!_mutex) - { - rt_kprintf("[ServoManager][E] init: mutex not created\n"); + if (!mutex_) { return -RT_ENOMEM; } - rt_mutex_take(_mutex, RT_WAITING_FOREVER); + rt_mutex_take(mutex_, RT_WAITING_FOREVER); - for (size_t i = 0; i < _cfg.size(); ++i) - { - const auto& c = _cfg[i]; - - rt_device_pwm* dev = (rt_device_pwm*)rt_device_find(c.pwm_dev_name); - if (!dev) - { - rt_kprintf("[ServoManager][E] init: rt_device_find failed, i=%d id=%s pwm=%s ch=%d\n", - (int)i, c.servo_id ? c.servo_id : "null", c.pwm_dev_name ? c.pwm_dev_name : "null", c.pwm_channel); - rt_mutex_release(_mutex); - return -RT_ENOSYS; - } - _pwm_dev[i] = dev; - - const float init_angle = go_home ? c.home_angle_deg : c.min_angle_deg; - const rt_uint32_t pulse = angleToPulseNs(c, init_angle); - - rt_err_t err = rt_pwm_set(_pwm_dev[i], c.pwm_channel, c.period_ns, pulse); - if (err != RT_EOK) - { - rt_kprintf("[ServoManager][E] init: rt_pwm_set failed, i=%d id=%s pwm=%s ch=%d period=%u pulse=%u err=%d\n", - (int)i, c.servo_id ? c.servo_id : "null", c.pwm_dev_name ? c.pwm_dev_name : "null", - c.pwm_channel, (unsigned)c.period_ns, (unsigned)pulse, (int)err); - rt_mutex_release(_mutex); - return err; - } - _last_pulse[i] = pulse; - - if (enable_after_init) - { - err = rt_pwm_enable(_pwm_dev[i], c.pwm_channel); - if (err != RT_EOK) - { - rt_kprintf("[ServoManager][E] init: rt_pwm_enable failed, i=%d id=%s pwm=%s ch=%d err=%d\n", - (int)i, c.servo_id ? c.servo_id : "null", c.pwm_dev_name ? c.pwm_dev_name : "null", - c.pwm_channel, (int)err); - rt_mutex_release(_mutex); - return err; - } - } - - - rt_kprintf("[ServoManager] init OK: i=%d id=%s pwm=%s ch=%d\n", (int)i, c.servo_id, c.pwm_dev_name, c.pwm_channel); - } - - rt_mutex_release(_mutex); - - // - rt_kprintf("[ServoManager] init OK: count=%d enable=%d go_home=%d\n", - (int)_cfg.size(), (int)enable_after_init, (int)go_home); - - return RT_EOK; -} - - -rt_err_t ServoManager::enable(const char* servo_id, bool on) -{ - if (!servo_id) return -RT_EINVAL; - if (!_mutex) return -RT_ENOMEM; - - rt_mutex_take(_mutex, RT_WAITING_FOREVER); - - int order = findOrderByIdNoLock(servo_id); - if (order < 0) - { - rt_mutex_release(_mutex); - return -RT_ENOSYS; - } - - rt_err_t err = enableNoLock((size_t)order, on); - rt_mutex_release(_mutex); - return err; -} - -rt_err_t ServoManager::setAngle(const char* servo_id, float angle_deg) -{ - if (!servo_id) return -RT_EINVAL; - if (!_mutex) return -RT_ENOMEM; - - rt_mutex_take(_mutex, RT_WAITING_FOREVER); - - int order = findOrderByIdNoLock(servo_id); - if (order < 0) - { - rt_mutex_release(_mutex); - return -RT_ENOSYS; - } - - rt_err_t err = setAngleNoLock((size_t)order, angle_deg); - rt_mutex_release(_mutex); - return err; -} - -rt_err_t ServoManager::setAllAngles(const std::vector& angles_deg) -{ - if (angles_deg.size() < _cfg.size()) return -RT_EINVAL; - return setAllAngles(angles_deg.data(), angles_deg.size()); -} - -rt_err_t ServoManager::setAllAngles(const float* angles_deg, size_t n) -{ - if (!angles_deg || n < _cfg.size()) return -RT_EINVAL; - if (!_mutex) return -RT_ENOMEM; - - rt_mutex_take(_mutex, RT_WAITING_FOREVER); - - for (size_t i = 0; i < _cfg.size(); ++i) - { - rt_err_t err = setAngleNoLock(i, angles_deg[i]); - if (err != RT_EOK) - { - rt_mutex_release(_mutex); + for (size_t i = 0; i < controls_.size(); ++i) { + const rt_err_t err = controls_[i].init(enable_after_init, go_home); + if (err != RT_EOK) { + rt_mutex_release(mutex_); return err; } } - rt_mutex_release(_mutex); + rt_mutex_release(mutex_); return RT_EOK; } -rt_err_t ServoManager::setAngles(const std::vector& cmds) +ServoControl* ServoManager::get(const std::string& servo_id) { - if (cmds.empty()) return -RT_EINVAL; - return setAngles(cmds.data(), cmds.size()); -} - -rt_err_t ServoManager::setAngles(const ServoCmd* cmds, size_t n) -{ - if (!cmds || n == 0) return -RT_EINVAL; - if (!_mutex) return -RT_ENOMEM; - - rt_mutex_take(_mutex, RT_WAITING_FOREVER); - - for (size_t i = 0; i < n; ++i) - { - if (!cmds[i].id) - { - rt_mutex_release(_mutex); - return -RT_EINVAL; - } - - int order = findOrderByIdNoLock(cmds[i].id); - if (order < 0) - { - rt_mutex_release(_mutex); - return -RT_ENOSYS; - } - - rt_err_t err = setAngleNoLock((size_t)order, cmds[i].angle_deg); - if (err != RT_EOK) - { - rt_mutex_release(_mutex); - return err; - } + if (!mutex_) { + return RT_NULL; } - rt_mutex_release(_mutex); - return RT_EOK; + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const int index = findIndexByIdNoLock(servo_id); + ServoControl* control = (index >= 0) ? &controls_[(size_t)index] : RT_NULL; + rt_mutex_release(mutex_); + return control; } -const char* ServoManager::idAt(size_t order) const +const ServoControl* ServoManager::get(const std::string& servo_id) const { - if (order >= _cfg.size()) return RT_NULL; - return _cfg[order].servo_id; -} - -int ServoManager::orderOf(const char* servo_id) const -{ - if (!servo_id) return -1; - for (size_t i = 0; i < _cfg.size(); ++i) - { - const char* sid = _cfg[i].servo_id; - if (sid && (rt_strcmp(sid, servo_id) == 0)) return (int)i; + if (!mutex_) { + return RT_NULL; } - return -1; + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + const int index = findIndexByIdNoLock(servo_id); + const ServoControl* control = (index >= 0) ? &controls_[(size_t)index] : RT_NULL; + rt_mutex_release(mutex_); + return control; } -// ---------- private ---------- - -rt_err_t ServoManager::enableNoLock(size_t order, bool on) +size_t ServoManager::count() const { - if (order >= _cfg.size()) return -RT_EINVAL; - if (!_pwm_dev[order]) return -RT_ERROR; - - return on ? rt_pwm_enable(_pwm_dev[order], _cfg[order].pwm_channel) - : rt_pwm_disable(_pwm_dev[order], _cfg[order].pwm_channel); + return controls_.size(); } -rt_err_t ServoManager::setAngleNoLock(size_t order, float angle_deg) +rt_err_t ServoManager::start() { - if (order >= _cfg.size()) return -RT_EINVAL; - if (!_pwm_dev[order]) return -RT_ERROR; + if (!mutex_ || !wake_sem_) { + return -RT_ENOMEM; + } - const rt_uint32_t pulse = angleToPulseNs(_cfg[order], angle_deg); + if (running_) { + return RT_EOK; + } - if (pulse != _last_pulse[order]) - { - rt_err_t err = rt_pwm_set(_pwm_dev[order], _cfg[order].pwm_channel, _cfg[order].period_ns, pulse); - if (err != RT_EOK) return err; - _last_pulse[order] = pulse; + running_ = true; + + thread_ = rt_thread_create("sv_mgr", + ServoManager::threadEntry, + this, + 4096, + 20, + 1); + if (!thread_) { + running_ = false; + return -RT_ENOMEM; + } + + const rt_err_t err = rt_thread_startup(thread_); + if (err != RT_EOK) { + running_ = false; + rt_thread_delete(thread_); + thread_ = RT_NULL; + return err; } return RT_EOK; } -int ServoManager::findOrderByIdNoLock(const char* servo_id) const +void ServoManager::stop() { - for (size_t i = 0; i < _cfg.size(); ++i) - { - const char* sid = _cfg[i].servo_id; - if (sid && (rt_strcmp(sid, servo_id) == 0)) + if (!running_) { + return; + } + + running_ = false; + wake(); + + if (thread_) { + rt_thread_mdelay((rt_int32_t)(update_period_ms_ + 1)); + rt_thread_delete(thread_); + thread_ = RT_NULL; + } +} + +void ServoManager::setUpdatePeriodMs(rt_uint32_t update_period_ms) +{ + const rt_uint32_t clamped = (update_period_ms == 0) ? 1 : update_period_ms; + + if (!mutex_) { + update_period_ms_ = clamped; + return; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + update_period_ms_ = clamped; + rt_mutex_release(mutex_); +} + +void ServoManager::wake() +{ + if (thread_ && wake_sem_) { + rt_sem_release(wake_sem_); + } +} + +void ServoManager::threadEntry(void* parameter) +{ + ServoManager* self = static_cast(parameter); + if (!self) { + return; + } + + self->run(); +} + +void ServoManager::run() +{ + while (running_) { + rt_tick_t wait_ticks = rt_tick_from_millisecond(update_period_ms_); + if (wait_ticks == 0) { + wait_ticks = 1; + } + + if (wake_sem_) { + rt_sem_take(wake_sem_, wait_ticks); + } else { + rt_thread_mdelay((rt_int32_t)update_period_ms_); + } + + if (!running_) { + break; + } + + const rt_tick_t now_tick = rt_tick_get(); + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + for (size_t i = 0; i < controls_.size(); ++i) { + controls_[i].update(now_tick); + } + rt_mutex_release(mutex_); + } + + thread_ = RT_NULL; +} + +int ServoManager::findIndexByIdNoLock(const std::string& servo_id) const +{ + if (servo_id.empty()) { + return -1; + } + + for (size_t i = 0; i < controls_.size(); ++i) { + if (controls_[i].id() == servo_id) { return (int)i; + } } + return -1; } - -float ServoManager::clampf(float v, float lo, float hi) -{ - if (v < lo) return lo; - if (v > hi) return hi; - return v; -} - -rt_uint32_t ServoManager::angleToPulseNs(const ServoConfig& c, float angle_deg) -{ - const float a = clampf(angle_deg, c.min_angle_deg, c.max_angle_deg); - - const float in_span = (c.max_angle_deg - c.min_angle_deg); - const float out_span = (float)(c.max_pulse_ns - c.min_pulse_ns); - - float t = 0.0f; - if (in_span > 1e-6f) - t = (a - c.min_angle_deg) / in_span; - - float pulse = (float)c.min_pulse_ns + t * out_span; - - if (pulse < (float)c.min_pulse_ns) pulse = (float)c.min_pulse_ns; - if (pulse > (float)c.max_pulse_ns) pulse = (float)c.max_pulse_ns; - - return (rt_uint32_t)(pulse + 0.5f); -} diff --git a/rtconfig.h b/rtconfig.h index 7fb38ed..d6bedf8 100644 --- a/rtconfig.h +++ b/rtconfig.h @@ -148,6 +148,7 @@ #define RT_USING_ADC #define RT_USING_PWM #define RT_USING_SPI +#define RT_USING_QSPI #define RT_USING_PIN #define RT_USING_KTIME #define RT_USING_CHERRYUSB @@ -452,6 +453,7 @@ #define BSP_USING_PWM #define BSP_USING_PWM1 #define BSP_USING_PWM1_CH1 +#define BSP_USING_QSPI #define BSP_USING_ADC #define BSP_USING_ADC1 /* end of On-chip Peripheral Drivers */