293 lines
7.8 KiB
C
293 lines
7.8 KiB
C
|
#include "app_motor.h"
|
|||
|
|
|
|||
|
|
/* 当前位置相关：用于CiA402位置反馈 */
|
|||
|
|
float current_absolute_pos_mm = 0.0f;
|
|||
|
|
float last_move_start_pos_mm = 0.0f;
|
|||
|
|
|
|||
|
|
/* 电机内部状态机 */
|
|||
|
|
Motor_State_t internal_state = STATE_SWITCH_ON_DISABLED;
|
|||
|
|
Homing_State_t homing_state = HOMING_IDLE;
|
|||
|
|
|
|||
|
|
/* 兼容旧调试变量：值由StepLossMonitor同步更新 */
|
|||
|
|
float error = 0.0f;
|
|||
|
|
float distance = 0.0f;
|
|||
|
|
float encoder_distance = 0.0f;
|
|||
|
|
uint32_t steps = 0;
|
|||
|
|
|
|||
|
|
#define STEPLOSS_SETTLE_MS 200U
|
|||
|
|
#define STEPLOSS_THRESHOLD_MM_DEFAULT 0.1f
|
|||
|
|
|
|||
|
|
/* 丢步检测运行态：集中管理，避免状态分散在多个全局变量 */
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
/* 实时观测量 */
|
|||
|
|
float abs_error_mm;
|
|||
|
|
float cmd_distance_mm;
|
|||
|
|
float enc_distance_mm;
|
|||
|
|
uint32_t cmd_steps;
|
|||
|
|
|
|||
|
|
/* 编码器16位计数扩展 */
|
|||
|
|
int32_t accum_pulses;
|
|||
|
|
uint16_t prev_raw_cnt;
|
|||
|
|
uint8_t prev_raw_valid;
|
|||
|
|
uint8_t motion_active;
|
|||
|
|
|
|||
|
|
/* 停止后延时上报 */
|
|||
|
|
uint8_t wait_stop_settle;
|
|||
|
|
uint32_t stop_tick_ms;
|
|||
|
|
uint8_t report_ready;
|
|||
|
|
float report_error_mm;
|
|||
|
|
float threshold_mm;
|
|||
|
|
Stepper_state_t prev_state;
|
|||
|
|
} StepLossMonitor_t;
|
|||
|
|
|
|||
|
|
static StepLossMonitor_t s_step_loss = {
|
|||
|
|
.threshold_mm = STEPLOSS_THRESHOLD_MM_DEFAULT,
|
|||
|
|
.prev_state = STOP,
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
static float Normalize_FollowingErrorThreshold(float threshold_mm, float fallback_mm)
|
|||
|
|
{
|
|||
|
|
if (!(threshold_mm == threshold_mm))
|
|||
|
|
{
|
|||
|
|
return fallback_mm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (threshold_mm <= 0.0f)
|
|||
|
|
{
|
|||
|
|
return fallback_mm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (threshold_mm > SOFT_LIMIT_MAX_MM)
|
|||
|
|
{
|
|||
|
|
return SOFT_LIMIT_MAX_MM;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return threshold_mm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void Sync_FollowingErrorThreshold_From_OD(void)
|
|||
|
|
{
|
|||
|
|
OD_entry_t *entry_ferr = OD_find(OD, CIA402_INDEX_FOLLOWING_ERROR_WINDOW);
|
|||
|
|
|
|||
|
|
if (entry_ferr == NULL)
|
|||
|
|
{
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float32_t threshold_mm = 0.0f;
|
|||
|
|
if (OD_get_f32(entry_ferr, 0, &threshold_mm, true) == ODR_OK)
|
|||
|
|
{
|
|||
|
|
s_step_loss.threshold_mm =
|
|||
|
|
Normalize_FollowingErrorThreshold((float)threshold_mm, s_step_loss.threshold_mm);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void Update_PP_FollowingError_StatusBit(float stop_error_mm)
|
|||
|
|
{
|
|||
|
|
OD_entry_t *entry_mode = OD_find(OD, CIA402_INDEX_OP_MODE);
|
|||
|
|
OD_entry_t *entry_sw = OD_find(OD, CIA402_INDEX_STATUSWORD);
|
|||
|
|
|
|||
|
|
if (entry_mode == NULL || entry_sw == NULL)
|
|||
|
|
{
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int8_t mode = 0;
|
|||
|
|
OD_get_i8(entry_mode, 0, &mode, true);
|
|||
|
|
if (mode != 1)
|
|||
|
|
{
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
CO_LOCK_OD(CO->CANmodule);
|
|||
|
|
uint16_t sw = 0;
|
|||
|
|
OD_get_u16(entry_sw, 0, &sw, true);
|
|||
|
|
|
|||
|
|
if (stop_error_mm > s_step_loss.threshold_mm)
|
|||
|
|
{
|
|||
|
|
sw |= CIA402_STATUS_OMS_13;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
sw &= (uint16_t)(~CIA402_STATUS_OMS_13);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
OD_set_u16(entry_sw, 0, sw, true);
|
|||
|
|
CO_UNLOCK_OD(CO->CANmodule);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 核心运动分发：先做安全判断，再按模式进入回零/位置模式处理 */
|
|||
|
|
static void Process_Motion_Logic(void)
|
|||
|
|
{
|
|||
|
|
/* 未使能运行时，强制停机并维持状态反馈 */
|
|||
|
|
if ((internal_state != STATE_OPERATION_ENABLED) && (stepper_1.run_flag == 0u))
|
|||
|
|
{
|
|||
|
|
if (stepper_1.state != STOP)
|
|||
|
|
{
|
|||
|
|
Int_TMC2209_stop();
|
|||
|
|
stepper_1.state = STOP;
|
|||
|
|
}
|
|||
|
|
homing_state = HOMING_IDLE;
|
|||
|
|
Update_Motion_State_To_OD();
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Update_Motion_State_To_OD();
|
|||
|
|
|
|||
|
|
OD_entry_t *entry_mode = OD_find(OD, CIA402_INDEX_OP_MODE);
|
|||
|
|
OD_entry_t *entry_mode_disp = OD_find(OD, CIA402_INDEX_OP_MODE_DISPLAY);
|
|||
|
|
|
|||
|
|
if (entry_mode != NULL)
|
|||
|
|
{
|
|||
|
|
int8_t mode = 0;
|
|||
|
|
OD_get_i8(entry_mode, 0, &mode, true);
|
|||
|
|
|
|||
|
|
if (entry_mode_disp != NULL)
|
|||
|
|
{
|
|||
|
|
OD_set_i8(entry_mode_disp, 0, mode, true);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
switch (mode)
|
|||
|
|
{
|
|||
|
|
case 6:
|
|||
|
|
Handle_Homing_Mode();
|
|||
|
|
break;
|
|||
|
|
case 1:
|
|||
|
|
Handle_Position_Mode();
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void App_Motor_Init(void)
|
|||
|
|
{
|
|||
|
|
/* 统一初始化运行态与位置基准 */
|
|||
|
|
current_absolute_pos_mm = 0.0f;
|
|||
|
|
last_move_start_pos_mm = 0.0f;
|
|||
|
|
homing_state = HOMING_IDLE;
|
|||
|
|
internal_state = STATE_SWITCH_ON_DISABLED;
|
|||
|
|
Int_Encoder_Init();
|
|||
|
|
debug_printf("App Motor Initialized.");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void App_Motor_Process(void)
|
|||
|
|
{
|
|||
|
|
Sync_FollowingErrorThreshold_From_OD();
|
|||
|
|
|
|||
|
|
/* 先跑CiA402状态机，再执行业务运动逻辑 */
|
|||
|
|
Process_StateMachine();
|
|||
|
|
Process_Motion_Logic();
|
|||
|
|
|
|||
|
|
float stop_error_mm = 0.0f;
|
|||
|
|
if (App_Motor_TryGetStopError(&stop_error_mm))
|
|||
|
|
{
|
|||
|
|
Update_PP_FollowingError_StatusBit(stop_error_mm);
|
|||
|
|
|
|||
|
|
/* 回零流程不输出StepLoss误差日志，避免与“回零成功/失败”语义混淆 */
|
|||
|
|
if (homing_state == HOMING_IDLE)
|
|||
|
|
{
|
|||
|
|
debug_printf("[StepLoss] stop+200ms error = %.6f mm", stop_error_mm);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void App_Motor_StepLossCheck(void)
|
|||
|
|
{
|
|||
|
|
/* 周期读取硬件计数与已发步数（TIM12中断调用） */
|
|||
|
|
uint16_t raw_cnt = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1);
|
|||
|
|
s_step_loss.cmd_steps = stepper_1.step_count;
|
|||
|
|
|
|||
|
|
/* 新动作开始时重置累计基准，避免跨动作串扰 */
|
|||
|
|
if (stepper_1.state != STOP)
|
|||
|
|
{
|
|||
|
|
if (!s_step_loss.motion_active)
|
|||
|
|
{
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if (s_step_loss.motion_active)
|
|||
|
|
{
|
|||
|
|
s_step_loss.motion_active = 0U;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 首次采样仅建立基准，不计算位移增量 */
|
|||
|
|
if (!s_step_loss.prev_raw_valid)
|
|||
|
|
{
|
|||
|
|
s_step_loss.prev_raw_cnt = raw_cnt;
|
|||
|
|
s_step_loss.prev_raw_valid = 1U;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 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;
|
|||
|
|
|
|||
|
|
/* 输出扩展后的总脉冲给上层使用 */
|
|||
|
|
encoder_1.pulses = s_step_loss.accum_pulses;
|
|||
|
|
encoder_1.overflow = (uint16_t)(abs(encoder_1.pulses) / 65536);
|
|||
|
|
|
|||
|
|
/* 统一换算为mm并计算绝对误差 */
|
|||
|
|
s_step_loss.cmd_distance_mm = (float)s_step_loss.cmd_steps / STEPS_PER_MM;
|
|||
|
|
s_step_loss.enc_distance_mm = (float)abs(encoder_1.pulses) * STICK_LEAD / (float)ENCODER_PULSES_PER_CIRCLE;
|
|||
|
|
s_step_loss.abs_error_mm = s_step_loss.enc_distance_mm - s_step_loss.cmd_distance_mm;
|
|||
|
|
if (s_step_loss.abs_error_mm < 0.0f)
|
|||
|
|
{
|
|||
|
|
s_step_loss.abs_error_mm = -s_step_loss.abs_error_mm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 同步旧调试变量，便于观察窗口/旧日志继续使用 */
|
|||
|
|
steps = s_step_loss.cmd_steps;
|
|||
|
|
distance = s_step_loss.cmd_distance_mm;
|
|||
|
|
encoder_distance = s_step_loss.enc_distance_mm;
|
|||
|
|
error = s_step_loss.abs_error_mm;
|
|||
|
|
|
|||
|
|
/* 运动中不出最终结论，仅标记“停稳后需要上报一次” */
|
|||
|
|
if (stepper_1.state != STOP)
|
|||
|
|
{
|
|||
|
|
s_step_loss.wait_stop_settle = 1U;
|
|||
|
|
s_step_loss.report_ready = 0U;
|
|||
|
|
s_step_loss.prev_state = stepper_1.state;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 从运动态切换到STOP时，开始计时稳定窗口 */
|
|||
|
|
if (s_step_loss.prev_state != STOP)
|
|||
|
|
{
|
|||
|
|
s_step_loss.prev_state = STOP;
|
|||
|
|
s_step_loss.stop_tick_ms = HAL_GetTick();
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* STOP持续到阈值后，锁存一次最终误差 */
|
|||
|
|
if (s_step_loss.wait_stop_settle &&
|
|||
|
|
((uint32_t)(HAL_GetTick() - s_step_loss.stop_tick_ms) >= STEPLOSS_SETTLE_MS))
|
|||
|
|
{
|
|||
|
|
s_step_loss.report_error_mm = s_step_loss.abs_error_mm;
|
|||
|
|
s_step_loss.report_ready = 1U;
|
|||
|
|
s_step_loss.wait_stop_settle = 0U;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* 一次性读取“停止后最终误差”；读取成功后自动清ready标志 */
|
|||
|
|
bool App_Motor_TryGetStopError(float *err_mm)
|
|||
|
|
{
|
|||
|
|
if (!s_step_loss.report_ready)
|
|||
|
|
{
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (err_mm != NULL)
|
|||
|
|
{
|
|||
|
|
*err_mm = s_step_loss.report_error_mm;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
s_step_loss.report_ready = 0U;
|
|||
|
|
return true;
|
|||
|
|
}
|