jiujialin
/
Train_Camp_PLSR
1
0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

解决临时修改频率可能导致程序卡死的问题原因是TIM6的更新频率过高了,然后脉冲数不足加减速过程时导致脉冲停止发送,是因为脉冲计算函数,在这种情况下处理不当

2
JIU JIALIN 1 month ago
parent
32a19a6815
commit
7dce13df3d
5 changed files with 735 additions and 624 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +5
    -0
      PLSR/PLSR/Core/Inc/tim.h
  2. +268
    -162
      PLSR/PLSR/Core/Src/tim.c
  3. +2
    -2
      PLSR/PLSR/EWARM/settings/test.1.dnx
  4. +460
    -460
      PLSR/PLSR/EWARM/test.1.dep
  5. BIN
      PLSR/PLSR/EWARM/test.1/Exe/test.1.sim

+ 5
- 0
PLSR/PLSR/Core/Inc/tim.h View File

@@ -274,6 +274,11 @@ void PLSR_SectionSwitchTask(void *p_arg);
void PLSR_SectionSwitchInit(void); //<段切换信号量初始化
void PLSR_SectionSwitchSignal(void); //<发送段切换信号量(中断中调用)

uint32_t BinarySearchOptimalFreq(uint32_t v0, uint32_t vt_desired,
int32_t total_pulses, uint32_t a, uint32_t d,
uint8_t is_accel); // 二分搜索优化目标频率
uint32_t OptimalIntermediateFrequency(uint32_t v0, uint32_t vt_desired,
int32_t total_pulses, uint32_t a, uint32_t d); // 计算最优中间频率
// ==================== PLSR全局变量声明 ====================
extern PLSR_RouteConfig_t g_plsr_route; // 全局PLSR路径控制结构体
extern uint8_t g_plsr_ext_event_flag; // 外部事件标志


+ 268
- 162
PLSR/PLSR/Core/Src/tim.c View File

