|
|
@@ -1,17 +1,22 @@ |
|
|
/** |
|
|
/** |
|
|
* @file plsr_run_control.c |
|
|
* @file plsr_run_control.c |
|
|
* @brief 完成模式直线加减速(手册梯形/三角) |
|
|
|
|
|
|
|
|
* @brief 完成方式 / 后续方式 运行控制(加减速形状由 accel_mode 决定) |
|
|
* |
|
|
* |
|
|
* 完成方式要点: |
|
|
|
|
|
* - 段开始:阶跃到起速,再按时间直线升/降到本段目标(相时间按脉冲周期 µs 累加,避免 10ms 台阶) |
|
|
|
|
|
* - 升降相按时间走完,不按脉冲数切相 |
|
|
|
|
|
* - 剩余脉冲 <= 预留减速脉冲时进入减速;非末段不预留减速 |
|
|
|
|
|
* - 末段减到终止速度后停机(落到 0) |
|
|
|
|
|
|
|
|
* 完成方式(PLSR_SEND_COMPLETE): |
|
|
|
|
|
* - 上一段发完后,再在本段开头加/减速到本段目标频率 |
|
|
|
|
|
* - 非末段:加速→匀速(f_end=本段目标,无段末过渡);末段再减到终止速度 |
|
|
|
|
|
* - 段间停表,用 s_chain_freq 把当前频传给下一段作起速 |
|
|
|
|
|
* |
|
|
|
|
|
* 后续方式(PLSR_SEND_FOLLOW): |
|
|
|
|
|
* - 上一段脉冲发完时,频率已到达下一段目标(过渡占用本段末尾脉冲) |
|
|
|
|
|
* - 首段:起速→本段目标→再过渡到下一段目标 |
|
|
|
|
|
* - 中间段:已在本段目标上匀速→段末过渡到下一段目标 |
|
|
|
|
|
* - 末段:匀速→减到终止速度后停 |
|
|
|
|
|
* - 同向无等待时不停表衔接;换向/等待则停表再启 |
|
|
* |
|
|
* |
|
|
* 【硬约束】脉冲 UPDATE ISR(PlsrRunControlOnPulseIsr)内禁止重计算: |
|
|
* 【硬约束】脉冲 UPDATE ISR(PlsrRunControlOnPulseIsr)内禁止重计算: |
|
|
* - 禁止 FitRampTimeMs / EstimatePulses 等 O(N) 或查表迭代 |
|
|
* - 禁止 FitRampTimeMs / EstimatePulses 等 O(N) 或查表迭代 |
|
|
* - 禁止 S 曲线、正弦等将来更重的轮廓递推/拟合 |
|
|
|
|
|
* - ISR 只做:计数、O(1) 插值取频、改预装载 ARR;耗时规划放在 BeginSeg/任务上下文 |
|
|
|
|
|
|
|
|
* - ISR 只做:计数、O(1) 形状取频、改预装载 ARR;耗时规划放在 BeginSeg/任务上下文 |
|
|
*/ |
|
|
*/ |
|
|
#include "plsr_run_control.h" |
|
|
#include "plsr_run_control.h" |
|
|
#include "plsr_path_plan.h" |
|
|
#include "plsr_path_plan.h" |
|
|
@@ -30,9 +35,9 @@ typedef enum { |
|
|
} RcState_e; |
|
|
} RcState_e; |
|
|
|
|
|
|
|
|
typedef enum { |
|
|
typedef enum { |
|
|
PH_APPROACH = 0, /* 起速 → 本段目标/峰值,按时间 */ |
|
|
|
|
|
|
|
|
PH_APPROACH = 0, /* 起速 → 本段目标/峰值 */ |
|
|
PH_CONST, /* 匀速 */ |
|
|
PH_CONST, /* 匀速 */ |
|
|
PH_DECEL /* → 终止速度,按时间 */ |
|
|
|
|
|
|
|
|
PH_DECEL /* 段末过渡 → f_end(可升可降:完成=止速;后续=下一段目标) */ |
|
|
} RunPhase_e; |
|
|
} RunPhase_e; |
|
|
|
|
|
|
|
|
static volatile RcState_e s_state; |
|
|
static volatile RcState_e s_state; |
|
|
@@ -62,6 +67,35 @@ static uint8_t s_pwm_on; |
|
|
|
|
|
|
|
|
static void PlsrRunControlAfterSegDone(void); |
|
|
static void PlsrRunControlAfterSegDone(void); |
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
|
|
|
* 后续方式:本段末拍后能否同向不停表直接进下一段。 |
|
|
|
|
|
* (有下一段、同向、且本段无等待。)ISR 内 O(1)。 |
|
|
|
|
|
*/ |
|
|
|
|
|
static uint8_t PlsrRunControlWillFollowKeep(uint16_t cur_seg) |
|
|
|
|
|
{ |
|
|
|
|
|
PlsrCfg_t *cfg = PlsrParamGetCfg(); |
|
|
|
|
|
int16_t next0; |
|
|
|
|
|
|
|
|
|
|
|
if (cfg->send_mode != PLSR_SEND_FOLLOW) |
|
|
|
|
|
{ |
|
|
|
|
|
return 0U; |
|
|
|
|
|
} |
|
|
|
|
|
next0 = PlsrPathPlanNextSeg(cur_seg); |
|
|
|
|
|
if (next0 < 0) |
|
|
|
|
|
{ |
|
|
|
|
|
return 0U; |
|
|
|
|
|
} |
|
|
|
|
|
if (PlsrPathPlanIsForward((uint16_t)next0, s_acc_pulse) != s_forward) |
|
|
|
|
|
{ |
|
|
|
|
|
return 0U; |
|
|
|
|
|
} |
|
|
|
|
|
if (PlsrPathPlanGetWaitMs(cur_seg) > 0U) |
|
|
|
|
|
{ |
|
|
|
|
|
return 0U; |
|
|
|
|
|
} |
|
|
|
|
|
return 1U; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
static INT32U PlsrRunControlMsToTicks(uint32_t ms) |
|
|
static INT32U PlsrRunControlMsToTicks(uint32_t ms) |
|
|
{ |
|
|
{ |
|
|
INT32U t; |
|
|
INT32U t; |
|
|
@@ -172,6 +206,11 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) |
|
|
uint32_t den; |
|
|
uint32_t den; |
|
|
uint32_t ref_ms; |
|
|
uint32_t ref_ms; |
|
|
uint32_t f_end; |
|
|
uint32_t f_end; |
|
|
|
|
|
uint32_t old_dec_n; |
|
|
|
|
|
uint32_t old_t_dec; |
|
|
|
|
|
|
|
|
|
|
|
old_dec_n = s_accel_plan.dec_n; |
|
|
|
|
|
old_t_dec = s_accel_plan.t_dec_ms; |
|
|
|
|
|
|
|
|
PlsrRunControlEnterPhase(PH_DECEL, from_hz); |
|
|
PlsrRunControlEnterPhase(PH_DECEL, from_hz); |
|
|
f_end = s_accel_plan.f_end; |
|
|
f_end = s_accel_plan.f_end; |
|
|
@@ -191,6 +230,17 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) |
|
|
return; |
|
|
return; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
|
* 规划阶段已按 dec_n 拟合过 T(含 S/正弦)且剩余脉冲仍等于规划值: |
|
|
|
|
|
* 直接沿用,避免 ISR 内 O(1) 平均公式把形状时间改歪。 |
|
|
|
|
|
* 脉冲被改频/截断等导致 remain 变化时,再走下面 O(1) 回退。 |
|
|
|
|
|
*/ |
|
|
|
|
|
if ((old_t_dec > 0U) && (old_dec_n > 0U) && (remain_pulses == old_dec_n)) |
|
|
|
|
|
{ |
|
|
|
|
|
s_accel_plan.t_dec_ms = old_t_dec; |
|
|
|
|
|
return; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
/* 参数斜率决定的理想减速时间 */ |
|
|
/* 参数斜率决定的理想减速时间 */ |
|
|
df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz); |
|
|
df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz); |
|
|
if (f_end > from_hz) |
|
|
if (f_end > from_hz) |
|
|
@@ -414,11 +464,21 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, |
|
|
{ |
|
|
{ |
|
|
f_lo = 1U; |
|
|
f_lo = 1U; |
|
|
} |
|
|
} |
|
|
PlsrPulseDriverLockPscRange(f_lo, f_hi); |
|
|
|
|
|
|
|
|
/* |
|
|
|
|
|
* 后续同向不停表时 PWM 仍在跑:禁止重锁 PSC(只改了映像、硬件 PSC 未变, |
|
|
|
|
|
* 会导致 ARR 按新 PSC 算错频)。完成方式段间已停表,可安全重锁。 |
|
|
|
|
|
*/ |
|
|
|
|
|
if (s_pwm_on == 0U) |
|
|
|
|
|
{ |
|
|
|
|
|
PlsrPulseDriverLockPscRange(f_lo, f_hi); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
s_cur_freq = f_from; |
|
|
|
|
|
s_approach_from = f_from; |
|
|
s_approach_from = f_from; |
|
|
s_decel_from = s_accel_plan.f_tgt; |
|
|
s_decel_from = s_accel_plan.f_tgt; |
|
|
|
|
|
if (s_pwm_on == 0U) |
|
|
|
|
|
{ |
|
|
|
|
|
s_cur_freq = f_from; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
if (f_from == s_accel_plan.f_tgt) |
|
|
if (f_from == s_accel_plan.f_tgt) |
|
|
{ |
|
|
{ |
|
|
@@ -445,6 +505,22 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, |
|
|
} |
|
|
} |
|
|
PlsrRunControlEnterPhase(PH_APPROACH, f_from); |
|
|
PlsrRunControlEnterPhase(PH_APPROACH, f_from); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/* 段末过渡时间按 dec_n 拟合(后续对准下一段 / 完成末段到止速) */ |
|
|
|
|
|
if ((s_accel_plan.dec_n > 0U) && |
|
|
|
|
|
(s_accel_plan.f_tgt != s_accel_plan.f_end)) |
|
|
|
|
|
{ |
|
|
|
|
|
s_accel_plan.t_dec_ms = |
|
|
|
|
|
PlsrAccelCurveFitRampTimeMs(s_accel_plan.f_tgt, |
|
|
|
|
|
s_accel_plan.f_end, |
|
|
|
|
|
s_accel_plan.dec_n, |
|
|
|
|
|
s_accel_plan.t_dec_ms, |
|
|
|
|
|
s_accel_plan.mode); |
|
|
|
|
|
if (s_accel_plan.t_dec_ms < 1U) |
|
|
|
|
|
{ |
|
|
|
|
|
s_accel_plan.t_dec_ms = 1U; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
static void PlsrRunControlFinishAll(void) |
|
|
static void PlsrRunControlFinishAll(void) |
|
|
@@ -482,36 +558,31 @@ static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg) |
|
|
static void PlsrRunControlAfterSegDone(void) |
|
|
static void PlsrRunControlAfterSegDone(void) |
|
|
{ |
|
|
{ |
|
|
PlsrCfg_t *cfg = PlsrParamGetCfg(); |
|
|
PlsrCfg_t *cfg = PlsrParamGetCfg(); |
|
|
int16_t next0; |
|
|
|
|
|
uint16_t wait_ms; |
|
|
uint16_t wait_ms; |
|
|
uint8_t follow; |
|
|
|
|
|
uint8_t keep_pwm; |
|
|
uint8_t keep_pwm; |
|
|
|
|
|
|
|
|
/* 先脱离 RUN,避免脉冲 ISR 重入 */ |
|
|
/* 先脱离 RUN,避免脉冲 ISR 重入 */ |
|
|
s_state = RC_IDLE; |
|
|
s_state = RC_IDLE; |
|
|
|
|
|
|
|
|
next0 = PlsrPathPlanNextSeg(s_cur_seg); |
|
|
|
|
|
follow = (cfg->send_mode == PLSR_SEND_FOLLOW) ? 1U : 0U; |
|
|
|
|
|
keep_pwm = 0U; |
|
|
|
|
|
|
|
|
keep_pwm = PlsrRunControlWillFollowKeep(s_cur_seg); |
|
|
|
|
|
|
|
|
if (follow == 0U) |
|
|
|
|
|
|
|
|
if (cfg->send_mode == PLSR_SEND_COMPLETE) |
|
|
{ |
|
|
{ |
|
|
s_chain_freq = s_cur_freq; |
|
|
s_chain_freq = s_cur_freq; |
|
|
s_chain_valid = 1U; |
|
|
s_chain_valid = 1U; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
if ((follow != 0U) && (next0 >= 0) && |
|
|
|
|
|
(PlsrPathPlanIsForward((uint16_t)next0, s_acc_pulse) == s_forward)) |
|
|
|
|
|
|
|
|
if (keep_pwm != 0U) |
|
|
{ |
|
|
{ |
|
|
|
|
|
/* 后续同向:频率已在下一段目标,不停表直接 BeginSeg */ |
|
|
s_follow_cont = 1U; |
|
|
s_follow_cont = 1U; |
|
|
keep_pwm = 1U; |
|
|
|
|
|
} |
|
|
} |
|
|
else |
|
|
else |
|
|
{ |
|
|
{ |
|
|
PlsrPulseDriverStop(); |
|
|
PlsrPulseDriverStop(); |
|
|
s_follow_cont = 0U; |
|
|
s_follow_cont = 0U; |
|
|
s_pwm_on = 0U; |
|
|
s_pwm_on = 0U; |
|
|
if (follow != 0U) |
|
|
|
|
|
|
|
|
if (cfg->send_mode == PLSR_SEND_FOLLOW) |
|
|
{ |
|
|
{ |
|
|
s_cur_freq = 0U; |
|
|
s_cur_freq = 0U; |
|
|
s_chain_valid = 0U; |
|
|
s_chain_valid = 0U; |
|
|
@@ -521,6 +592,7 @@ static void PlsrRunControlAfterSegDone(void) |
|
|
wait_ms = PlsrPathPlanGetWaitMs(s_cur_seg); |
|
|
wait_ms = PlsrPathPlanGetWaitMs(s_cur_seg); |
|
|
if (wait_ms > 0U) |
|
|
if (wait_ms > 0U) |
|
|
{ |
|
|
{ |
|
|
|
|
|
/* WillFollowKeep 在有等待时已为 0;此处兜底停表 */ |
|
|
if (keep_pwm != 0U) |
|
|
if (keep_pwm != 0U) |
|
|
{ |
|
|
{ |
|
|
PlsrPulseDriverStop(); |
|
|
PlsrPulseDriverStop(); |
|
|
@@ -602,6 +674,7 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0) |
|
|
|
|
|
|
|
|
if (s_follow_cont != 0U) |
|
|
if (s_follow_cont != 0U) |
|
|
{ |
|
|
{ |
|
|
|
|
|
/* 同向不停表:上一段末速应已是本段目标 */ |
|
|
f_from = s_cur_freq; |
|
|
f_from = s_cur_freq; |
|
|
} |
|
|
} |
|
|
else if ((cfg->send_mode == PLSR_SEND_COMPLETE) && (s_chain_valid != 0U)) |
|
|
else if ((cfg->send_mode == PLSR_SEND_COMPLETE) && (s_chain_valid != 0U)) |
|
|
@@ -615,6 +688,10 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0) |
|
|
|
|
|
|
|
|
if (cfg->send_mode == PLSR_SEND_FOLLOW) |
|
|
if (cfg->send_mode == PLSR_SEND_FOLLOW) |
|
|
{ |
|
|
{ |
|
|
|
|
|
/* |
|
|
|
|
|
* 后续:段末 f_end 对准下一段目标(末段对准终止速度)。 |
|
|
|
|
|
* AccelCurvePlan 据此预留段末过渡脉冲;中间段 f_from≈f_tgt → 仅匀速+过渡。 |
|
|
|
|
|
*/ |
|
|
if (is_last == 0U) |
|
|
if (is_last == 0U) |
|
|
{ |
|
|
{ |
|
|
PlsrSeg_t *nseg = PlsrParamGetSeg((uint16_t)next0); |
|
|
PlsrSeg_t *nseg = PlsrParamGetSeg((uint16_t)next0); |
|
|
@@ -659,7 +736,9 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0) |
|
|
|
|
|
|
|
|
if (s_follow_cont != 0U) |
|
|
if (s_follow_cont != 0U) |
|
|
{ |
|
|
{ |
|
|
|
|
|
PlsrPulseDriverClearOnePulseStop(); |
|
|
PlsrRunControlRefreshProfile(0U); |
|
|
PlsrRunControlRefreshProfile(0U); |
|
|
|
|
|
PlsrPulseDriverApplyPending(); |
|
|
s_state = RC_RUN; |
|
|
s_state = RC_RUN; |
|
|
s_follow_cont = 0U; |
|
|
s_follow_cont = 0U; |
|
|
} |
|
|
} |
|
|
@@ -878,10 +957,14 @@ void PlsrRunControlOnPulseIsr(void) |
|
|
|
|
|
|
|
|
s_cur_freq = s_pending_freq; |
|
|
s_cur_freq = s_pending_freq; |
|
|
|
|
|
|
|
|
/* 最后一拍:下一 UPDATE 停表,避免再多出一个上升沿 */ |
|
|
|
|
|
|
|
|
/* 最后一拍:完成方式 / 后续末段或断链时 OPM 停表,避免多一个上升沿。 |
|
|
|
|
|
* 后续同向不停表衔接时禁止 OPM,否则会在段界把定时器停掉。 */ |
|
|
if (s_done == (s_target - 1)) |
|
|
if (s_done == (s_target - 1)) |
|
|
{ |
|
|
{ |
|
|
PlsrPulseDriverArmOnePulseStop(); |
|
|
|
|
|
|
|
|
if (PlsrRunControlWillFollowKeep(s_cur_seg) == 0U) |
|
|
|
|
|
{ |
|
|
|
|
|
PlsrPulseDriverArmOnePulseStop(); |
|
|
|
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
if ((s_phase == PH_APPROACH) || (s_phase == PH_DECEL)) |
|
|
if ((s_phase == PH_APPROACH) || (s_phase == PH_DECEL)) |
|
|
|