|
@@ -894,10 +894,6 @@ void PLSR_PWM_SetFrequency(uint32_t frequency) |
|
|
*/ |
|
|
*/ |
|
|
void PLSR_TIM6_SetUpdateFreq(uint32_t freq_us) |
|
|
void PLSR_TIM6_SetUpdateFreq(uint32_t freq_us) |
|
|
{ |
|
|
{ |
|
|
// 频率范围限制 - 确保在合理的控制范围内 |
|
|
|
|
|
if (freq_us < 100) freq_us = 100; // 最小100微秒,避免过高的中断频率 |
|
|
|
|
|
if (freq_us > 100000) freq_us = 100000; // 最大100毫秒,确保响应及时性 |
|
|
|
|
|
|
|
|
|
|
|
// 保存新的频率设置 |
|
|
// 保存新的频率设置 |
|
|
s_tim6_update_freq_us = freq_us; |
|
|
s_tim6_update_freq_us = freq_us; |
|
|
|
|
|
|
|
@@ -1809,7 +1805,8 @@ static uint8_t PLSR_SetDirectionPin(PLSR_RouteConfig_t* route, PLSR_SectionConfi |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
// 如果设置了方向延时时间,则等待指定时间 |
|
|
// 如果设置了方向延时时间,则等待指定时间 |
|
|
if (route->dir_delay > 0) { |
|
|
|
|
|
|
|
|
if (route->dir_delay > 0 && direction_changed) |
|
|
|
|
|
{ |
|
|
// 使用HAL库延时函数,单位为毫秒 |
|
|
// 使用HAL库延时函数,单位为毫秒 |
|
|
HAL_Delay(route->dir_delay); |
|
|
HAL_Delay(route->dir_delay); |
|
|
} |
|
|
} |
|
@@ -1847,8 +1844,6 @@ void PLSR_Section_StartNewSection(PLSR_RouteConfig_t* route) |
|
|
{ |
|
|
{ |
|
|
current_section->actual_pulse = ((-current_section->target_pulse) - __HAL_TIM_GET_COUNTER(&htim2)); |
|
|
current_section->actual_pulse = ((-current_section->target_pulse) - __HAL_TIM_GET_COUNTER(&htim2)); |
|
|
} |
|
|
} |
|
|
// // 相对模式:直接使用目标脉冲数 |
|
|
|
|
|
// current_section->actual_pulse = current_section->target_pulse; |
|
|
|
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
// 设置方向端子 - 使用计算后的actual_pulse确定方向 |
|
|
// 设置方向端子 - 使用计算后的actual_pulse确定方向 |
|
@@ -2136,6 +2131,7 @@ void PLSR_Section_SwitchNext(PLSR_RouteConfig_t* route, uint8_t is_pulse_complet |
|
|
// 外部事件触发时保持当前频率不变,确保频率连续性 |
|
|
// 外部事件触发时保持当前频率不变,确保频率连续性 |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
/** |
|
|
* @brief 根据当前脉冲位置计算频率 |
|
|
* @brief 根据当前脉冲位置计算频率 |
|
|
* @param route 路径控制结构体指针 |
|
|
* @param route 路径控制结构体指针 |
|
@@ -2153,9 +2149,25 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc |
|
|
|
|
|
|
|
|
// 获取当前脉冲位置 |
|
|
// 获取当前脉冲位置 |
|
|
uint32_t current_tim2_count = __HAL_TIM_GET_COUNTER(&htim2); |
|
|
uint32_t current_tim2_count = __HAL_TIM_GET_COUNTER(&htim2); |
|
|
|
|
|
|
|
|
|
|
|
uint32_t executed_pulses = 0; |
|
|
|
|
|
uint32_t next_executed_pulses = 0; |
|
|
// 计算当前部分已经执行的脉冲数 |
|
|
// 计算当前部分已经执行的脉冲数 |
|
|
uint32_t executed_pulses = current_tim2_count; |
|
|
|
|
|
|
|
|
if(is_accel) |
|
|
|
|
|
{ |
|
|
|
|
|
executed_pulses = current_tim2_count; |
|
|
|
|
|
next_executed_pulses = current_tim2_count + 1; |
|
|
|
|
|
} |
|
|
|
|
|
else |
|
|
|
|
|
{ |
|
|
|
|
|
// 减速过程:使用全局脉冲计数器 |
|
|
|
|
|
executed_pulses = g_plsr_route.decel_pulse_count - current_tim2_count; |
|
|
|
|
|
next_executed_pulses = executed_pulses-1; |
|
|
|
|
|
if(next_executed_pulses < 0) |
|
|
|
|
|
{ |
|
|
|
|
|
next_executed_pulses = 0; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 使用速度位移公式 vt^2 = v0^2 + 2ax 计算当前频率,其中v0使用initial_freq作为初始速度 |
|
|
// 使用速度位移公式 vt^2 = v0^2 + 2ax 计算当前频率,其中v0使用initial_freq作为初始速度 |
|
|
uint32_t v0 = route->initial_freq; // 使用initial_freq作为初始速度v0 |
|
|
uint32_t v0 = route->initial_freq; // 使用initial_freq作为初始速度v0 |
|
@@ -2193,7 +2205,7 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc |
|
|
uint64_t v0_squared = (uint64_t)v0 * v0; |
|
|
uint64_t v0_squared = (uint64_t)v0 * v0; |
|
|
if (v0_squared >= 2 * a * executed_pulses) |
|
|
if (v0_squared >= 2 * a * executed_pulses) |
|
|
{ |
|
|
{ |
|
|
uint64_t freq_squared_start = v0_squared - 2 * a * executed_pulses; |
|
|
|
|
|
|
|
|
uint64_t freq_squared_start = 2 * a * executed_pulses; |
|
|
freq_start = integer_sqrt_64(freq_squared_start); |
|
|
freq_start = integer_sqrt_64(freq_squared_start); |
|
|
} |
|
|
} |
|
|
else |
|
|
else |
|
@@ -2202,20 +2214,21 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 计算当前脉冲结束时的频率(executed_pulses + 1位置) |
|
|
// 计算当前脉冲结束时的频率(executed_pulses + 1位置) |
|
|
if (is_accel) |
|
|
if (is_accel) |
|
|
{ |
|
|
{ |
|
|
uint64_t v0_squared = (uint64_t)v0 * v0; |
|
|
uint64_t v0_squared = (uint64_t)v0 * v0; |
|
|
uint64_t freq_squared_end = v0_squared + 2 * a * (executed_pulses + 1); |
|
|
|
|
|
|
|
|
uint64_t freq_squared_end = v0_squared + 2 * a * next_executed_pulses; |
|
|
freq_end = integer_sqrt_64(freq_squared_end); |
|
|
freq_end = integer_sqrt_64(freq_squared_end); |
|
|
} |
|
|
} |
|
|
else |
|
|
else |
|
|
{ |
|
|
{ |
|
|
uint64_t v0_squared = (uint64_t)v0 * v0; |
|
|
uint64_t v0_squared = (uint64_t)v0 * v0; |
|
|
if (v0_squared >= 2 * a * (executed_pulses + 1)) |
|
|
|
|
|
|
|
|
if (v0_squared >= 2 * a * next_executed_pulses) |
|
|
{ |
|
|
{ |
|
|
uint64_t freq_squared_end = v0_squared - 2 * a * (executed_pulses + 1); |
|
|
|
|
|
|
|
|
uint64_t freq_squared_end = 2 * a * next_executed_pulses; |
|
|
freq_end = integer_sqrt_64(freq_squared_end); |
|
|
freq_end = integer_sqrt_64(freq_squared_end); |
|
|
} |
|
|
} |
|
|
else |
|
|
else |
|
|