lm测试

CMakeLists.txt

@@ -59,6 +59,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/component/user_math.c
     User/component/filter.c
     User/component/PowerControl.c
+    User/component/mixer.c
     # User/component/toolbox sources (C++)
     User/component/toolbox/matrix.cpp
     User/component/toolbox/robotics.cpp

Core/Src/usart.c

@@ -40,10 +40,10 @@ void MX_USART3_UART_Init(void)
 
   /* USER CODE END USART3_Init 1 */
   huart3.Instance = USART3;
-  huart3.Init.BaudRate = 115200;
+  huart3.Init.BaudRate = 100000;
-  huart3.Init.WordLength = UART_WORDLENGTH_8B;
+  huart3.Init.WordLength = UART_WORDLENGTH_9B;
   huart3.Init.StopBits = UART_STOPBITS_1;
-  huart3.Init.Parity = UART_PARITY_NONE;
+  huart3.Init.Parity = UART_PARITY_EVEN;
   huart3.Init.Mode = UART_MODE_TX_RX;
   huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
   huart3.Init.OverSampling = UART_OVERSAMPLING_16;

User/bsp/uart.c

@@ -23,7 +23,7 @@ static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
 
 /* Private function  -------------------------------------------------------- */
 static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
-  if (huart->Instance == UART5)
+  if (huart->Instance == USART3)
     return BSP_UART_RC;
   else
     return BSP_UART_ERR;

User/component/arm_kinematics/arm6dof.h

@@ -50,6 +50,24 @@ typedef struct {
     float q[6];  // 6个关节角度 (rad)
 } Arm6dof_JointAngles_t;
 
+/**
+ * @brief 机械臂控制类型枚举
+ * - REMOTE_CARTESIAN: 遥控器/PC端控制 - 目标末端位姿（笛卡尔空间）
+ * - CUSTOM_JOINT:     自定义控制器  - 目标六个关节角度
+ */
+typedef enum {
+    ARM_CTRL_REMOTE_CARTESIAN = 0,  /* 遥控器控制 - 目标末端位姿（笛卡尔空间） */
+    ARM_CTRL_CUSTOM_JOINT,          /* 自定义控制器 - 目标六个关节角度 */
+} Arm_CtrlType_t;
+
+/** 机械臂命令结构体 */
+typedef struct {
+    bool                  enable;               /* 使能开关 */
+    bool                  set_target_as_current; /* true: 将目标位姿同步为当前实际位姿 */
+    Arm_CtrlType_t        ctrl_type;             /* 控制类型 */
+    Arm6dof_Pose_t        target_pose;           /* 目标末端位姿（遥控器模式，笛卡尔空间） */
+    Arm6dof_JointAngles_t target_joints;         /* 目标六个关节角度（自定义控制器模式） */
+} Arm_CMD_t;
 /**
  * @brief 初始化机械臂运动学模型
  * @param dh_params 6个关节的DH参数数组

User/module/arm_oop.hpp

@@ -108,9 +108,8 @@ public:
         }
         gravity_comp_.scale = 1.0f;
         inverseKinematics_.valid = false;
-
+        memset(&inverseKinematics_.angles, 0, sizeof(inverseKinematics_.angles));
-        memset(&traj_.start, 0, sizeof(traj_.start));
+        memset(&traj_,  0, sizeof(traj_));
-        memset(&traj_.goal,  0, sizeof(traj_.goal));
         traj_.t           = 0.0f;
         traj_.active      = false;
         traj_.max_lin_vel = 0.15f;  // 150 mm/s，可通过 SetLinVelLimit() 调整
@@ -252,7 +251,7 @@ public:
             q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
         }
 
-        if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) != 0) {
+        if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.01f, 50) != 0) {
             traj_.valid = false;
             traj_.active = false;
             control_.state = MotionState::ERROR;
@@ -293,21 +292,21 @@ public:
             }
         }
 
-        // // 突变检查：轨迹模式下每帧步长极小，若仍触发突变说明严重异常
+        // 突变检查：轨迹模式下每帧步长极小，若仍触发突变说明严重异常
-        // // 阈值比直接IK时更严：max_lin_vel*dt对应的关节角变化通常远小于0.3rad
+        // 阈值比直接IK时更严：max_lin_vel*dt对应的关节角变化通常远小于0.3rad
-        // static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f;  // rad ≈ 17°/帧
+        static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f;  // rad ≈ 17°/帧
-        // for (size_t i = 0; i < JOINT_NUM; ++i) {
+        for (size_t i = 0; i < JOINT_NUM; ++i) {
-        //     if (joints_[i]) {
+            if (joints_[i]) {
-        //         float delta = fabsf(traj_.angles.q[i] -
+                float delta = fabsf(traj_.angles.q[i] -
-        //                             joints_[i]->GetCurrentAngle());
+                                    joints_[i]->GetCurrentAngle());
-        //         if (delta > TRAJ_MAX_JOINT_DELTA) {
+                if (delta > TRAJ_MAX_JOINT_DELTA) {
-        //             traj_.valid = false;
+                    traj_.valid = false;
-        //             traj_.active = false;
+                    traj_.active = false;
-        //             control_.state = MotionState::ERROR;
+                    control_.state = MotionState::ERROR;
-        //             return;
+                    return;
-        //         }
+                }
-        //     }
+            }
-        // }
+        }
 
         traj_.valid = true;
         end_effector_.target = interp;

User/module/chassis.c

@@ -206,7 +206,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
         case CHASSIS_MODE_RELAX:
         case CHASSIS_MODE_BREAK:
         case CHASSIS_MODE_INDEPENDENT:
-            c->move_vec.wz = 0.0f;
+            c->move_vec.wz = c_cmd->ctrl_vec.wz;
             break;
         case CHASSIS_MODE_OPEN:
 						c->move_vec.wz = c_cmd->ctrl_vec.wz;

User/module/cmd/cmd.c

@@ -34,6 +34,7 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
   /* 摇杆控制移动 */
   ctx->output.chassis.cmd.ctrl_vec.vx = ctx->input.rc.joy_right.x;
   ctx->output.chassis.cmd.ctrl_vec.vy = ctx->input.rc.joy_right.y;
