|
|
@@ -101,18 +101,6 @@ uint8_t PLSR_Wait_CheckTime(PLSR_RouteConfig_t* route) |
|
|
|
return 0; // 等待时间未到 |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 获取系统时钟计数 |
|
|
|
* @param None |
|
|
|
* @retval 系统时钟计数(ms) |
|
|
|
*/ |
|
|
|
uint32_t PLSR_GetWaitTick(void) |
|
|
|
{ |
|
|
|
return s_wait_time_counter; |
|
|
|
} |
|
|
|
|
|
|
|
// 辅助函数声明已在文件开头声明 |
|
|
|
|
|
|
|
/* USER CODE END 1 */ |
|
|
|
|
|
|
|
TIM_HandleTypeDef htim2; // TIM2恢复用于脉冲计数 |
|
|
@@ -661,12 +649,6 @@ void PLSR_PWM_Init(void) |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 启动PLSR PWM输出 |
|
|
|
* @param None |
|
|
|
* @retval None |
|
|
|
* @note 启动TIM10 PWM输出和更新中断 |
|
|
|
*/ |
|
|
|
/** |
|
|
|
* @brief 启动PWM输出 |
|
|
|
* @retval None |
|
|
@@ -1232,6 +1214,7 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route) |
|
|
|
|
|
|
|
uint64_t temp_calc = (uint64_t)(v0 + vt) * part1_time; |
|
|
|
part1_pulse_num = (uint32_t)(temp_calc / 2000); |
|
|
|
if(part1_pulse_num == 0) part1_pulse_num = 1; // 防止为0 |
|
|
|
} |
|
|
|
else if (v0 > vt) { |
|
|
|
// 需要减速 |
|
|
@@ -1241,6 +1224,7 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route) |
|
|
|
|
|
|
|
uint64_t temp_calc = (uint64_t)(v0 + vt) * part1_time; |
|
|
|
part1_pulse_num = (uint32_t)(temp_calc / 2000); |
|
|
|
if(part1_pulse_num == 0) part1_pulse_num = 1; // 防止为0 |
|
|
|
} |
|
|
|
else { |
|
|
|
// v0 == vt,无需第一部分 |
|
|
@@ -1258,6 +1242,7 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route) |
|
|
|
// 从vt减速到0的脉冲数:(vt + 0) * part3_time / 2000 |
|
|
|
uint64_t temp_calc = (uint64_t)vt * part3_time; |
|
|
|
part3_pulse_num = (uint32_t)(temp_calc / 2000); |
|
|
|
if(part3_pulse_num == 0) part3_pulse_num = 1; // 防止为0 |
|
|
|
} else { |
|
|
|
// 目标频率已经是0,无需第三部分 |
|
|
|
part3_pulse_num = 0; |
|
|
@@ -1380,7 +1365,6 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route) |
|
|
|
} |
|
|
|
} |
|
|
|
else { |
|
|
|
// used_pulses == total_pulses,刚好用完,无匀速阶段 |
|
|
|
part2_pulse_num = 0; |
|
|
|
} |
|
|
|
|
|
|
@@ -1925,10 +1909,11 @@ void PLSR_SetupThreePartExecution(PLSR_RouteConfig_t* route) |
|
|
|
/** |
|
|
|
* @brief 段切换到下一段 |
|
|
|
* @param route: 路径控制结构体指针 |
|
|
|
* @param is_pulse_complete: 是否为脉冲完成触发的切换(1-脉冲完成, 0-外部事件触发) |
|
|
|
* @retval None |
|
|
|
* @note 切换到下一段或结束路径执行 |
|
|
|
* @note 切换到下一段或结束路径执行,根据触发原因正确累加脉冲数 |
|
|
|
*/ |
|
|
|
void PLSR_Section_SwitchNext(PLSR_RouteConfig_t* route) |
|
|
|
void PLSR_Section_SwitchNext(PLSR_RouteConfig_t* route, uint8_t is_pulse_complete) |
|
|
|
{ |
|
|
|
if (route == NULL) return; |
|
|
|
|
|
|
@@ -1936,8 +1921,18 @@ void PLSR_Section_SwitchNext(PLSR_RouteConfig_t* route) |
|
|
|
uint8_t next_section_num = current_section->next_section; |
|
|
|
|
|
|
|
// 只在相对模式下累加脉冲计数,绝对模式下不累加 |
|
|
|
if (route->mode == PLSR_MODE_RELATIVE) { |
|
|
|
route->prevPulseCount += current_section->target_pulse; |
|
|
|
if (route->mode == PLSR_MODE_RELATIVE) |
|
|
|
{ |
|
|
|
if (is_pulse_complete) |
|
|
|
{ |
|
|
|
// 脉冲完成触发:累加整个目标脉冲数 |
|
|
|
route->prevPulseCount += current_section->target_pulse; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
// 外部事件触发:直接使用全局脉冲计数器作为累计脉冲数 |
|
|
|
route->prevPulseCount = g_plsr_total_pulse_count; |
|
|
|
} |
|
|
|
} |
|
|
|
// 绝对模式下prevPulseCount保持不变,因为每段的target_pulse已经是绝对位置 |
|
|
|
// 检查下一段是否有效 |
|
|
@@ -1968,8 +1963,13 @@ void PLSR_Section_SwitchNext(PLSR_RouteConfig_t* route) |
|
|
|
// 清除新段的外部事件标志,确保每段都是干净的状态 |
|
|
|
PLSR_ClearExtEvent(route); |
|
|
|
|
|
|
|
// 更新当前频率为上一段的目标频率 |
|
|
|
route->current_freq = current_section->target_freq; |
|
|
|
// 根据切换原因更新当前频率 |
|
|
|
if (is_pulse_complete) |
|
|
|
{ |
|
|
|
// 脉冲完成触发:更新为上一段的目标频率 |
|
|
|
route->current_freq = current_section->target_freq; |
|
|
|
} |
|
|
|
// 外部事件触发时保持当前频率不变,确保频率连续性 |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
@@ -2112,23 +2112,6 @@ void PLSR_Accel_SetDefaultParams(PLSR_RouteConfig_t* route, uint32_t accel_time_ |
|
|
|
PLSR_Accel_UpdateRates(route); |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 等待状态处理 |
|
|
|
* @param route: 路径控制结构体指针 |
|
|
|
* @retval None |
|
|
|
* @note 处理等待状态逻辑 |
|
|
|
*/ |
|
|
|
void PLSR_ChackWait_End(PLSR_RouteConfig_t* route) |
|
|
|
{ |
|
|
|
if (route == NULL) return; |
|
|
|
|
|
|
|
// 检查等待条件是否满足 |
|
|
|
if (PLSR_Section_CheckWaitCondition(route)) |
|
|
|
{ |
|
|
|
// 等待条件满足,切换到下一段 |
|
|
|
PLSR_Section_SwitchNext(route); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 任务中执行段切换逻辑 |
|
|
@@ -2147,11 +2130,12 @@ void PLSR_TaskSectionSwitch(PLSR_RouteConfig_t* route) |
|
|
|
if (PLSR_Wait_CheckExtEvent(route)) |
|
|
|
{ |
|
|
|
/* 外部事件满足,切换到下一段 */ |
|
|
|
PLSR_Section_SwitchNext(route); |
|
|
|
PLSR_Section_SwitchNext(route, 0); // 外部事件触发,传入0 |
|
|
|
/* 启动新段,设置新的脉冲参数和频率 */ |
|
|
|
PLSR_Section_StartNewSection(route); |
|
|
|
/* 启动PWM输出 */ |
|
|
|
PLSR_PWM_Start(); |
|
|
|
PLSR_ClearGpioTriggerFlag(); // 清除GPIO触发标志 |
|
|
|
} |
|
|
|
return; // 等待外部事件时不需要继续处理 |
|
|
|
} |
|
|
@@ -2176,7 +2160,7 @@ void PLSR_TaskSectionSwitch(PLSR_RouteConfig_t* route) |
|
|
|
if(PLSR_Section_CheckPulseComplete(route)) |
|
|
|
{ |
|
|
|
/* 脉冲发送完成,直接切换到下一段 */ |
|
|
|
PLSR_Section_SwitchNext(route); |
|
|
|
PLSR_Section_SwitchNext(route, 1); // 脉冲完成触发,传入1 |
|
|
|
/* 启动新段,设置新的脉冲参数和频率 */ |
|
|
|
PLSR_Section_StartNewSection(route); |
|
|
|
/* 启动PWM输出 */ |
|
|
@@ -2189,7 +2173,7 @@ void PLSR_TaskSectionSwitch(PLSR_RouteConfig_t* route) |
|
|
|
if (PLSR_Section_CheckWaitCondition(route)) |
|
|
|
{ |
|
|
|
/* 等待条件满足,切换到下一段 */ |
|
|
|
PLSR_Section_SwitchNext(route); |
|
|
|
PLSR_Section_SwitchNext(route, 0); // 其他等待条件触发,传入0 |
|
|
|
/* 启动新段,设置新的脉冲参数和频率 */ |
|
|
|
PLSR_Section_StartNewSection(route); |
|
|
|
/* 启动PWM输出 */ |
|
|
@@ -2307,7 +2291,7 @@ void PLSR_SetExtEvent(PLSR_RouteConfig_t* route) |
|
|
|
g_plsr_ext_event_flag = 0; // 清除全局标志 |
|
|
|
} |
|
|
|
|
|
|
|
void PLSR_ClearExtEvent(PLSR_RouteConfig_t* route) |
|
|
|
void PLSR_ClearExtEvent(PLSR_RouteConfig_t* route) |
|
|
|
{ |
|
|
|
if (route == NULL) return; |
|
|
|
|
|
|
@@ -2375,7 +2359,6 @@ void PLSR_SectionSwitchTask(void *p_arg) |
|
|
|
if (err == OS_ERR_NONE) { |
|
|
|
/* 信号量获取成功,执行段切换逻辑 */ |
|
|
|
PLSR_TaskSectionSwitch(&g_plsr_route); |
|
|
|
PLSR_ClearGpioTriggerFlag(); // 清除GPIO触发标志 |
|
|
|
} |
|
|
|
/* 如果信号量获取失败,继续等待下一次信号 */ |
|
|
|
} |
|
|
|