// /*
//  * far♂蛇模块
//  */

// #pragma once

// #include <stdbool.h>
// #include <stddef.h>
// #include <stdint.h>
// #ifdef __cplusplus
// extern "C" {
// #endif

// #include "main.h"
// #include "component/pid.h"
// #include "component/arm_kinematics/arm6dof.h"
// #include "device/motor_dm.h"
// #include "device/motor_lz.h"
// /* Exported constants ------------------------------------------------------- */
// #define ARM_JOINT_NUM 6  // 关节数量

// /* Exported macro ----------------------------------------------------------- */
// /* Exported types ----------------------------------------------------------- */

// // 电机类型
// typedef enum {
//   JOINT_MOTOR_TYPE_LZ = 0,  // 凌控电机
//   JOINT_MOTOR_TYPE_DM,      // 达妙电机
// } Joint_MotorType_t;

// // 控制模式
// typedef enum {
//   ARM_MODE_IDLE = 0,           // 空闲模式（电机松弛）
//   ARM_MODE_JOINT_POSITION,     // 关节空间位置控制
//   ARM_MODE_CARTESIAN_POSITION, // 笛卡尔空间位置控制
//   ARM_MODE_TRAJECTORY,         // 轨迹跟踪模式
//   ARM_MODE_TEACH,              // 示教模式（力控）
// } Arm_ControlMode_t;

// // 运动状态
// typedef enum {
//   ARM_STATE_STOPPED = 0,  // 停止
//   ARM_STATE_MOVING,       // 运动中
//   ARM_STATE_REACHED,      // 到达目标
//   ARM_STATE_ERROR,        // 错误
// } Arm_MotionState_t;

//   struct {
//     union {
//       MOTOR_LZ_Param_t lzMotor;
//       MOTOR_DM_Param_t dmMotor;
//     };
//     KPID_Params_t pid;
//   } *Joint_MotorParams_t;
// typedef struct {
//   // 关节标识
//   uint8_t id;                    // 关节编号（0-5）
//   Joint_MotorType_t motor_type;  // 电机类型
  
//   // 运动学参数（DH参数）
//   Arm6dof_DHParams_t dh_params;  // DH参数（包含限位）
//   float q_offset;                // 零位偏移
  
//   // 参数
//  *params;
  
//   // 电机实例
//   union {
//     MOTOR_LZ_t lz_motor;
//     MOTOR_DM_t dm_motor;
//   } motor;
  
//   // 控制器
//   KPID_t pid;                    // PID控制器
  
//   // 状态变量
//   struct {
//     float current_angle;         // 当前角度（去除零位偏移后）
//     float current_velocity;      // 当前速度
//     float current_torque;        // 当前力矩
//     float target_angle;          // 目标角度
//     float target_velocity;       // 目标速度
//     bool online;                 // 在线状态
//   } state;
  
//   // 控制输出（根据motor_type选择使用）
//   union {
//     MOTOR_LZ_MotionParam_t lz_output;
//     MOTOR_MIT_Output_t dm_output;
//   } output;
  
// } Joint_t;

// // ============================================================================
// // Arm（机械臂）- 由多个关节组成
// // ============================================================================
// typedef struct {
//   // 关节数组（面向对象：机械臂由6个关节对象组成）
//   Joint_t joints[ARM_JOINT_NUM];
  
//   // 全局运动学参数
//   struct {
//     float x, y, z;           // 工具中心偏移 (mm)
//     float roll, pitch, yaw;  // 工具姿态偏移 (rad)
//   } tool_offset;
  
//   // 时间管理
//   struct {
//     float now;               // 当前时间（秒）
//     uint64_t last_wakeup;    // 上次唤醒时间（微秒）
//     float dt;                // 控制周期（秒）
//   } timer;
  
//   // 末端状态（笛卡尔空间）
//   struct {
//     Arm6dof_Pose_t current;  // 当前末端位姿（正运动学结果）
//     Arm6dof_Pose_t target;   // 目标末端位姿
//   } end_effector;
  
//   // 控制状态
//   struct {
//     Arm_ControlMode_t mode;      // 控制模式
//     Arm_MotionState_t state;     // 运动状态
//     float position_tolerance;    // 位置到达容差（rad）
//     float velocity_tolerance;    // 速度到达容差（rad/s）
//     bool enable;                 // 使能标志
//   } control;
  
// } Arm_t;

// // 兼容旧接口的参数结构体
// typedef struct {
//   Arm6dof_Params_t arm6dof_params;
//   KPID_Params_t joint_pid_params[6];
//   MOTOR_LZ_Param_t jointMotor1_params;
//   MOTOR_LZ_Param_t jointMotor2_params;
//   MOTOR_LZ_Param_t jointMotor3_params;
//   MOTOR_DM_Param_t jointMotor4_params;
//   MOTOR_DM_Param_t jointMotor5_params;
//   MOTOR_DM_Param_t jointMotor6_params;
//   float q_offset[6];
// } Arm_Params_t;