+  ctx->output.chassis.cmd.ctrl_vec.wz = ctx->input.rc.joy_left.x;
 }
 #endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS */
 
@@ -251,6 +252,78 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
 }
 #endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT */
 
+/* 从 RC 输入生成机械臂命令 */
+#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM
+static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
+  /*
+   * 左拨杆映射：
+   *   UP   -> enable = false（失能）
+   *   MID  -> enable = true，正常笛卡尔增量控制
+   *   DOWN -> enable = true，set_target_as_current = true（目标位姿吸附为当前位姿）
+   */
+  ctx->output.arm.cmd.set_target_as_current = false;
+    switch (ctx->input.rc.sw[0]) {
+      case CMD_SW_MID:
+          ctx->output.arm.cmd.enable = true;
+          break;
+      case CMD_SW_DOWN:
+          ctx->output.arm.cmd.enable = true;
+          break;
+      default: 
+          ctx->output.arm.cmd.enable = false;
+          goto end;
+          break;
+  }
+  if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
+      ctx->output.arm.cmd.set_target_as_current = true;
+  }
+
+  /* 遥控器模式使用笛卡尔位姿控制 */
+  ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
+
+  if (!ctx->output.arm.cmd.enable) return;
+  /* set_target_as_current 置位时不叠加摇杆增量，由上层模块负责同步位姿 */
+  if (ctx->output.arm.cmd.set_target_as_current) return;
+
+  /* 摆杆增量控制末端位姿 */
+  float pos_speed = 0.3f * ctx->timer.dt;   /* 位置平移速度 (m/s) */
+  float rot_speed = 1.0f * ctx->timer.dt;   /* 姿态旋转速度 (rad/s) */
+    switch (ctx->input.rc.sw[1]) {
+      case CMD_SW_UP:
+            ctx->output.arm.cmd.target_pose.x   += ctx->input.rc.joy_right.x * pos_speed;
+            ctx->output.arm.cmd.target_pose.y   += ctx->input.rc.joy_right.y * pos_speed;
+            ctx->output.arm.cmd.target_pose.z   += ctx->input.rc.joy_left.y  * pos_speed;
+            ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_left.x  * rot_speed;
+          break;
+      case CMD_SW_MID:
+            ctx->output.arm.cmd.target_pose.roll   += ctx->input.rc.joy_left.x * pos_speed;
+            ctx->output.arm.cmd.target_pose.pitch  += ctx->input.rc.joy_right.y * pos_speed;
+          break;
+      default:  
+          break;
+  }
+  end:
+  return;
+}
+#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM */
+
+/* 从 PC 输入生成机械臂命令 */
+#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_ARM
+static void CMD_PC_BuildArmCmd(CMD_t *ctx) {
+  if (!ctx->input.online[CMD_SRC_PC]) {
+      ctx->output.arm.cmd.enable = false;
+      return;
+  }
+  ctx->output.arm.cmd.enable = true;
+  ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
+
+  /* 鼠标控制末端位坐 XY，其他轴可根据需要扩展 */
+  float sens = ctx->config->sensitivity.mouse_sens * 0.001f;
+  ctx->output.arm.cmd.target_pose.x += (float)ctx->input.pc.mouse.x * sens * ctx->timer.dt;
+  ctx->output.arm.cmd.target_pose.y += (float)ctx->input.pc.mouse.y * sens * ctx->timer.dt;
+}
+#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_ARM */
+
 /* 离线安全模式 */
 static void CMD_SetOfflineMode(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_CHASSIS
@@ -265,6 +338,9 @@ static void CMD_SetOfflineMode(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_TRACK
   ctx->output.track.cmd.enable = false;
 #endif
+#if CMD_ENABLE_MODULE_ARM
+  ctx->output.arm.cmd.enable = false;
+#endif
 }
 
 /* ========================================================================== */
@@ -310,6 +386,7 @@ typedef struct {
   CMD_BuildCommandFunc gimbalFunc;
   CMD_BuildCommandFunc shootFunc;
   CMD_BuildCommandFunc trackFunc;
+  CMD_BuildCommandFunc armFunc;
 } CMD_SourceHandler_t;
 
 /* Build-function conditional references */
@@ -353,6 +430,16 @@ typedef struct {
 #else
   #define _FN_PC_TRACK    NULL
 #endif
+#if CMD_ENABLE_MODULE_ARM && CMD_ENABLE_SRC_RC
+  #define _FN_RC_ARM      CMD_RC_BuildArmCmd
+#else
+  #define _FN_RC_ARM      NULL
+#endif
+#if CMD_ENABLE_MODULE_ARM && CMD_ENABLE_SRC_PC
+  #define _FN_PC_ARM      CMD_PC_BuildArmCmd
+#else
+  #define _FN_PC_ARM      NULL
+#endif
 #if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_NUC
   #define _FN_NUC_GIMBAL  CMD_NUC_BuildGimbalCmd
 #else
@@ -366,16 +453,16 @@ typedef struct {
 
 CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
 #if CMD_ENABLE_SRC_RC
-  [CMD_SRC_RC]  = {CMD_SRC_RC,  _FN_RC_CHASSIS, _FN_RC_GIMBAL, _FN_RC_SHOOT, _FN_RC_TRACK},
+  [CMD_SRC_RC]  = {CMD_SRC_RC,  _FN_RC_CHASSIS, _FN_RC_GIMBAL, _FN_RC_SHOOT, _FN_RC_TRACK, _FN_RC_ARM},
 #endif
 #if CMD_ENABLE_SRC_PC
-  [CMD_SRC_PC]  = {CMD_SRC_PC,  _FN_PC_CHASSIS, _FN_PC_GIMBAL, _FN_PC_SHOOT, _FN_PC_TRACK},
+  [CMD_SRC_PC]  = {CMD_SRC_PC,  _FN_PC_CHASSIS, _FN_PC_GIMBAL, _FN_PC_SHOOT, _FN_PC_TRACK, _FN_PC_ARM},
 #endif
 #if CMD_ENABLE_SRC_NUC
-  [CMD_SRC_NUC] = {CMD_SRC_NUC, NULL, _FN_NUC_GIMBAL, _FN_NUC_SHOOT, NULL},
+  [CMD_SRC_NUC] = {CMD_SRC_NUC, NULL, _FN_NUC_GIMBAL, _FN_NUC_SHOOT, NULL, NULL},
 #endif
 #if CMD_ENABLE_SRC_REF
-  [CMD_SRC_REF] = {CMD_SRC_REF, NULL, NULL, NULL, NULL},
+  [CMD_SRC_REF] = {CMD_SRC_REF, NULL, NULL, NULL, NULL, NULL},
 #endif
 };
 
@@ -432,6 +519,9 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
 #if CMD_ENABLE_MODULE_TRACK
   ctx->output.track.source = ctx->active_source;
 #endif
+#if CMD_ENABLE_MODULE_ARM
+  ctx->output.arm.source = ctx->active_source;
+#endif
 
   CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE);
 
@@ -485,6 +575,11 @@ int8_t CMD_GenerateCommands(CMD_t *ctx) {
   if (sourceHandlers[ctx->output.track.source].trackFunc != NULL) {
     sourceHandlers[ctx->output.track.source].trackFunc(ctx);
   }
+#endif
+#if CMD_ENABLE_MODULE_ARM
+  if (sourceHandlers[ctx->output.arm.source].armFunc != NULL) {
+    sourceHandlers[ctx->output.arm.source].armFunc(ctx);
+  }
 #endif
   return CMD_OK;
 }

User/module/cmd/cmd.h

@@ -21,6 +21,9 @@
 #if CMD_ENABLE_MODULE_TRACK
   #include "module/track.h"
 #endif
+#if CMD_ENABLE_MODULE_ARM
+  #include "component/arm_kinematics/arm6dof.h"
+#endif
 #include <stdint.h>
 
 #ifdef __cplusplus
@@ -60,6 +63,13 @@ typedef struct {
 } CMD_TrackOutput_t;
 #endif
 
+#if CMD_ENABLE_MODULE_ARM
+typedef struct {
+    CMD_InputSource_t source;
+    Arm_CMD_t         cmd;
+} CMD_ArmOutput_t;
+#endif
+
 /* ========================================================================== */
 /*                           配置结构                                           */
 /* ========================================================================== */
@@ -146,6 +156,9 @@ typedef struct CMD_Context {
 #endif
 #if CMD_ENABLE_MODULE_TRACK
     CMD_TrackOutput_t track;
+#endif
+#if CMD_ENABLE_MODULE_ARM
+    CMD_ArmOutput_t arm;
 #endif
     } output;
 } CMD_t;
