v1.2修复了EEPROM的读写以及掉电保持参数的功能,现在丢步的误差会保存在Canopen字典的0x60F4位置,网关那边可以通过SDO读取到
This commit is contained in:
@@ -31,6 +31,7 @@ typedef struct
|
||||
uint16_t prev_raw_cnt;
|
||||
uint8_t prev_raw_valid;
|
||||
uint8_t motion_active;
|
||||
uint32_t prev_cmd_steps;
|
||||
|
||||
/* 停止后延时上报 */
|
||||
uint8_t wait_stop_settle;
|
||||
@@ -44,6 +45,7 @@ typedef struct
|
||||
static StepLossMonitor_t s_step_loss = {
|
||||
.threshold_mm = STEPLOSS_THRESHOLD_MM_DEFAULT,
|
||||
.prev_state = STOP,
|
||||
.prev_cmd_steps = 0U,
|
||||
};
|
||||
|
||||
static float Normalize_FollowingErrorThreshold(float threshold_mm, float fallback_mm)
|
||||
@@ -117,6 +119,28 @@ static void Update_PP_FollowingError_StatusBit(float stop_error_mm)
|
||||
CO_UNLOCK_OD(CO->CANmodule);
|
||||
}
|
||||
|
||||
static void Update_FollowingErrorActual_To_OD(float abs_error_mm)
|
||||
{
|
||||
OD_entry_t *entry_ferr_actual = OD_find(OD, CIA402_INDEX_FOLLOWING_ERROR_ACTUAL);
|
||||
float32_t ferr_mm = (float32_t)abs_error_mm;
|
||||
|
||||
if (entry_ferr_actual == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (CO != NULL)
|
||||
{
|
||||
CO_LOCK_OD(CO->CANmodule);
|
||||
(void)OD_set_f32(entry_ferr_actual, 0, ferr_mm, true);
|
||||
CO_UNLOCK_OD(CO->CANmodule);
|
||||
}
|
||||
else
|
||||
{
|
||||
(void)OD_set_f32(entry_ferr_actual, 0, ferr_mm, true);
|
||||
}
|
||||
}
|
||||
|
||||
/* 核心运动分发:先做安全判断,再按模式进入回零/位置模式处理 */
|
||||
static void Process_Motion_Logic(void)
|
||||
{
|
||||
@@ -176,6 +200,7 @@ void App_Motor_Init(void)
|
||||
void App_Motor_Process(void)
|
||||
{
|
||||
Sync_FollowingErrorThreshold_From_OD();
|
||||
Update_FollowingErrorActual_To_OD(s_step_loss.abs_error_mm);
|
||||
|
||||
/* 先跑CiA402状态机,再执行业务运动逻辑 */
|
||||
Process_StateMachine();
|
||||
@@ -198,17 +223,19 @@ void App_Motor_StepLossCheck(void)
|
||||
{
|
||||
/* 周期读取硬件计数与已发步数(TIM12中断调用) */
|
||||
uint16_t raw_cnt = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1);
|
||||
uint8_t rebase_now = 0U;
|
||||
s_step_loss.cmd_steps = stepper_1.step_count;
|
||||
|
||||
/* 新动作开始时重置累计基准,避免跨动作串扰 */
|
||||
if (stepper_1.state != STOP)
|
||||
{
|
||||
if (!s_step_loss.motion_active)
|
||||
if ((!s_step_loss.motion_active) || (s_step_loss.cmd_steps < s_step_loss.prev_cmd_steps))
|
||||
{
|
||||
s_step_loss.motion_active = 1U;
|
||||
s_step_loss.accum_pulses = 0;
|
||||
s_step_loss.prev_raw_cnt = raw_cnt;
|
||||
s_step_loss.prev_raw_valid = 1U;
|
||||
rebase_now = 1U;
|
||||
}
|
||||
}
|
||||
else if (s_step_loss.motion_active)
|
||||
@@ -224,9 +251,18 @@ void App_Motor_StepLossCheck(void)
|
||||
}
|
||||
|
||||
/* int16差分可自然处理16位计数器回绕(溢出/下溢) */
|
||||
int16_t delta = (int16_t)(raw_cnt - s_step_loss.prev_raw_cnt);
|
||||
s_step_loss.accum_pulses += (int32_t)delta;
|
||||
s_step_loss.prev_raw_cnt = raw_cnt;
|
||||
int16_t delta = 0;
|
||||
if (rebase_now)
|
||||
{
|
||||
/* New move baseline: skip one diff sample to avoid counter-reset jump. */
|
||||
}
|
||||
else
|
||||
{
|
||||
delta = (int16_t)(raw_cnt - s_step_loss.prev_raw_cnt);
|
||||
s_step_loss.accum_pulses += (int32_t)delta;
|
||||
s_step_loss.prev_raw_cnt = raw_cnt;
|
||||
}
|
||||
s_step_loss.prev_cmd_steps = s_step_loss.cmd_steps;
|
||||
|
||||
/* 输出扩展后的总脉冲给上层使用 */
|
||||
encoder_1.pulses = s_step_loss.accum_pulses;
|
||||
|
||||
Reference in New Issue
Block a user