问题描述

问题出现的环境背景

调试GM6020速度环的时候p给大就是高频震荡，给小了就没劲，调节d项也解决不了，然后我在pid控制获取电机反馈速度的时候加了一个低通滤波，情况是好了很多，但是它还是在目标速度来回波动导致轻微震荡.

下面这个是我想直接输入电流控制电流转换为控制值

int16_t current_to_ctrl_value(float current_A, dji_motor_type_e type)

{

switch(type)

{

case DJI_2006:

//-10A ~ 10A 对应 -16384 ~ 16384

return (int16_t)(current_A * 16384.0f / 10.0f);

    case DJI_6020:
        //-3A ~ 3A 对应 -16384 ~ 16384
        return (int16_t)(current_A * 16384.0f / 3.0f);

    default:
        return 0;
}

}

//这里我用的是度，添加了第二种rad/s

void dji_motor_send_velocity(FDCAN_HandleTypeDef* hfdcan, dji_motor_t *motor, uint16_t identifier, float expect_velocity)

{

//一阶低通滤波，去除掉了由于电机颤动导致的突变，过滤掉大部分外界因素

float A = 0.49f;

float feedback_angle_speed = 0;

float current_A = 0;
int16_t ctrl_value = 0; 


current_A = pid_calc(&motor->dji_pid_velocity, expect_velocity, motor->rx_fdpara.angle_speed);


feedback_angle_speed = A*motor->rx_fdpara.angle_speed + (1-A)*feedback_angle_speed;

current_A = pid_calc(&motor->dji_pid_velocity, expect_velocity, feedback_angle_speed);

// motor->last_angular_speed = motor->rx_fdpara.angle_speed;

//电流输出
if(motor->type == DJI_6020)
{
    ctrl_value = current_to_ctrl_value(current_A, DJI_6020);
}
else if(motor->type == DJI_2006)
{
    ctrl_value = current_to_ctrl_value(current_A, DJI_2006);
}

int16_t value1 = 0, value2 = 0, value3 = 0; uint16_t value4 = 0;

switch(motor->identifier_id)
{
    case 0x204: 
        value1 = ctrl_value; 
    break;
    case 0x205:
        value2 = ctrl_value;
    break;
    case 3: 
        value3 = ctrl_value;
    break;
    case 0x202:
        value4 = (uint16_t)ctrl_value;
    break;
}

dji_motor_ctrl(hfdcan, identifier, value1, value2, value3, value4);

}