// /* Exported functions prototypes -------------------------------------------- */

// /**
//  * @brief 初始化机械臂（兼容旧接口）
//  */
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq);

// /**
//  * @brief 更新机械臂状态
//  */
// int8_t Arm_Updata(Arm_t *arm);

// /**
//  * @brief 机械臂控制
//  */
// int8_t Arm_Ctrl(Arm_t *arm);

// // ============================================================================
// // 关节级别操作（面向对象接口）
// // ============================================================================

// /**
//  * @brief 初始化单个关节
//  * @param joint 关节指针
//  * @param id 关节编号（0-5）
//  * @param motor_type 电机类型
//  * @param dh_params DH参数
//  * @param pid_params PID参数
//  * @param freq 控制频率
//  * @return 0: 成功, -1: 失败
//  */
// int8_t Joint_Init(Joint_t *joint, uint8_t id, Joint_MotorType_t motor_type,
//                   const Arm6dof_DHParams_t *dh_params, const KPID_Params_t *pid_params, float freq);

// /**
//  * @brief 更新关节状态（读取电机反馈）
//  * @param joint 关节指针
//  * @return 0: 成功, -1: 失败
//  */
// int8_t Joint_Update(Joint_t *joint);

// /**
//  * @brief 关节位置控制
//  * @param joint 关节指针
//  * @param target_angle 目标角度（rad）
//  * @param dt 控制周期（s）
//  * @return 0: 成功, -1: 失败
//  */
// int8_t Joint_PositionControl(Joint_t *joint, float target_angle, float dt);

// /**
//  * @brief 关节松弛
//  * @param joint 关节指针
//  * @return 0: 成功, -1: 失败
//  */
// int8_t Joint_Relax(Joint_t *joint);

// /**
//  * @brief 检查关节是否到达目标
//  * @param joint 关节指针
//  * @param tolerance 位置容差（rad）
//  * @return true: 已到达, false: 未到达
//  */
// bool Joint_IsReached(const Joint_t *joint, float tolerance);

// // ============================================================================
// // 机械臂级别操作（继续保持现有接口）
// // ============================================================================

// /**
//  * @brief 设置控制模式
//  * @param arm 机械臂实例
//  * @param mode 控制模式
//  * @return 0: 成功, -1: 失败
//  */
// int8_t Arm_SetMode(Arm_t *arm, Arm_ControlMode_t mode);

// /**
//  * @brief 使能/失能机械臂
//  * @param arm 机械臂实例
//  * @param enable true: 使能, false: 失能
//  * @return 0: 成功, -1: 失败
//  */
// int8_t Arm_Enable(Arm_t *arm, bool enable);

// /**
//  * @brief 关节空间位置控制：设置目标关节角度
//  * @param arm 机械臂实例
//  * @param target_angles 目标关节角度（rad）
//  * @return 0: 成功, -1: 失败
//  */
// int8_t Arm_MoveJoint(Arm_t *arm, const Arm6dof_JointAngles_t* target_angles);

// /**
//  * @brief 笛卡尔空间位置控制：设置目标末端位姿
//  * @param arm 机械臂实例
//  * @param target_pose 目标末端位姿
//  * @return 0: 成功（含逆解成功）, -1: 失败（含逆解失败）
//  */
// int8_t Arm_MoveCartesian(Arm_t *arm, const Arm6dof_Pose_t* target_pose);

// /**
//  * @brief 急停：立即停止所有运动
//  * @param arm 机械臂实例
//  * @return 0: 成功, -1: 失败
//  */
// int8_t Arm_Stop(Arm_t *arm);

// /**
//  * @brief 获取当前运动状态
//  * @param arm 机械臂实例
//  * @return 运动状态
//  */
// Arm_MotionState_t Arm_GetState(Arm_t *arm);

// /**
//  * @brief ;
//   }output;
// */
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq);
// int8_t Arm_Updata(Arm_t *arm);
// int8_t Arm_Ctrl(Arm_t *arm);

// /**
//  * @brief 逆运动学求解：根据目标末端位姿计算关节角度
//  * @param arm 机械臂实例
//  * @param target_pose 目标末端位姿
//  * @param result_angles 输出的关节角度解
//  * @return 0: 成功, -1: 失败
//  * @note 使用当前关节角度作为初始猜测值，迭代50次，误差容限1.0mm
//  */
// int8_t Arm_SolveIK(Arm_t *arm, const Arm6dof_Pose_t* target_pose, Arm6dof_JointAngles_t* result_angles);

// #ifdef __cplusplus
// }
// #endif