diff --git a/CMakeLists.txt b/CMakeLists.txt
index f327827..2e2822f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -79,6 +79,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/module/config.c
     User/module/arm.cpp
     User/module/chassis.cpp
+    User/module/dr16.c
 
     # User/task sources
     User/task/atti_esit.c
diff --git a/User/device/motor_dm.c b/User/device/motor_dm.c
index ca3a814..4d26145 100644
--- a/User/device/motor_dm.c
+++ b/User/device/motor_dm.c
@@ -81,18 +81,18 @@ static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data
     motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12);  // (-12.0,12.0)
     motor->motor.feedback.temp = (float)(data[6]);
 
-    while (motor->motor.feedback.rotor_abs_angle < 0) {
-        motor->motor.feedback.rotor_abs_angle += M_2PI;
-    }
-    while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
-        motor->motor.feedback.rotor_abs_angle -= M_2PI;
-    }
+    // while (motor->motor.feedback.rotor_abs_angle < 0) {
+    //     motor->motor.feedback.rotor_abs_angle += M_2PI;
+    // }
+    // while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
+    //     motor->motor.feedback.rotor_abs_angle -= M_2PI;
+    // }
 
-    if (motor->param.reverse) {
-        motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
-        motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
-        motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
-    }
+    // if (motor->param.reverse) {
+    //     motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
+    //     motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
+    //     motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
+    // }
     return DEVICE_OK;
 }
 
diff --git a/User/device/motor_lz.c b/User/device/motor_lz.c
index aed92fd..73e39fe 100644
--- a/User/device/motor_lz.c
+++ b/User/device/motor_lz.c
@@ -204,18 +204,18 @@ static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
     uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
     float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
 
-    while (angle <0){
-        angle += M_2PI;
-    }
-    while (angle > M_2PI){
-        angle -= M_2PI;
-    }
-    // 自动反向
-    if (motor->param.reverse) {
-        angle = M_2PI - angle;
-        velocity = -velocity;
-        torque = -torque;
-    }
+    // while (angle <0){
+    //     angle += M_2PI;
+    // }
+    // while (angle > M_2PI){
+    //     angle -= M_2PI;
+    // }
+    // // 自动反向
+    // if (motor->param.reverse) {
+    //     angle = M_2PI - angle;
+    //     velocity = -velocity;
+    //     torque = -torque;
+    // }
 
     motor->lz_feedback.current_angle = angle;
     motor->lz_feedback.current_velocity = velocity;
diff --git a/User/module/arm.cpp b/User/module/arm.cpp
index 543f513..979a97a 100644
--- a/User/module/arm.cpp
+++ b/User/module/arm.cpp
@@ -1,7 +1,27 @@
 #include "module/arm.hpp"
+#include "component/pid.h"
+#include "module/dr16.h"
+#include "task/user_task.h"
 #include "bsp/time.h"
