diff --git a/.config b/.config index 672bc9e..a1f7e9b 100644 --- a/.config +++ b/.config @@ -1531,6 +1531,9 @@ 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_PWM1_CH2=y +CONFIG_BSP_USING_PWM1_CH3=y +CONFIG_BSP_USING_PWM1_CH4=y CONFIG_BSP_USING_QSPI=y CONFIG_BSP_USING_ADC=y CONFIG_BSP_USING_ADC1=y diff --git a/CMakeLists.txt b/CMakeLists.txt index 4cbec69..52852b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,10 +105,10 @@ 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/servo_service_server.cpp applications/erpc/proto/generated/servo_service_interface.cpp + applications/erpc/service/src/servo_service_impl.cpp applications/erpc/service/src/erpc_server_main.cpp ) @@ -118,12 +118,12 @@ SET(RT_APPLICATIONS_SOURCES 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/osal/usb_osal_rtthread.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/port/dwc2/usb_glue_st.c rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_dc_dwc2.c + rt-thread/components/drivers/usb/cherryusb/platform/rtthread/usb_check.c + rt-thread/components/drivers/usb/cherryusb/platform/rtthread/usb_msh.c + rt-thread/components/drivers/usb/cherryusb/port/dwc2/usb_glue_st.c ) SET(RT_COMMON_SOURCES @@ -147,8 +147,8 @@ SET(RT_CONTROLLER_SOURCES ) SET(RT_CPP_SOURCES - rt-thread/components/libc/cplusplus/cxx_crt_init.c rt-thread/components/libc/cplusplus/cxx_crt.cpp + rt-thread/components/libc/cplusplus/cxx_crt_init.c ) SET(RT_DEVICEDRIVERS_SOURCES @@ -188,30 +188,30 @@ SET(RT_DRIVERS_SOURCES ) SET(RT_ERPC_SOURCES - packages/erpc-1.14.0/erpc_c/infra/erpc_message_loggers.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/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/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/transports/erpc_usb_cdc_transport.cpp + packages/erpc-1.14.0/erpc_c/port/erpc_port_rtt.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_message_loggers.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_server.cpp + packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_static.cpp + packages/erpc-1.14.0/erpc_c/setup/erpc_setup_usb_cdc.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_simple_server.cpp packages/erpc-1.14.0/erpc_c/infra/erpc_message_buffer.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_transport_arbitrator.cpp + packages/erpc-1.14.0/erpc_c/setup/erpc_server_setup.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_framed_transport.cpp + packages/erpc-1.14.0/erpc_c/setup/erpc_setup_mbf_dynamic.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_basic_codec.cpp + packages/erpc-1.14.0/erpc_c/infra/erpc_crc16.cpp + packages/erpc-1.14.0/erpc_c/port/erpc_threading_pthreads.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/msh.c + rt-thread/components/finsh/msh_parse.c + rt-thread/components/finsh/shell.c ) SET(RT_KERNEL_SOURCES @@ -234,14 +234,14 @@ SET(RT_KERNEL_SOURCES SET(RT_KLIBC_SOURCES rt-thread/src/klibc/rt_vsscanf.c + rt-thread/src/klibc/kstdio.c rt-thread/src/klibc/kstring.c rt-thread/src/klibc/kerrno.c - rt-thread/src/klibc/kstdio.c ) SET(RT_KTIME_SOURCES - rt-thread/components/drivers/ktime/src/boottime.c rt-thread/components/drivers/ktime/src/hrtimer.c + rt-thread/components/drivers/ktime/src/boottime.c rt-thread/components/drivers/ktime/src/cputimer.c ) @@ -255,35 +255,35 @@ SET(RT_LIBCPU_SOURCES ) SET(RT_LIBRARIES_SOURCES - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_gpio.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_adc_ex.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_qspi.c - libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc_ex.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_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.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_rcc.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp_ex.c + libraries/STM32H7xx_HAL/CMSIS/Device/ST/STM32H7xx/Source/Templates/system_stm32h7xx.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr.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_adc_ex.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_crc_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_mdma.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_lptim.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_usart.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_qspi.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_comp.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_gpio.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_sram.c libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_crc.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cryp.c + libraries/STM32H7xx_HAL/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc.c ) SET(RT_POSIX_SOURCES @@ -301,9 +301,9 @@ SET(RT_POSIX_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 + applications/servo_manager/src/servo_control.cpp ) SET(RT_UTC_UTEST_SOURCES diff --git a/applications/common/type/servo_types.h b/applications/common/type/servo_types.h index 236fd59..8479722 100644 --- a/applications/common/type/servo_types.h +++ b/applications/common/type/servo_types.h @@ -4,10 +4,12 @@ #ifndef RTTHREAD_SERVO_TYPES_H #define RTTHREAD_SERVO_TYPES_H + enum class MotionMode { - Immediate, // 直接到达 - SCurve // S 曲线 + Immediate, + SCurve, + Follow }; -#endif //RTTHREAD_SERVO_TYPES_H +#endif // RTTHREAD_SERVO_TYPES_H diff --git a/applications/erpc/proto/generated/c_servo_service_client.cpp b/applications/erpc/proto/generated/c_servo_service_client.cpp index 42bf2fc..9fa05ed 100644 --- a/applications/erpc/proto/generated/c_servo_service_client.cpp +++ b/applications/erpc/proto/generated/c_servo_service_client.cpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -28,6 +28,14 @@ bool setConstraints(const char * id, float max_velocity_rad, float max_accelerat return result; } +bool setPositionGain(const char * id, float position_gain) +{ + bool result; + result = s_servo_service_client->setPositionGain(id, position_gain); + + return result; +} + bool setMode(RpcMotionMode mode) { bool result; diff --git a/applications/erpc/proto/generated/c_servo_service_client.h b/applications/erpc/proto/generated/c_servo_service_client.h index bea4765..371a54d 100644 --- a/applications/erpc/proto/generated/c_servo_service_client.h +++ b/applications/erpc/proto/generated/c_servo_service_client.h @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -25,16 +25,19 @@ 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, + kservo_service_setPositionGain_id = 2, + kservo_service_setMode_id = 3, + kservo_service_setUpdatePeriodMs_id = 4, + kservo_service_move_id = 5, + kservo_service_moveJ_id = 6, }; //! @name servo_service //@{ bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad); +bool setPositionGain(const char * id, float position_gain); + bool setMode(RpcMotionMode mode); bool setUpdatePeriodMs(uint32_t ms); diff --git a/applications/erpc/proto/generated/c_servo_service_server.cpp b/applications/erpc/proto/generated/c_servo_service_server.cpp index bc8fadd..1ac9159 100644 --- a/applications/erpc/proto/generated/c_servo_service_server.cpp +++ b/applications/erpc/proto/generated/c_servo_service_server.cpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -29,6 +29,14 @@ class servo_service_server: public servo_service_interface return result; } + bool setPositionGain(const char * id, float position_gain) + { + bool result; + result = ::setPositionGain(id, position_gain); + + return result; + } + bool setMode(RpcMotionMode mode) { bool result; diff --git a/applications/erpc/proto/generated/c_servo_service_server.h b/applications/erpc/proto/generated/c_servo_service_server.h index 3ff8cf8..4efd69b 100644 --- a/applications/erpc/proto/generated/c_servo_service_server.h +++ b/applications/erpc/proto/generated/c_servo_service_server.h @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -26,16 +26,19 @@ 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, + kservo_service_setPositionGain_id = 2, + kservo_service_setMode_id = 3, + kservo_service_setUpdatePeriodMs_id = 4, + kservo_service_move_id = 5, + kservo_service_moveJ_id = 6, }; //! @name servo_service //@{ bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad); +bool setPositionGain(const char * id, float position_gain); + bool setMode(RpcMotionMode mode); bool setUpdatePeriodMs(uint32_t ms); diff --git a/applications/erpc/proto/generated/servo_service_client.cpp b/applications/erpc/proto/generated/servo_service_client.cpp index 276af2f..b19738c 100644 --- a/applications/erpc/proto/generated/servo_service_client.cpp +++ b/applications/erpc/proto/generated/servo_service_client.cpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -162,6 +162,75 @@ bool servo_service_client::setConstraints(const char * id, float max_velocity_ra return result; } +// servo_service interface setPositionGain function client shim. +bool servo_service_client::setPositionGain(const char * id, float position_gain) +{ + 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_setPositionGainId, request.getSequence()); + + { + uint32_t id_len = strlen((const char*)id); + + codec->writeString(id_len, (const char*)id); + } + + codec->write(position_gain); + + // 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_setPositionGainId); + +#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 setMode function client shim. bool servo_service_client::setMode(RpcMotionMode mode) { diff --git a/applications/erpc/proto/generated/servo_service_client.hpp b/applications/erpc/proto/generated/servo_service_client.hpp index 9923a99..ed52458 100644 --- a/applications/erpc/proto/generated/servo_service_client.hpp +++ b/applications/erpc/proto/generated/servo_service_client.hpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -24,6 +24,8 @@ class servo_service_client: public servo_service_interface virtual bool setConstraints(const char * id, float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad); + virtual bool setPositionGain(const char * id, float position_gain); + virtual bool setMode(RpcMotionMode mode); virtual bool setUpdatePeriodMs(uint32_t ms); diff --git a/applications/erpc/proto/generated/servo_service_common.h b/applications/erpc/proto/generated/servo_service_common.h index 368145a..e318de9 100644 --- a/applications/erpc/proto/generated/servo_service_common.h +++ b/applications/erpc/proto/generated/servo_service_common.h @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -31,7 +31,8 @@ extern "C" typedef enum RpcMotionMode { RpcMotionModeImmediate = 0, - RpcMotionModeSCurve = 1 + RpcMotionModeSCurve = 1, + RpcMotionModeFollow = 2 } RpcMotionMode; // Aliases data types declarations diff --git a/applications/erpc/proto/generated/servo_service_common.hpp b/applications/erpc/proto/generated/servo_service_common.hpp index 7cf4024..b0b69b9 100644 --- a/applications/erpc/proto/generated/servo_service_common.hpp +++ b/applications/erpc/proto/generated/servo_service_common.hpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -26,7 +26,8 @@ typedef enum RpcMotionMode { RpcMotionModeImmediate = 0, - RpcMotionModeSCurve = 1 + RpcMotionModeSCurve = 1, + RpcMotionModeFollow = 2 } RpcMotionMode; // Aliases data types declarations diff --git a/applications/erpc/proto/generated/servo_service_interface.cpp b/applications/erpc/proto/generated/servo_service_interface.cpp index 54cfb64..c9f35e3 100644 --- a/applications/erpc/proto/generated/servo_service_interface.cpp +++ b/applications/erpc/proto/generated/servo_service_interface.cpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ diff --git a/applications/erpc/proto/generated/servo_service_interface.hpp b/applications/erpc/proto/generated/servo_service_interface.hpp index 8dc58e8..fece815 100644 --- a/applications/erpc/proto/generated/servo_service_interface.hpp +++ b/applications/erpc/proto/generated/servo_service_interface.hpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -20,15 +20,18 @@ 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; + static const uint8_t m_setPositionGainId = 2; + static const uint8_t m_setModeId = 3; + static const uint8_t m_setUpdatePeriodMsId = 4; + static const uint8_t m_moveId = 5; + static const uint8_t m_moveJId = 6; 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 setPositionGain(const char * id, float position_gain) = 0; + virtual bool setMode(RpcMotionMode mode) = 0; virtual bool setUpdatePeriodMs(uint32_t ms) = 0; diff --git a/applications/erpc/proto/generated/servo_service_server.cpp b/applications/erpc/proto/generated/servo_service_server.cpp index 1ce45d6..d9d836a 100644 --- a/applications/erpc/proto/generated/servo_service_server.cpp +++ b/applications/erpc/proto/generated/servo_service_server.cpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -166,6 +166,12 @@ erpc_status_t servo_service_service::handleInvocation(uint32_t methodId, uint32_ break; } + case servo_service_interface::m_setPositionGainId: + { + erpcStatus = setPositionGain_shim(codec, messageFactory, transport, sequence); + break; + } + case servo_service_interface::m_setModeId: { erpcStatus = setMode_shim(codec, messageFactory, transport, sequence); @@ -269,6 +275,69 @@ erpc_status_t servo_service_service::setConstraints_shim(Codec * codec, MessageB return err; } +// Server shim for setPositionGain of servo_service interface. +erpc_status_t servo_service_service::setPositionGain_shim(Codec * codec, MessageBufferFactory *messageFactory, Transport * transport, uint32_t sequence) +{ + erpc_status_t err = kErpcStatus_Success; + + char * id = NULL; + float position_gain; + bool result; + + // startReadMessage() was already called before this shim was invoked. + + { + 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(position_gain); + + err = codec->getStatus(); + if (err == kErpcStatus_Success) + { + // Invoke the actual served function. +#if ERPC_NESTED_CALLS_DETECTION + nestingDetection = true; +#endif + result = m_handler->setPositionGain(id, position_gain); +#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_setPositionGainId, sequence); + + codec->write(result); + + err = codec->getStatus(); + } + + erpc_free(id); + + return err; +} + // 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) { diff --git a/applications/erpc/proto/generated/servo_service_server.hpp b/applications/erpc/proto/generated/servo_service_server.hpp index a3114aa..633603e 100644 --- a/applications/erpc/proto/generated/servo_service_server.hpp +++ b/applications/erpc/proto/generated/servo_service_server.hpp @@ -1,5 +1,5 @@ /* - * Generated by erpcgen 1.14.0 on Wed Apr 22 16:48:46 2026. + * Generated by erpcgen 1.14.0 on Thu Apr 23 10:54:10 2026. * * AUTOGENERATED - DO NOT EDIT */ @@ -42,6 +42,9 @@ private: /*! @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 setPositionGain of servo_service interface. */ + erpc_status_t setPositionGain_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); diff --git a/applications/erpc/proto/servo_service.erpc b/applications/erpc/proto/servo_service.erpc index 103136a..2e1cc54 100644 --- a/applications/erpc/proto/servo_service.erpc +++ b/applications/erpc/proto/servo_service.erpc @@ -2,12 +2,14 @@ // Generator examples: // .\erpcgen.exe -o .\generated\ -g c .\servo_service.erpc // .\erpcgen.exe -g py .\servo_service.erpc +// 注意：interface 客户端和服务端erpc里面的函数顺序一定要一样 program servo_service enum RpcMotionMode { RpcMotionModeImmediate = 0, - RpcMotionModeSCurve = 1 + RpcMotionModeSCurve = 1, + RpcMotionModeFollow = 2 } struct ServoCmd { string id @@ -19,6 +21,8 @@ interface servo_service float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad) -> bool + setPositionGain(string id, + float position_gain) -> bool setMode(RpcMotionMode mode)->bool setUpdatePeriodMs(uint32 ms) -> bool diff --git a/applications/erpc/service/include/servo_service_impl.h b/applications/erpc/service/include/servo_service_impl.h index bd582a7..38ab14b 100644 --- a/applications/erpc/service/include/servo_service_impl.h +++ b/applications/erpc/service/include/servo_service_impl.h @@ -25,6 +25,8 @@ public: float max_velocity_rad, float max_acceleration_rad, float max_jerk_rad) override; + bool setPositionGain(const char* id, + float position_gain) override; bool setMode(RpcMotionMode mode) override; bool setUpdatePeriodMs(uint32_t ms) override; int32_t move(const list_ServoCmd_1_t* cmds) override; diff --git a/applications/erpc/service/src/servo_service_impl.cpp b/applications/erpc/service/src/servo_service_impl.cpp index 2c95c6a..7fe6e53 100644 --- a/applications/erpc/service/src/servo_service_impl.cpp +++ b/applications/erpc/service/src/servo_service_impl.cpp @@ -8,6 +8,9 @@ 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.5f * PI, 0.5f * PI, 0.0f}, +{"eye_l_down", "pwm1", 3, 20000000, 500000, 2500000, -0.5f * PI, 0.5f * PI, 0.0f}, +{"eye_r_down", "pwm1", 4, 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}, // ... }; @@ -31,7 +34,7 @@ ServoServiceImpl::ServoServiceImpl() return; } - manager_.setUpdatePeriodMs(1); + manager_.setUpdatePeriodMs(10); for (const auto& cfg : kServoCfg) { ServoControl* servo = manager_.get(cfg.servo_id); @@ -42,9 +45,9 @@ ServoServiceImpl::ServoServiceImpl() } servo->setConstraints( - 3.14, - 31.4, - 31.4); + 6.28, + 100, + 500); } init_err_ = manager_.start(); @@ -103,6 +106,41 @@ bool ServoServiceImpl::setConstraints(const char* id, return true; } +bool ServoServiceImpl::setPositionGain(const char* id, float position_gain) +{ + if (init_err_ != RT_EOK) { + rt_kprintf("[ServoService][E] setPositionGain: service not ready, init_err=%d\n", + (int)init_err_); + return false; + } + + if (!id) { + rt_kprintf("[ServoService][E] setPositionGain: id is null\n"); + return false; + } + + if (position_gain < 0.0f) { + rt_kprintf("[ServoService][E] setPositionGain: invalid gain, id=%s gain=%.4f\n", + id, + (double)position_gain); + return false; + } + + ServoControl* servo = manager_.get(std::string(id)); + if (!servo) { + rt_kprintf("[ServoService][E] setPositionGain: servo not found, id=%s\n", id); + return false; + } + + servo->setPositionGain(position_gain); + manager_.wake(); + + rt_kprintf("[ServoService] setPositionGain: id=%s gain=%.4f\n", + id, + (double)position_gain); + return true; +} + bool ServoServiceImpl::setMode(RpcMotionMode mode) { if (init_err_ != RT_EOK) { @@ -117,13 +155,16 @@ bool ServoServiceImpl::setMode(RpcMotionMode mode) } MotionMode motion_mode; - switch (mode) { + switch (static_cast(mode)) { case RpcMotionModeImmediate: motion_mode = MotionMode::Immediate; break; case RpcMotionModeSCurve: motion_mode = MotionMode::SCurve; break; + case 2: + motion_mode = MotionMode::Follow; + break; default: rt_kprintf("[ServoService][E] setMode: invalid mode=%d\n", (int)mode); return false; @@ -205,7 +246,7 @@ int32_t ServoServiceImpl::move(const list_ServoCmd_1_t* cmds) } } - if (motion_mode == MotionMode::SCurve) { + if (motion_mode != MotionMode::Immediate) { manager_.wake(); } @@ -269,7 +310,7 @@ int32_t ServoServiceImpl::moveJ(const list_float_1_t* angles_rad) } } - if (motion_mode == MotionMode::SCurve) { + if (motion_mode != MotionMode::Immediate) { manager_.wake(); } diff --git a/applications/main.cpp b/applications/main.cpp index 0721cd6..6c8b5b5 100644 --- a/applications/main.cpp +++ b/applications/main.cpp @@ -17,95 +17,12 @@ /* 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++) { diff --git a/applications/servo_manager/include/servo_control.h b/applications/servo_manager/include/servo_control.h index 6c2da72..973775b 100644 --- a/applications/servo_manager/include/servo_control.h +++ b/applications/servo_manager/include/servo_control.h @@ -8,11 +8,12 @@ #include #include -#include "common/type/servo_types.h" -#include "common/curve/include/s_curve.h" -#include "servo_manager/include/servo_driver.h" #include +#include "common/curve/include/s_curve.h" +#include "common/type/servo_types.h" +#include "planner/s_curve_planner/include/s_curve_position_planner.h" +#include "servo_manager/include/servo_driver.h" class ServoControl { @@ -28,6 +29,7 @@ public: void setConstraints(double max_velocity_rad, double max_acceleration_rad, double max_jerk_rad); + void setPositionGain(double position_gain); void update(rt_tick_t now_tick); @@ -44,17 +46,20 @@ private: double ticksToSeconds(rt_tick_t delta_ticks) const; private: - // 角度变化阈值，rad static constexpr float ESP = 1e-4f; + ServoDriver driver_; cmvr::SCurve curve_; + cmvr::SCurvePositionPlanner1D follow_planner_; float current_angle_rad_; float current_velocity_rad_per_sec_; float target_angle_rad_; rt_tick_t start_tick_; + rt_tick_t last_update_tick_; uint32_t generation_; + MotionMode active_mode_; bool active_; bool dirty_; @@ -64,4 +69,4 @@ private: rt_mutex_t mutex_; }; -#endif // SERVO_CONTROL_H \ No newline at end of file +#endif // SERVO_CONTROL_H diff --git a/applications/servo_manager/include/servo_manager.h b/applications/servo_manager/include/servo_manager.h index 9f8cacb..a349853 100644 --- a/applications/servo_manager/include/servo_manager.h +++ b/applications/servo_manager/include/servo_manager.h @@ -47,7 +47,7 @@ private: rt_sem_t wake_sem_; rt_thread_t thread_; - rt_uint32_t update_period_ms_{1}; + rt_uint32_t update_period_ms_{10}; volatile bool running_; }; diff --git a/applications/servo_manager/src/servo_control.cpp b/applications/servo_manager/src/servo_control.cpp index 71f4163..c766dd8 100644 --- a/applications/servo_manager/src/servo_control.cpp +++ b/applications/servo_manager/src/servo_control.cpp @@ -6,20 +6,25 @@ #include - ServoControl::ServoControl(const ServoDriver::Config& cfg) : driver_(cfg), curve_(), + follow_planner_(6.28, + 100, + 500), current_angle_rad_(cfg.home_angle_rad), current_velocity_rad_per_sec_(0.0f), target_angle_rad_(cfg.home_angle_rad), start_tick_(0), + last_update_tick_(0), generation_(0), + active_mode_(MotionMode::Immediate), active_(false), dirty_(false), profile_(), mutex_(RT_NULL) { + follow_planner_.setPositionGain(8.0); mutex_ = rt_mutex_create("sv_ctl", RT_IPC_FLAG_PRIO); } @@ -46,14 +51,21 @@ rt_err_t ServoControl::init(bool enable_after_init, bool go_home) const float init_angle = go_home ? driver_.config().home_angle_rad : driver_.config().min_angle_rad; + const rt_tick_t now_tick = rt_tick_get(); current_angle_rad_ = init_angle; current_velocity_rad_per_sec_ = 0.0f; target_angle_rad_ = init_angle; - start_tick_ = rt_tick_get(); + start_tick_ = now_tick; + last_update_tick_ = now_tick; generation_ = 0; + active_mode_ = MotionMode::Immediate; active_ = false; dirty_ = false; + profile_ = cmvr::SCurveProfile(); + + follow_planner_.initialize(init_angle, 0.0, 0.0); + follow_planner_.setTarget(init_angle); rt_mutex_release(mutex_); return RT_EOK; @@ -77,6 +89,8 @@ rt_err_t ServoControl::move(float angle_rad, MotionMode mode) current_angle_rad_ = angle_rad; current_velocity_rad_per_sec_ = 0.0f; start_tick_ = now_tick; + last_update_tick_ = now_tick; + active_mode_ = MotionMode::Immediate; active_ = false; dirty_ = false; @@ -84,53 +98,76 @@ rt_err_t ServoControl::move(float angle_rad, MotionMode mode) return driver_.setAngle(angle_rad); } - if (mode != MotionMode::SCurve) { - rt_mutex_release(mutex_); - return -RT_EINVAL; - } + if (mode == MotionMode::SCurve) { + const float start_angle_rad = current_angle_rad_; + const float start_velocity_rad_per_sec = current_velocity_rad_per_sec_; - 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); - } + current_angle_rad_, angle_rad, current_velocity_rad_per_sec_, 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); + if (profile_.total_time <= 0.0) { + profile_ = curve_.calculateProfile( + current_angle_rad_, angle_rad, 0.0, 0.0); } - current_angle_rad_ = angle_rad; - current_velocity_rad_per_sec_ = 0.0f; - active_ = false; + + start_tick_ = now_tick; + last_update_tick_ = now_tick; + active_mode_ = MotionMode::SCurve; + + 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_mode_ = MotionMode::Immediate; + 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 driver_.setAngle(angle_rad); + return RT_EOK; } - 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); - } + if (mode == MotionMode::Follow) { + if (active_mode_ != MotionMode::Follow) { + follow_planner_.initialize(current_angle_rad_, + current_velocity_rad_per_sec_, + 0.0); + } - active_ = true; - dirty_ = false; + follow_planner_.setTarget(angle_rad); + start_tick_ = now_tick; + last_update_tick_ = now_tick; + active_mode_ = MotionMode::Follow; + active_ = true; + dirty_ = false; + + rt_mutex_release(mutex_); + return RT_EOK; + } rt_mutex_release(mutex_); - return RT_EOK; + return -RT_EINVAL; } rt_err_t ServoControl::stop() @@ -148,6 +185,8 @@ rt_err_t ServoControl::stop() target_angle_rad_ = current_angle_rad_; current_velocity_rad_per_sec_ = 0.0f; start_tick_ = now_tick; + last_update_tick_ = now_tick; + active_mode_ = MotionMode::Immediate; active_ = false; dirty_ = false; @@ -163,11 +202,25 @@ void ServoControl::setConstraints(double max_velocity_rad, { if (!mutex_) { curve_.setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad); + follow_planner_.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); + follow_planner_.setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad); + rt_mutex_release(mutex_); +} + +void ServoControl::setPositionGain(double position_gain) +{ + if (!mutex_) { + follow_planner_.setPositionGain(position_gain); + return; + } + + rt_mutex_take(mutex_, RT_WAITING_FOREVER); + follow_planner_.setPositionGain(position_gain); rt_mutex_release(mutex_); } @@ -263,18 +316,35 @@ void ServoControl::sampleStateNoLock(rt_tick_t now_tick) 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; + if (active_mode_ == MotionMode::SCurve) { + 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)); return; } - current_angle_rad_ = static_cast( - curve_.getPositionAtTime(profile_, elapsed_sec)); - current_velocity_rad_per_sec_ = static_cast( - curve_.getVelocityAtTime(profile_, elapsed_sec)); + if (active_mode_ == MotionMode::Follow) { + const double dt = ticksToSeconds(now_tick - last_update_tick_); + if (dt <= 0.0) { + return; + } + + last_update_tick_ = now_tick; + current_angle_rad_ = static_cast(follow_planner_.update(dt)); + current_velocity_rad_per_sec_ = static_cast(follow_planner_.getVelocity()); + target_angle_rad_ = static_cast(follow_planner_.getTargetPosition()); + active_ = follow_planner_.isMoving(); + return; + } } double ServoControl::ticksToSeconds(rt_tick_t delta_ticks) const diff --git a/board/CubeMX_Config/CubeMX_Config.ioc b/board/CubeMX_Config/CubeMX_Config.ioc index 688e16d..4dce237 100644 --- a/board/CubeMX_Config/CubeMX_Config.ioc +++ b/board/CubeMX_Config/CubeMX_Config.ioc @@ -48,14 +48,17 @@ Mcu.Pin14=PF8 Mcu.Pin15=PF10 Mcu.Pin16=PF9 Mcu.Pin17=PE9 -Mcu.Pin18=PB1 -Mcu.Pin19=PD9 +Mcu.Pin18=PE11 +Mcu.Pin19=PB1 Mcu.Pin2=PA14 (JTCK/SWCLK) -Mcu.Pin20=PD8 -Mcu.Pin21=VP_SYS_VS_Systick -Mcu.Pin22=VP_TIM1_VS_ClockSourceINT -Mcu.Pin23=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS -Mcu.Pin24=VP_MEMORYMAP_VS_MEMORYMAP +Mcu.Pin20=PE13 +Mcu.Pin21=PD9 +Mcu.Pin22=PE14 +Mcu.Pin23=PD8 +Mcu.Pin24=VP_SYS_VS_Systick +Mcu.Pin25=VP_TIM1_VS_ClockSourceINT +Mcu.Pin26=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS +Mcu.Pin27=VP_MEMORYMAP_VS_MEMORYMAP Mcu.Pin3=PC15-OSC32_OUT (OSC32_OUT) Mcu.Pin4=PC14-OSC32_IN (OSC32_IN) Mcu.Pin5=PA13 (JTMS/SWDIO) @@ -63,7 +66,7 @@ Mcu.Pin6=PA10 Mcu.Pin7=PA9 Mcu.Pin8=PA12 Mcu.Pin9=PA11 -Mcu.PinsNb=25 +Mcu.PinsNb=28 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32H750XBHx @@ -116,6 +119,11 @@ PD8.Signal=USART3_TX PD9.Locked=true PD9.Mode=Asynchronous PD9.Signal=USART3_RX +PE11.Locked=true +PE11.Signal=S_TIM1_CH2 +PE13.Locked=true +PE13.Signal=S_TIM1_CH3 +PE14.Signal=S_TIM1_CH4 PE9.Locked=true PE9.Signal=S_TIM1_CH1 PF10.Mode=Single Bank 1 2IOs @@ -262,8 +270,17 @@ SH.ADCx_INP5.0=ADC1_INP5,IN5-Single-Ended SH.ADCx_INP5.ConfNb=1 SH.S_TIM1_CH1.0=TIM1_CH1,PWM Generation1 CH1 SH.S_TIM1_CH1.ConfNb=1 +SH.S_TIM1_CH2.0=TIM1_CH2,PWM Generation2 CH2 +SH.S_TIM1_CH2.ConfNb=1 +SH.S_TIM1_CH3.0=TIM1_CH3,PWM Generation3 CH3 +SH.S_TIM1_CH3.ConfNb=1 +SH.S_TIM1_CH4.0=TIM1_CH4,PWM Generation4 CH4 +SH.S_TIM1_CH4.ConfNb=1 TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 -TIM1.IPParameters=Channel-PWM Generation1 CH1 +TIM1.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2 +TIM1.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3 +TIM1.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4 +TIM1.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Channel-PWM Generation4 CH4 USART1.IPParameters=VirtualMode-Asynchronous USART1.VirtualMode-Asynchronous=VM_ASYNC USART2.IPParameters=VirtualMode-Asynchronous diff --git a/board/CubeMX_Config/Src/main.c b/board/CubeMX_Config/Src/main.c index a25be63..1d2d1cf 100644 --- a/board/CubeMX_Config/Src/main.c +++ b/board/CubeMX_Config/Src/main.c @@ -348,6 +348,18 @@ static void MX_TIM1_Init(void) { Error_Handler(); } + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) + { + Error_Handler(); + } sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; diff --git a/board/CubeMX_Config/Src/stm32h7xx_hal_msp.c b/board/CubeMX_Config/Src/stm32h7xx_hal_msp.c index fa3fb8e..96609f6 100644 --- a/board/CubeMX_Config/Src/stm32h7xx_hal_msp.c +++ b/board/CubeMX_Config/Src/stm32h7xx_hal_msp.c @@ -294,8 +294,11 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim) __HAL_RCC_GPIOE_CLK_ENABLE(); /**TIM1 GPIO Configuration PE9 ------> TIM1_CH1 + PE11 ------> TIM1_CH2 + PE13 ------> TIM1_CH3 + PE14 ------> TIM1_CH4 */ - GPIO_InitStruct.Pin = GPIO_PIN_9; + GPIO_InitStruct.Pin = GPIO_PIN_9|GPIO_PIN_11|GPIO_PIN_13|GPIO_PIN_14; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; diff --git a/board/Kconfig b/board/Kconfig index f279cab..a37beaf 100644 --- a/board/Kconfig +++ b/board/Kconfig @@ -194,6 +194,15 @@ menu "On-chip Peripheral Drivers" config BSP_USING_PWM1_CH1 bool "Enable PWM1 channel1" default n + config BSP_USING_PWM1_CH2 + bool "Enable PWM1 channel2" + default n + config BSP_USING_PWM1_CH3 + bool "Enable PWM1 channel3" + default n + config BSP_USING_PWM1_CH4 + bool "Enable PWM1 channel4" + default n endif endif config BSP_USING_QSPI diff --git a/rtconfig.h b/rtconfig.h index d6bedf8..8d8294c 100644 --- a/rtconfig.h +++ b/rtconfig.h @@ -453,6 +453,9 @@ #define BSP_USING_PWM #define BSP_USING_PWM1 #define BSP_USING_PWM1_CH1 +#define BSP_USING_PWM1_CH2 +#define BSP_USING_PWM1_CH3 +#define BSP_USING_PWM1_CH4 #define BSP_USING_QSPI #define BSP_USING_ADC #define BSP_USING_ADC1