From ce08cb4d4460bbfc48a50e5e49b35685dc8cbced Mon Sep 17 00:00:00 2001 From: Feelyx Date: Wed, 14 Jan 2026 21:02:56 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=BA=E4=BB=80=E4=B9=88=E8=87=82=E5=8F=88?= =?UTF-8?q?=E6=AD=BB=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/motor_rm.c | 22 +++++++------- User/module/arm.cpp | 69 +++++++++++++++++++++++++++++++----------- User/module/arm.hpp | 15 +++++---- 3 files changed, 71 insertions(+), 35 deletions(-) diff --git a/User/device/motor_rm.c b/User/device/motor_rm.c index 88c1ef4..4387b10 100644 --- a/User/device/motor_rm.c +++ b/User/device/motor_rm.c @@ -139,17 +139,17 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) { motor->feedback.rotor_speed = rotor_speed; motor->feedback.torque_current = torque_current; } - while (motor->feedback.rotor_abs_angle < 0) { - motor->feedback.rotor_abs_angle += M_2PI; - } - while (motor->feedback.rotor_abs_angle >= M_2PI) { - motor->feedback.rotor_abs_angle -= M_2PI; - } - if (motor->motor.reverse) { - motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; - motor->feedback.rotor_speed = -motor->feedback.rotor_speed; - motor->feedback.torque_current = -motor->feedback.torque_current; - } + // while (motor->feedback.rotor_abs_angle < 0) { + // motor->feedback.rotor_abs_angle += M_2PI; + // } + // while (motor->feedback.rotor_abs_angle >= M_2PI) { + // motor->feedback.rotor_abs_angle -= M_2PI; + // } + // if (motor->motor.reverse) { + // motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; + // motor->feedback.rotor_speed = -motor->feedback.rotor_speed; + // motor->feedback.torque_current = -motor->feedback.torque_current; + // } motor->feedback.temp = msg->data[6]; } diff --git a/User/module/arm.cpp b/User/module/arm.cpp index fbd0dc1..3c9c811 100644 --- a/User/module/arm.cpp +++ b/User/module/arm.cpp @@ -1,5 +1,7 @@ #include "module/arm.hpp" #include "component/pid.h" +#include "device/dr16.h" +#include "device/motor_rm.h" #include "module/dr16.h" #include "task/user_task.h" #include "bsp/time.h" @@ -20,8 +22,8 @@ #define AXIS_6_MIN_ANGLE -0.25f #define AXIS_6_MAX_ANGLE 0.25f -#define DJI_MOTOR_OPEN 60 -#define DJI_MOTOR_CLOSE 0 +#define DJI_MOTOR_OPEN 0.2f +#define DJI_MOTOR_CLOSE 1.37f Arm::Arm(){ //dm电机参数设置 @@ -217,22 +219,22 @@ Arm::Arm(){ }; pid_params[6] = KPID_Params_t{ .k=1, - .p=0.2, + .p=3, .i=0, .d=0, - .i_limit=0.5, - .out_limit=1, - .d_cutoff_freq=100, + .i_limit=2, + .out_limit=50, + .d_cutoff_freq=-1.0f, .range = -1, }; pid_v_params[6] = KPID_Params_t{ - .k=1, - .p=0.2, + .k=0.2, + .p=0.15, .i=0, .d=0, .i_limit=0.5, .out_limit=1, - .d_cutoff_freq=100, + .d_cutoff_freq=-1, .range = -1, }; } @@ -274,6 +276,7 @@ void Arm::init(){ joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle; arm_relax(); last_time = BSP_TIME_Get_us(); + last_sw_l = dr16.data.sw_l; // MOTOR_LZ_SetZero(&lz_motor_params[2]); // BSP_TIME_Delay(2); // MOTOR_LZ_SetZero(&lz_motor_params[1]); @@ -294,16 +297,17 @@ void Arm::operator()(){ // calc_motor_angle(); calc_pid(); //发送控制命令 + if(dr16.data.sw_l == DR16_SW_DOWN){ // 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]); + arm_relax(); + }else { + send_control(); + } + if(last_sw_l != dr16.data.sw_l && last_sw_l != DR16_SW_UP){ + arm_enable(); + } + last_sw_l = dr16.data.sw_l; + } void Arm::arm_relax(){ @@ -313,6 +317,8 @@ void Arm::arm_relax(){ MOTOR_LZ_Relax(&lz_motor_params[0]); MOTOR_LZ_Relax(&lz_motor_params[1]); MOTOR_LZ_Relax(&lz_motor_params[2]); + MOTOR_RM_Relax(&motor_dji_param); + MOTOR_RM_Ctrl(&motor_dji_param); } void Arm::arm_enable(){ @@ -322,6 +328,15 @@ void Arm::arm_enable(){ MOTOR_LZ_Enable(&lz_motor_params[0]); MOTOR_LZ_Enable(&lz_motor_params[1]); MOTOR_LZ_Enable(&lz_motor_params[2]); + BSP_TIME_Delay(2); + read_data(); + 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(); } void Arm::read_data(){ @@ -364,6 +379,8 @@ void Arm::read_data(){ 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; + current_pos[6] = arm_motor_dji.feedback.rotor_abs_angle; + current_vel[6] = arm_motor_dji.feedback.rotor_speed; } void Arm::calc_joint_angle(){ @@ -437,6 +454,12 @@ void Arm::calc_motor_angle(){ joint_angle[4] = joint_angle[4]; // 保持当前角度 joint_angle[5] = joint_angle[5]; // 保持当前角度 } + if(dr16.data.sw_l == DR16_SW_UP){ + joint_angle[6] = DJI_MOTOR_CLOSE; + }else { + joint_angle[6] = DJI_MOTOR_OPEN; + } + } void Arm::calc_pid(){ @@ -446,12 +469,20 @@ void Arm::calc_pid(){ output[i] = PID_Calc(&pid_v[i], delta, current_vel[i],0, dt/1000000.0f); } + if(dr16.data.sw_l == DR16_SW_UP){ + output[6] = 0.2f; + }else if(dr16.data.sw_l == DR16_SW_MID){ + output[6] = -0.2f; + }else { + output[6] = 0.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]; + MOTOR_RM_SetOutput(&motor_dji_param, output[6]); } void Arm::send_control(){ @@ -462,5 +493,7 @@ void Arm::send_control(){ 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]); + MOTOR_RM_SetOutput(&motor_dji_param, output[6]); + MOTOR_RM_Ctrl(&motor_dji_param); } diff --git a/User/module/arm.hpp b/User/module/arm.hpp index 241ce34..cccd458 100644 --- a/User/module/arm.hpp +++ b/User/module/arm.hpp @@ -7,6 +7,7 @@ #include "device/motor_rm.h" #include "device/motor_dm.h" #include "component/pid.h" +#include class Arm { public: @@ -21,7 +22,8 @@ private: void arm_relax(); void arm_enable(); void calc_pid(); - float joint_angle[6] = {0}; //关节角度 + void change_mode(); + float joint_angle[7] = {0}; //关节角度 // float motor_angle[6] = {0}; //电机角度 MOTOR_DM_Param_t dm_motor_params[3]; MOTOR_DM_t arm_dm_motor[3]; @@ -31,14 +33,15 @@ private: MOTOR_MIT_Output_t arm_dm_output[3]; KPID_Params_t pid_params[7]; KPID_Params_t pid_v_params[7]; - float output[6] = {0}; - KPID_t pid[6]; - KPID_t pid_v[6]; - float current_pos[6]; - float current_vel[6]; + float output[7] = {0}; + KPID_t pid[7]; + KPID_t pid_v[7]; + float current_pos[7]; + float current_vel[7]; uint64_t time = 0; uint64_t last_time = 0; uint64_t dt = 0; MOTOR_RM_Param_t motor_dji_param; MOTOR_RM_t arm_motor_dji; + uint8_t last_sw_l; }; \ No newline at end of file