@@ -222,6 +235,13 @@ static inline Track_CMD_t* CMD_GetTrackCmd(CMD_t *ctx) {
 }
 #endif
 
+/* 获取机械臂命令 */
+#if CMD_ENABLE_MODULE_ARM
+static inline Arm_CMD_t* CMD_GetArmCmd(CMD_t *ctx) {
+    return &ctx->output.arm.cmd;
+}
+#endif
+
 #ifdef __cplusplus
 }
 #endif

User/module/cmd/cmd_behavior.c

@@ -107,6 +107,9 @@ int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) {
 #endif
 #if CMD_ENABLE_MODULE_SHOOT
       ctx->output.shoot.source = CMD_SRC_RC;
+#endif
+#if CMD_ENABLE_MODULE_ARM
+      ctx->output.arm.source = CMD_SRC_RC;
 #endif
     } else if(ctx->active_source == CMD_SRC_RC) {
       ctx->active_source = CMD_SRC_PC;
@@ -118,6 +121,9 @@ int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) {
 #endif
 #if CMD_ENABLE_MODULE_SHOOT
       ctx->output.shoot.source = CMD_SRC_PC;
+#endif
+#if CMD_ENABLE_MODULE_ARM
+      ctx->output.arm.source = CMD_SRC_PC;
 #endif
     }
     return CMD_OK;

