xm/arm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

夹爪

Browse Source
This commit is contained in:
xxxxm 2026-03-20 04:14:00 +08:00
parent d5933612aa
commit 6309cd5bd2
5 changed files with 328 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -81,6 +81,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/module/cmd/cmd_adapter.c
User/module/cmd/cmd_behavior.c
User/module/cmd/cmd_example.c
User/module/gripper_rm.cpp
# User/task sources
User/task/arm_main.cpp
User/task/blink.c

163
User/module/cmd/cmd.c
View File

@ -257,103 +257,92 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM
static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
/*
* 遥控器控制机械臂：拨杆选择 pose 或 joint
*
* 左拨杆 SW_L (sw[0]) —— 使能与控制类型:
* UP → 失能（电机松弛/重力补偿）
* MID → 使能，POSE 笛卡尔增量控制
* DOWN → 使能，JOINT 关节角增量控制
*
* 右拨杆 SW_R (sw[1]) —— 子模式/分组 + 夹爪状态切换:
*
* POSE(ctrl_type=ARM_CTRL_REMOTE_CARTESIAN):
* UP 【位置模式】3轴平移 + 偏航
* 右摇杆X (RX) → X 平移
* 右摇杆Y (RY) → Y 平移
* 左摇杆Y (LY) → Z 平移（升降）
* 左摇杆X (LX) → Yaw 偏航旋转
*
* MID 【姿态模式】俯仰/横滚/偏航 + 升降
* 右摇杆X (RX) → Yaw 偏航旋转
* 右摇杆Y (RY) → Pitch 俯仰旋转
* 左摇杆X (LX) → Roll 横滚旋转
* 左摇杆Y (LY) → Z 平移（升降）
*
* JOINT(ctrl_type=ARM_CTRL_CUSTOM_JOINT):
* UP → J1/J2/J3 由 RX/RY/LY 增量控制
* MID → J4/J5/J6 由 RX/RY/LY 增量控制
*
* 通用:
* SW_R 进入 DOWN 的边沿触发一次 gripper_toggle，
* 并翻转 gripper_close（true=夹紧, false=松开）。
* 遥控器机械臂行为组（按需求定制）：
*
* 左拨杆 SW_L：
* UP → 机械臂 relax
* MID → 机械臂关节控制分组（由 SW_R 选择）
* DOWN → 仅控制夹爪
*
* SW_L=MID 且 SW_R=UP：
* LY → J2/J3 同向联动增量（底盘摇杆控制由底盘模块保持）
*
* SW_L=MID 且 SW_R=MID：
* LX → J1, LY → J2, RY → J3
*
* SW_L=MID 且 SW_R=DOWN：
* RX → J4, RY → J5, LY → J6
*
* SW_L=DOWN（夹爪控制）：
* SW_R=UP → 松开
* SW_R=MID → 保持当前
* SW_R=DOWN → 夹紧
*/
ctx->output.arm.cmd.set_target_as_current = false;
ctx->output.arm.cmd.gripper_toggle = false;
/* 右拨杆 DOWN 边沿：夹爪状态切换置位（仅输出控制量，不在此执行夹爪动作） */
if (ctx->input.rc.sw[1] == CMD_SW_DOWN && ctx->last_input.rc.sw[1] != CMD_SW_DOWN) {
ctx->output.arm.cmd.gripper_close = !ctx->output.arm.cmd.gripper_close;
ctx->output.arm.cmd.gripper_toggle = true;
}
switch (ctx->input.rc.sw[0]) {
case CMD_SW_MID:
ctx->output.arm.cmd.enable = true;
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
break;
case CMD_SW_DOWN:
ctx->output.arm.cmd.enable = true;
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_CUSTOM_JOINT;
break;
default:
ctx->output.arm.cmd.enable = false;
goto end;
}
/* 按控制类型分别处理增量输入 */
const float pos_scale = 0.3f; /* 末端线速度上限 (m/s) */
const float rot_scale = 1.0f; /* 末端角速度上限 (rad/s) */
const float joint_scale = 1.0f; /* 关节角速度上限 (rad/s) */
const float dt = ctx->timer.dt;
if (ctx->output.arm.cmd.ctrl_type == ARM_CTRL_REMOTE_CARTESIAN) {
switch (ctx->input.rc.sw[1]) {
case CMD_SW_UP:
/* 位置模式：3轴平移 + 偏航 */
ctx->output.arm.cmd.target_pose.x += ctx->input.rc.joy_right.x * pos_scale * dt;
ctx->output.arm.cmd.target_pose.y += ctx->input.rc.joy_right.y * pos_scale * dt;
ctx->output.arm.cmd.target_pose.z += ctx->input.rc.joy_left.y * pos_scale * dt;
ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_left.x * rot_scale * dt;
break;
case CMD_SW_MID:
/* 姿态模式：俯仰 + 横滚 + 偏航 + 升降 */
ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_right.x * rot_scale * dt;
ctx->output.arm.cmd.target_pose.pitch += ctx->input.rc.joy_right.y * rot_scale * dt;
ctx->output.arm.cmd.target_pose.roll += ctx->input.rc.joy_left.x * rot_scale * dt;
ctx->output.arm.cmd.target_pose.z += ctx->input.rc.joy_left.y * pos_scale * dt;
break;
default:
/* SW_R=DOWN 预留给夹爪切换，不做机械臂增量 */
break;
}
} else {
switch (ctx->input.rc.sw[1]) {
case CMD_SW_UP:
ctx->output.arm.cmd.target_joints.q[0] += ctx->input.rc.joy_right.x * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[1] += ctx->input.rc.joy_right.y * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[2] += ctx->input.rc.joy_left.y * joint_scale * dt;
break;
case CMD_SW_MID:
ctx->output.arm.cmd.target_joints.q[3] += ctx->input.rc.joy_right.x * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[4] += ctx->input.rc.joy_right.y * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[5] += ctx->input.rc.joy_left.y * joint_scale * dt;
break;
default:
/* SW_R=DOWN 预留给夹爪切换，不做机械臂增量 */
break;
switch (ctx->input.rc.sw[0]) {
case CMD_SW_UP:
ctx->output.arm.cmd.enable = false;
break;
case CMD_SW_MID:
ctx->output.arm.cmd.enable = true;
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_CUSTOM_JOINT;
switch (ctx->input.rc.sw[1]) {
case CMD_SW_UP: {
const float d = ctx->input.rc.joy_left.y * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[1] += d;
ctx->output.arm.cmd.target_joints.q[2] += d;
break;
}
case CMD_SW_MID:
ctx->output.arm.cmd.target_joints.q[0] += ctx->input.rc.joy_left.x * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[1] += ctx->input.rc.joy_left.y * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[2] += ctx->input.rc.joy_right.y * joint_scale * dt;
break;
case CMD_SW_DOWN:
ctx->output.arm.cmd.target_joints.q[3] += ctx->input.rc.joy_right.x * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[4] += ctx->input.rc.joy_right.y * joint_scale * dt;
ctx->output.arm.cmd.target_joints.q[5] += ctx->input.rc.joy_left.y * joint_scale * dt;
break;
default:
break;
}
break;
case CMD_SW_DOWN: {
// 左拨杆下：只控制夹爪，机械臂保持使能但不注入新的关节目标
ctx->output.arm.cmd.enable = true;
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_CUSTOM_JOINT;
bool has_cmd = false;
bool target_close = ctx->output.arm.cmd.gripper_close;
if (ctx->input.rc.sw[1] == CMD_SW_UP) {
has_cmd = true;
target_close = false;
} else if (ctx->input.rc.sw[1] == CMD_SW_DOWN) {
has_cmd = true;
target_close = true;
}
if (has_cmd && target_close != ctx->output.arm.cmd.gripper_close) {
ctx->output.arm.cmd.gripper_close = target_close;
ctx->output.arm.cmd.gripper_toggle = true;
}
break;
}
default:
ctx->output.arm.cmd.enable = false;
break;
}
end:
return;
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM */

192
User/module/gripper_rm.cpp Normal file
View File

@ -0,0 +1,192 @@
#include "module/gripper_rm.hpp"
#include <cmath>
#include "task/user_task.h"
namespace arm {
GripperState gripper_state_dbg = GripperState::OPEN_HOLD;
bool gripper_target_close = false;
float gripper_output_dbg = 0.0f;
float gripper_out_open = -0.35f;
float gripper_out_close_fast = 0.55f;
float gripper_out_preload = 0.72f;
float gripper_out_hold = 0.46f;
float gripper_out_boost = 0.64f;
float gripper_contact_current_th = 4200.0f;
float gripper_contact_speed_th = 2.0f;
float gripper_slip_speed_th = 1.6f;
uint16_t gripper_contact_debounce_ms = 18;
uint16_t gripper_preload_ms = 180;
uint16_t gripper_boost_ms = 120;
uint16_t gripper_open_timeout_ms = 1200;
uint16_t gripper_close_timeout_ms = 1500;
static MOTOR_RM_Param_t g_gripper_rm_param = {};
static bool g_gripper_inited = false;
static uint16_t GripperMsToTicks(uint16_t ms) {
const uint32_t freq = ARM_MAIN_FREQ;
uint32_t ticks = ((uint32_t)ms * freq + 999u) / 1000u;
if (ticks == 0u) {
ticks = 1u;
}
return (uint16_t)ticks;
}
static void GripperSetOutput(float out_norm) {
if (out_norm > 1.0f) out_norm = 1.0f;
if (out_norm < -1.0f) out_norm = -1.0f;
gripper_output_dbg = out_norm;
(void)MOTOR_RM_SetOutput(&g_gripper_rm_param, out_norm);
}
static void GripperStateMachineStep(bool target_close) {
static uint16_t state_ticks = 0;
static uint16_t contact_ticks = 0;
static uint16_t slip_ticks = 0;
static uint16_t boost_ticks = 0;
static bool prev_target_close = false;
const bool target_changed = (target_close != prev_target_close);
prev_target_close = target_close;
MOTOR_RM_t* gm = MOTOR_RM_GetMotor(&g_gripper_rm_param);
const bool online = (gm != nullptr) && gm->motor.header.online;
const float fb_speed = online ? gm->feedback.rotor_speed : 0.0f;
const float fb_current = online ? gm->feedback.torque_current : 0.0f;
const bool contact_now = fabsf(fb_current) > gripper_contact_current_th &&
fabsf(fb_speed) < gripper_contact_speed_th;
const bool slip_now = fabsf(fb_speed) > gripper_slip_speed_th;
if (target_changed) {
state_ticks = 0;
contact_ticks = 0;
slip_ticks = 0;
boost_ticks = 0;
gripper_state_dbg = target_close ? GripperState::CLOSING : GripperState::OPENING;
}
if (!online) {
gripper_state_dbg = GripperState::FAULT;
GripperSetOutput(0.0f);
return;
}
++state_ticks;
switch (gripper_state_dbg) {
case GripperState::OPENING:
GripperSetOutput(gripper_out_open);
if (target_close) {
gripper_state_dbg = GripperState::CLOSING;
state_ticks = 0;
contact_ticks = 0;
} else if (state_ticks >= GripperMsToTicks(gripper_open_timeout_ms)) {
gripper_state_dbg = GripperState::OPEN_HOLD;
state_ticks = 0;
}
break;
case GripperState::OPEN_HOLD:
GripperSetOutput(0.0f);
if (target_close) {
gripper_state_dbg = GripperState::CLOSING;
state_ticks = 0;
contact_ticks = 0;
}
break;
case GripperState::CLOSING:
GripperSetOutput(gripper_out_close_fast);
if (!target_close) {
gripper_state_dbg = GripperState::OPENING;
state_ticks = 0;
contact_ticks = 0;
break;
}
contact_ticks = contact_now ? (uint16_t)(contact_ticks + 1u) : 0u;
if (contact_ticks >= GripperMsToTicks(gripper_contact_debounce_ms)) {
gripper_state_dbg = GripperState::PRELOAD;
state_ticks = 0;
} else if (state_ticks >= GripperMsToTicks(gripper_close_timeout_ms)) {
// 稳夹优先：闭合超时后转入保持而不是松开
gripper_state_dbg = GripperState::CLOSE_HOLD;
state_ticks = 0;
}
break;
case GripperState::PRELOAD:
GripperSetOutput(gripper_out_preload);
if (!target_close) {
gripper_state_dbg = GripperState::OPENING;
state_ticks = 0;
} else if (state_ticks >= GripperMsToTicks(gripper_preload_ms)) {
gripper_state_dbg = GripperState::CLOSE_HOLD;
state_ticks = 0;
}
break;
case GripperState::CLOSE_HOLD:
if (!target_close) {
gripper_state_dbg = GripperState::OPENING;
state_ticks = 0;
slip_ticks = 0;
boost_ticks = 0;
break;
}
if (boost_ticks > 0) {
GripperSetOutput(gripper_out_boost);
--boost_ticks;
} else {
GripperSetOutput(gripper_out_hold);
}
// 检测到可能打滑时短时增压，提高稳夹能力
if (slip_now) {
++slip_ticks;
} else {
slip_ticks = 0;
}
if (slip_ticks >= GripperMsToTicks(30)) {
boost_ticks = GripperMsToTicks(gripper_boost_ms);
slip_ticks = 0;
}
break;
case GripperState::FAULT:
default:
GripperSetOutput(0.0f);
if (online) {
gripper_state_dbg = target_close ? GripperState::CLOSING : GripperState::OPENING;
state_ticks = 0;
}
break;
}
}
int8_t Gripper_Init(const MOTOR_RM_Param_t& param) {
g_gripper_rm_param = param;
g_gripper_inited = (MOTOR_RM_Register(&g_gripper_rm_param) == DEVICE_OK);
return g_gripper_inited ? DEVICE_OK : DEVICE_ERR;
}
void Gripper_Step(bool target_close) {
if (!g_gripper_inited) {
return;
}
gripper_target_close = target_close;
(void)MOTOR_RM_Update(&g_gripper_rm_param);
GripperStateMachineStep(target_close);
(void)MOTOR_RM_Ctrl(&g_gripper_rm_param);
}
} // namespace arm

44
User/module/gripper_rm.hpp Normal file
View File

@ -0,0 +1,44 @@
#pragma once
#include <stdint.h>
#include "device/motor_rm.h"
namespace arm {
enum class GripperState : uint8_t {
OPENING = 0,
OPEN_HOLD,
CLOSING,
PRELOAD,
CLOSE_HOLD,
FAULT,
};
// 调试变量与可调参数，保留为全局便于Watch实时观察/修改。
extern GripperState gripper_state_dbg;
extern bool gripper_target_close;
extern float gripper_output_dbg;
// 夹爪输出（MOTOR_RM_SetOutput 归一化范围：[-1, 1]）
extern float gripper_out_open;
extern float gripper_out_close_fast;
extern float gripper_out_preload;
extern float gripper_out_hold;
extern float gripper_out_boost;
// 接触/防滑判据
extern float gripper_contact_current_th;
extern float gripper_contact_speed_th;
extern float gripper_slip_speed_th;
extern uint16_t gripper_contact_debounce_ms;
extern uint16_t gripper_preload_ms;
extern uint16_t gripper_boost_ms;
extern uint16_t gripper_open_timeout_ms;
extern uint16_t gripper_close_timeout_ms;
int8_t Gripper_Init(const MOTOR_RM_Param_t& param);
void Gripper_Step(bool target_close);
} // namespace arm

15
User/task/arm_main.cpp
View File

@ -7,6 +7,7 @@
#include "task/user_task.h"
#include "module/arm_oop.hpp"
#include "module/gripper_rm.hpp"
#include "module/motor_base.hpp"
#include "module/joint.hpp"
#include "bsp/can.h"
@ -66,6 +67,16 @@ RoboticArm* robot_arm = nullptr;
Arm_CMD_t arm_cmd; // 当前机械臂控制命令（含 target_pose / set_target_as_current）
static MOTOR_RM_Param_t MakeGripperRMParam() {
MOTOR_RM_Param_t param{};
param.can = BSP_CAN_1;
param.id = 0x201;
param.module = MOTOR_M3508;
param.reverse = false;
param.gear = true;
return param;
}
// ============================================================================
// 测试用调试变量（可在调试器Watch窗口直接观察和修改）
// ============================================================================
@ -311,6 +322,7 @@ void Task_arm_main(void* argument) {
BSP_CAN_Init();
MOTOR_LZ_Init(); // 注册LZ电机ID解析器
Gripper_Init(MakeGripperRMParam());
// 初始化运动学模型
Arm6dof_Init(dh_params);
@ -377,6 +389,9 @@ void Task_arm_main(void* argument) {
arm_cmd.gripper_toggle = false;
arm_cmd.set_target_as_current = false;
}
// 夹爪两态控制：直接跟随 gripper_close，内部状态机负责稳夹优先策略
Gripper_Step(arm_cmd.gripper_close);
robot_arm->Enable(arm_cmd.enable);