+#define STEP 0.002f
+#define STEP_DE 0.002f
+
+//限位定义
+#define AXIS_1_MIN_ANGLE -6.0f
+#define AXIS_1_MAX_ANGLE 6.0f
+#define AXIS_2_MIN_ANGLE -4.0f
+#define AXIS_2_MAX_ANGLE 4.0f
+#define AXIS_3_MIN_ANGLE -2.0f
+#define AXIS_3_MAX_ANGLE 2.0f
+#define AXIS_4_MIN_ANGLE -1.0f
+#define AXIS_4_MAX_ANGLE 1.0f
+#define AXIS_5_MIN_ANGLE -0.5ff
+#define AXIS_5_MAX_ANGLE 0.5f
+#define AXIS_6_MIN_ANGLE -0.25f
+#define AXIS_6_MAX_ANGLE 0.25f
 
 Arm::Arm(){
+    //dm电机参数设置
     dm_motor_params[0] = MOTOR_DM_Param_t{
         .can = BSP_CAN_1,
         .master_id = 0x14,
@@ -23,6 +43,7 @@ Arm::Arm(){
         .module = MOTOR_DM_J4310,
         .reverse = false
     };
+    //lz电机参数设置
     lz_motor_params[0] = MOTOR_LZ_Param_t{
         .can = BSP_CAN_1,
         .motor_id = 127,
@@ -47,40 +68,223 @@ Arm::Arm(){
         .reverse = false,
         .mode = MOTOR_LZ_MODE_MOTION
     };
+    //dm电机控制参数初始化
+    for (int i = 0; i < 3; i++) {
+        arm_dm_output[i].velocity = 0;
+        arm_dm_output[i].kp = 0;
+        arm_dm_output[i].kd = 0.8;
+        arm_dm_output[i].angle = 0;
+        arm_dm_output[i].torque = 0;
+    }
+    //lz电机控制参数初始化
+    for (int i = 0; i < 3; i++) {
+        arm_lz_output[i].target_velocity = 0;
+        arm_lz_output[i].kp = 0;
+        arm_lz_output[i].kd = 0;
+        arm_lz_output[i].target_angle = 0;
+        arm_lz_output[i].torque = 0;
+    }
+
+    pid_params[0] = KPID_Params_t{
+        .k=13,
+        .p=1.5,
+        .i=0,
+        .d=0,
+        .i_limit=0,
+        .out_limit=100,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_params[1] = KPID_Params_t{
+        .k=13,
+        .p=1.5,
+        .i=0,
+        .d=0,
+        .i_limit=0,
+        .out_limit=100,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_params[2] = KPID_Params_t{
+        .k=13,
+        .p=1.5,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_params[3] = KPID_Params_t{
+        .k=4.5,
+        .p=1,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_params[4] = KPID_Params_t{
+        .k=5.5,
+        .p=1,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=10,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_params[5] = KPID_Params_t{
+        .k=5,
+        .p=1,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_v_params[0] = KPID_Params_t{
+        .k=8,
+        .p=1,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_v_params[1] = KPID_Params_t{
+        .k=8,
+        .p=1,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_v_params[2] = KPID_Params_t{
+        .k=8,
+        .p=1,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=20,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_v_params[3] = KPID_Params_t{
+        .k=2,
+        .p=0.5,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=12,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_v_params[4] = KPID_Params_t{
+        .k=2.5,
+        .p=0.5,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=12,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_v_params[5] = KPID_Params_t{
+        .k=2.5,
+        .p=0.5,
+        .i=0,
+        .d=0,
+        .i_limit=10,
+        .out_limit=12,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
 }
 
 void Arm::init(){
+    for(int i = 0; i < 6; i++){
+        PID_Init(&pid[i], KPID_MODE_NO_D, CTRL_ARM_FREQ, &pid_params[i]);
+        PID_Init(&pid_v[i], KPID_MODE_NO_D, CTRL_ARM_FREQ, &pid_v_params[i]);
+    }
     MOTOR_LZ_Init();
-    MOTOR_LZ_Register(&lz_motor_params[0]);
-    MOTOR_LZ_Enable(&lz_motor_params[0]);
-    BSP_TIME_Delay(2);
-    MOTOR_LZ_Register(&lz_motor_params[1]);
-    MOTOR_LZ_Enable(&lz_motor_params[1]);
-    BSP_TIME_Delay(2);
-    MOTOR_LZ_Register(&lz_motor_params[2]);
-    MOTOR_LZ_Enable(&lz_motor_params[2]);
-    BSP_TIME_Delay(2);
+    for (int i = 0; i < 3; i++) {
+        MOTOR_LZ_Register(&lz_motor_params[i]);
+        MOTOR_LZ_Enable(&lz_motor_params[i]);
+    }
 
-    MOTOR_DM_Register(&dm_motor_params[0]);
-    MOTOR_DM_Enable(&dm_motor_params[0]);
-    BSP_TIME_Delay(2);
-    MOTOR_DM_Register(&dm_motor_params[1]);
-    MOTOR_DM_Enable(&dm_motor_params[1]);
-    BSP_TIME_Delay(2);
-    MOTOR_DM_Register(&dm_motor_params[2]);
-    MOTOR_DM_Enable(&dm_motor_params[2]);
-    BSP_TIME_Delay(2);
+    for (int i = 0; i < 3; i++) {
+        MOTOR_DM_Register(&dm_motor_params[i]);
+        MOTOR_DM_Enable(&dm_motor_params[i]);
+    }
+    
+    BSP_TIME_Delay(1000); //等待电机上线
+    read_adta();
+    joint_angle[0] = arm_lz_motor[0].lz_feedback.current_angle;
+    joint_angle[1] = arm_lz_motor[1].lz_feedback.current_angle;
+    joint_angle[2] = arm_lz_motor[2].lz_feedback.current_angle;
+    joint_angle[3] = arm_dm_motor[0].motor.feedback.rotor_abs_angle;
+    joint_angle[4] = arm_dm_motor[1].motor.feedback.rotor_abs_angle;
+    joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
+    arm_relax();
+    BSP_TIME_Delay(1000); //等待电机上线
+    read_adta();
+    joint_angle[0] = arm_lz_motor[0].lz_feedback.current_angle;
+    joint_angle[1] = arm_lz_motor[1].lz_feedback.current_angle;
+    joint_angle[2] = arm_lz_motor[2].lz_feedback.current_angle;
+    joint_angle[3] = arm_dm_motor[0].motor.feedback.rotor_abs_angle;
+    joint_angle[4] = arm_dm_motor[1].motor.feedback.rotor_abs_angle;
+    joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
+    arm_relax();
+    last_time = BSP_TIME_Get_us();
+    // MOTOR_LZ_SetZero(&lz_motor_params[2]);
+    // BSP_TIME_Delay(2);
+    // MOTOR_LZ_SetZero(&lz_motor_params[1]);
+    // BSP_TIME_Delay(2);
 
 }
 
 void Arm::operator()(){
-    MOTOR_DM_Relax(&dm_motor_params[0]);
-    BSP_TIME_Delay(2);
-    MOTOR_DM_Relax(&dm_motor_params[1]);
-    BSP_TIME_Delay(2);
-    MOTOR_DM_Relax(&dm_motor_params[2]);
-    BSP_TIME_Delay(2);
+    last_time = time;
+    time = BSP_TIME_Get_us();
+    dt = time - last_time;
+    // arm_relax();
+    //读取反馈数据
+    read_adta();
+    //根据遥控器计算关节角度
+    calc_motor_angle();
+    //根据关节角度计算电机角度
+    // calc_motor_angle();
+    calc_pid();
+    //发送控制命令
+    send_control();
+    // arm_relax();
+    // //控制数据
+    // for (int i = 1; i < 3; i++) {
+    //     arm_dm_output[i].angle = arm_dm_motor[i].motor.feedback.rotor_abs_angle + STEP;
+    //     MOTOR_DM_MITCtrl(&dm_motor_params[i], &arm_dm_output[i]);
+    // }
 
+    // arm_lz_output[0].target_angle = arm_lz_motor[0].motor.feedback.rotor_abs_angle + STEP;
+    // MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);
+}
+
+void Arm::arm_relax(){
+    MOTOR_DM_Relax(&dm_motor_params[0]);
+    MOTOR_DM_Relax(&dm_motor_params[1]);
+    MOTOR_DM_Relax(&dm_motor_params[2]);
+    MOTOR_LZ_Relax(&lz_motor_params[0]);
+    MOTOR_LZ_Relax(&lz_motor_params[1]);        
+    MOTOR_LZ_Relax(&lz_motor_params[2]);
+}
+
+void Arm::read_adta(){
+   //读取dm数据
     MOTOR_DM_UpdateAll();
 
     MOTOR_DM_t *motor_dm = NULL;
@@ -90,20 +294,7 @@ void Arm::operator()(){
     arm_dm_motor[1] = *motor_dm;
     motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[2]);
     arm_dm_motor[2] = *motor_dm;
-
-    arm_output[0].angle = 1.0f;
-    arm_output[0].velocity = 1.0f;
-    arm_output[0].kp = 0.5f;
-    arm_output[0].kd = 1.0f;
-    // MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_output[0]);
-
-    MOTOR_LZ_Relax(&lz_motor_params[0]);
-    BSP_TIME_Delay(2);
-    MOTOR_LZ_Relax(&lz_motor_params[1]);
-    BSP_TIME_Delay(2);
-    MOTOR_LZ_Relax(&lz_motor_params[2]);
-    BSP_TIME_Delay(2);
-
+    //读取lz数据
     MOTOR_LZ_UpdateAll();
 
     MOTOR_LZ_t *motor_lz = NULL;
@@ -113,10 +304,140 @@ void Arm::operator()(){
     arm_lz_motor[1] = *motor_lz;
     motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[2]);
     arm_lz_motor[2] = *motor_lz;
-    // arm_lz_motion[0].target_angle = 1.0f;
-    arm_lz_motion[0].target_velocity = 1.0f;
-    // arm_lz_motion[0].kp = 0.5f;
-    arm_lz_motion[0].kd = 0.5f;
-    // MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_motion[0]);
 
+    current_pos[0] = arm_lz_motor[0].lz_feedback.current_angle;
+    current_pos[1] = arm_lz_motor[1].lz_feedback.current_angle;
+    current_pos[2] = arm_lz_motor[2].lz_feedback.current_angle;
+    current_pos[3] = arm_dm_motor[0].motor.feedback.rotor_abs_angle;
+    current_pos[4] = arm_dm_motor[1].motor.feedback.rotor_abs_angle;
+    current_pos[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
+    current_vel[0] = arm_lz_motor[0].lz_feedback.current_velocity;
+    current_vel[1] = arm_lz_motor[1].lz_feedback.current_velocity;
+    current_vel[2] = arm_lz_motor[2].lz_feedback.current_velocity;
+    current_vel[3] = arm_dm_motor[0].motor.feedback.rotor_speed;
+    current_vel[4] = arm_dm_motor[1].motor.feedback.rotor_speed;
+    current_vel[5] = arm_dm_motor[2].motor.feedback.rotor_speed;
+}
+
+void Arm::calc_joint_angle(){
+    
+}
+
+void Arm::calc_motor_angle(){
+    if(dr16.data.sw_r == DR16_SW_MID){
+        if(dr16.data.ch_r_x > 0.4f){
+            joint_angle[0] = joint_angle[0] + dr16.data.ch_r_x * STEP;
+        }else if(dr16.data.ch_r_x < -0.4f){
+            joint_angle[0] = joint_angle[0] + dr16.data.ch_r_x * STEP_DE;
+        }else{
+            joint_angle[0] = joint_angle[0];  // 保持当前角度
+        }
+        
+        if(dr16.data.ch_r_y > 0.4f){
+            joint_angle[1] = joint_angle[1] - dr16.data.ch_r_y * STEP_DE;
+            joint_angle[2] = joint_angle[2] + dr16.data.ch_r_y * STEP;
+        }else if(dr16.data.ch_r_y < -0.4f){
+            joint_angle[1] = joint_angle[1] - dr16.data.ch_r_y * STEP;
+            joint_angle[2] = joint_angle[2] + dr16.data.ch_r_y * STEP_DE;
+        }else{
+            joint_angle[1] = joint_angle[1];  // 保持当前角度
+            joint_angle[2] = joint_angle[2];  // 保持当前角度
+        }
+        
+        if(dr16.data.ch_l_y > 0.4f){
+            joint_angle[2] = joint_angle[2] - dr16.data.ch_l_y * STEP_DE;
+        }else if(dr16.data.ch_l_y < -0.4f){
+            joint_angle[2] = joint_angle[2] - dr16.data.ch_l_y * STEP;
+        }else{
+            joint_angle[2] = joint_angle[2];
+        }
+        
+        if(dr16.data.ch_l_x > 0.4f){
+            joint_angle[3] = joint_angle[3] + dr16.data.ch_l_x * STEP;
+        }else if(dr16.data.ch_l_x < -0.4f){
+            joint_angle[3] = joint_angle[3] + dr16.data.ch_l_x * STEP_DE;
+        }else{
+            joint_angle[3] = joint_angle[3];  // 保持当前角度
+        }
+        joint_angle[4] = joint_angle[4];  // 保持当前角度
+        joint_angle[5] = joint_angle[5];  // 保持当前角度
+    
+    }else if(dr16.data.sw_r == DR16_SW_UP){
+        if(dr16.data.ch_r_y > 0.4f){
+            joint_angle[4] = joint_angle[4] + dr16.data.ch_r_y * STEP;
+        }else if(dr16.data.ch_r_y < -0.4f){
+            joint_angle[4] = joint_angle[4] + dr16.data.ch_r_y * STEP_DE;
+        }else{
+            joint_angle[4] = joint_angle[4];  // 保持当前角度
+        }
+        
+        if(dr16.data.ch_r_x > 0.4f){
+            joint_angle[5] = joint_angle[5] + dr16.data.ch_r_x * STEP;
+        }else if(dr16.data.ch_r_x < -0.4f){
+            joint_angle[5] = joint_angle[5] + dr16.data.ch_r_x * STEP_DE;
+        }else{
+            joint_angle[5] = joint_angle[5];  // 保持当前角度
+        }
+        joint_angle[0] = joint_angle[0];  // 保持当前角度
+        joint_angle[1] = joint_angle[1];  // 保持当前角度
+        joint_angle[2] = joint_angle[2];  // 保持当前角度
+        joint_angle[3] = joint_angle[3];  // 保持当前角度
+    }else {
+        joint_angle[0] = joint_angle[0];  // 保持当前角度
+        joint_angle[1] = joint_angle[1];  // 保持当前角度
+        joint_angle[2] = joint_angle[2];  // 保持当前角度
+        joint_angle[3] = joint_angle[3];  // 保持当前角度
+        joint_angle[4] = joint_angle[4];  // 保持当前角度
+        joint_angle[5] = joint_angle[5];  // 保持当前角度
+    }
+}
+
+void Arm::calc_pid(){
+    float delta = 0;
+    for(int i = 0; i < 6; i++){
+        delta = PID_Calc(&pid[i], joint_angle[i], current_pos[i],current_vel[i], dt/1000000.0f);
+        output[i] = PID_Calc(&pid_v[i], delta, current_vel[i],0, dt/1000000.0f);
+    }
+    
+    arm_lz_output[0].torque = output[0];
+    arm_lz_output[1].torque = output[1];
+    arm_lz_output[2].torque = output[2];
+    arm_dm_output[0].torque = output[3];
+    arm_dm_output[1].torque = output[4];
+    arm_dm_output[2].torque = output[5];
+}
+
+void Arm::send_control(){
+    MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);
+    MOTOR_LZ_MotionControl(&lz_motor_params[1], &arm_lz_output[1]);
+    MOTOR_LZ_MotionControl(&lz_motor_params[2], &arm_lz_output[2]);
+    MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_dm_output[0]);
+    MOTOR_DM_MITCtrl(&dm_motor_params[1], &arm_dm_output[1]);
+    MOTOR_DM_MITCtrl(&dm_motor_params[2], &arm_dm_output[2]);
+    
+    // if(dr16.data.sw_r == DR16_SW_MID){
+    //     if(dr16.data.ch_r_x >0.4f || dr16.data.ch_r_x < -0.4f){
+    //         MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);
+
+    //     }if(dr16.data.ch_r_y >0.4f || dr16.data.ch_r_y < -0.4f){
+    //         MOTOR_LZ_MotionControl(&lz_motor_params[1], &arm_lz_output[1]);
+
+    //     }if(dr16.data.ch_l_y > 0.4f || dr16.data.ch_l_y < -0.4f || dr16.data.ch_r_y > 0.4f || dr16.data.ch_r_y < -0.4f){
+    //         MOTOR_LZ_MotionControl(&lz_motor_params[2], &arm_lz_output[2]);
+
+    //     }if(dr16.data.ch_l_x >0.4f || dr16.data.ch_l_x < -0.4f){
+            
+    //         MOTOR_DM_MITCtrl(&dm_motor_params[0], &arm_dm_output[0]);
+    //     }
+    // }else if(dr16.data.sw_r == DR16_SW_UP){
+    //     if(dr16.data.ch_r_y >0.4f || dr16.data.ch_r_y < -0.4f){
+            
+    //         MOTOR_DM_MITCtrl(&dm_motor_params[1], &arm_dm_output[1]);
+
+    //     }if(dr16.data.ch_r_x >0.4f || dr16.data.ch_r_x < -0.4f){
+               
+    //         MOTOR_DM_MITCtrl(&dm_motor_params[2], &arm_dm_output[2]);
+
+    //     }
+    // }
 }
diff --git a/User/module/arm.hpp b/User/module/arm.hpp
index bc379a4..5103d5f 100644
--- a/User/module/arm.hpp
+++ b/User/module/arm.hpp
@@ -6,7 +6,7 @@
 #include "device/motor_lz.h"
 #include "device/motor_rm.h"
 #include "device/motor_dm.h"
-#include <stdint.h>
+#include "component/pid.h"
 
 class Arm {
 public:
@@ -14,11 +14,29 @@ public:
     void init();
     void operator()();
 private:
+    void read_adta();
+    void calc_joint_angle();
+    void calc_motor_angle();
+    void send_control();
+    void arm_relax();
+    void calc_pid();
+    float joint_angle[6] = {0}; //关节角度
+    // float motor_angle[6] = {0}; //电机角度
     MOTOR_DM_Param_t dm_motor_params[3];
     MOTOR_DM_t arm_dm_motor[3];
     MOTOR_LZ_Param_t lz_motor_params[3];
     MOTOR_LZ_t arm_lz_motor[3];
-    MOTOR_LZ_MotionParam_t arm_lz_motion[3];
-    MOTOR_MIT_Output_t arm_output[3];
+    MOTOR_LZ_MotionParam_t arm_lz_output[3];
+    MOTOR_MIT_Output_t arm_dm_output[3];
+    KPID_Params_t pid_params[6];
+    KPID_Params_t pid_v_params[6];
+    float output[6] = {0};
+    KPID_t pid[6];
+    KPID_t pid_v[6];
+    float current_pos[6];
+    float current_vel[6];
+    uint64_t time = 0;
+    uint64_t last_time = 0;
+    uint64_t dt = 0;
 
 };
\ No newline at end of file
diff --git a/User/module/dr16.c b/User/module/dr16.c
new file mode 100644
index 0000000..517b983
--- /dev/null
+++ b/User/module/dr16.c
@@ -0,0 +1,16 @@
+#include "module/dr16.h"
+
+DR16_t dr16;
+
+void dr16_module_init(){
+    DR16_Init(&dr16);
+}
+
+void dr16_module_update(){
+    DR16_StartDmaRecv(&dr16);
+    if (DR16_WaitDmaCplt(20)) {
+      DR16_ParseData(&dr16);
+    } else {
+      DR16_Offline(&dr16);
+    }
+}
diff --git a/User/module/dr16.h b/User/module/dr16.h
new file mode 100644
index 0000000..ee45cfe
--- /dev/null
+++ b/User/module/dr16.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "main.h"
+#include "device/dr16.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern DR16_t dr16;
+
+void dr16_module_init();
+void dr16_module_update();
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/User/task/rc.c b/User/task/rc.c
index 5674578..9816b3a 100644
--- a/User/task/rc.c
+++ b/User/task/rc.c
@@ -6,7 +6,7 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-#include "device/dr16.h"
+#include "module/dr16.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -14,7 +14,7 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-DR16_t dr16;
+
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -30,18 +30,13 @@ void Task_rc(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
-  DR16_Init(&dr16);
+  dr16_module_init();
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-    DR16_StartDmaRecv(&dr16);
-    if (DR16_WaitDmaCplt(20)) {
-      DR16_ParseData(&dr16);
-    } else {
-      DR16_Offline(&dr16);
-    }
+    dr16_module_update();
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }