283 lines
8.4 KiB
C
283 lines
8.4 KiB
C
// /*
|
||
// * 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
|
||
|
||
|
||
|