330 lines
11 KiB
C
330 lines
11 KiB
C
#include "app_CiA402.h"
|
||
|
||
/**
|
||
* @brief 辅助函数：CiA402 状态机管理
|
||
* @note 使用 cia402_defs.h 标准宏定义重构，确保状态流转逻辑与原版本一致
|
||
*/
|
||
void Process_StateMachine(void)
|
||
{
|
||
// 1. 获取对象字典条目句柄
|
||
OD_entry_t *entry_ctrl = OD_find(OD, CIA402_INDEX_CONTROLWORD);
|
||
OD_entry_t *entry_status = OD_find(OD, CIA402_INDEX_STATUSWORD);
|
||
|
||
uint16_t cw = 0;
|
||
uint16_t sw = 0;
|
||
|
||
// 2. 读取当前的控制字与状态字
|
||
OD_get_u16(entry_ctrl, 0, &cw, true);
|
||
OD_get_u16(entry_status, 0, &sw, true);
|
||
|
||
// 3. 屏蔽状态字低 7 位 (Bit 0-6)，准备根据当前状态重新填充反馈位
|
||
sw &= 0xFF80;
|
||
|
||
// 4. 标准 CiA402 状态流转逻辑处理
|
||
switch (internal_state)
|
||
{
|
||
case STATE_SWITCH_ON_DISABLED:
|
||
// 检查是否收到 Shutdown 命令 (对应 0x0006)
|
||
if ((cw & CIA402_CMD_SHUTDOWN) == CIA402_CMD_SHUTDOWN)
|
||
internal_state = STATE_READY_TO_SWITCH_ON;
|
||
// 反馈状态：Switch on disabled
|
||
sw |= CIA402_STATUS_SWITCH_ON_DISABLED;
|
||
break;
|
||
|
||
case STATE_READY_TO_SWITCH_ON:
|
||
// 检查是否收到 Switch On 命令 (对应 0x0007)
|
||
if ((cw & CIA402_CMD_SWITCH_ON) == CIA402_CMD_SWITCH_ON)
|
||
internal_state = STATE_SWITCHED_ON;
|
||
// 反馈状态：Ready to switch on + Quick stop (标志正常运行位)
|
||
sw |= (CIA402_STATUS_READY_TO_SWITCH_ON | CIA402_STATUS_QUICK_STOP);
|
||
break;
|
||
|
||
case STATE_SWITCHED_ON:
|
||
// 检查是否收到 Enable Operation 命令 (对应 0x000F)
|
||
if ((cw & CIA402_CMD_ENABLE_OP) == CIA402_CMD_ENABLE_OP)
|
||
internal_state = STATE_OPERATION_ENABLED;
|
||
else if ((cw & CIA402_CMD_SWITCH_ON) != CIA402_CMD_SWITCH_ON)
|
||
internal_state = STATE_READY_TO_SWITCH_ON;
|
||
// 反馈状态：Switched on 等组合
|
||
sw |= (CIA402_STATUS_READY_TO_SWITCH_ON | CIA402_STATUS_SWITCHED_ON | CIA402_STATUS_QUICK_STOP);
|
||
break;
|
||
|
||
case STATE_OPERATION_ENABLED:
|
||
// 如果控制字不再满足使能条件，退回上一状态
|
||
if ((cw & CIA402_CMD_ENABLE_OP) != CIA402_CMD_ENABLE_OP)
|
||
internal_state = STATE_SWITCHED_ON;
|
||
// 反馈状态：使能运行全标志 (对应掩码 0x0027)
|
||
sw |= CIA402_STATUS_MASK_OP_ENABLE;
|
||
break;
|
||
|
||
default:
|
||
internal_state = STATE_SWITCH_ON_DISABLED;
|
||
break;
|
||
}
|
||
|
||
// 5. 更新状态字到对象字典
|
||
OD_set_u16(entry_status, 0, sw, true);
|
||
}
|
||
|
||
// 根据脉冲更新绝对位置并写入 OD
|
||
void Update_Motion_State_To_OD(void)
|
||
{
|
||
// 1. 获取 OD 条目句柄
|
||
OD_entry_t *entry_pos = OD_find(OD, CIA402_INDEX_POS_ACTUAL);
|
||
OD_entry_t *entry_vel = OD_find(OD, CIA402_INDEX_VEL_ACTUAL);
|
||
OD_entry_t *entry_sw = OD_find(OD, CIA402_INDEX_STATUSWORD);
|
||
|
||
if (!entry_pos || !entry_vel || !entry_sw)
|
||
return;
|
||
|
||
// 2. 物理位置逻辑计算
|
||
float dist_moved_mm = (float)stepper_1.step_count / STEPS_PER_MM;
|
||
|
||
if (stepper_1.dir == GPIO_PIN_RESET)
|
||
current_absolute_pos_mm = last_move_start_pos_mm + dist_moved_mm;
|
||
else
|
||
current_absolute_pos_mm = last_move_start_pos_mm - dist_moved_mm;
|
||
|
||
// 3. 计算并四舍五入位置值
|
||
int32_t pos_to_set = 0;
|
||
if (current_absolute_pos_mm >= 0)
|
||
pos_to_set = (int32_t)(current_absolute_pos_mm + 0.5f);
|
||
else
|
||
pos_to_set = (int32_t)(current_absolute_pos_mm - 0.5f);
|
||
|
||
// 4. 获取当前速度值
|
||
int32_t vel_to_set = (stepper_1.state != STOP) ? (int32_t)stepper_1.current_speed : 0;
|
||
|
||
// 5. 写入 OD (使用锁保护原子性)
|
||
// 注意：电机板作为从机，通常直接使用 CO 指针访问
|
||
CO_LOCK_OD(CO->CANmodule);
|
||
|
||
OD_set_i32(entry_pos, 0, pos_to_set, true);
|
||
OD_set_i32(entry_vel, 0, vel_to_set, true);
|
||
|
||
// 6. 更新状态字 (Bit 10: Target Reached)
|
||
uint16_t sw = 0;
|
||
OD_get_u16(entry_sw, 0, &sw, true);
|
||
|
||
if (stepper_1.state == STOP)
|
||
{
|
||
// 仅在非回零或回零完成且 Operation Enabled 时置位
|
||
if (homing_state == HOMING_IDLE || homing_state == HOMING_DONE)
|
||
{
|
||
// 使用掩码检查 Ready+SwOn+OpEn+NoQuickStop (0x0027)
|
||
if ((sw & CIA402_STATUS_MASK_OP_ENABLE) == CIA402_STATUS_MASK_OP_ENABLE)
|
||
{
|
||
sw |= CIA402_STATUS_TARGET_REACHED; // 置位目标到达 (Bit 10)
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 运动中清除已到达标志
|
||
sw &= ~CIA402_STATUS_TARGET_REACHED;
|
||
}
|
||
|
||
OD_set_u16(entry_sw, 0, sw, true);
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
}
|
||
|
||
// @brief 处理 Mode 6 (回零模式) 的逻辑
|
||
void Handle_Homing_Mode(void)
|
||
{
|
||
// 1. 获取 OD 条目句柄
|
||
OD_entry_t *entry_ctrl = OD_find(OD, CIA402_INDEX_CONTROLWORD);
|
||
OD_entry_t *entry_status = OD_find(OD, CIA402_INDEX_STATUSWORD);
|
||
OD_entry_t *entry_pos = OD_find(OD, CIA402_INDEX_POS_ACTUAL);
|
||
|
||
if (!entry_ctrl || !entry_status || !entry_pos)
|
||
return;
|
||
|
||
uint16_t cw = 0;
|
||
OD_get_u16(entry_ctrl, 0, &cw, true); // 读取控制字
|
||
|
||
// A. 启动回零 (Bit 4 上升沿)
|
||
if ((cw & CIA402_CONTROL_HM_START) && homing_state == HOMING_IDLE)
|
||
{
|
||
debug_printf("[App] Homing Start...");
|
||
last_move_start_pos_mm = current_absolute_pos_mm;
|
||
|
||
stepper_1.dir = GPIO_PIN_SET; // 物理向右 (找开关)
|
||
stepper_1.distance = 230.0f; // 设大距离
|
||
stepper_1.speed = 5.0f; // 慢速
|
||
stepper_1.acc = 10.0f;
|
||
stepper_1.start_speed = 1.0f;
|
||
|
||
stepper_1.x_zero = 0; // 清除标志
|
||
Int_TMC2209_start(&stepper_1, &encoder_1);
|
||
|
||
homing_state = HOMING_MOVING;
|
||
|
||
// 原子化更新状态字：清除 Bit 10 (Target Reached) 和 Bit 12 (Homing Attained)
|
||
CO_LOCK_OD(CO->CANmodule);
|
||
uint16_t sw = 0;
|
||
OD_get_u16(entry_status, 0, &sw, true);
|
||
sw &= ~(CIA402_STATUS_TARGET_REACHED | CIA402_STATUS_OMS_12);
|
||
OD_set_u16(entry_status, 0, sw, true);
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
}
|
||
|
||
// B. 正在回零 (检测开关)
|
||
if (homing_state == HOMING_MOVING)
|
||
{
|
||
if (stepper_1.x_zero == 1) // 撞到开关,到达零点
|
||
{
|
||
Int_TMC2209_stop();
|
||
stepper_1.state = STOP; // 强制停止状态
|
||
stepper_1.step_count = 0; // 清除底层脉冲
|
||
|
||
/* --- 核心修复：原子化重置坐标系，防止 TPDO 跳变 --- */
|
||
CO_LOCK_OD(CO->CANmodule);
|
||
|
||
// 1. 内部浮点坐标重置
|
||
current_absolute_pos_mm = 0.0f;
|
||
last_move_start_pos_mm = 0.0f;
|
||
|
||
// 2. 通过接口更新对象字典位置为 0
|
||
// 这会自动标记 TPDO 为待发送状态
|
||
OD_set_i32(entry_pos, 0, 0, true);
|
||
|
||
// 3. 更新状态字 (Bit 10: Reached, Bit 12: Attained)
|
||
uint16_t sw = 0;
|
||
OD_get_u16(entry_status, 0, &sw, true);
|
||
sw |= (CIA402_STATUS_TARGET_REACHED | CIA402_STATUS_OMS_12);
|
||
OD_set_u16(entry_status, 0, sw, true);
|
||
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
|
||
homing_state = HOMING_DONE;
|
||
debug_printf("[App] Homing Success, Position Reset to 0.");
|
||
}
|
||
else if (stepper_1.state == STOP) // 没撞开关就停了
|
||
{
|
||
homing_state = HOMING_IDLE;
|
||
}
|
||
}
|
||
}
|
||
|
||
// @brief 处理 Mode 1 (位置模式) 的逻辑
|
||
void Handle_Position_Mode(void)
|
||
{
|
||
homing_state = HOMING_IDLE; // 复位回零状态
|
||
static uint16_t last_cw = 0;
|
||
|
||
// 1. 获取 OD 条目句柄
|
||
OD_entry_t *entry_cw = OD_find(OD, CIA402_INDEX_CONTROLWORD);
|
||
OD_entry_t *entry_sw = OD_find(OD, CIA402_INDEX_STATUSWORD);
|
||
OD_entry_t *entry_tpos = OD_find(OD, CIA402_INDEX_TARGET_POS);
|
||
OD_entry_t *entry_tvel = OD_find(OD, CIA402_INDEX_TARGET_VEL);
|
||
OD_entry_t *entry_acc = OD_find(OD, CIA402_INDEX_PROFILE_ACC);
|
||
OD_entry_t *entry_dec = OD_find(OD, CIA402_INDEX_PROFILE_DEC);
|
||
|
||
if (!entry_cw || !entry_sw || !entry_tpos || !entry_tvel || !entry_acc || !entry_dec)
|
||
return;
|
||
|
||
uint16_t cw = 0;
|
||
OD_get_u16(entry_cw, 0, &cw, true); // 读取当前控制字
|
||
|
||
// A. 触发新运动 (Bit 4 上升沿：New set-point)
|
||
if ((cw & CIA402_CONTROL_PP_NEW_SET_POINT) && !(last_cw & CIA402_CONTROL_PP_NEW_SET_POINT))
|
||
{
|
||
int32_t target_pos_raw = 0;
|
||
int32_t target_vel_raw = 0;
|
||
uint32_t target_acc_raw = 0;
|
||
uint32_t target_dec_raw = 0;
|
||
|
||
// 通过接口获取目标参数
|
||
OD_get_i32(entry_tpos, 0, &target_pos_raw, true);
|
||
OD_get_i32(entry_tvel, 0, &target_vel_raw, true);
|
||
OD_get_u32(entry_acc, 0, &target_acc_raw, true);
|
||
OD_get_u32(entry_dec, 0, &target_dec_raw, true);
|
||
|
||
float target_pos = (float)target_pos_raw;
|
||
float target_vel = (float)target_vel_raw;
|
||
float target_acc = (float)target_acc_raw;
|
||
float target_dec = (float)target_dec_raw;
|
||
|
||
if (target_pos > SOFT_LIMIT_MAX_MM || target_pos < SOFT_LIMIT_MIN_MM)
|
||
{
|
||
// 如果超出 230mm 或小于 0mm，拦截指令并报错
|
||
debug_printf("[App] ERROR: Target %.1f out of Range [0, 230]! Command Ignored.\r\n", target_pos);
|
||
|
||
// 为了完成 CiA402 握手，即便不移动也需要置位 Bit 12 (Ack)
|
||
CO_LOCK_OD(CO->CANmodule);
|
||
uint16_t sw = 0;
|
||
OD_get_u16(entry_sw, 0, &sw, true); // 读取当前状态字
|
||
sw |= CIA402_STATUS_OMS_12;
|
||
OD_set_u16(entry_sw, 0, sw, true);
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
last_cw = cw;
|
||
return;
|
||
}
|
||
|
||
// 默认参数保护
|
||
if (target_vel < 0.1f)
|
||
target_vel = 5.0f;
|
||
if (target_acc < 0.1f)
|
||
target_acc = 50.0f;
|
||
if (target_dec < 0.1f)
|
||
target_dec = 50.0f;
|
||
|
||
float delta = target_pos - current_absolute_pos_mm;
|
||
debug_printf("[App] PP Move: Tgt=%.1f, Delta=%.1f", target_pos, delta);
|
||
|
||
// 设置方向与行程
|
||
if (delta >= 0)
|
||
{
|
||
stepper_1.dir = GPIO_PIN_RESET;
|
||
stepper_1.distance = delta;
|
||
}
|
||
else
|
||
{
|
||
stepper_1.dir = GPIO_PIN_SET;
|
||
stepper_1.distance = -delta;
|
||
}
|
||
|
||
stepper_1.speed = target_vel;
|
||
stepper_1.acc = target_acc;
|
||
stepper_1.dec = target_dec;
|
||
stepper_1.start_speed = 5.0f;
|
||
|
||
// 启动运动或直接置位到达标志
|
||
CO_LOCK_OD(CO->CANmodule); // 进入保护区修改状态字
|
||
uint16_t sw = 0;
|
||
OD_get_u16(entry_sw, 0, &sw, true);
|
||
|
||
/* 清除丢步标志位 */
|
||
sw &= (uint16_t)(~CIA402_STATUS_OMS_13);
|
||
|
||
if (stepper_1.distance > 0.05f)
|
||
{
|
||
last_move_start_pos_mm = current_absolute_pos_mm; // 记录起点
|
||
Int_TMC2209_start(&stepper_1, &encoder_1);
|
||
|
||
sw &= ~CIA402_STATUS_TARGET_REACHED; // 清除 Target Reached (Bit 10)
|
||
sw |= CIA402_STATUS_OMS_12; // 置位 Set-point Acknowledge (Bit 12)
|
||
}
|
||
else
|
||
{
|
||
sw |= (CIA402_STATUS_TARGET_REACHED | CIA402_STATUS_OMS_12); // 已在目标位置
|
||
}
|
||
|
||
OD_set_u16(entry_sw, 0, sw, true);
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
}
|
||
|
||
// B. 握手信号处理 (Bit 4 下降沿清除 Ack 反馈)
|
||
if (!(cw & 0x0010) && (last_cw & 0x0010))
|
||
{
|
||
CO_LOCK_OD(CO->CANmodule);
|
||
uint16_t sw = 0;
|
||
OD_get_u16(entry_sw, 0, &sw, true);
|
||
sw &= ~CIA402_STATUS_OMS_12; // 清除 Set-point Acknowledge
|
||
OD_set_u16(entry_sw, 0, sw, true);
|
||
CO_UNLOCK_OD(CO->CANmodule);
|
||
}
|
||
|
||
last_cw = cw;
|
||
}
|