444 lines
14 KiB
C++
444 lines
14 KiB
C++
#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,
|
|
.can_id = 0x04,
|
|
.module = MOTOR_DM_J4310,
|
|
.reverse = false
|
|
};
|
|
dm_motor_params[1] = MOTOR_DM_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.master_id = 0x15,
|
|
.can_id = 0x05,
|
|
.module = MOTOR_DM_J4310,
|
|
.reverse = false
|
|
};
|
|
dm_motor_params[2] = MOTOR_DM_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.master_id = 0x16,
|
|
.can_id = 0x06,
|
|
.module = MOTOR_DM_J4310,
|
|
.reverse = false
|
|
};
|
|
//lz电机参数设置
|
|
lz_motor_params[0] = MOTOR_LZ_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.motor_id = 127,
|
|
.host_id = 0xff,
|
|
.module= MOTOR_LZ_RSO3,
|
|
.reverse = false,
|
|
.mode = MOTOR_LZ_MODE_MOTION
|
|
};
|
|
lz_motor_params[1] = MOTOR_LZ_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.motor_id = 126,
|
|
.host_id = 0xff,
|
|
.module= MOTOR_LZ_RSO3,
|
|
.reverse = false,
|
|
.mode = MOTOR_LZ_MODE_MOTION
|
|
};
|
|
lz_motor_params[2] = MOTOR_LZ_Param_t{
|
|
.can = BSP_CAN_1,
|
|
.motor_id = 125,
|
|
.host_id = 0xff,
|
|
.module= MOTOR_LZ_RSO3,
|
|
.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();
|
|
for (int i = 0; i < 3; i++) {
|
|
MOTOR_LZ_Register(&lz_motor_params[i]);
|
|
MOTOR_LZ_Enable(&lz_motor_params[i]);
|
|
}
|
|
|
|
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()(){
|
|
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;
|
|
motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[0]);
|
|
arm_dm_motor[0] = *motor_dm;
|
|
motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[1]);
|
|
arm_dm_motor[1] = *motor_dm;
|
|
motor_dm = MOTOR_DM_GetMotor(&dm_motor_params[2]);
|
|
arm_dm_motor[2] = *motor_dm;
|
|
//读取lz数据
|
|
MOTOR_LZ_UpdateAll();
|
|
|
|
MOTOR_LZ_t *motor_lz = NULL;
|
|
motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[0]);
|
|
arm_lz_motor[0] = *motor_lz;
|
|
motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[1]);
|
|
arm_lz_motor[1] = *motor_lz;
|
|
motor_lz = MOTOR_LZ_GetMotor(&lz_motor_params[2]);
|
|
arm_lz_motor[2] = *motor_lz;
|
|
|
|
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]);
|
|
|
|
// }
|
|
// }
|
|
}
|