jiujialin
/
Train_Camp_PLSR
1
0
Форк 0
Код Проблеми 0 Запити на злиття 0 Релізи 0 Вікі Активність
Переглянути джерело

解决昨日加速不到目标频率的问题,原因在于频率的计算公式有问题,但小加速度使用之前的频率计算公式是可行的.当前存在如果加速过程中加速会导致脉冲卡死的情况需要解决

1
JIU JIALIN 1 місяць тому
джерело
761a49a62c
коміт
2d953cefad
3 змінених файлів з 803 додано та 564 видалено
Розділений перегляд
Параметри порівняння
Показати статистику Завантажити патч Завантажити файл різниці
  1. +166
    -123
      PLSR/PLSR/Core/Src/tim.c
  2. +637
    -441
      PLSR/PLSR/EWARM/test.1.dep
  3. BIN
      PLSR/PLSR/EWARM/test.1/Exe/test.1.sim

+ 166
- 123
PLSR/PLSR/Core/Src/tim.c Переглянути файл

@@ -738,89 +738,108 @@ void PLSR_PWM_Stop(void)
*/
static void PLSR_CalculateTimerParams(uint32_t frequency, uint16_t* prescaler, uint32_t* period)
{
// STM32F4系列定时器时钟频率（APB1定时器通常为84MHz，APB2定时器为168MHz）
// 这里使用168MHz，适用于APB2上的高速定时器（如TIM1、TIM8等）
if (frequency < PLSR_PWM_FREQ_MIN) frequency = PLSR_PWM_FREQ_MIN;
if (frequency > PLSR_PWM_FREQ_MAX) frequency = PLSR_PWM_FREQ_MAX;

uint32_t psc;
uint32_t arr;
uint32_t f = (uint32_t)(frequency + 0.5f);

// PLSR_Calc_TimerParams((uint32_t)(f_hz + 0.5f), &psc, &arr);
uint32_t timer_clock = 168000000;
// 定时器频率计算原理：
// 输出频率 = 定时器时钟频率 / ((预分频器 + 1) * (自动重装载值 + 1))
// 因此：(预分频器 + 1) * (自动重装载值 + 1) = 定时器时钟频率 / 目标频率
// 频率为0时的异常处理
if (frequency == 0) {
*prescaler = 65535; // 最大预分频器
*period = 65535; // 最大周期，输出最低频率
return;
}
// 频率过高时的处理（超过定时器时钟频率）
if (frequency > timer_clock) {
*prescaler = 0; // 最小预分频器
*period = 0; // 最小周期，输出最高频率
return;
psc = 1;
while (timer_clock / (psc + 1) / f > 65535) {
psc++;
if (psc > 0XFFFF) psc = 0XFFFF;
}
// 计算总的计数值：定时器时钟频率除以目标频率
uint64_t total_count = (uint64_t)timer_clock / frequency;
// 如果总计数值超出范围，直接设置为最低频率
if (total_count > (uint64_t)65536 * 65536) {
*prescaler = 65535;
*period = 65535;
return;
}
// 优化搜索策略：寻找最佳的预分频器和自动重装载值组合
// 目标：找到误差最小且分辨率最高的组合
uint32_t best_prescaler = 65535;
uint32_t best_period = 65535;
uint32_t min_error = UINT32_MAX;
// 计算合理的搜索范围，避免无效遍历
uint32_t max_psc = (total_count > 65535) ? 65535 : (uint32_t)total_count;
uint32_t min_psc = (total_count + 65535) / 65536; // 确保ARR不超过65535
if (min_psc == 0) min_psc = 1;
// 遍历预分频器值，寻找最佳组合
for (uint32_t psc = min_psc; psc <= max_psc; psc++) {
// 计算对应的自动重装载值（四舍五入）
uint32_t arr = (uint32_t)((total_count + psc/2) / psc);
arr = (timer_clock / (psc + 1)) / f - 1;
if (arr < 2) arr = 2;
*prescaler = psc;
*period = arr;

// // STM32F4系列定时器时钟频率（APB1定时器通常为84MHz，APB2定时器为168MHz）
// // 这里使用168MHz，适用于APB2上的高速定时器（如TIM1、TIM8等）
// uint32_t timer_clock = 168000000;
// // 定时器频率计算原理：
// // 输出频率 = 定时器时钟频率 / ((预分频器 + 1) * (自动重装载值 + 1))
// // 因此：(预分频器 + 1) * (自动重装载值 + 1) = 定时器时钟频率 / 目标频率
// // 频率为0时的异常处理
// if (frequency == 0) {
// *prescaler = 65535; // 最大预分频器
// *period = 65535; // 最大周期，输出最低频率
// return;
// }
// // 频率过高时的处理（超过定时器时钟频率）
// if (frequency > timer_clock) {
// *prescaler = 0; // 最小预分频器
// *period = 0; // 最小周期，输出最高频率
// return;
// }
// // 计算总的计数值：定时器时钟频率除以目标频率
// uint64_t total_count = (uint64_t)timer_clock / frequency;
// // 如果总计数值超出范围，直接设置为最低频率
// if (total_count > (uint64_t)65536 * 65536) {
// *prescaler = 65535;
// *period = 65535;
// return;
// }
// // 优化搜索策略：寻找最佳的预分频器和自动重装载值组合
// // 目标：找到误差最小且分辨率最高的组合
// uint32_t best_prescaler = 65535;
// uint32_t best_period = 65535;
// uint32_t min_error = UINT32_MAX;
// // 计算合理的搜索范围，避免无效遍历
// uint32_t max_psc = (total_count > 65535) ? 65535 : (uint32_t)total_count;
// uint32_t min_psc = (total_count + 65535) / 65536; // 确保ARR不超过65535
// if (min_psc == 0) min_psc = 1;
// // 遍历预分频器值，寻找最佳组合
// for (uint32_t psc = min_psc; psc <= max_psc; psc++) {
// // 计算对应的自动重装载值（四舍五入）
// uint32_t arr = (uint32_t)((total_count + psc/2) / psc);
// 检查自动重装载值是否在有效范围内
if (arr >= 1 && arr <= 65536) {
// 计算实际输出频率和误差
uint32_t actual_total = psc * arr;
uint32_t actual_freq = timer_clock / actual_total;
uint32_t error = (actual_freq > frequency) ?
(actual_freq - frequency) : (frequency - actual_freq);
// // 检查自动重装载值是否在有效范围内
// if (arr >= 1 && arr <= 65536) {
// // 计算实际输出频率和误差
// uint32_t actual_total = psc * arr;
// uint32_t actual_freq = timer_clock / actual_total;
// uint32_t error = (actual_freq > frequency) ?
// (actual_freq - frequency) : (frequency - actual_freq);
// 选择误差最小的组合，误差相同时选择分辨率更高的（ARR更大）
if (error < min_error || (error == min_error && arr > best_period + 1)) {
min_error = error;
best_prescaler = psc - 1; // 转换为寄存器值
best_period = arr - 1; // 转换为寄存器值
// // 选择误差最小的组合，误差相同时选择分辨率更高的（ARR更大）
// if (error < min_error || (error == min_error && arr > best_period + 1)) {
// min_error = error;
// best_prescaler = psc - 1; // 转换为寄存器值
// best_period = arr - 1; // 转换为寄存器值
// 如果找到精确匹配，直接返回
if (error == 0) {
*prescaler = best_prescaler;
*period = best_period;
return;
}
}
}
}
// 返回找到的最佳组合
*prescaler = best_prescaler;
*period = best_period;
// 如果没有找到合理的组合，使用默认的1kHz配置
if (min_error == UINT32_MAX) {
// 预分频器 = 167 (实际分频168)，自动重装载 = 999 (实际计数1000)
// 输出频率 = 168MHz / (168 * 1000) = 1kHz
*prescaler = 167; // 168MHz / 168 = 1MHz
*period = 999; // 1MHz / 1000 = 1kHz
}
// // 如果找到精确匹配，直接返回
// if (error == 0) {
// *prescaler = best_prescaler;
// *period = best_period;
// return;
// }
// }
// }
// }
// // 返回找到的最佳组合
// *prescaler = best_prescaler;
// *period = best_period;
// // 如果没有找到合理的组合，使用默认的1kHz配置
// if (min_error == UINT32_MAX) {
// // 预分频器 = 167 (实际分频168)，自动重装载 = 999 (实际计数1000)
// // 输出频率 = 168MHz / (168 * 1000) = 1kHz
// *prescaler = 167; // 168MHz / 168 = 1MHz
// *period = 999; // 1MHz / 1000 = 1kHz
// }
}


@@ -1044,6 +1063,9 @@ uint32_t integer_sqrt_64(uint64_t x)
return (uint32_t)result;
}

