采用达妙4310电机通过mit模式读取力矩值来控制抓取。首先代码分为三个模式初始态复位找0状态打开状态闭合状态每2毫秒运行一次函数查找对应什么状态执行对应逻辑。每2ms运行0.11度。1弧度每秒。此时底层500hz的s型曲线并不会卡顿。因为在2ms后电机还每刹车下一个2ms已经达到所以伪造出了一个连续的匀速运动轨迹。首先是复位找0得到3个关键参数闭合点角度张开点角度此时角度。假设是闭合逻辑第一次2ms进入角度减少一点点第二次2ms进入发现已经达到力矩阈值直接break跳出输出上一次的位置值电机保持不动。也可能第一次进入直接达到阈值输出grip_close_target电机保持原位void gripper_fsm_update(float dt) { float actual_tor fabsf(motor[GRIP_IDX].para.tor); // 正常运行步进量与复位试探步进量 float step GRIP_SPEED * dt; float calib_step CALIB_SPEED * dt;先读取力矩绝对值actual_tordt是0.002秒GRIP_SPEED开合速度CALIB_SPEED复位找0的速度case GRIP_CALIBRATING: // 顺时针(角度减小)缓慢张开试探机械限位 current_pos_set - calib_step; // 当力矩达到张开阈值 0.3Nm说明已顶死在最大张开限位上 if (actual_tor 0.3) { // 1. 记录此时真实的物理位置作为永恒的“最大张开点” grip_open_target motor[GRIP_IDX].para.pos; // 2. 顺时针为张开(-)逆时针为闭合() // 闭合点 张开点 行程 grip_close_target grip_open_target GRIP_STROKE_RAD; // 3. 复位完成进入待机状态 current_pos_set grip_open_target; gripper_state GRIP_OPENED; } break;复位找0态current_pos_set最后会传给电机输出位置结构体motor[].cmd.pos_set所以一直张开爪子对应current_pos_set一直减小达到极限位置会卡住力矩比0.3大时记录此时位置最小值。此时位置张开的极限位置总转动角度195度闭合位置grip_close_target该值非常重要用于张开与闭合态case GRIP_OPENING: // 1. 张开力矩保护力矩 TOR_LIMIT_OPEN 时尝试停止 if (actual_tor TOR_LIMIT_OPEN) { // 计算当前【真实的物理位置】距离【完全闭合点】有多远 (顺时针张开角度减小所以用 闭合点 - 当前点) float moved_distance grip_close_target - motor[GRIP_IDX].para.pos; // 如果张开的物理距离已经大于100 度说明确实到底了正常停止 if (moved_distance 2.60f) { gripper_state GRIP_OPENED; break; } // 否则 (小于40度)说明是起步阶段的物理卡顿。 // 代码不执行 break无视此次力矩报警继续往下执行匀速插补 } // 2. 匀速减小角度逼近张开点 if (current_pos_set grip_open_target)//角度还没达到最小 { current_pos_set - step; if (current_pos_set grip_open_target) { current_pos_set grip_open_target; gripper_state GRIP_OPENED; // 到达极限 } } else//角度比初始化时更小 { current_pos_set grip_open_target;//不能比初始化态更小会损坏结构 gripper_state GRIP_OPENED; } break;有时候爪子会因为物理结构卡住一点点导致容易到达阈值现实中却没张到最大。所以还需要加入条件用完全闭合角度此时角度最大-此时角度计算的结果需要大于100度才算张开结束。正常张开过程角度一直减小直到等于复位找0态的值一定要有输出结果为grip_open_target张开的极限值确保不会一直张开撑爆硬件case GRIP_CLOSING: // 1. 抓取力矩检测力矩 0.30Nm 时认为咬住物体 if (actual_tor TOR_LIMIT_CLOSE) { // 冻结目标点利用 Kp 40 产生恒定夹紧力 gripper_state GRIP_GRABBED; break; } // 2. 匀速增加角度逼近闭合点 if (current_pos_set grip_close_target) { current_pos_set step; if (current_pos_set grip_close_target) { current_pos_set grip_close_target; gripper_state GRIP_GRABBED; // 空抓到底 (走完170度行程) } } else { current_pos_set grip_close_target; gripper_state GRIP_GRABBED; } break; /* ---------------------------------- * 保持状态逻辑 * ---------------------------------- */ case GRIP_OPENED: case GRIP_GRABBED: // 虚拟目标点停止步进维持现有姿态与力矩 break;跟张开的逻辑一样一定要有输出结果为grip_close_target闭合的极限值确保不会一直闭合#include gripper_ctrl.h #include dm4310_ctrl.h #include math.h /* * * 夹爪核心物理参数配置 * */ #define GRIP_IDX 6 // 夹爪在 motor 数组中的索引 (第7轴) #define PI 3.1415926535f // 行程转换为弧度 #define GRIP_STROKE_RAD (195.0f * PI / 180.0f) // 力矩阈值配置 (Nm) #define TOR_LIMIT_CLOSE 0.43f // 闭合时 认为抓到物体并夹紧 #define TOR_LIMIT_OPEN 0.43f // 张开时 触发保护/认定为到达极限 // 控制刚度与速度 #define GRIP_KP 40.0f // 夹爪位置刚度 (遇阻时的弹性夹紧力) #define GRIP_KD 1.0f // 夹爪运行阻尼 #define GRIP_SPEED 1.0f // 正常开合速度 (rad/s) #define CALIB_SPEED 1.0f // 找零时的缓慢试探速度 (rad/s)防止撞击过猛 /* */ gripper_state_e gripper_state GRIP_UNINIT; // 动态目标点 (在复位找零完成后自动生成) static float grip_open_target 0.0f; static float grip_close_target 0.0f; static float current_pos_set 0.0f; // 实时步进的虚拟目标点 void gripper_init(void) { // 获取上电时的初始位置防止起步瞬间产生大跳变 current_pos_set motor[GRIP_IDX].para.pos; // 初始化 MIT 控制参数 motor[GRIP_IDX].cmd.kp_set GRIP_KP; motor[GRIP_IDX].cmd.kd_set GRIP_KD; motor[GRIP_IDX].cmd.tor_set 0.0f; motor[GRIP_IDX].cmd.vel_set 0.0f; motor[GRIP_IDX].cmd.pos_set current_pos_set; // 上电后直接进入“复位找零”状态 gripper_state GRIP_CALIBRATING; } void gripper_set_signal(uint8_t sig) { // 安全锁如果还在复位找零阶段拒绝响应任何指令 if (gripper_state GRIP_UNINIT || gripper_state GRIP_CALIBRATING) { return; } if (sig 1) { gripper_state GRIP_CLOSING; // 信号1触发闭合 } else if (sig 0) { gripper_state GRIP_OPENING; // 信号0触发张开 } } // 核心状态机 (由底层 500Hz DT0.002s 控制循环调用) void gripper_fsm_update(float dt) { // 读取当前物理反馈绝对力矩 (取绝对值) float actual_tor fabsf(motor[GRIP_IDX].para.tor); // 正常运行步进量与复位试探步进量 float step GRIP_SPEED * dt; float calib_step CALIB_SPEED * dt; switch (gripper_state) { case GRIP_UNINIT: break; /* ---------------------------------- 上电自动复位找零 * ---------------------------------- */ case GRIP_CALIBRATING: // 顺时针(角度减小)缓慢张开试探机械限位 current_pos_set - calib_step; // 当力矩达到张开阈值 0.3Nm说明已顶死在最大张开限位上 if (actual_tor 0.3) { // 1. 记录此时真实的物理位置作为永恒的“最大张开点” grip_open_target motor[GRIP_IDX].para.pos; // 2. 顺时针为张开(-)逆时针为闭合() // 闭合点 张开点 行程 grip_close_target grip_open_target GRIP_STROKE_RAD; // 3. 复位完成进入待机状态 current_pos_set grip_open_target; gripper_state GRIP_OPENED; } break; /* ---------------------------------- * 正常张开过程 (顺时针角度减小) * ---------------------------------- */ case GRIP_OPENING: // 1. 张开力矩保护力矩 TOR_LIMIT_OPEN 时尝试停止 if (actual_tor TOR_LIMIT_OPEN) { // 计算当前【真实的物理位置】距离【完全闭合点】有多远 (顺时针张开角度减小所以用 闭合点 - 当前点) float moved_distance grip_close_target - motor[GRIP_IDX].para.pos; // 如果张开的物理距离已经大于100 度说明确实到底了正常停止 if (moved_distance 2.60f) { gripper_state GRIP_OPENED; break; } // 否则 (小于40度)说明是起步阶段的物理卡顿。 // 代码不执行 break无视此次力矩报警继续往下执行匀速插补 } // 2. 匀速减小角度逼近张开点 if (current_pos_set grip_open_target) { current_pos_set - step; if (current_pos_set grip_open_target) { current_pos_set grip_open_target; gripper_state GRIP_OPENED; // 到达极限 } } else { current_pos_set grip_open_target; gripper_state GRIP_OPENED; } break; /* ---------------------------------- * 正常闭合过程 逆时针角度增加 * ---------------------------------- */ case GRIP_CLOSING: // 1. 抓取力矩检测力矩 0.30Nm 时认为咬住物体 if (actual_tor TOR_LIMIT_CLOSE) { // 冻结目标点利用 Kp 40 产生恒定夹紧力 gripper_state GRIP_GRABBED; break; } // 2. 匀速增加角度逼近闭合点 if (current_pos_set grip_close_target) { current_pos_set step; if (current_pos_set grip_close_target) { current_pos_set grip_close_target; gripper_state GRIP_GRABBED; // 空抓到底 (走完170度行程) } } else { current_pos_set grip_close_target; gripper_state GRIP_GRABBED; } break; /* ---------------------------------- * 保持状态逻辑 * ---------------------------------- */ case GRIP_OPENED: case GRIP_GRABBED: // 虚拟目标点停止步进维持现有姿态与力矩 break; } // 最终下发给电机结构体 motor[GRIP_IDX].cmd.pos_set current_pos_set; motor[GRIP_IDX].cmd.kp_set GRIP_KP; motor[GRIP_IDX].cmd.kd_set GRIP_KD; motor[GRIP_IDX].cmd.tor_set 0.0f; }