diff --git a/CMakeLists.txt b/CMakeLists.txt index 98432ca..fa9d362 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index f2541d0..f238ca5 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -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 */ diff --git a/User/module/gripper_rm.cpp b/User/module/gripper_rm.cpp new file mode 100644 index 0000000..89c95d1 --- /dev/null +++ b/User/module/gripper_rm.cpp @@ -0,0 +1,192 @@ +#include "module/gripper_rm.hpp" + +#include + +#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 diff --git a/User/module/gripper_rm.hpp b/User/module/gripper_rm.hpp new file mode 100644 index 0000000..81ae4fe --- /dev/null +++ b/User/module/gripper_rm.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include + +#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 diff --git a/User/task/arm_main.cpp b/User/task/arm_main.cpp index 1c0e9d6..cd62f26 100644 --- a/User/task/arm_main.cpp +++ b/User/task/arm_main.cpp @@ -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);