Feelyx/mv_arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

为什么臂又死了

Browse Source
This commit is contained in:
Feelyx 2026-01-14 21:02:56 +08:00
parent 02ca6c8130
commit ce08cb4d44
3 changed files with 71 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
User/device/motor_rm.c
View File

@ -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.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current; motor->feedback.torque_current = torque_current;
} }
while (motor->feedback.rotor_abs_angle < 0) { // while (motor->feedback.rotor_abs_angle < 0) {
motor->feedback.rotor_abs_angle += M_2PI; // motor->feedback.rotor_abs_angle += M_2PI;
} // }
while (motor->feedback.rotor_abs_angle >= M_2PI) { // while (motor->feedback.rotor_abs_angle >= M_2PI) {
motor->feedback.rotor_abs_angle -= M_2PI; // motor->feedback.rotor_abs_angle -= M_2PI;
} // }
if (motor->motor.reverse) { // if (motor->motor.reverse) {
motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; // motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
motor->feedback.rotor_speed = -motor->feedback.rotor_speed; // motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
motor->feedback.torque_current = -motor->feedback.torque_current; // motor->feedback.torque_current = -motor->feedback.torque_current;
} // }
motor->feedback.temp = msg->data[6]; motor->feedback.temp = msg->data[6];
} }

69
User/module/arm.cpp
View File

@ -1,5 +1,7 @@
#include "module/arm.hpp" #include "module/arm.hpp"
#include "component/pid.h" #include "component/pid.h"
#include "device/dr16.h"
#include "device/motor_rm.h"
#include "module/dr16.h" #include "module/dr16.h"
#include "task/user_task.h" #include "task/user_task.h"
#include "bsp/time.h" #include "bsp/time.h"
@ -20,8 +22,8 @@
#define AXIS_6_MIN_ANGLE -0.25f #define AXIS_6_MIN_ANGLE -0.25f
#define AXIS_6_MAX_ANGLE 0.25f #define AXIS_6_MAX_ANGLE 0.25f
#define DJI_MOTOR_OPEN 60 #define DJI_MOTOR_OPEN 0.2f
#define DJI_MOTOR_CLOSE 0 #define DJI_MOTOR_CLOSE 1.37f
Arm::Arm(){ Arm::Arm(){
//dm电机参数设置 //dm电机参数设置
@ -217,22 +219,22 @@ Arm::Arm(){
}; };
pid_params[6] = KPID_Params_t{ pid_params[6] = KPID_Params_t{
.k=1, .k=1,
.p=0.2, .p=3,
.i=0, .i=0,
.d=0, .d=0,
.i_limit=0.5, .i_limit=2,
.out_limit=1, .out_limit=50,
.d_cutoff_freq=100, .d_cutoff_freq=-1.0f,
.range = -1, .range = -1,
}; };
pid_v_params[6] = KPID_Params_t{ pid_v_params[6] = KPID_Params_t{
.k=1, .k=0.2,
.p=0.2, .p=0.15,
.i=0, .i=0,
.d=0, .d=0,
.i_limit=0.5, .i_limit=0.5,
.out_limit=1, .out_limit=1,
.d_cutoff_freq=100, .d_cutoff_freq=-1,
.range = -1, .range = -1,
}; };
} }
@ -274,6 +276,7 @@ void Arm::init(){
joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle; joint_angle[5] = arm_dm_motor[2].motor.feedback.rotor_abs_angle;
arm_relax(); arm_relax();
last_time = BSP_TIME_Get_us(); last_time = BSP_TIME_Get_us();
last_sw_l = dr16.data.sw_l;
// MOTOR_LZ_SetZero(&lz_motor_params[2]); // MOTOR_LZ_SetZero(&lz_motor_params[2]);
// BSP_TIME_Delay(2); // BSP_TIME_Delay(2);
// MOTOR_LZ_SetZero(&lz_motor_params[1]); // MOTOR_LZ_SetZero(&lz_motor_params[1]);
@ -294,16 +297,17 @@ void Arm::operator()(){
// calc_motor_angle(); // calc_motor_angle();
calc_pid(); calc_pid();
//发送控制命令 //发送控制命令
if(dr16.data.sw_l == DR16_SW_DOWN){
// send_control(); // send_control();
arm_relax(); arm_relax();
// //控制数据 }else {
// for (int i = 1; i < 3; i++) { send_control();
// 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]); if(last_sw_l != dr16.data.sw_l && last_sw_l != DR16_SW_UP){
// } arm_enable();
}
// arm_lz_output[0].target_angle = arm_lz_motor[0].motor.feedback.rotor_abs_angle + STEP; last_sw_l = dr16.data.sw_l;
// MOTOR_LZ_MotionControl(&lz_motor_params[0], &arm_lz_output[0]);
} }
void Arm::arm_relax(){ 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[0]);
MOTOR_LZ_Relax(&lz_motor_params[1]); MOTOR_LZ_Relax(&lz_motor_params[1]);
MOTOR_LZ_Relax(&lz_motor_params[2]); MOTOR_LZ_Relax(&lz_motor_params[2]);
MOTOR_RM_Relax(&motor_dji_param);
MOTOR_RM_Ctrl(&motor_dji_param);
} }
void Arm::arm_enable(){ 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[0]);
MOTOR_LZ_Enable(&lz_motor_params[1]); MOTOR_LZ_Enable(&lz_motor_params[1]);
MOTOR_LZ_Enable(&lz_motor_params[2]); 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(){ 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[3] = arm_dm_motor[0].motor.feedback.rotor_speed;
current_vel[4] = arm_dm_motor[1].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_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(){ void Arm::calc_joint_angle(){
@ -437,6 +454,12 @@ void Arm::calc_motor_angle(){
joint_angle[4] = joint_angle[4]; // 保持当前角度 joint_angle[4] = joint_angle[4]; // 保持当前角度
joint_angle[5] = joint_angle[5]; // 保持当前角度 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(){ 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); 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[0].torque = output[0];
arm_lz_output[1].torque = output[1]; arm_lz_output[1].torque = output[1];
arm_lz_output[2].torque = output[2]; arm_lz_output[2].torque = output[2];
arm_dm_output[0].torque = output[3]; arm_dm_output[0].torque = output[3];
arm_dm_output[1].torque = output[4]; arm_dm_output[1].torque = output[4];
arm_dm_output[2].torque = output[5]; arm_dm_output[2].torque = output[5];
MOTOR_RM_SetOutput(&motor_dji_param, output[6]);
} }
void Arm::send_control(){ 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[0], &arm_dm_output[0]);
MOTOR_DM_MITCtrl(&dm_motor_params[1], &arm_dm_output[1]); MOTOR_DM_MITCtrl(&dm_motor_params[1], &arm_dm_output[1]);
MOTOR_DM_MITCtrl(&dm_motor_params[2], &arm_dm_output[2]); 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);
} }

