feat: add real time follow mode

.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

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/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/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

applications/common/type/servo_types.h

@@ -4,10 +4,12 @@
 
 #ifndef RTTHREAD_SERVO_TYPES_H
 #define RTTHREAD_SERVO_TYPES_H
+
 enum class MotionMode
 {
-    Immediate,   // 直接到达
+    Immediate,
-    SCurve       // S 曲线
+    SCurve,
+    Follow
 };
 
-#endif //RTTHREAD_SERVO_TYPES_H
+#endif // RTTHREAD_SERVO_TYPES_H

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;

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_setPositionGain_id = 2,
-    kservo_service_setUpdatePeriodMs_id = 3,
+    kservo_service_setMode_id = 3,
-    kservo_service_move_id = 4,
+    kservo_service_setUpdatePeriodMs_id = 4,
-    kservo_service_moveJ_id = 5,
+    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);

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;

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_setPositionGain_id = 2,
-    kservo_service_setUpdatePeriodMs_id = 3,
+    kservo_service_setMode_id = 3,
-    kservo_service_move_id = 4,
+    kservo_service_setUpdatePeriodMs_id = 4,
-    kservo_service_moveJ_id = 5,
+    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);

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)
 {

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);

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

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

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
  */

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_setPositionGainId = 2;
-        static const uint8_t m_setUpdatePeriodMsId = 3;
+        static const uint8_t m_setModeId = 3;
-        static const uint8_t m_moveId = 4;
+        static const uint8_t m_setUpdatePeriodMsId = 4;
-        static const uint8_t m_moveJId = 5;
+        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;

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)
 {

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);
 

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

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;

applications/erpc/service/src/servo_service_impl.cpp

@@ -8,6 +8,9 @@
 
 const std::vector<ServoDriver::Config> 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,
+            6.28,
-            31.4,
+            100,
-            31.4);
+            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<int>(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();
     }
 

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<rt_uint64_t>(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<rt_uint64_t>(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++)
     {

applications/servo_manager/include/servo_control.h

@@ -8,11 +8,12 @@
 #include <stdint.h>
 #include <string>
 
-#include "common/type/servo_types.h"
-#include "common/curve/include/s_curve.h"
-#include "servo_manager/include/servo_driver.h"
 #include <rtthread.h>
 
+#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_;

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

applications/servo_manager/src/servo_control.cpp

@@ -6,20 +6,25 @@
 
 #include <cmath>
 
-
 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) {
+    if (mode == MotionMode::SCurve) {
-        rt_mutex_release(mutex_);
+        const float start_angle_rad = current_angle_rad_;
-        return -RT_EINVAL;
+        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) {
-
+            profile_ = curve_.calculateProfile(
-    if (profile_.total_time <= 0.0) {
+                current_angle_rad_, angle_rad, 0.0, 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;
+        start_tick_ = now_tick;
-        active_ = false;
+        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 (mode == MotionMode::Follow) {
-    if (profile_.total_time <= tick_sec) {
+        if (active_mode_ != MotionMode::Follow) {
-        rt_kprintf("[ServoControl][W] scurve duration too short, id=%s duration=%.6f tick=%.6f\n",
+            follow_planner_.initialize(current_angle_rad_,
-                   id().c_str(),
+                                       current_velocity_rad_per_sec_,
-                   profile_.total_time,
+                                       0.0);
-                   tick_sec);
+        }
-    }
 
-    active_ = true;
+        follow_planner_.setTarget(angle_rad);
-    dirty_ = false;
+        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 (active_mode_ == MotionMode::SCurve) {
-    if (elapsed_sec >= profile_.total_time) {
+        const double elapsed_sec = ticksToSeconds(now_tick - start_tick_);
-        current_angle_rad_ = target_angle_rad_;
+        if (elapsed_sec >= profile_.total_time) {
-        current_velocity_rad_per_sec_ = 0.0f;
+            current_angle_rad_ = target_angle_rad_;
-        active_ = false;
+            current_velocity_rad_per_sec_ = 0.0f;
+            active_ = false;
+            return;
+        }
+
+        current_angle_rad_ = static_cast<float>(
+            curve_.getPositionAtTime(profile_, elapsed_sec));
+        current_velocity_rad_per_sec_ = static_cast<float>(
+            curve_.getVelocityAtTime(profile_, elapsed_sec));
         return;
     }
 
-    current_angle_rad_ = static_cast<float>(
+    if (active_mode_ == MotionMode::Follow) {
-        curve_.getPositionAtTime(profile_, elapsed_sec));
+        const double dt = ticksToSeconds(now_tick - last_update_tick_);
-    current_velocity_rad_per_sec_ = static_cast<float>(
+        if (dt <= 0.0) {
-        curve_.getVelocityAtTime(profile_, elapsed_sec));
+            return;
+        }
+
+        last_update_tick_ = now_tick;
+        current_angle_rad_ = static_cast<float>(follow_planner_.update(dt));
+        current_velocity_rad_per_sec_ = static_cast<float>(follow_planner_.getVelocity());
+        target_angle_rad_ = static_cast<float>(follow_planner_.getTargetPosition());
+        active_ = follow_planner_.isMoving();
+        return;
+    }
 }
 
 double ServoControl::ticksToSeconds(rt_tick_t delta_ticks) const

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.Pin18=PE11
-Mcu.Pin19=PD9
+Mcu.Pin19=PB1
 Mcu.Pin2=PA14 (JTCK/SWCLK)
-Mcu.Pin20=PD8
+Mcu.Pin20=PE13
-Mcu.Pin21=VP_SYS_VS_Systick
+Mcu.Pin21=PD9
-Mcu.Pin22=VP_TIM1_VS_ClockSourceINT
+Mcu.Pin22=PE14
-Mcu.Pin23=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
+Mcu.Pin23=PD8
-Mcu.Pin24=VP_MEMORYMAP_VS_MEMORYMAP
+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

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;

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;

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

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