// ==================== PLSR 路径计算函数实现 ====================
//在加速度较小时,可能会出现计算脉冲数为0的情况,此时会导致无法进入加速状态
void Calculate_PluseNum_Simplified(PLSR_RouteConfig_t *route)
{
int32_t accel_pulse_num = 0; // 加速过程脉冲数
@@ -1762,8 +1784,15 @@ static uint8_t PLSR_SetDirectionPin(PLSR_RouteConfig_t* route, PLSR_SectionConfi
GPIO_PinState dir_pin_state;
// 根据计算后的实际可发脉冲数确定方向（正值为正向，负值为反向）
// 在绝对模式下，actual_pulse已经计算为目标位置与当前位置的差值
uint8_t is_forward = (current_section->actual_pulse >= 0);
uint8_t is_forward;
if(route->mode == PLSR_MODE_ABSOLUTE)
{
is_forward = (current_section->actual_pulse >= 0);
}
else
{
is_forward = (current_section->target_pulse >= 0);
}
// 检测方向是否发生变化
uint8_t direction_changed = (s_last_direction != 0xFF && s_last_direction != is_forward);
@@ -1807,8 +1836,16 @@ static uint8_t PLSR_SetDirectionPin(PLSR_RouteConfig_t* route, PLSR_SectionConfi
// 如果设置了方向延时时间，则等待指定时间
if (route->dir_delay > 0 && direction_changed)
{
// 使用HAL库延时函数，单位为毫秒
HAL_Delay(route->dir_delay);
if(route->dir_delay > 10)
{
// 使用HAL库延时函数，单位为毫秒
HAL_Delay(route->dir_delay-1);
}
else
{
HAL_Delay(route->dir_delay);
}

}
// 返回方向变化状态
@@ -2143,7 +2180,12 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc
{
if (route == NULL) return 0;
// 使用全局变量g_last_freq替代static变量
// 静态变量用于缓存上次计算的结果，避免重复计算
static uint32_t s_last_executed_pulses = 0xFFFFFFFF; // 初始化为无效值
static uint32_t s_last_calculated_freq = 0;
static uint8_t s_last_is_accel = 0xFF; // 初始化为无效值
static uint8_t s_last_section_num = 0xFF; // 初始化为无效值
// 获取当前段配置
PLSR_SectionConfig_t* current_section = &route->section[route->current_section_num - 1];
@@ -2167,6 +2209,15 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc
next_executed_pulses = 0;
}
}
// 检查是否需要重新计算：脉冲步数、加减速状态或段号发生变化时才重新计算
if (executed_pulses == s_last_executed_pulses &&
is_accel == s_last_is_accel &&
route->current_section_num == s_last_section_num)
{
// 脉冲步数、状态和段号都没有变化，直接返回上次计算的结果
return s_last_calculated_freq;
}

// 使用速度位移公式 vt^2 = v0^2 + 2ax 计算当前频率，其中v0使用initial_freq作为初始速度
@@ -2202,16 +2253,9 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc
}
else
{
uint64_t v0_squared = (uint64_t)v0 * v0;
if (v0_squared >= 2 * a * executed_pulses)
{
uint64_t freq_squared_start = 2 * a * executed_pulses;
freq_start = integer_sqrt_64(freq_squared_start);
}
else
{
freq_start = 0;
}
uint64_t freq_squared_start = 2 * a * executed_pulses;
freq_start = integer_sqrt_64(freq_squared_start);

}
}