15
User/module/arm.hpp
View File

@ -7,6 +7,7 @@
#include "device/motor_rm.h" #include "device/motor_rm.h"
#include "device/motor_dm.h" #include "device/motor_dm.h"
#include "component/pid.h" #include "component/pid.h"
#include <cstdint>
class Arm { class Arm {
public: public:
@ -21,7 +22,8 @@ private:
void arm_relax(); void arm_relax();
void arm_enable(); void arm_enable();
void calc_pid(); void calc_pid();
float joint_angle[6] = {0}; //关节角度 void change_mode();
float joint_angle[7] = {0}; //关节角度
// float motor_angle[6] = {0}; //电机角度 // float motor_angle[6] = {0}; //电机角度
MOTOR_DM_Param_t dm_motor_params[3]; MOTOR_DM_Param_t dm_motor_params[3];
MOTOR_DM_t arm_dm_motor[3]; MOTOR_DM_t arm_dm_motor[3];
@ -31,14 +33,15 @@ private:
MOTOR_MIT_Output_t arm_dm_output[3]; MOTOR_MIT_Output_t arm_dm_output[3];
KPID_Params_t pid_params[7]; KPID_Params_t pid_params[7];
KPID_Params_t pid_v_params[7]; KPID_Params_t pid_v_params[7];
float output[6] = {0}; float output[7] = {0};
KPID_t pid[6]; KPID_t pid[7];
KPID_t pid_v[6]; KPID_t pid_v[7];
float current_pos[6]; float current_pos[7];
float current_vel[6]; float current_vel[7];
uint64_t time = 0; uint64_t time = 0;
uint64_t last_time = 0; uint64_t last_time = 0;
uint64_t dt = 0; uint64_t dt = 0;
MOTOR_RM_Param_t motor_dji_param; MOTOR_RM_Param_t motor_dji_param;
MOTOR_RM_t arm_motor_dji; MOTOR_RM_t arm_motor_dji;
uint8_t last_sw_l;
}; };