夹爪

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

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]) —— 使能与控制类型:
+  * 左拨杆 SW_L：
-   *   UP   → 失能（电机松弛/重力补偿）
+  *   UP   → 机械臂 relax
-  *   MID  → 使能，POSE 笛卡尔增量控制
+  *   MID  → 机械臂关节控制分组（由 SW_R 选择）
-  *   DOWN → 使能，JOINT 关节角增量控制
+  *   DOWN → 仅控制夹爪
-   *
+  *
-  * 右拨杆 SW_R (sw[1]) —— 子模式/分组 + 夹爪状态切换:
+  * SW_L=MID 且 SW_R=UP：
-   *
+  *   LY → J2/J3 同向联动增量（底盘摇杆控制由底盘模块保持）
-  * POSE(ctrl_type=ARM_CTRL_REMOTE_CARTESIAN):
+  *
-  *   UP  【位置模式】3轴平移 + 偏航
+  * SW_L=MID 且 SW_R=MID：
-   *         右摇杆X (RX) → X 平移
+  *   LX → J1, LY → J2, RY → J3
-   *         右摇杆Y (RY) → Y 平移
+  *
-   *         左摇杆Y (LY) → Z 平移（升降）
+  * SW_L=MID 且 SW_R=DOWN：
-   *         左摇杆X (LX) → Yaw 偏航旋转
+  *   RX → J4, RY → J5, LY → J6
-   *
+  *
-   *   MID 【姿态模式】俯仰/横滚/偏航 + 升降
+  * SW_L=DOWN（夹爪控制）：
-   *         右摇杆X (RX) → Yaw  偏航旋转
+  *   SW_R=UP   → 松开
-   *         右摇杆Y (RY) → Pitch 俯仰旋转
+  *   SW_R=MID  → 保持当前
-   *         左摇杆X (LX) → Roll  横滚旋转
+  *   SW_R=DOWN → 夹紧
-   *         左摇杆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=松开）。
    */
   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[0]) {
-    switch (ctx->input.rc.sw[1]) {
+    case CMD_SW_UP:
-      case CMD_SW_UP:
+      ctx->output.arm.cmd.enable = false;
-        /* 位置模式：3轴平移 + 偏航 */
+      break;
-        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;
+    case CMD_SW_MID:
-        ctx->output.arm.cmd.target_pose.z   += ctx->input.rc.joy_left.y  * pos_scale * dt;
+      ctx->output.arm.cmd.enable = true;
-        ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_left.x  * rot_scale * dt;
+      ctx->output.arm.cmd.ctrl_type = ARM_CTRL_CUSTOM_JOINT;
-        break;
+
-      case CMD_SW_MID:
+      switch (ctx->input.rc.sw[1]) {
-        /* 姿态模式：俯仰 + 横滚 + 偏航 + 升降 */
+        case CMD_SW_UP: {
-        ctx->output.arm.cmd.target_pose.yaw   += ctx->input.rc.joy_right.x * rot_scale * dt;
+          const float d = ctx->input.rc.joy_left.y * joint_scale * dt;
-        ctx->output.arm.cmd.target_pose.pitch += ctx->input.rc.joy_right.y * rot_scale * dt;
+          ctx->output.arm.cmd.target_joints.q[1] += d;
-        ctx->output.arm.cmd.target_pose.roll  += ctx->input.rc.joy_left.x  * rot_scale * dt;
+          ctx->output.arm.cmd.target_joints.q[2] += d;
-        ctx->output.arm.cmd.target_pose.z     += ctx->input.rc.joy_left.y  * pos_scale * dt;
+          break;
-        break;
+        }
-      default:
+        case CMD_SW_MID:
-        /* SW_R=DOWN 预留给夹爪切换，不做机械臂增量 */
+          ctx->output.arm.cmd.target_joints.q[0] += ctx->input.rc.joy_left.x  * joint_scale * dt;
-        break;
+          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;
-  } else {
+          break;
-    switch (ctx->input.rc.sw[1]) {
+        case CMD_SW_DOWN:
-      case CMD_SW_UP:
+          ctx->output.arm.cmd.target_joints.q[3] += ctx->input.rc.joy_right.x * joint_scale * dt;
-        ctx->output.arm.cmd.target_joints.q[0] += 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[1] += 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;
-        ctx->output.arm.cmd.target_joints.q[2] += ctx->input.rc.joy_left.y  * joint_scale * dt;
+          break;
-        break;
+        default:
-      case CMD_SW_MID:
+          break;
-        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;
+      break;
-        ctx->output.arm.cmd.target_joints.q[5] += ctx->input.rc.joy_left.y  * joint_scale * dt;
+
-        break;
+    case CMD_SW_DOWN: {
-      default:
+      // 左拨杆下：只控制夹爪，机械臂保持使能但不注入新的关节目标
-        /* SW_R=DOWN 预留给夹爪切换，不做机械臂增量 */
+      ctx->output.arm.cmd.enable = true;
-        break;
+      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 */

User/module/gripper_rm.cpp

@@ -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

User/module/gripper_rm.hpp

@@ -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

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);
@@ -378,6 +390,9 @@ void Task_arm_main(void* argument) {
       arm_cmd.set_target_as_current = false;
     }
 
+    // 夹爪两态控制：直接跟随 gripper_close，内部状态机负责稳夹优先策略
+    Gripper_Step(arm_cmd.gripper_close);
+    
     robot_arm->Enable(arm_cmd.enable);
 
     // 使能上升沿自动同步：避免使能时 target_pose 与当前实际位姿不一致导致初始跳变