@@ -2226,15 +2270,8 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc
else
{
uint64_t v0_squared = (uint64_t)v0 * v0;
if (v0_squared >= 2 * a * next_executed_pulses)
{
uint64_t freq_squared_end = 2 * a * next_executed_pulses;
freq_end = integer_sqrt_64(freq_squared_end);
}
else
{
freq_end = 0;
}
uint64_t freq_squared_end = 2 * a * next_executed_pulses;
freq_end = integer_sqrt_64(freq_squared_end);
}
// 计算当前脉冲的平均频率
@@ -2242,6 +2279,10 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc
// 更新全局变量（保存当前脉冲结束时的频率，供下次使用）
g_last_freq = freq_end;
// if(executed_pulses > 61)
// {
// printf("freq_start: %lu, freq_end: %lu, calculated_freq: %lu\r\n", freq_start, freq_end, calculated_freq);
// }
// 限制频率范围
if (calculated_freq < PLSR_PWM_FREQ_MIN)
@@ -2253,6 +2294,12 @@ uint32_t PLSR_Calculate_FreqByPosition(PLSR_RouteConfig_t* route, uint8_t is_acc
calculated_freq = PLSR_PWM_FREQ_MAX;
}
// 缓存本次计算结果，供下次比较使用
s_last_executed_pulses = executed_pulses;
s_last_calculated_freq = calculated_freq;
s_last_is_accel = is_accel;
s_last_section_num = route->current_section_num;
return calculated_freq;
}

