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.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];
}

65
User/module/arm.cpp
View File

@ -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]);
// }
}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;
// 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(){
@ -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);
}

15
User/module/arm.hpp
View File

@ -7,6 +7,7 @@
#include "device/motor_rm.h"
#include "device/motor_dm.h"
#include "component/pid.h"
#include <cstdint>
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;
};