User/module/cmd/cmd_feature.h

@@ -38,6 +38,9 @@
 /** 履带模块 (需要 module/track.h) */
 #define CMD_ENABLE_MODULE_TRACK    0
 
+/** 机械臂模块 (需要 component/arm_kinematics/arm6dof.h) */
+#define CMD_ENABLE_MODULE_ARM      1
+
 /* ========================================================================== */
 /*                          合法性检查                                          */
 /* ========================================================================== */

User/module/cmd/cmd_types.h

@@ -85,12 +85,19 @@ extern "C" {
   #define _X_MOD_TRACK(X)
 #endif
 
+#if CMD_ENABLE_MODULE_ARM
+  #define _X_MOD_ARM(X)      X(ARM,     Arm_CMD_t,     arm)
+#else
+  #define _X_MOD_ARM(X)
+#endif
+
 /* 输出模块配置宏表 */
 #define CMD_OUTPUT_MODULE_TABLE(X) \
     _X_MOD_CHASSIS(X) \
     _X_MOD_GIMBAL(X)  \
     _X_MOD_SHOOT(X)   \
-    _X_MOD_TRACK(X)
+    _X_MOD_TRACK(X)   \
+    _X_MOD_ARM(X)
 
 
 /* ========================================================================== */
@@ -219,7 +226,8 @@ typedef enum {
     CMD_MODULE_GIMBAL  = (1 << 2),
     CMD_MODULE_SHOOT   = (1 << 3),
     CMD_MODULE_TRACK   = (1 << 4),
-    CMD_MODULE_ALL     = 0x1E
+    CMD_MODULE_ARM     = (1 << 5),
+    CMD_MODULE_ALL     = 0x3E
 
 } CMD_ModuleMask_t;
 

User/module/config.c

@@ -107,8 +107,8 @@ Config_RobotParam_t robot_config = {
         },
         .rc_mode_map = {
             .sw_left_up   = CHASSIS_MODE_RELAX,
-            .sw_left_mid  = CHASSIS_MODE_FOLLOW_GIMBAL,
+            .sw_left_mid  = CHASSIS_MODE_RELAX,
-            .sw_left_down = CHASSIS_MODE_RELAX,
+            .sw_left_down = CHASSIS_MODE_INDEPENDENT,
 
         },
     },

User/module/joint.hpp

@@ -21,6 +21,7 @@ private:
     IMotor* motor_;                 // 电机指针（多态）
     Arm6dof_DHParams_t dh_params_;  // DH参数
     float q_offset_;                // 零位偏移
