diff --git a/e3_176_ref/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c b/e3_176_ref/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c index e74ae36e..a696620d 100644 --- a/e3_176_ref/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c +++ b/e3_176_ref/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c @@ -124,13 +124,22 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint float abs_left_rear_speed = 0; float abs_right_rear_speed = 0; - static float previous_time2 = 0.0f; + static uint32_t previous_time2 = 0; + uint32_t time1 = getCurrentTime(); + uint32_t dt_ticks; - float time1 = (float)getCurrentTime(); - float dt = (time1 - previous_time2) / PERIOD_TICK; + if(time1 < previous_time2)//如果当前时间小于上一次保存的时间,那表示溢出了。 + { + dt_ticks = (0xFFFFFFFF - previous_time2) + time1 + 1; + } + else + { + dt_ticks = (time1 - previous_time2) ; + } + + float dt = (float)dt_ticks / PERIOD_TICK; // 最后才转换为浮点 previous_time2 = time1; - - + abs_left_front_speed = dynamic_current_limit(&diff_data.out_torq_last[0],-diff_data.out_torq[0],dt); abs_right_front_speed = dynamic_current_limit(&diff_data.out_torq_last[1],diff_data.out_torq[1],dt); abs_left_rear_speed = dynamic_current_limit(&diff_data.out_torq_last[2],-diff_data.out_torq[2],dt); @@ -512,11 +521,32 @@ float mapRemoteControlSpeed( static void diffProcess(void *signal_id) { (void)signal_id; - static float previous_time1 = 0.0f; - - float time1 = (float)getCurrentTime(); - float dt = (time1 - previous_time1) / PERIOD_TICK; - previous_time1 = time1; +// static float previous_time1 = 0.0f; +// +// float time1 = (float)getCurrentTime(); +// float dt = (time1 - previous_time1) / PERIOD_TICK; +// previous_time1 = time1; + + static uint32_t previous_time1 = 0; + uint32_t time1 = getCurrentTime(); + uint32_t dt_ticks; + + if(time1 < previous_time1)//如果当前时间小于上一次保存的时间,那表示溢出了。 + { + dt_ticks = (0xFFFFFFFF - previous_time1) + time1 + 1; + } + else + { + dt_ticks = (time1 - previous_time1) ; + } + + float dt = (float)dt_ticks / PERIOD_TICK; // 最后才转换为浮点 + previous_time1 = time1; + + + + + // 计算当前状态,包括当前速度、角速度、加速度、减速度、最大速度 calculateCurrentState(dt); diff --git a/e3_176_ref/boards/e3_176_ref/app_demo/eth-xip/sf/drive_rs04.c b/e3_176_ref/boards/e3_176_ref/app_demo/eth-xip/sf/drive_rs04.c index 410221d1..d248696e 100644 --- a/e3_176_ref/boards/e3_176_ref/app_demo/eth-xip/sf/drive_rs04.c +++ b/e3_176_ref/boards/e3_176_ref/app_demo/eth-xip/sf/drive_rs04.c @@ -189,6 +189,22 @@ int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdoda */ float dynamic_current_limit(float *last_command, float target_current, float delta_time) { + // 1. 参数有效性检查 + if (last_command == NULL) + { + return 0.0f; // 或者返回安全默认值 + } + + if (!isfinite(*last_command) || !isfinite(target_current) || !isfinite(delta_time)) { + return *last_command; // 输入异常时保持原值 + } + + // 2. 时间间隔安全性检查 + if (delta_time <= 0.0f) { + // 使用最小安全时间间隔或直接返回原值 + delta_time = 0.001f; // 1ms默认值 + } + // 计算期望的变化量 float desired_change = target_current - *last_command;