@@ -671,8 +671,8 @@ void PLSR_PWM_Start(void)
switch(g_plsr_route.output_port)
{
case 0: // TIM10
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, htim10.Init.Period / 2); // 设置占空比为50%
//HAL_TIM_PWM_Start_IT(&htim10, TIM_CHANNEL_1);
//__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, htim10.Init.Period / 2); // 设置占空比为50%
HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
break;
case 1: // TIM11
@@ -715,8 +715,8 @@ void PLSR_PWM_Stop(void)
switch(g_plsr_route.output_port)
{
case 0: // TIM10
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, htim10.Init.Period); //
//HAL_TIM_PWM_Stop(&htim10, TIM_CHANNEL_1);
//__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, htim10.Init.Period); //
HAL_TIM_PWM_Stop(&htim10, TIM_CHANNEL_1);
break;
case 1: // TIM11
@@ -1170,7 +1170,8 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route)
if (route->accel_config.accel_algorithm == PLSR_ACCEL_LINEAR)
{
uint32_t v0 = route->current_freq; // 起始频率
uint32_t vt = current_section->target_freq; // 期望目标频率
uint32_t vt_desired = current_section->target_freq; // 期望目标频率
uint32_t vt = vt_desired; // 实际目标频率（可能会被调整）
uint32_t vf = 0; // 最终频率（必须为0）
uint32_t a = route->accel_rate; // 加速度
uint32_t d = route->decel_rate; // 减速度
@@ -1182,186 +1183,152 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route)
if (a == 0) a = 1;
if (d == 0) d = 1;
// 三部分运动规划：
// 第一部分：从v0到vt（加速或减速或保持）
// 第二部分：在vt保持匀速
// 第三部分：从vt减速到0
// 初始计算：按理想情况计算各部分脉冲数
PLSR_RunState_t part1_state = PLSR_STATE_CONST;
PLSR_RunState_t part2_state = PLSR_STATE_CONST;
PLSR_RunState_t part3_state = PLSR_STATE_DECEL;
// 计算第一部分：v0 -> vt
PLSR_RunState_t part1_state = PLSR_STATE_CONST; // 默认匀速
// 计算理想情况下的第一部分：v0 -> vt
if (v0 < vt)
{
// 需要加速
part1_state = PLSR_STATE_ACCEL;
part1_time = (vt - v0) / a;
if (part1_time == 0) part1_time = 1;
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
if(part1_pulse_num == 0) part1_pulse_num = 1;
}
else if (v0 > vt)
{
// 需要减速
part1_state = PLSR_STATE_DECEL;
part1_time = (v0 - vt) / d;
if (part1_time == 0) part1_time = 1;
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
if(part1_pulse_num == 0) part1_pulse_num = 1;
}
else {
// v0 == vt，无需第一部分
part1_state = PLSR_STATE_CONST;
part1_pulse_num = 0;
part1_time = 0;
}
// 计算第三部分：vt -> 0（必须减速到0）
PLSR_RunState_t part3_state = PLSR_STATE_DECEL;
// 计算理想情况下的第三部分：vt -> 0
if (vt > 0)
{
part3_time = vt / d;
if (part3_time == 0) part3_time = 1;
// 从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
if(part3_pulse_num == 0) part3_pulse_num = 1;
}
else
{
// 目标频率已经是0，无需第三部分
part3_pulse_num = 0;
part3_time = 0;
}
// 计算第二部分：匀速部分
PLSR_RunState_t part2_state = PLSR_STATE_CONST;
// 检查脉冲数是否足够
int32_t used_pulses = part1_pulse_num + part3_pulse_num;
if (used_pulses < total_pulses)
if (used_pulses <= total_pulses)
{
// 有剩余脉冲用于匀速阶段
// 脉冲数足够，计算匀速部分
part2_pulse_num = total_pulses - used_pulses;
// 计算匀速时间
if (vt > 0) {
if (vt > 0 && part2_pulse_num > 0) {
part2_time = (part2_pulse_num * 1000) / vt;
}
}
else if (used_pulses > total_pulses)
else
{
// 脉冲数不足，需要调整运动参数
// 策略：优先保证能减速到0，然后调整第一部分
if (part3_pulse_num <= total_pulses) {
// 第三部分可以完成，调整第一部分
int32_t remaining_pulses = total_pulses - part3_pulse_num;
if (remaining_pulses > 0) {
if (v0 < vt)
{
// 加速情况：计算实际能加速到的频率
// vm^2 = v0^2 + remaining_pulses * 2000 * a
uint64_t v0_squared = (uint64_t)v0 * v0;
uint64_t addition = (uint64_t)remaining_pulses * 2000ULL * a;
uint64_t vm_squared = v0_squared + addition;
if (vm_squared <= 0xFFFFFFFFULL) {
uint32_t vm = integer_sqrt((uint32_t)vm_squared);
if (vm > vt) vm = vt; // 不超过期望目标频率
// 重新计算第一部分和第三部分
vt = vm; // 更新实际目标频率
part1_time = (vt - v0) / a;
if (part1_time == 0) part1_time = 1;
part1_pulse_num = remaining_pulses;
// 重新计算第三部分
part3_time = vt / d;
if (part3_time == 0) part3_time = 1;
uint64_t temp_calc = (uint64_t)vt * part3_time;
part3_pulse_num = (uint32_t)(temp_calc / 2000);
}
}
else if (v0 > vt) {
// 减速情况：计算实际能减速到的频率
// vm^2 = v0^2 - remaining_pulses * 2000 * d
uint64_t v0_squared = (uint64_t)v0 * v0;
uint64_t reduction = (uint64_t)remaining_pulses * 2000ULL * d;
if (v0_squared > reduction) {
uint32_t vm_squared = (uint32_t)(v0_squared - reduction);
uint32_t vm = integer_sqrt(vm_squared);
if (vm < vt) vm = vt; // 不低于期望目标频率
// 更新实际目标频率
vt = vm;
part1_time = (v0 - vt) / d;
if (part1_time == 0) part1_time = 1;
part1_pulse_num = remaining_pulses;
// 重新计算第三部分
part3_time = vt / d;
if (part3_time == 0) part3_time = 1;
uint64_t temp_calc = (uint64_t)vt * part3_time;
part3_pulse_num = (uint32_t)(temp_calc / 2000);
}
}
else {
// v0 == vt，直接分配给第一部分作为匀速
part1_pulse_num = remaining_pulses;
part1_state = PLSR_STATE_CONST;
}
} else {
// remaining_pulses == 0，只有第三部分
part1_pulse_num = 0;
}
part2_pulse_num = 0; // 无匀速阶段
}
else {
// 连第三部分都无法完成，重新规划整个运动
// 计算在total_pulses内能实现的运动
// 简化处理：直接从v0减速，看能减速到什么频率
// vf^2 = v0^2 - total_pulses * 2000 * d
uint64_t v0_squared = (uint64_t)v0 * v0;
uint64_t reduction = (uint64_t)total_pulses * 2000ULL * d;
// 脉冲数不足，需要重新规划运动
part2_pulse_num = 0; // 没有匀速阶段
// 优化策略：寻找最优的中间频率vm，使得总脉冲数刚好等于available_pulses
uint32_t vm = OptimalIntermediateFrequency(v0, vt_desired, total_pulses, a, d);
if (vm != v0) // 找到了有效的中间频率
{
vt = vm; // 更新实际目标频率
if (v0_squared > reduction) {
uint32_t vf_squared = (uint32_t)(v0_squared - reduction);
uint32_t actual_vf = integer_sqrt(vf_squared);
// 重新计算第一部分
if (v0 < vm)
{
part1_state = PLSR_STATE_ACCEL;
// 使用运动学公式：s = (v0 + vm) * t / 2，其中 t = (vm - v0) / a
// 所以：s = (v0 + vm) * (vm - v0) / (2 * a) = (vm^2 - v0^2) / (2 * a)
uint64_t numerator = (uint64_t)vm * vm - (uint64_t)v0 * v0;
part1_pulse_num = (uint32_t)(numerator / (2000ULL * a));
if (part1_pulse_num == 0) part1_pulse_num = 1;
// 设置为单一减速阶段
part1_state = PLSR_STATE_DECEL;
part1_pulse_num = total_pulses;
part1_time = (v0 - actual_vf) / d;
part1_time = (vm - v0) / a;
if (part1_time == 0) part1_time = 1;
}
else if (v0 > vm)
{
part1_state = PLSR_STATE_DECEL;
// 减速情况：s = (v0^2 - vm^2) / (2 * d)
uint64_t numerator = (uint64_t)v0 * v0 - (uint64_t)vm * vm;
part1_pulse_num = (uint32_t)(numerator / (2000ULL * d));
if (part1_pulse_num == 0) part1_pulse_num = 1;
vt = actual_vf; // 更新目标频率
part2_pulse_num = 0;
part3_pulse_num = 0;
part1_time = (v0 - vm) / d;
if (part1_time == 0) part1_time = 1;
}
else {
// 数学上不可行，保持当前频率
else
{
part1_state = PLSR_STATE_CONST;
part1_pulse_num = total_pulses;
part2_pulse_num = 0;
part1_pulse_num = 0;
part1_time = 0;
}
// 重新计算第三部分：vm -> 0
if (vm > 0)
{
part3_state = PLSR_STATE_DECEL;
// s = vm^2 / (2 * d)
uint64_t numerator = (uint64_t)vm * vm;
part3_pulse_num = (uint32_t)(numerator / (2000ULL * d));
if (part3_pulse_num == 0) part3_pulse_num = 1;
part3_time = vm / d;
if (part3_time == 0) part3_time = 1;
}
else
{
part3_pulse_num = 0;
vt = v0;
part3_time = 0;
}
// 确保总脉冲数不超过限制
int32_t calculated_total = part1_pulse_num + part3_pulse_num;
if (calculated_total > total_pulses)
{
// 按比例调整
if (part1_pulse_num > 0) {
part1_pulse_num = (part1_pulse_num * total_pulses) / calculated_total;
}
if (part3_pulse_num > 0) {
part3_pulse_num = total_pulses - part1_pulse_num;
}
}
}
else
{
// 无法找到有效的运动规划，保持当前频率
part1_state = PLSR_STATE_CONST;
part1_pulse_num = total_pulses;
part1_time = (total_pulses * 1000) / v0;
part3_pulse_num = 0;
part3_time = 0;
vt = v0; // 实际目标频率等于起始频率
}
}
else {
part2_pulse_num = 0;
}
// 保存三个部分的状态和脉冲计数到结构体
// 使用现有的字段来存储三个部分的信息
// 第一部分
// 保存计算结果到结构体
if (part1_state == PLSR_STATE_ACCEL)
{
route->accel_pulse_count = part1_pulse_num;
@@ -1370,41 +1337,181 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route)
{
route->accel_pulse_count = (part1_state == PLSR_STATE_DECEL) ? part1_pulse_num : 0;
}

// if(route->accel_pulse_count > 1)
// {
// route->accel_pulse_count -= 1; // 减1防止过冲
// }
// 第二部分（匀速）
route->const_pulse_count = part2_pulse_num;
// 第三部分（减速到0）
route->decel_pulse_count = part3_pulse_num;
route->part1_state = part1_state; // 保存第一部分状态
route->part2_state = part2_state; // 保存第二部分状态
route->part3_state = part3_state; // 保存第三部分状态
route->part1_target_freq = vt; // 第一部分结束频率
route->part2_target_freq = vt; // 第二部分结束频率
route->part3_target_freq = 0; // 第三部分结束频率必须为0
route->part1_state = part1_state;
route->part2_state = part2_state;
route->part3_state = part3_state;
route->part1_target_freq = vt;
route->part2_target_freq = vt;
route->part3_target_freq = 0;
// 设置初始运行状态
if (part1_pulse_num > 0) {
if (part1_pulse_num > 0)
{
route->run_state = part1_state;
route->target_freq = vt;
} else if (part2_pulse_num > 0) {
} else if (part2_pulse_num > 0)
{
route->run_state = PLSR_STATE_CONST;
route->target_freq = vt;
} else if (part3_pulse_num > 0) {
} else if (part3_pulse_num > 0)
{
route->run_state = PLSR_STATE_DECEL;
route->target_freq = 0;
} else {
} else
{
route->run_state = PLSR_STATE_CONST;
route->target_freq = v0;
}
}
}

/**
* @brief 计算最优中间频率
* @param v0 起始频率
* @param vt_desired 期望目标频率
* @param total_pulses 可用总脉冲数
* @param a 加速度
* @param d 减速度
* @return 最优中间频率vm
*/
uint32_t OptimalIntermediateFrequency(uint32_t v0, uint32_t vt_desired,
int32_t total_pulses, uint32_t a, uint32_t d)
{
// 数学推导：
// 设中间频率为vm
// 第一阶段脉冲数：s1 = |vm^2 - v0^2| / (2000 * rate1)
// 第三阶段脉冲数：s3 = vm^2 / (2000 * d)
// 约束条件：s1 + s3 = total_pulses
uint32_t vm = v0; // 默认返回起始频率
if (total_pulses <= 0) return vm;
// 情况1：需要加速 (v0 < vt_desired)
if (v0 < vt_desired)
{
// s1 = (vm^2 - v0^2) / (2000 * a)
// s3 = vm^2 / (2000 * d)
// s1 + s3 = total_pulses
// (vm^2 - v0^2) / (2000 * a) + vm^2 / (2000 * d) = total_pulses
// vm^2 * (1/(2000*a) + 1/(2000*d)) - v0^2/(2000*a) = total_pulses
// vm^2 * (d + a)/(2000*a*d) = total_pulses + v0^2/(2000*a)
uint64_t coefficient = (uint64_t)(a + d) * 1000ULL; // 分母：2000*a*d，这里先乘1000
uint64_t constant_term = ((uint64_t)v0 * v0) / (2ULL * a); // v0^2/(2*a)，单位是脉冲*1000
uint64_t rhs = (uint64_t)total_pulses * 1000ULL + constant_term; // 右边，单位是脉冲*1000
// vm^2 = rhs * 2000 * a * d / (a + d) / 1000
uint64_t vm_squared = (rhs * 2000ULL * a * d) / coefficient;
if (vm_squared > 0 && vm_squared <= 0xFFFFFFFFULL)
{
vm = integer_sqrt((uint32_t)vm_squared);
// 限制vm不能超过期望目标频率太多
if (vm > vt_desired * 2) vm = vt_desired * 2;
// 验证计算结果
uint32_t s1_calc = ((uint64_t)vm * vm - (uint64_t)v0 * v0) / (2000ULL * a);
uint32_t s3_calc = ((uint64_t)vm * vm) / (2000ULL * d);
if ((int32_t)(s1_calc + s3_calc) > total_pulses) {
// 结果超出限制，进行二分搜索优化
vm = BinarySearchOptimalFreq(v0, vt_desired, total_pulses, a, d, 1);
}
}
}
// 情况2：需要减速 (v0 > vt_desired)
else if (v0 > vt_desired)
{
// s1 = (v0^2 - vm^2) / (2000 * d)
// s3 = vm^2 / (2000 * d)
// s1 + s3 = (v0^2 - vm^2) / (2000 * d) + vm^2 / (2000 * d) = v0^2 / (2000 * d)
// 这种情况下总脉冲数固定为 v0^2 / (2000 * d)
uint32_t required_pulses = ((uint64_t)v0 * v0) / (2000ULL * d);
if (required_pulses <= (uint32_t)total_pulses)
{
// 脉冲数足够，可以减速到任意频率
// 选择一个合理的中间频率，优先选择期望频率
if (vt_desired < v0) {
vm = vt_desired;
} else {
vm = v0 / 2; // 选择一个中间值
}
}
else
{
// 脉冲数不足，计算能减速到的最低频率
// vm^2 = v0^2 - total_pulses * 2000 * d
uint64_t v0_squared = (uint64_t)v0 * v0;
uint64_t reduction = (uint64_t)total_pulses * 2000ULL * d;
if (v0_squared > reduction) {
uint32_t vm_squared = (uint32_t)(v0_squared - reduction);
vm = integer_sqrt(vm_squared);
} else {
vm = 0; // 理论上可以减速到0
}
}
}
return vm;
}

/**
* @brief 二分搜索寻找最优频率
* @param v0 起始频率
* @param vt_desired 期望目标频率
* @param total_pulses 可用总脉冲数
* @param a 加速度
* @param d 减速度
* @param is_accel 是否为加速情况
* @return 最优频率
*/
uint32_t BinarySearchOptimalFreq(uint32_t v0, uint32_t vt_desired,
int32_t total_pulses, uint32_t a, uint32_t d,
uint8_t is_accel)
{
uint32_t low = is_accel ? v0 : 0;
uint32_t high = is_accel ? (vt_desired * 2) : v0;
uint32_t best_vm = v0;
for (int iterations = 0; iterations < 20; iterations++) // 限制迭代次数
{
uint32_t mid = low + (high - low) / 2;
// 计算当前频率下的脉冲数
uint32_t s1, s3;
if (is_accel) {
s1 = ((uint64_t)mid * mid - (uint64_t)v0 * v0) / (2000ULL * a);
} else {
s1 = ((uint64_t)v0 * v0 - (uint64_t)mid * mid) / (2000ULL * d);
}
s3 = ((uint64_t)mid * mid) / (2000ULL * d);
int32_t total_calc = s1 + s3;
if (total_calc == total_pulses) {
return mid; // 找到精确解
} else if (total_calc < total_pulses) {
best_vm = mid;
low = mid + 1;
} else {
high = mid - 1;
}
if (low > high) break;
}
return best_vm;
}

void PLSR_HandleSectionEnd(void)
{
// 清零所有部分的脉冲计数
@@ -1466,7 +1573,6 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
PLSR_PWM_SetFrequency(g_plsr_route.target_freq);
__HAL_TIM_SetAutoreload(&htim2, g_plsr_route.const_pulse_count);

__HAL_TIM_SET_COUNTER(&htim2, 0);
PLSR_PWM_Start();
break;
@@ -1637,7 +1743,7 @@ void PLSR_Route_Init(PLSR_RouteConfig_t* route)
// 初始化全局变量
g_last_freq = 0; // 清零上一次计算的频率

PLSR_TIM6_SetUpdateFreq(10); //初始化TIM6更新频率为1000us(1ms)
PLSR_TIM6_SetUpdateFreq(100); //初始化TIM6更新频率为1000us(1ms)
PLSR_PWM_Stop(); // 确保PWM停止，避免意外输出
}

@@ -2135,21 +2241,21 @@ void PLSR_Section_SwitchNext(PLSR_RouteConfig_t* route, uint8_t is_pulse_complet
route->prevPulseCount = g_plsr_location;
}
}
// 检查下一段是否有效
if(next_section_num == 0 && current_section->section_num == route->section_num)
{
// 如果是最后一段且下一段为0，结束路径
route->route_state = PLSR_ROUTE_COMPLETED;
PLSR_Route_Stop(route);
return;
}
if (next_section_num > PLSR_MAX_SECTIONS)
{
// 路径结束
route->route_state = PLSR_ROUTE_COMPLETED;
PLSR_Route_Stop(route);
return;
}
// // 检查下一段是否有效
// if(next_section_num == 0 && current_section->section_num == route->section_num)
// {
// // 如果是最后一段且下一段为0，结束路径
// route->route_state = PLSR_ROUTE_COMPLETED;
// PLSR_Route_Stop(route);
// return;
// }
// if (next_section_num > PLSR_MAX_SECTIONS)
// {
// // 路径结束
// route->route_state = PLSR_ROUTE_COMPLETED;
// PLSR_Route_Stop(route);
// return;
// }
if(next_section_num == 0)
{
route->current_section_num = current_section->section_num + 1;


+ 2
- 2
PLSR/PLSR/EWARM/settings/test.1.dnx View File

@@ -12,12 +12,12 @@
<ByteLimit>50</ByteLimit>
</Stack>
<StLinkDriver>
<stlinkserialNo>46232557</stlinkserialNo>
<stlinkfoundProbes />
<CStepIntDis>_ 0</CStepIntDis>
<LeaveTargetRunning>_ 0</LeaveTargetRunning>
<stlinkResetStyle>0</stlinkResetStyle>
<stlinkResetStrategy>2</stlinkResetStrategy>
<stlinkserialNo>46232557</stlinkserialNo>
<stlinkfoundProbes />
</StLinkDriver>
<DebugChecksum>
<Checksum>1150792608</Checksum>


+ 460
- 460
PLSR/PLSR/EWARM/test.1.dep
File diff suppressed because it is too large
View File


BIN
PLSR/PLSR/EWARM/test.1/Exe/test.1.sim View File


Write Preview
Loading…
Cancel
Save