+    const Joint* coupled_joint_;    // 耦合源关节（J3 与 J2 耦合）
     
     // 状态
     struct {
@@ -36,7 +37,8 @@ public:
     // 构造函数
     Joint(uint8_t id, IMotor* motor, const Arm6dof_DHParams_t& dh_params, 
           float q_offset, float freq)
-        : id_(id), motor_(motor), dh_params_(dh_params), q_offset_(q_offset) {
+        : id_(id), motor_(motor), dh_params_(dh_params), q_offset_(q_offset),
+          coupled_joint_(nullptr) {
                 
         state_.current_angle = 0.0f;
         state_.current_velocity = 0.0f;
@@ -62,6 +64,14 @@ public:
     // Setters
     void SetOffset(float offset) { q_offset_ = offset; }
 
+    /**
+     * @brief 设置机械耦合源关节
+     * @param joint 耦合源关节指针（读取角度时减去其角度，写入时加回）
+     * @note  用于关节间机械耦合补偿，如 J3 与 J2 耦合：
+     *        实际J3角度 = 电机J3原始角度 - J2当前角度
+     */
+    void SetCoupledJoint(const Joint* joint) { coupled_joint_ = joint; }
+    
     void SetTargetAngle(float target) { 
         state_.target_angle = target; 
     }
@@ -97,6 +107,12 @@ public:
             if (raw < -M_PI) raw += 2.0f * M_PI;
         }
 
+        // 机械耦合补偿：减去耦合源关节的当前角度以得到真实关节角度
+        // 例：J3 与 J2 耦合时，真实J3 = 电机J3原始角 - J2当前角
+        if (coupled_joint_) {
+            raw -= coupled_joint_->GetCurrentAngle();
+        }
+
         state_.current_angle    = raw;
         state_.current_velocity = motor_->GetVelocity();
         state_.current_torque   = motor_->GetTorque();
@@ -129,8 +145,14 @@ public:
         if (kp <= 0.0f) kp = (id_ < 3) ? 10.0f : 50.0f;  // LZ vs DM默认值
         if (kd <= 0.0f) kd = (id_ < 3) ? 0.5f : 3.0f;
         
+        // 机械耦合补偿：发送给电机的目标需加回耦合源关节的角度
+        float motor_target = target_angle;
+        if (coupled_joint_) {
+            motor_target += coupled_joint_->GetCurrentAngle();
+        }
+
         return motor_->PositionControl(
-            target_angle, 
+            motor_target, 
             0.0f,                      // 速度
             kp, kd,                    // 刚度和阻尼
             total_feedforward          // 前馈力矩（重力补偿）

User/task/arm_main.cpp

@@ -3,6 +3,7 @@
  * @brief 机械臂主任务 - C++版本
  */
 
+#include "cmsis_os2.h"
 #include "task/user_task.h"
 
 #include "module/arm_oop.hpp"
@@ -49,10 +50,10 @@ static Arm6dof_DHParams_t dh_params[6] = {
   // J3: 小臂（LZ电机，负载中等）  j3 rpy=(0,0,-90)
   {.theta=0.0f, .d=0.002795f,  .a=0.047775f, .alpha=-M_PI_2,  .theta_offset=-M_PI_2,  .qmin=-1.0f,    .qmax=3.0f,   .m=0.72964f, .rc={ 0.08696299f,  0.00167372f, -0.02500400f},  .kp=20.0f,  .kd=0.0f},
   // J4: 腕部旋转（DM电机，roll轴，0到2π循环）  j4 rpy=(-90,0,-90)
-  {.theta=0.0f, .d=0.241f,     .a=0.0f,      .alpha=M_PI_2,   .theta_offset=M_PI,     .qmin=0.0f,     .qmax=6.3f,  .m=0.60215f, .rc={ 0.00207070f, -0.14360000f,  0.00004920f},  .kp=10.0f,  .kd=0.0f},
+  {.theta=0.0f, .d=0.241f,     .a=0.0f,      .alpha=M_PI_2,   .theta_offset=M_PI,     .qmin=0.0f,     .qmax=6.3f,  .m=0.60215f, .rc={ 0.00207070f, -0.14360000f,  0.00004920f},  .kp=3.0f,  .kd=0.0f},
   // J5: 腕部俯仰（DM电机，负载小，可提高响应）  j5 rpy=(90,0,-180)
-  {.theta=0.0f, .d=0.0f,       .a=0.1065f,   .alpha=-M_PI_2,  .theta_offset=0.0f,     .qmin=-1.9f,   .qmax=1.9f,  .m=0.21817f, .rc={ 0.00001750f,  0.00297341f,  0.05816899f},  .kp=15.0f,  .kd=0.0f},
+  // {.theta=0.0f, .d=0.0f,       .a=0.1065f,   .alpha=-M_PI_2,  .theta_offset=0.0f,     .qmin=-1.9f,   .qmax=1.9f,  .m=0.21817f, .rc={ 0.00001750f,  0.00297341f,  0.05816899f},  .kp=15.0f,  .kd=0.0f},
-  //  {.theta=0.0f, .d=0.0f,       .a=0.1065f,   .alpha=-M_PI_2,  .theta_offset=M_PI_2,   .qmin=-1.9f,   .qmax=1.9f,  .m=0.21817f, .rc={ 0.00001750f,  0.00297341f,  0.05816899f},  .kp=15.0f,  .kd=0.0f},
+   {.theta=0.0f, .d=0.0f,       .a=0.1065f,   .alpha=-M_PI_2,  .theta_offset=M_PI_2,   .qmin=-1.9f,   .qmax=1.9f,  .m=0.21817f, .rc={ 0.00001750f,  0.00297341f,  0.05816899f},  .kp=5.0f,  .kd=0.0f},
   // J6: 末端Roll（DM电机，最小负载，roll轴，0到2π循环）  j6 rpy=(90,0,180)
   {.theta=0.0f, .d=0.0040025f, .a=0.0f,      .alpha=0.0f,     .theta_offset=M_PI,     .qmin=0.0f,     .qmax=6.3f,   .m=0.57513f, .rc={-0.00002134f,  0.09993500f,  0.00053860f},  .kp=5.0f,  .kd=0.0f},
 };
@@ -64,7 +65,7 @@ MotorLZ* motors_lz[3] = {nullptr};   // 用于调试查看
 MotorDM* motors_dm[3] = {nullptr};   // 用于调试查看
 Joint* joints[6] = {nullptr};
 RoboticArm* robot_arm = nullptr;
-
+Arm_CMD_t arm_cmd;  // 当前机械臂控制命令
 Arm6dof_Pose_t target_pose = {0};  // 控制输入：目标末端位姿，单位 m/rad（内部计算全链路SI单位）
 
 // ============================================================================
@@ -90,14 +91,24 @@ float gravity_comp_scale = 1.0f;
 // 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成
 float traj_progress_dbg = 0.0f;
 // 轨迹速度设置：可在调试器中修改这两个值，重启case4生效
-float traj_lin_vel = 0.15f;   // 末端线速度 (m/s)，默认 150mm/s
+float traj_lin_vel = 0.05f;   // 末端线速度 (m/s)，默认 150mm/s
-float traj_ang_vel = 1.0f;    // 末端角速度 (rad/s)，默认 ~57°/s
+float traj_ang_vel = 0.3f;    // 末端角速度 (rad/s)，默认 ~57°/s
 // 同步控制：设置为1时将target_pose同步为当前位姿（世界系，供MoveCartesian使用），同步后自动清零
 int sync_target_to_current = 0;
 // 航向系同步：设置为1时将target_pose同步为当前位姿的航向系表示（供MoveCartesianHeading使用），同步后自动清零
 // 位置：p_h = Rz(-yaw)*p_w
 // 姿态：R_h = Rz(-yaw)*R_cur 提取RPY，同步后直接修改 yaw/pitch/roll 即可直观控制末端各轴旋转
 int sync_heading_to_current = 0;
+
+// ============================================================================
+// IK测试调试变量
+// ============================================================================
+// ik_test_enable：设置为1时每帧调用一次IK测试；设置为0停止
+//   - 输入：target_pose（目标位姿）+ current_angles（当前角度作为初始猜测）
+//   - 结果：ik_test_result（IK解算出的关节角度），ik_test_ret（返回码：0=成功 -1=失败）
+int ik_test_enable = 0;
+Arm6dof_JointAngles_t ik_test_result = {0};  // IK解算结果（rad），调试器Watch观察
+int ik_test_ret = 0;                          // IK返回码：0=成功，-1=无解或未初始化
 // 工具系同步：设置为1时将target_pose同步为当前位姿的工具系表示（供MoveCartesianTool使用），同步后自动清零
 // 位置：p_tool = R_cur^T * p_world
 // 姿态：roll=pitch=yaw=0 即 R_target=I，机械臂保持当前末端姿态不变
@@ -136,12 +147,15 @@ void Task_arm_main(void* argument) {
   for (int i = 0; i < 6; ++i) {
       joints[i] = new Joint(i, motors[i], dh_params[i],  q_offset[i], ARM_MAIN_FREQ);
   }
+  // 关节3与关节2存在机械耦合：J3电机读数包含J2的运动，需减去J2角度才是真实J3角度
+  joints[2]->SetCoupledJoint(joints[1]);
+
   robot_arm = new RoboticArm();
   for (int i = 0; i < 6; ++i) {
       robot_arm->AddJoint(i, joints[i]);
   }
   robot_arm->Init();
-  robot_arm->Enable(true);
+  // robot_arm->Enable(true);
   
   // 使能重力补偿
   robot_arm->EnableGravityCompensation(true);
@@ -153,7 +167,9 @@ void Task_arm_main(void* argument) {
   // JOINT_POSITION: 关节位置控制 + 重力补偿前馈
   robot_arm->SetMode(ControlMode::GRAVITY_COMP);
 
+
   // 将 target_pose 初始化为当前末端位姿，防止 stage4 首次进入时进行大范围跳变
+  osDelay(100);
   robot_arm->Update();  // 先跨一帧读取当前状态
   target_pose = robot_arm->GetEndPose();
   
@@ -180,7 +196,13 @@ void Task_arm_main(void* argument) {
     // 更新机械臂状态（读取关节角度、FK、重力补偿计算）
     robot_arm->Update();
 
-    // 同步target到current（调试时手动触发）
+    osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);  
+    
+    if (!arm_cmd.enable) {
+      robot_arm->Enable(false);
+    }
+
+
     if (sync_target_to_current) {
       target_pose = robot_arm->GetEndPose();
       sync_target_to_current = 0;  // 自动清零
@@ -236,6 +258,9 @@ void Task_arm_main(void* argument) {
     end_pose_mm_dbg = { ep.x * 1000.0f, ep.y * 1000.0f, ep.z * 1000.0f,
                         ep.roll, ep.pitch, ep.yaw };
 
+      
+      osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
+
       switch (test_stage) {
         case 0:
           // 阶段1：仅计算，不输出。观察 gravity_torques_dbg[0~5] 是否量级合理
@@ -275,13 +300,25 @@ void Task_arm_main(void* argument) {
           // robot_arm->MoveCartesianTool(target_pose);
           // robot_arm->MoveCartesianHeading(target_pose);
           robot_arm->MoveCartesian(target_pose);
+          
+        
+          // robot_arm->StepTrajectory();
           robot_arm->Control();
+          // for (int i = 0; i < 6; ++i) motors[i]->Relax();
 
           ik_from_fk_result  = robot_arm->GetIkAngles();
           traj_progress_dbg  = robot_arm->GetTrajProgress();
           break;
       }
 
+    // IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
+    // 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret
+    if (ik_test_enable) {
+      ik_test_ret = Arm6dof_InverseKinematics(&target_pose, &current_angles,
+                                              &ik_test_result, 0.001f, 50);
+      ik_test_ret++;                                       
+    }
+
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
 } 

User/task/cmd.cpp

@@ -27,7 +27,7 @@ DR16_t cmd_dr16;
 AT9S_t cmd_at9s;
 #endif
 Chassis_CMD_t  *cmd_for_chassis;
-
+Arm_CMD_t *cmd_for_arm;
 static CMD_t cmd;
 /* USER STRUCT END */
 
@@ -57,16 +57,16 @@ void Task_cmd(void *argument) {
     osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0);
     #endif
 
-    /* 从CAN2接收AI命令 */
-
     CMD_Update(&cmd);
     
     /* 获取命令发送到各模块 */
     cmd_for_chassis = CMD_GetChassisCmd(&cmd);
-
+    cmd_for_arm = CMD_GetArmCmd(&cmd);
 	osMessageQueueReset(task_runtime.msgq.chassis.cmd);
     osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0);
-   /* USER CODE END */
+   
+    osMessageQueuePut(task_runtime.msgq.arm.cmd, cmd_for_arm, 0, 0);
+    /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
   

User/task/ctrl_chassis.c

@@ -37,25 +37,25 @@ void Task_ctrl_chassis(void *argument) {
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
 
-  // Config_RobotParam_t *cfg = Config_GetRobotParam();
+  Config_RobotParam_t *cfg = Config_GetRobotParam();
-  // Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ);
+  Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ);
-  // chassis.mech_zero=0.57f;
+  chassis.mech_zero=0.57f;
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-    // float encoder_gimbalYawMotor;
+    float encoder_gimbalYawMotor;
-    // osMessageQueueGet(task_runtime.msgq.chassis.yaw, &encoder_gimbalYawMotor, NULL, 0);
+    osMessageQueueGet(task_runtime.msgq.chassis.yaw, &encoder_gimbalYawMotor, NULL, 0);
-		// osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);
+		osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);
-    // chassis.feedback.encoder_gimbalYawMotor=encoder_gimbalYawMotor;
+    chassis.feedback.encoder_gimbalYawMotor=encoder_gimbalYawMotor;
     
-	  // Chassis_UpdateFeedback(&chassis);
+	  Chassis_UpdateFeedback(&chassis);
-    // Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
+    Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
 
-    // Chassis_Power_Control(&chassis,max);
+    Chassis_Power_Control(&chassis,max);
     
-    // Chassis_Output(&chassis);
+    Chassis_Output(&chassis);
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/init.c

@@ -9,7 +9,7 @@
 /* USER INCLUDE BEGIN */
 #include "device/dr16.h"
 #include "module/chassis.h"
-
+#include "component/arm_kinematics/arm6dof.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -36,7 +36,7 @@ void Task_Init(void *argument) {
   task_runtime.thread.arm_main = osThreadNew(Task_arm_main, NULL, &attr_arm_main);
   task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
   task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
-
+  task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
   // 创建消息队列
   /* USER MESSAGE BEGIN */
   task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
@@ -47,6 +47,7 @@ void Task_Init(void *argument) {
 #endif
   task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(float), NULL);
   task_runtime.msgq.chassis.cmd= osMessageQueueNew(3u, sizeof(Chassis_CMD_t), NULL);
+  task_runtime.msgq.arm.cmd= osMessageQueueNew(3u, sizeof(Arm_CMD_t), NULL);
 
   /* USER MESSAGE END */
 

User/task/rc.c

@@ -52,8 +52,8 @@ void Task_rc(void *argument) {
   AT9S_StartDmaRecv(cmd_buffer);
   #endif
   #ifdef DR16
-  // DR16_Init(&dr16);
+  DR16_Init(&dr16);
-  // DR16_StartDmaRecv(&dr16);  // 立即启动第一次接收
+  DR16_StartDmaRecv(&dr16);  // 立即启动第一次接收
   #endif
   /* USER CODE INIT END */
   
@@ -68,16 +68,16 @@ void Task_rc(void *argument) {
     osMessageQueuePut(task_runtime.msgq.cmd.rc, &at9s, 0, 0);
     #endif
     #ifdef DR16
-    // // 等待DMA接收完成
+    // 等待DMA接收完成
-    // if (DR16_WaitDmaCplt(20)) {
+    if (DR16_WaitDmaCplt(20)) {
-    //   DR16_ParseData(&dr16);
+      DR16_ParseData(&dr16);
-    // } else {
+    } else {
-    //   DR16_Offline(&dr16);
+      DR16_Offline(&dr16);
-    // }
+    }
-    // osMessageQueueReset(task_runtime.msgq.cmd.rc);
+    osMessageQueueReset(task_runtime.msgq.cmd.rc);
-    // osMessageQueuePut(task_runtime.msgq.cmd.rc, &dr16, 0, 0);
+    osMessageQueuePut(task_runtime.msgq.cmd.rc, &dr16, 0, 0);
-    // // 启动下一次DMA接收
+    // 启动下一次DMA接收
-    // DR16_StartDmaRecv(&dr16);
+    DR16_StartDmaRecv(&dr16);
     #endif
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

User/task/user_task.c

@@ -27,7 +27,7 @@ const osThreadAttr_t attr_rc = {
 const osThreadAttr_t attr_ctrl_chassis = {
     .name = "ctrl_chassis",
     .priority = osPriorityNormal,
-    .stack_size = 512 * 16,
+    .stack_size = 512 * 4,
 };
 const osThreadAttr_t attr_cmd = {
     .name = "cmd",

User/task/user_task.h

@@ -49,17 +49,20 @@ typedef struct {
     /* USER MESSAGE BEGIN */
     struct {
         osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
-        		struct {
+        struct {
-            osMessageQueueId_t imu;
+          osMessageQueueId_t imu;
-            osMessageQueueId_t cmd;
+          osMessageQueueId_t cmd;
-            osMessageQueueId_t yaw;
+          osMessageQueueId_t yaw;
         }chassis;
-        		struct{
+        struct{
-			osMessageQueueId_t rc; 
+			    osMessageQueueId_t rc; 
-            osMessageQueueId_t pc; 
+          osMessageQueueId_t pc; 
-            osMessageQueueId_t nuc; 
+          osMessageQueueId_t nuc; 
-            osMessageQueueId_t ref; 
+          osMessageQueueId_t ref; 
         }cmd;
+        struct{
+			    osMessageQueueId_t cmd; 
+        }arm;
     } msgq;
     /* USER MESSAGE END */
 

arm.ioc

@@ -209,8 +209,11 @@ TIM5.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
 TIM5.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Period,Prescaler
 TIM5.Period=65535
 TIM5.Prescaler=0
-USART3.IPParameters=VirtualMode
+USART3.BaudRate=100000
+USART3.IPParameters=VirtualMode,BaudRate,WordLength,Parity
+USART3.Parity=PARITY_EVEN
 USART3.VirtualMode=VM_ASYNC
+USART3.WordLength=WORDLENGTH_9B
 VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
 VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
 VP_SYS_VS_Systick.Mode=SysTick

ozone/arm.jdebug.user

@@ -1,14 +1,27 @@
 
 
 
-Breakpoint=D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp:264, State=BP_STATE_ON
+Breakpoint=D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp:290, State=BP_STATE_ON
-Breakpoint=D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp:280, State=BP_STATE_ON
+GraphedExpression="ik_test_ret", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f, Show=0
+OpenDocument="robotics.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/robotics.h", Line=309
+OpenDocument="arm_main.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/arm_main.cpp", Line=265
+OpenDocument="memcpy.c", FilePath="/build/gnu-tools-for-stm32_13.3.rel1.20250523-0900/src/newlib/newlib/libc/string/memcpy.c", Line=0
+OpenDocument="stm32f4xx_it.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/stm32f4xx_it.c", Line=77
+OpenDocument="init.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/init.c", Line=6
+OpenDocument="ctrl_chassis.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/ctrl_chassis.c", Line=6
+OpenDocument="cmd.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/cmd.cpp", Line=22
+OpenDocument="cmd.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/cmd/cmd.c", Line=278
+OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32Projects_HAL/board-F4/arm/startup_stm32f407xx.s", Line=48
+OpenDocument="arm_oop.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp", Line=266
+OpenDocument="motor_base.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/motor_base.hpp", Line=121
+OpenDocument="joint.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/joint.hpp", Line=29
+OpenDocument="robotics.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/robotics.cpp", Line=251
 OpenDocument="arm6dof.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/arm_kinematics/arm6dof.h", Line=0
 OpenDocument="can.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/can.c", Line=0
-OpenDocument="arm_oop.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp", Line=242
+OpenDocument="arm6dof.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/arm_kinematics/arm6dof.cpp", Line=175
-OpenDocument="arm_main.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/arm_main.cpp", Line=139
+OpenDocument="matrix.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/matrix.h", Line=24
-OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32Projects_HAL/board-F4/arm/startup_stm32f407xx.s", Line=39
 OpenDocument="main.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/main.c", Line=45
+OpenDocument="rc.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/rc.c", Line=20
 OpenToolbar="Debug", Floating=0, x=0, y=0
 OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=2, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
@@ -17,17 +30,17 @@ OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=463, h=544, FilterBarShown=0
 OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="928;117", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1143;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1164;0"
+OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="201;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1143;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1164;0"
 OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
-TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[250;229;145;128;125]
+TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;639]
-TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;613]
+TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[250;229;145;279;100]
 TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Class";"Source"], ColWidths=[1594;104;100;100;27;100]
 TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
 TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
 TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1334]
-TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ik_test_ret"], ColWidths=[100;100;100]
-TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;134;100;134;154;110;126;134]
+TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;100;100;100;100;110;126;126]
 TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
 WatchedExpression="robot_arm", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="ik_from_fk_result", RefreshRate=5, Window=Watched Data 1
@@ -35,3 +48,9 @@ WatchedExpression="target_pose", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="current_angles", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="setzero", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="sync_target_to_current", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="sync_heading_to_current", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="sync_tool_to_current", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="cmd", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="ik_test_enable", RefreshRate=1, Window=Watched Data 1
+WatchedExpression="ik_test_result", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="ik_test_ret", RefreshRate=5, Window=Watched Data 1