|
|
@@ -225,9 +225,7 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route) |
|
|
|
// 获取当前段配置(段号从1开始,数组索引从0开始) |
|
|
|
PLSR_SectionConfig_t* current_section = &route->section[route->current_section_num - 1]; |
|
|
|
|
|
|
|
// 只处理线性加减速算法 |
|
|
|
if (route->accel_config.accel_algorithm == PLSR_ACCEL_LINEAR) |
|
|
|
{ |
|
|
|
|
|
|
|
uint32_t v0 = route->current_freq; // 起始频率 |
|
|
|
uint32_t vt_desired = current_section->target_freq; // 期望目标频率 |
|
|
|
uint32_t vt = vt_desired; // 实际目标频率(可能会被调整) |
|
|
@@ -424,6 +422,8 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route) |
|
|
|
route->part1_state = part1_state; |
|
|
|
route->part2_state = part2_state; |
|
|
|
route->part3_state = part3_state; |
|
|
|
route->part1_time = part1_time; |
|
|
|
route->part3_time = part3_time; |
|
|
|
route->part1_target_freq = vt; |
|
|
|
route->part2_target_freq = vt; |
|
|
|
route->part3_target_freq = 0; |
|
|
@@ -446,7 +446,6 @@ void Calculate_PluseNum(PLSR_RouteConfig_t *route) |
|
|
|
route->run_state = PLSR_STATE_CONST; |
|
|
|
route->target_freq = v0; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@@ -618,363 +617,355 @@ void PLSR_Accel_UpdateRates(PLSR_RouteConfig_t* route) |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
/* ==================== 快速立方根近似 ==================== */ |
|
|
|
static uint32_t cbrt_approx(uint64_t x) |
|
|
|
{ |
|
|
|
if (x == 0) return 0; |
|
|
|
|
|
|
|
/* 使用位操作快速初始估算 */ |
|
|
|
uint32_t guess; |
|
|
|
if (x >= 1000000000ULL) guess = 1000; |
|
|
|
else if (x >= 1000000ULL) guess = 100; |
|
|
|
else if (x >= 1000ULL) guess = 10; |
|
|
|
else guess = 1; |
|
|
|
|
|
|
|
/* 牛顿法迭代:x_{n+1} = (2*x_n + a/x_n²) / 3 */ |
|
|
|
for (int i = 0; i < 3; i++) { |
|
|
|
uint64_t guess_sq = (uint64_t)guess * guess; |
|
|
|
if (guess_sq == 0) break; |
|
|
|
guess = (uint32_t)((2ULL * guess + x / guess_sq) / 3); |
|
|
|
} |
|
|
|
|
|
|
|
return guess; |
|
|
|
} |
|
|
|
|
|
|
|
/* ==================== S曲线参数初始化函数 ==================== */ |
|
|
|
void SCurve_InitializeParameters(PLSR_RouteConfig_t* route) |
|
|
|
/** |
|
|
|
* @brief S曲线加减速参数初始化 |
|
|
|
* @param route 路径配置结构体指针 |
|
|
|
* @param accel_time_ms 加速时间(毫秒) |
|
|
|
* @param decel_time_ms 减速时间(毫秒) |
|
|
|
*/ |
|
|
|
void SCurve_InitializeParameters(PLSR_RouteConfig_t* route, uint32_t accel_time_ms, uint32_t decel_time_ms) |
|
|
|
{ |
|
|
|
if (route == NULL) return; |
|
|
|
|
|
|
|
/* 输入参数验证和预处理 */ |
|
|
|
const uint32_t accel_rate = (route->accel_rate == 0) ? 1 : route->accel_rate; // Hz/ms |
|
|
|
const uint32_t decel_rate = (route->decel_rate == 0) ? 1 : route->decel_rate; // Hz/ms |
|
|
|
|
|
|
|
/* ============ 加速段参数计算 ============ */ |
|
|
|
uint32_t vel_start_accel = route->initial_freq; |
|
|
|
uint32_t vel_target_accel = route->part1_target_freq; |
|
|
|
if (vel_target_accel > PLSR_PWM_FREQ_MAX) { |
|
|
|
vel_target_accel = PLSR_PWM_FREQ_MAX; |
|
|
|
} |
|
|
|
|
|
|
|
uint32_t delta_vel_accel = (vel_target_accel > vel_start_accel) ? |
|
|
|
(vel_target_accel - vel_start_accel) : 0; |
|
|
|
|
|
|
|
if (delta_vel_accel > 0) |
|
|
|
if (accel_time_ms > 0) |
|
|
|
{ |
|
|
|
/* 计算加速时间参数 */ |
|
|
|
uint32_t total_accel_time_ms = CLAMP_MIN(delta_vel_accel / accel_rate, 1); |
|
|
|
uint32_t jerk_time_ms = CLAMP_MIN(total_accel_time_ms / 2, 1); |
|
|
|
|
|
|
|
/* 关键公式:Δv = J·Tj²,因此 J = Δv / Tj² */ |
|
|
|
uint32_t jerk = (uint32_t)DIV_ROUND((uint64_t)delta_vel_accel * 1000ULL, |
|
|
|
(uint64_t)jerk_time_ms * jerk_time_ms); |
|
|
|
uint32_t max_accel = (jerk * jerk_time_ms) / 1000; // 转换单位 |
|
|
|
|
|
|
|
/* 脉冲数计算 - 使用精确积分公式 */ |
|
|
|
uint64_t T = jerk_time_ms; |
|
|
|
uint64_t T_sq = T * T; |
|
|
|
uint64_t T_cu = T_sq * T; |
|
|
|
|
|
|
|
/* 第一阶段: s₁ = v₀·T + (1/6)·J·T³ */ |
|
|
|
uint64_t phase1_displacement = (uint64_t)vel_start_accel * T + |
|
|
|
((uint64_t)jerk * T_cu) / 6000; // 除以6000而不是6是因为单位换算 |
|
|
|
uint32_t phase1_pulses = (uint32_t)DIV_ROUND(phase1_displacement, 1000); |
|
|
|
|
|
|
|
/* 第二阶段:中间速度计算 */ |
|
|
|
uint32_t vel_mid = vel_start_accel + (uint32_t)((jerk * T_sq) / 2000); |
|
|
|
uint32_t vel_start = route->initial_freq; // V0 |
|
|
|
uint32_t vel_target = route->part1_target_freq; // Vt |
|
|
|
|
|
|
|
/* 第二阶段位移:s₂ = v_mid·T - (1/6)·J·T³ (注意减号,因为是减jerk阶段)*/ |
|
|
|
uint64_t phase2_displacement = (uint64_t)vel_mid * T - |
|
|
|
((uint64_t)jerk * T_cu) / 6000; |
|
|
|
uint32_t phase2_pulses = (uint32_t)DIV_ROUND(phase2_displacement, 1000); |
|
|
|
|
|
|
|
/* 归一化到实际脉冲总数,防止舍入误差 */ |
|
|
|
uint32_t calculated_total = phase1_pulses + phase2_pulses; |
|
|
|
uint32_t actual_total = route->accel_pulse_count; |
|
|
|
if (calculated_total != actual_total && calculated_total > 0) { |
|
|
|
if (actual_total > calculated_total) |
|
|
|
{ |
|
|
|
phase2_pulses += (actual_total - calculated_total); |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
phase1_pulses = (uint32_t)(((uint64_t)phase1_pulses * actual_total) / calculated_total); |
|
|
|
phase2_pulses = actual_total - phase1_pulses; |
|
|
|
} |
|
|
|
if (vel_target > vel_start) |
|
|
|
{ |
|
|
|
uint32_t delta_v = vel_target - vel_start; |
|
|
|
uint32_t Tj = accel_time_ms / 2; // 到达最大加速度的时间(加速时间的一半) |
|
|
|
|
|
|
|
// 加加速度计算:J = 4*Vt/(Tt^2) = 4*delta_v/(accel_time_ms^2) |
|
|
|
uint64_t jerk_numerator = 4ULL * delta_v * 1000ULL; // 乘1000进行单位换算 |
|
|
|
uint64_t jerk_denominator = (uint64_t)accel_time_ms * accel_time_ms; |
|
|
|
uint32_t jerk = (uint32_t)(jerk_numerator / jerk_denominator); // Hz/ms² |
|
|
|
|
|
|
|
// 最大加速度:Amax = J * Tj |
|
|
|
uint32_t max_accel = (jerk * Tj) / 1000; // Hz/ms |
|
|
|
|
|
|
|
// 加加速过程的脉冲数:N1 = (1/6) * J * Tj^3 |
|
|
|
uint64_t Tj_cubed = (uint64_t)Tj * Tj * Tj; |
|
|
|
uint64_t phase1_pulses_calc = (jerk * Tj_cubed) / (6ULL * 1000ULL * 1000ULL); // 单位换算 |
|
|
|
|
|
|
|
// 减加速过程的脉冲数:N2 = V0*Tj + (5/6)*J*Tj^3 |
|
|
|
uint64_t phase2_term2 = (5ULL * jerk * Tj_cubed) / (6ULL * 1000ULL * 1000ULL); |
|
|
|
uint64_t phase2_pulses_calc = phase2_term2; |
|
|
|
|
|
|
|
// 存储加速段参数 |
|
|
|
route->scurve.accel_jerk = jerk; |
|
|
|
route->scurve.accel_max = max_accel; |
|
|
|
route->scurve.accel_jerk_time_ms = Tj; |
|
|
|
route->scurve.accel_phase1_pulses = (uint32_t)phase1_pulses_calc; |
|
|
|
route->scurve.accel_phase2_pulses = (uint32_t)phase2_pulses_calc; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
// 无需加速,清零参数 |
|
|
|
route->scurve.accel_jerk = 0; |
|
|
|
route->scurve.accel_max = 0; |
|
|
|
route->scurve.accel_jerk_time_ms = 0; |
|
|
|
route->scurve.accel_phase1_pulses = 0; |
|
|
|
route->scurve.accel_phase2_pulses = 0; |
|
|
|
} |
|
|
|
|
|
|
|
/* 存储加速段参数 */ |
|
|
|
route->scurve.accel_jerk = jerk; |
|
|
|
route->scurve.accel_max = max_accel; |
|
|
|
route->scurve.accel_jerk_time_ms = jerk_time_ms; |
|
|
|
route->scurve.accel_phase1_pulses = phase1_pulses; |
|
|
|
route->scurve.accel_phase2_pulses = phase2_pulses; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
/* 无加速段,清零参数 */ |
|
|
|
route->scurve.accel_jerk = 0; |
|
|
|
route->scurve.accel_max = 0; |
|
|
|
route->scurve.accel_jerk_time_ms = 0; |
|
|
|
route->scurve.accel_phase1_pulses = 0; |
|
|
|
route->scurve.accel_phase2_pulses = 0; |
|
|
|
} |
|
|
|
|
|
|
|
/* ============ 减速段参数计算 ============ */ |
|
|
|
uint32_t vel_start_decel = route->initial_freq; |
|
|
|
uint32_t vel_target_decel = route->part3_target_freq; |
|
|
|
if (vel_start_decel > PLSR_PWM_FREQ_MAX) { |
|
|
|
vel_start_decel = PLSR_PWM_FREQ_MAX; |
|
|
|
} |
|
|
|
|
|
|
|
uint32_t delta_vel_decel = (vel_start_decel > vel_target_decel) ? |
|
|
|
(vel_start_decel - vel_target_decel) : 0; |
|
|
|
|
|
|
|
if (delta_vel_decel > 0) { |
|
|
|
/* 计算减速时间参数 */ |
|
|
|
uint32_t total_decel_time_ms = CLAMP_MIN(delta_vel_decel / decel_rate, 1); |
|
|
|
uint32_t jerk_time_ms = CLAMP_MIN(total_decel_time_ms / 2, 1); |
|
|
|
|
|
|
|
/* 减速jerk计算 */ |
|
|
|
uint32_t jerk = (uint32_t)DIV_ROUND((uint64_t)delta_vel_decel * 1000ULL, |
|
|
|
(uint64_t)jerk_time_ms * jerk_time_ms); |
|
|
|
uint32_t max_decel = (jerk * jerk_time_ms) / 1000; |
|
|
|
|
|
|
|
/* 减速段脉冲数计算 */ |
|
|
|
uint64_t T = jerk_time_ms; |
|
|
|
uint64_t T_sq = T * T; |
|
|
|
uint64_t T_cu = T_sq * T; |
|
|
|
|
|
|
|
/* 第一阶段(减速开始): s₁ = v₀·T - (1/6)·J·T³ (减号,因为是减速)*/ |
|
|
|
uint64_t phase1_displacement = (uint64_t)vel_start_decel * T - |
|
|
|
((uint64_t)jerk * T_cu) / 6000; |
|
|
|
/* 防止负数 */ |
|
|
|
if ((int64_t)phase1_displacement < 0) phase1_displacement = 0; |
|
|
|
uint32_t phase1_pulses = (uint32_t)DIV_ROUND(phase1_displacement, 1000); |
|
|
|
|
|
|
|
/* 第二阶段:中间速度 */ |
|
|
|
uint32_t vel_mid = (vel_start_decel > (jerk * T_sq) / 2000) ? |
|
|
|
vel_start_decel - (jerk * T_sq) / 2000 : 0; |
|
|
|
|
|
|
|
/* 第二阶段位移:s₂ = v_mid·T + (1/6)·J·T³ (加号,因为是减jerk阶段)*/ |
|
|
|
uint64_t phase2_displacement = (uint64_t)vel_mid * T + |
|
|
|
((uint64_t)jerk * T_cu) / 6000; |
|
|
|
uint32_t phase2_pulses = (uint32_t)DIV_ROUND(phase2_displacement, 1000); |
|
|
|
if (decel_time_ms > 0) |
|
|
|
{ |
|
|
|
uint32_t vel_start_decel = route->part2_target_freq; // 减速起始速度 |
|
|
|
uint32_t vel_target_decel = route->part3_target_freq; // 减速目标速度 |
|
|
|
|
|
|
|
/* 归一化到实际脉冲总数 */ |
|
|
|
uint32_t calculated_total = phase1_pulses + phase2_pulses; |
|
|
|
uint32_t actual_total = route->decel_pulse_count; |
|
|
|
if (calculated_total != actual_total && calculated_total > 0) { |
|
|
|
if (actual_total > calculated_total) { |
|
|
|
phase2_pulses += (actual_total - calculated_total); |
|
|
|
} else { |
|
|
|
phase1_pulses = (uint32_t)(((uint64_t)phase1_pulses * actual_total) / calculated_total); |
|
|
|
phase2_pulses = actual_total - phase1_pulses; |
|
|
|
} |
|
|
|
if (vel_start_decel > vel_target_decel) |
|
|
|
{ |
|
|
|
uint32_t delta_v = vel_start_decel - vel_target_decel; |
|
|
|
uint32_t Tj = decel_time_ms / 2; // 到达最大减速度的时间 |
|
|
|
|
|
|
|
// 减加速度计算:J = 4*delta_v/(decel_time_ms^2) |
|
|
|
uint64_t jerk_numerator = 4ULL * delta_v * 1000ULL; |
|
|
|
uint64_t jerk_denominator = (uint64_t)decel_time_ms * decel_time_ms; |
|
|
|
uint32_t jerk = (uint32_t)(jerk_numerator / jerk_denominator); |
|
|
|
|
|
|
|
// 最大减速度 |
|
|
|
uint32_t max_decel = (jerk * Tj) / 1000; |
|
|
|
|
|
|
|
// 减减速过程的脉冲数:N1 = (1/6) * J * Tj^3 |
|
|
|
uint64_t Tj_cubed = (uint64_t)Tj * Tj * Tj; |
|
|
|
uint64_t phase1_pulses_calc = (jerk * Tj_cubed) / (6ULL * 1000ULL * 1000ULL); |
|
|
|
|
|
|
|
// 加减速过程的脉冲数:N2 = V0*Tj + (5/6)*J*Tj^3 |
|
|
|
uint64_t phase2_term2 = (5ULL * jerk * Tj_cubed) / (6ULL * 1000ULL * 1000ULL); |
|
|
|
uint64_t phase2_pulses_calc = phase2_term2; |
|
|
|
|
|
|
|
// 存储减速段参数 |
|
|
|
route->scurve.decel_jerk = jerk; |
|
|
|
route->scurve.decel_max = max_decel; |
|
|
|
route->scurve.decel_jerk_time_ms = Tj; |
|
|
|
route->scurve.decel_phase1_pulses = (uint32_t)phase2_pulses_calc; |
|
|
|
route->scurve.decel_phase2_pulses = (uint32_t)phase1_pulses_calc; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
// 无需减速,清零参数 |
|
|
|
route->scurve.decel_jerk = 0; |
|
|
|
route->scurve.decel_max = 0; |
|
|
|
route->scurve.decel_jerk_time_ms = 0; |
|
|
|
route->scurve.decel_phase1_pulses = 0; |
|
|
|
route->scurve.decel_phase2_pulses = 0; |
|
|
|
} |
|
|
|
|
|
|
|
/* 存储减速段参数 */ |
|
|
|
route->scurve.decel_jerk = jerk; |
|
|
|
route->scurve.decel_max = max_decel; |
|
|
|
route->scurve.decel_jerk_time_ms = jerk_time_ms; |
|
|
|
route->scurve.decel_phase1_pulses = phase1_pulses; |
|
|
|
route->scurve.decel_phase2_pulses = phase2_pulses; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
/* 无减速段,清零参数 */ |
|
|
|
route->scurve.decel_jerk = 0; |
|
|
|
route->scurve.decel_max = 0; |
|
|
|
route->scurve.decel_jerk_time_ms = 0; |
|
|
|
route->scurve.decel_phase1_pulses = 0; |
|
|
|
route->scurve.decel_phase2_pulses = 0; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 重置S曲线计算状态(用于段切换) |
|
|
|
*/ |
|
|
|
void PLSR_Reset_SCurve_State(void) { |
|
|
|
// 通过调用函数并传入无效参数来触发状态重置 |
|
|
|
// 实际使用中,在段切换时调用此函数 |
|
|
|
} |
|
|
|
|
|
|
|
/* ==================== 第一阶段多项式求解(加速或减速开始阶段)==================== */ |
|
|
|
static uint32_t SCurve_SolvePhase1_Polynomial(uint32_t vel_start, uint32_t jerk, |
|
|
|
uint64_t displacement, uint8_t is_deceleration) |
|
|
|
{ |
|
|
|
if (jerk == 0) return vel_start; |
|
|
|
|
|
|
|
/* 方程: s = v₀·t ± (1/6)·J·t³ |
|
|
|
对于加速: s = v₀·t + (1/6)·J·t³ (正号) |
|
|
|
对于减速: s = v₀·t - (1/6)·J·t³ (负号) |
|
|
|
*/ |
|
|
|
|
|
|
|
if (vel_start == 0) |
|
|
|
{ |
|
|
|
/* 特殊情况:从零速开始 */ |
|
|
|
if (is_deceleration) return 0; /* 减速从0开始不合理 */ |
|
|
|
|
|
|
|
/* 纯立方方程: s = (1/6)·J·t³ => t = ∛(6s/J) */ |
|
|
|
uint64_t t_cubed = (6000ULL * displacement) / jerk; /* 乘1000是单位换算 */ |
|
|
|
uint32_t time_ms = cbrt_approx(t_cubed); |
|
|
|
|
|
|
|
/* v = (1/2)·J·t² */ |
|
|
|
uint64_t velocity = ((uint64_t)jerk * time_ms * time_ms) / 2000; |
|
|
|
return CLAMP_FREQUENCY(velocity); |
|
|
|
|
|
|
|
static double cbrt_approx(double x) { |
|
|
|
if (x == 0) return 0; |
|
|
|
if (x < 0) return -cbrt_approx(-x); |
|
|
|
double guess = x / 3.0; |
|
|
|
for (int i = 0; i < 10; i++) { |
|
|
|
guess = (2.0 * guess + x / (guess * guess)) / 3.0; |
|
|
|
} |
|
|
|
|
|
|
|
/* 使用多项式近似求解 */ |
|
|
|
/* 设 x = J·s/(v₀³), 则 t/τ ≈ 1 + x/6 - x²/36,其中 τ = s/v₀ */ |
|
|
|
uint64_t v0_sq = (uint64_t)vel_start * vel_start; |
|
|
|
uint64_t v0_cu = v0_sq * vel_start; |
|
|
|
|
|
|
|
/* 基础时间估计: τ = s/v₀ */ |
|
|
|
uint64_t tau_ms = displacement / vel_start; |
|
|
|
|
|
|
|
/* 修正项计算: x = J·s/(v₀³) */ |
|
|
|
uint64_t x_numerator = (uint64_t)jerk * displacement; |
|
|
|
uint64_t x = x_numerator / v0_cu; |
|
|
|
|
|
|
|
/* 多项式修正: t ≈ τ·(1 + x/6 - x²/36) */ |
|
|
|
uint64_t correction = x / 6; |
|
|
|
if (x < 36) { /* 防止溢出和负数 */ |
|
|
|
uint64_t x_sq = x * x; |
|
|
|
if (correction > x_sq / 36) { |
|
|
|
correction -= x_sq / 36; |
|
|
|
return guess; |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 第一阶段:根据脉冲数计算时间(加加速段,牛顿迭代) |
|
|
|
* 方程: (1/6)*J_real*t^3 - S*1000 = 0 |
|
|
|
* @param pulse_num 当前脉冲数 |
|
|
|
* @param jerk 加加速度 (Hz/ms²) |
|
|
|
* @return 时间 (ms) |
|
|
|
*/ |
|
|
|
static double SCurve_GetTimeFromPulses_Phase1(uint32_t pulse_num, uint32_t jerk) { |
|
|
|
if (pulse_num == 0 || jerk == 0) return 0.0; |
|
|
|
|
|
|
|
double S_cont = (double)pulse_num * 1000.0; // 脉冲转换为连续量 |
|
|
|
double J_real = (double)jerk / 1000.0; // Hz/ms² |
|
|
|
|
|
|
|
// 初始猜测:用反函数近似值作为初始点 |
|
|
|
double t = cbrt_approx((6.0 * S_cont) / J_real); |
|
|
|
if (t <= 0.0) t = 1.0; // 避免负数或零 |
|
|
|
|
|
|
|
// 牛顿迭代 |
|
|
|
for (int i = 0; i < 40; ++i) { |
|
|
|
double t2 = t * t; |
|
|
|
double t3 = t2 * t; |
|
|
|
double F = (1.0 / 6.0) * J_real * t3 - S_cont; |
|
|
|
double dF = 0.5 * J_real * t2; |
|
|
|
|
|
|
|
if (fabs(dF) < 1e-12) break; |
|
|
|
|
|
|
|
double t_new = t - F / dF; |
|
|
|
if (t_new <= 0.0) { |
|
|
|
t_new = t * 0.5; |
|
|
|
} |
|
|
|
if (fabs(t_new - t) < 1e-7) { |
|
|
|
t = t_new; |
|
|
|
break; |
|
|
|
} |
|
|
|
t = t_new; |
|
|
|
} |
|
|
|
|
|
|
|
uint64_t time_ms = tau_ms + (tau_ms * correction) / 1000; |
|
|
|
if (time_ms == 0) time_ms = 1; |
|
|
|
|
|
|
|
/* 计算速度: v = v₀ ± (1/2)·J·t² */ |
|
|
|
uint64_t time_sq = (time_ms * time_ms) / 1000; /* 防止溢出 */ |
|
|
|
uint64_t accel_term = ((uint64_t)jerk * time_sq) / 2; |
|
|
|
|
|
|
|
uint64_t velocity; |
|
|
|
if (is_deceleration) { |
|
|
|
/* 减速: v = v₀ - (1/2)·J·t² */ |
|
|
|
velocity = ((uint64_t)vel_start * 1000 > accel_term) ? |
|
|
|
((uint64_t)vel_start * 1000 - accel_term) / 1000 : 0; |
|
|
|
} else { |
|
|
|
/* 加速: v = v₀ + (1/2)·J·t² */ |
|
|
|
velocity = vel_start + accel_term / 1000; |
|
|
|
} |
|
|
|
|
|
|
|
return CLAMP_FREQUENCY(velocity); |
|
|
|
|
|
|
|
if (t < 0.0) t = 0.0; |
|
|
|
return t; // ms |
|
|
|
} |
|
|
|
|
|
|
|
/* ==================== 第二阶段多项式求解(jerk减小阶段)==================== */ |
|
|
|
static uint32_t SCurve_SolvePhase2_Polynomial(uint32_t vel_start, uint32_t jerk, |
|
|
|
uint32_t jerk_time_ms, uint64_t displacement, |
|
|
|
uint8_t is_deceleration) |
|
|
|
{ |
|
|
|
if (jerk == 0 || jerk_time_ms == 0) return vel_start; |
|
|
|
|
|
|
|
/* 第二阶段的起点速度计算 */ |
|
|
|
uint64_t T_sq = (uint64_t)jerk_time_ms * jerk_time_ms; |
|
|
|
uint32_t vel_mid; |
|
|
|
|
|
|
|
if (is_deceleration) { |
|
|
|
/* 减速段:第二阶段起点是第一阶段结束点 */ |
|
|
|
vel_mid = ((uint64_t)vel_start * 1000 > (jerk * T_sq) / 2) ? |
|
|
|
vel_start - (uint32_t)((jerk * T_sq) / 2000) : 0; |
|
|
|
|
|
|
|
/* Phase2: pulses*1000 = Vmid*t + 0.5*Amax*t^2 - (1/6)*J_real*t^3 |
|
|
|
其中 Amax = J_real * Tj (Tj 单位 ms), J_real = jerk / 1000 */ |
|
|
|
static double SCurve_GetTimeFromPulses_Phase2(uint32_t pulse_num, uint32_t V0, |
|
|
|
uint32_t jerk, uint32_t Tj) { |
|
|
|
if (pulse_num == 0) return 0.0; |
|
|
|
|
|
|
|
double J_real = (double)jerk / 1000.0; /* Hz / ms^2 */ |
|
|
|
double Tj_d = (double)Tj; /* ms */ |
|
|
|
double Amax = J_real * Tj_d; /* Hz / ms (注意单位:a in Hz per ms) */ |
|
|
|
double Vmid = (double)V0; /* V0 parameter here is vmid (caller should pass vmid) */ |
|
|
|
|
|
|
|
double S_cont = (double)pulse_num * 1000.0; /* 连续量 */ |
|
|
|
|
|
|
|
double t; |
|
|
|
if (Vmid > 0.0) |
|
|
|
{ |
|
|
|
t = S_cont / Vmid; |
|
|
|
if (t <= 0.0) t = Tj_d / 2.0; |
|
|
|
} else { |
|
|
|
/* 加速段:第二阶段起点 */ |
|
|
|
vel_mid = vel_start + (uint32_t)((jerk * T_sq) / 2000); |
|
|
|
t = Tj_d / 2.0; |
|
|
|
} |
|
|
|
|
|
|
|
/* 第二阶段方程求解 */ |
|
|
|
/* 加速第二阶段: s = v_mid·t - (1/6)·J·t³ (减jerk) |
|
|
|
减速第二阶段: s = v_mid·t + (1/6)·J·t³ (减jerk,但整体仍是减速)*/ |
|
|
|
|
|
|
|
if (vel_mid == 0) { |
|
|
|
/* 特殊情况处理 */ |
|
|
|
uint64_t t_cubed = (6000ULL * displacement) / jerk; |
|
|
|
uint32_t time_ms = cbrt_approx(t_cubed); |
|
|
|
|
|
|
|
uint64_t velocity; |
|
|
|
if (is_deceleration) { |
|
|
|
velocity = ((uint64_t)jerk * time_ms * time_ms) / 2000; |
|
|
|
} else { |
|
|
|
velocity = vel_mid > ((uint64_t)jerk * time_ms * time_ms) / 2000 ? |
|
|
|
vel_mid - ((uint64_t)jerk * time_ms * time_ms) / 2000 : 0; |
|
|
|
|
|
|
|
/* 牛顿迭代求解 F(t) = Vmid*t + 0.5*Amax*t^2 - (1/6)*J_real*t^3 - S_cont = 0 */ |
|
|
|
for (int i = 0; i < 40; ++i) { |
|
|
|
double t2 = t * t; |
|
|
|
double t3 = t2 * t; |
|
|
|
double F = Vmid * t + 0.5 * Amax * t2 - (1.0/6.0) * J_real * t3 - S_cont; |
|
|
|
double dF = Vmid + Amax * t - 0.5 * J_real * t2; |
|
|
|
|
|
|
|
if (fabs(dF) < 1e-12) break; |
|
|
|
double t_new = t - F / dF; |
|
|
|
if (t_new <= 0.0) { |
|
|
|
t_new = t * 0.5; |
|
|
|
} |
|
|
|
return CLAMP_FREQUENCY(velocity); |
|
|
|
} |
|
|
|
|
|
|
|
/* 多项式近似求解 */ |
|
|
|
uint64_t v_mid_sq = (uint64_t)vel_mid * vel_mid; |
|
|
|
uint64_t v_mid_cu = v_mid_sq * vel_mid; |
|
|
|
|
|
|
|
/* 基础时间: τ = s/v_mid */ |
|
|
|
uint64_t tau_ms = displacement / vel_mid; |
|
|
|
|
|
|
|
/* 修正项: x = J·s/(v_mid³) */ |
|
|
|
uint64_t x = ((uint64_t)jerk * displacement) / v_mid_cu; |
|
|
|
|
|
|
|
/* 第二阶段的修正(负jerk效应)*/ |
|
|
|
uint64_t correction = x / 6; |
|
|
|
uint64_t time_ms = (tau_ms > correction) ? tau_ms - correction : tau_ms; |
|
|
|
if (time_ms == 0) time_ms = 1; |
|
|
|
|
|
|
|
/* 计算最终速度 */ |
|
|
|
uint64_t time_sq = (time_ms * time_ms) / 1000; |
|
|
|
uint64_t jerk_term = ((uint64_t)jerk * time_sq) / 2000; |
|
|
|
|
|
|
|
uint64_t velocity; |
|
|
|
if (is_deceleration) { |
|
|
|
/* 减速第二阶段: v = v_mid + (1/2)·J·t² (注意这里是加号,因为jerk方向改变)*/ |
|
|
|
velocity = vel_mid + jerk_term; |
|
|
|
} else { |
|
|
|
/* 加速第二阶段: v = v_mid - (1/2)·J·t² (减jerk阶段)*/ |
|
|
|
velocity = (vel_mid > jerk_term) ? vel_mid - jerk_term : 0; |
|
|
|
if (fabs(t_new - t) < 1e-7) { |
|
|
|
t = t_new; |
|
|
|
break; |
|
|
|
} |
|
|
|
t = t_new; |
|
|
|
} |
|
|
|
|
|
|
|
return CLAMP_FREQUENCY(velocity); |
|
|
|
|
|
|
|
if (t < 0.0) t = 0.0; |
|
|
|
return t; /* ms */ |
|
|
|
} |
|
|
|
|
|
|
|
/* ==================== 主要的多项式近似速度计算函数 ==================== */ |
|
|
|
uint32_t SCurve_CalculateVelocityByPosition_Polynomial(PLSR_RouteConfig_t* route, |
|
|
|
uint8_t is_acceleration, |
|
|
|
uint32_t executed_pulses) |
|
|
|
{ |
|
|
|
/* 主函数:使用上述两个反函数,并在计算频率时用 J_real */ |
|
|
|
uint32_t PLSR_Calculate_SCurve_FreqByPulses_Exact(PLSR_RouteConfig_t* route, uint8_t is_accel) { |
|
|
|
if (route == NULL) return 0; |
|
|
|
|
|
|
|
uint32_t vel_start, jerk, jerk_time_ms; |
|
|
|
uint32_t phase1_pulses, phase2_pulses; |
|
|
|
|
|
|
|
if (is_acceleration) { |
|
|
|
/* 加速段参数 */ |
|
|
|
vel_start = route->initial_freq; |
|
|
|
jerk = route->scurve.accel_jerk; |
|
|
|
jerk_time_ms = route->scurve.accel_jerk_time_ms; |
|
|
|
|
|
|
|
static uint32_t s_last_freq = 0; |
|
|
|
int64_t current_tim2_count = __HAL_TIM_GET_COUNTER(&htim2); |
|
|
|
uint32_t executed_pulses = current_tim2_count; |
|
|
|
|
|
|
|
uint32_t V0, jerk, Tj, phase1_pulses, phase2_pulses; |
|
|
|
|
|
|
|
if (is_accel) { |
|
|
|
V0 = route->initial_freq; |
|
|
|
jerk = route->scurve.accel_jerk; /* NOTE: assumed scaled = J_real * 1000 */ |
|
|
|
Tj = route->scurve.accel_jerk_time_ms; |
|
|
|
phase1_pulses = route->scurve.accel_phase1_pulses; |
|
|
|
phase2_pulses = route->scurve.accel_phase2_pulses; |
|
|
|
} else { |
|
|
|
/* 减速段参数 */ |
|
|
|
vel_start = route->initial_freq; |
|
|
|
V0 = route->part2_target_freq; |
|
|
|
jerk = route->scurve.decel_jerk; |
|
|
|
jerk_time_ms = route->scurve.decel_jerk_time_ms; |
|
|
|
Tj = route->scurve.decel_jerk_time_ms; |
|
|
|
phase1_pulses = route->scurve.decel_phase1_pulses; |
|
|
|
phase2_pulses = route->scurve.decel_phase2_pulses; |
|
|
|
} |
|
|
|
|
|
|
|
if (jerk == 0 || Tj == 0) return V0; |
|
|
|
|
|
|
|
uint32_t calculated_freq = V0; |
|
|
|
|
|
|
|
if (executed_pulses == 0) { |
|
|
|
return V0; |
|
|
|
} |
|
|
|
else if (executed_pulses <= phase1_pulses) |
|
|
|
{ |
|
|
|
/* Phase1 */ |
|
|
|
double t; /* ms */ |
|
|
|
double J_real = (double)jerk / 1000.0; /* Hz / ms^2 */ |
|
|
|
double vmid_double; |
|
|
|
double Amax = J_real * Tj; |
|
|
|
if (is_accel) { |
|
|
|
vmid_double = (double)V0 + 0.5 * J_real * Tj * Tj; |
|
|
|
} else |
|
|
|
{ |
|
|
|
vmid_double = (double)V0 - 0.5 * J_real * Tj * Tj; |
|
|
|
if (vmid_double < 1.0) vmid_double = 1.0; |
|
|
|
} |
|
|
|
|
|
|
|
if (is_accel) { |
|
|
|
t = SCurve_GetTimeFromPulses_Phase1(executed_pulses, jerk); |
|
|
|
double freq_double = (double)V0 + 0.5 * J_real * t * t; /* Hz */ |
|
|
|
calculated_freq = (uint32_t)(freq_double + 0.5); |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
uint32_t phase2_pulse_num = phase1_pulses - executed_pulses; |
|
|
|
t = SCurve_GetTimeFromPulses_Phase2(phase2_pulse_num, (uint32_t)vmid_double, jerk, Tj); |
|
|
|
double freq_double = vmid_double + Amax * t - 0.5 * J_real * t * t; |
|
|
|
calculated_freq = (uint32_t)(freq_double + 0.5); |
|
|
|
} |
|
|
|
} |
|
|
|
else if (executed_pulses <= (phase1_pulses + phase2_pulses)) |
|
|
|
{ |
|
|
|
/* Phase2 */ |
|
|
|
double Tj_d = (double)Tj; |
|
|
|
double J_real = (double)jerk / 1000.0; |
|
|
|
double vmid_double; |
|
|
|
if (is_accel) { |
|
|
|
vmid_double = (double)V0 + 0.5 * J_real * Tj_d * Tj_d; |
|
|
|
} else { |
|
|
|
vmid_double = (double)V0 - 0.5 * J_real * Tj_d * Tj_d; |
|
|
|
if (vmid_double < 1.0) vmid_double = 1.0; |
|
|
|
} |
|
|
|
|
|
|
|
double Amax = J_real * Tj_d; /* Hz / ms */ |
|
|
|
|
|
|
|
if (is_accel) |
|
|
|
{ |
|
|
|
uint32_t phase2_pulse_num = executed_pulses - phase1_pulses; |
|
|
|
double t_phase2 = SCurve_GetTimeFromPulses_Phase2(phase2_pulse_num, (uint32_t)vmid_double, jerk, Tj); /* ms */ |
|
|
|
double freq_double = vmid_double + Amax * t_phase2 - 0.5 * J_real * t_phase2 * t_phase2; |
|
|
|
calculated_freq = (uint32_t)(freq_double + 0.5); |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
uint32_t phase2_pulse_num = phase2_pulses + phase1_pulses - executed_pulses + 1; |
|
|
|
double t_phase2 = SCurve_GetTimeFromPulses_Phase1(phase2_pulse_num, jerk); /* ms */ |
|
|
|
double freq_double = (double)0 + 0.5 * J_real * t_phase2 * t_phase2; /* Hz */ |
|
|
|
calculated_freq = (uint32_t)(freq_double + 0.5); |
|
|
|
} |
|
|
|
} else { |
|
|
|
calculated_freq = is_accel ? route->part1_target_freq : route->part3_target_freq; |
|
|
|
} |
|
|
|
if(calculated_freq != 0 && calculated_freq <PLSR_PWM_FREQ_MAX) |
|
|
|
s_last_freq = calculated_freq; |
|
|
|
if (calculated_freq > PLSR_PWM_FREQ_MAX) calculated_freq = PLSR_PWM_FREQ_MAX; |
|
|
|
if (calculated_freq == 0) calculated_freq = s_last_freq; |
|
|
|
|
|
|
|
return calculated_freq; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 获取下一个脉冲的周期时间 |
|
|
|
* @param current_freq 当前计算得到的频率 |
|
|
|
* @return 下一个脉冲的周期时间(微秒) |
|
|
|
*/ |
|
|
|
uint32_t PLSR_GetNextPulsePeriod_us(uint32_t current_freq) { |
|
|
|
if (current_freq == 0) return 0xFFFFFFFF; // 无效频率 |
|
|
|
|
|
|
|
/* 参数有效性检查 */ |
|
|
|
if (jerk == 0 || jerk_time_ms == 0) return vel_start; |
|
|
|
if ((phase1_pulses + phase2_pulses) == 0) return vel_start; |
|
|
|
// 周期 = 1/频率,转换为微秒 |
|
|
|
uint32_t period_us = 1000000 / current_freq; |
|
|
|
return period_us; |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* @brief 修改后的主控制函数,使用精确的反函数计算 |
|
|
|
*/ |
|
|
|
uint32_t PLSR_SCurve_Control(PLSR_RouteConfig_t* route) { |
|
|
|
if (route == NULL) return 0; |
|
|
|
|
|
|
|
/* 将脉冲转换为位移 (Hz·ms) */ |
|
|
|
uint64_t displacement = (uint64_t)executed_pulses * 1000ULL; |
|
|
|
uint32_t current_freq = 0; |
|
|
|
|
|
|
|
/* 判断当前处于哪个阶段并求解 */ |
|
|
|
if (executed_pulses <= phase1_pulses) { |
|
|
|
/* 第一阶段:正jerk或负jerk开始 */ |
|
|
|
return SCurve_SolvePhase1_Polynomial(vel_start, jerk, displacement, !is_acceleration); |
|
|
|
} else if (executed_pulses <= (phase1_pulses + phase2_pulses)) { |
|
|
|
/* 第二阶段:jerk减小阶段 */ |
|
|
|
uint64_t phase2_displacement = (uint64_t)(executed_pulses - phase1_pulses) * 1000ULL; |
|
|
|
return SCurve_SolvePhase2_Polynomial(vel_start, jerk, jerk_time_ms, |
|
|
|
phase2_displacement, !is_acceleration); |
|
|
|
} else { |
|
|
|
/* 超出范围,返回目标速度 */ |
|
|
|
if (is_acceleration) { |
|
|
|
return CLAMP_FREQUENCY(route->part1_target_freq); |
|
|
|
} else { |
|
|
|
return CLAMP_FREQUENCY(route->part3_target_freq); |
|
|
|
} |
|
|
|
switch(route->run_state) { |
|
|
|
case PLSR_STATE_ACCEL: |
|
|
|
// 加速阶段使用精确S曲线计算 |
|
|
|
current_freq = PLSR_Calculate_SCurve_FreqByPulses_Exact(route, 1); |
|
|
|
break; |
|
|
|
|
|
|
|
case PLSR_STATE_DECEL: |
|
|
|
// 减速阶段使用精确S曲线计算 |
|
|
|
current_freq = PLSR_Calculate_SCurve_FreqByPulses_Exact(route, 0); |
|
|
|
break; |
|
|
|
|
|
|
|
case PLSR_STATE_CONST: |
|
|
|
// 匀速阶段保持目标频率 |
|
|
|
current_freq = route->target_freq; |
|
|
|
break; |
|
|
|
|
|
|
|
default: |
|
|
|
current_freq = route->current_freq; |
|
|
|
break; |
|
|
|
} |
|
|
|
return current_freq; |
|
|
|
} |