@@ -2303,16 +2350,16 @@ void PLSR_Accel_Process(PLSR_RouteConfig_t* route)
// 根据当前脉冲位置计算频率（传入1表示加速过程）
uint32_t calculated_freq = PLSR_Calculate_FreqByPosition(route, 1);
// 更新当前频率
route->current_freq = calculated_freq;
// 检查是否达到目标频率
if (route->current_freq >= route->target_freq) {
route->current_freq = route->target_freq; // 限制到目标频率
if (calculated_freq >= route->target_freq) {
calculated_freq = route->target_freq; // 限制到目标频率
}
// 更新PWM频率
PLSR_PWM_SetFrequency(route->current_freq);
// 只有当计算出的频率与当前频率不同时才更新
if (calculated_freq != route->current_freq) {
route->current_freq = calculated_freq;
PLSR_PWM_SetFrequency(route->current_freq);
}
// 如果是第一次设置非零频率，启动PWM输出
if(first_flag == 1 && route->current_freq != 0)
@@ -2327,12 +2374,9 @@ void PLSR_Accel_Process(PLSR_RouteConfig_t* route)
// 使用速度位移公式 vt^2 = v0^2 - 2ax 计算当前脉冲的下一个脉冲频率
uint32_t calculated_freq = PLSR_Calculate_FreqByPosition(route, 0);
// 更新当前频率
route->current_freq = calculated_freq;
// 检查是否达到目标频率
if (route->current_freq <= route->target_freq) {
route->current_freq = route->target_freq; // 限制到目标频率
if (calculated_freq <= route->target_freq) {
calculated_freq = route->target_freq; // 限制到目标频率
// 如果目标频率为0，说明减速到停止
if (route->target_freq == 0)
@@ -2343,15 +2387,14 @@ void PLSR_Accel_Process(PLSR_RouteConfig_t* route)
}
}
// 更新PWM频率或停止PWM
if (route->current_freq > 0)
{
PLSR_PWM_SetFrequency(route->current_freq);
} else
{
// 频率减速到0，停止PWM输出
route->run_state = PLSR_STATE_IDLE;
PLSR_PWM_Stop();
// 只有当计算出的频率与当前频率不同时才更新PWM频率
if (calculated_freq != route->current_freq) {
route->current_freq = calculated_freq;
if (route->current_freq > 0)
{
PLSR_PWM_SetFrequency(route->current_freq);
}
}
}
}


+ 637
- 441
PLSR/PLSR/EWARM/test.1.dep
Різницю між файлами не показано, бо вона завелика
Переглянути файл


BIN
PLSR/PLSR/EWARM/test.1/Exe/test.1.sim Переглянути файл


Писати Попередній перегляд
Завантаження…
Відмінити
Зберегти