engineering/application/gimbal/gimbal.cpp

#ifdef __cplusplus
extern "C" {
#endif
#include "gimbal.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "dmmotor.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "bmi088.h"
#include "user_lib.h"
#ifdef __cplusplus
}
#endif
#include "matrix.h"
#include "robotics.h"

static attitude_t *gimba_IMU_data; // 云台IMU数据
static DMMotorInstance *yaw_motor, *pitch_motor, *roll_motor;
static DMMotorInstance *diff_r_motor,*diff_l_motor;

static PIDInstance diff_pitch_loop,diff_roll_loop;
static PIDInstance diff_pitch_spd_loop,diff_roll_spd_loop;

float arm_gravity_feedforward = 0;
float GRAVITY_COMP = 5.5;

static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

first_order_filter_type_t pitch_spd_filter,yaw_spd_filter,roll_spd_filter;
static float pitch_spd,yaw_spd,roll_spd;

first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter;
static float diff_r_spd,diff_l_spd;

//机械臂参数初始化
float arm_q[5] = {0}; // 机械臂各关节位置
robotics::Link link[5];
robotics::Serial_Link<5> engineer_arm(link);
Matrixf<4, 4> fk_T; //正运动学  末端变换矩阵
Matrixf<3, 1> fk_p; //正运动学  末端位置向量
Matrixf<3, 1> fk_rpy; //正运动学  末端欧拉角

Matrixf<5, 2> ik_q; //逆运动学  关节位置
const float l1 = 0.151 ,l3 = 0.350, l4 = 0.139;
void Arm_Init()
{
    link[0] = robotics::Link(0,l1,0,PI/2);
    link[1] = robotics::Link(0,0,0,PI/2); link[1].offset_ = PI/2;
    link[2] = robotics::Link(0,l3,0,-PI/2);
    link[3] = robotics::Link(0,0,0,PI/2);
    link[4] = robotics::Link(0,l4,0,0);

    engineer_arm = robotics::Serial_Link<5>(link);
    engineer_arm.ikine_analytic = robotics::my_analytic_ikine;
}

void GimbalInit()
{
    Arm_Init();
    Motor_Init_Config_s yaw_motor_config = {
            .controller_param_init_config = {
                    .other_speed_feedback_ptr = &yaw_spd,
                    .speed_PID = {
                            .Kp = 3,//5,
                            .Ki = 3.0f,
                            .Kd = 0.02f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
                    .angle_PID = {
                            .Kp = 10.0f,//13.0f,
                            .Ki = 0.0f,
                            .Kd = 0.0f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
            },
            .controller_setting_init_config = {
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    .feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = DM6006,
    };

    yaw_motor_config.can_init_config.can_handle = &hcan2;
    yaw_motor_config.can_init_config.tx_id = 1;
    yaw_motor_config.can_init_config.rx_id = 2;
    yaw_motor = DMMotorInit(&yaw_motor_config);

    Motor_Init_Config_s pitch_motor_config = {
            .controller_param_init_config = {
                    .other_speed_feedback_ptr = &pitch_spd,
                    .current_feedforward_ptr = &arm_gravity_feedforward,
                    .speed_PID = {
                            .Kp = 2.5f,
                            .Ki = 0.8f,
                            .Kd = 0.0f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
                    .angle_PID = {
                            .Kp = 8.0f,
                            .Ki = 0.0f,
                            .Kd = 0.0f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
            },
            .controller_setting_init_config = {
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    .feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = DM6006,
    };

    pitch_motor_config.can_init_config.can_handle = &hcan2;
    pitch_motor_config.can_init_config.tx_id = 3;
    pitch_motor_config.can_init_config.rx_id = 4;
    pitch_motor = DMMotorInit(&pitch_motor_config);

    //@todo：roll轴机械固定不牢  待细调
    Motor_Init_Config_s roll_motor_config = {
            .controller_param_init_config = {
                    .other_speed_feedback_ptr = &roll_spd,
                    .speed_PID = {
                            .Kp = 0.8f,
                            .Ki = 0.3f,
                            .Kd = 0.02f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
                    .angle_PID = {
                            .Kp = 10.0f,
                            .Ki = 0.0f,
                            .Kd = 0.0f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
            },
            .controller_setting_init_config = {
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    //.feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = DM4310,
    };

    roll_motor_config.can_init_config.can_handle = &hcan2;
    roll_motor_config.can_init_config.tx_id = 5;
    roll_motor_config.can_init_config.rx_id = 6;
    roll_motor = DMMotorInit(&roll_motor_config);

    Motor_Init_Config_s diff_motor_config = {
            .controller_param_init_config = {
                    .speed_PID = {
                            .Kp = 0.6f,
                            .Ki = 0.1f,
                            .Kd = 0.02f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                    },
            },
            .controller_setting_init_config = {
                    .outer_loop_type = SPEED_LOOP,
                    .close_loop_type = SPEED_LOOP,
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    //.feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = DM4310,
    };

    diff_motor_config.can_init_config.can_handle = &hcan2;
    diff_motor_config.can_init_config.tx_id = 7;
    diff_motor_config.can_init_config.rx_id = 8;
    diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd;
    diff_r_motor = DMMotorInit(&diff_motor_config);

    diff_motor_config.can_init_config.can_handle = &hcan2;
    diff_motor_config.can_init_config.tx_id = 9;
    diff_motor_config.can_init_config.rx_id = 10;
    diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd;
    diff_l_motor = DMMotorInit(&diff_motor_config);

    const float spd_filter_num = 0.05f;
    first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num);
    const float spd_filter_num_yaw = 0.1f;
    first_order_filter_init(&yaw_spd_filter,5e-3,&spd_filter_num_yaw);
    const float spd_filter_num_roll = 0.05f;
    first_order_filter_init(&roll_spd_filter,5e-3,&spd_filter_num_roll);

    const float spd_filter_num_diff = 0.05f;
    first_order_filter_init(&diff_r_spd_filter,5e-3,&spd_filter_num_diff);
    first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff);

    PID_Init_Config_s diff_pitch_config= {
            .Kp = 15.0f,
            .Ki = 0,
            .Kd = 0,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };
    PIDInit(&diff_pitch_loop,&diff_pitch_config);

    PID_Init_Config_s diff_pitch_spd_config= {
            .Kp = 0.6f,
            .Ki = 2.0f,
            .Kd = 0.01f,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };
    PIDInit(&diff_pitch_spd_loop,&diff_pitch_spd_config);

    PID_Init_Config_s diff_roll_config= {
            .Kp = 14.0f,
            .Ki = 0,
            .Kd = 0,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };
    PIDInit(&diff_roll_loop,&diff_roll_config);

    PID_Init_Config_s diff_roll_spd_config= {
            .Kp = 1.0f,
            .Ki = 1.0f,
            .Kd = 0.02f,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };
    PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config);

    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
    // YAW
    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
}
//static void DMMotroEnable()
//{
//    if(gimbal_cmd_recv.MotorEnableFlag)
//    {
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,yaw_motor);
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,pitch_motor);
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,roll_motor);
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_r_motor );
//        DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_l_motor );
//    }
//}

/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{

    // 获取云台控制数据
    // 后续增加未收到数据的处理
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

    /* 控制参数计算 ------------------------------------------------------------------------*/
    //大臂重力补偿力矩
    arm_gravity_feedforward =  - GRAVITY_COMP * arm_cos_f32(- pitch_motor->measure.position);
    //电机速度滤波
    first_order_filter_cali(&pitch_spd_filter,pitch_motor->measure.velocity);
    pitch_spd = pitch_spd_filter.out;
    first_order_filter_cali(&yaw_spd_filter,yaw_motor->measure.velocity);
    yaw_spd = yaw_spd_filter.out;
    first_order_filter_cali(&roll_spd_filter,roll_motor->measure.velocity);
    roll_spd = roll_spd_filter.out;
    //手腕关节
    first_order_filter_cali(&diff_r_spd_filter,diff_r_motor->measure.velocity);
    diff_r_spd = diff_r_spd_filter.out;
    first_order_filter_cali(&diff_l_spd_filter,diff_l_motor->measure.velocity);
    diff_l_spd = diff_l_spd_filter.out;

    float diff_pitch_angle = (diff_r_motor->measure.position + (-diff_l_motor->measure.position))/2;
    float diff_roll_angle = (diff_r_motor->measure.position - (-diff_l_motor->measure.position))/2 * 18/52;

    float pitch_angle_out = PIDCalculate(&diff_pitch_loop,diff_pitch_angle,gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD);
    float roll_angle_out = PIDCalculate(&diff_roll_loop,diff_roll_angle,gimbal_cmd_recv.diff_roll * DEGREE_2_RAD);

//    float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2;
//    float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52;
//    float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out);
//    float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out);

    float r_speed_set = pitch_angle_out + roll_angle_out;
    float l_speed_set = pitch_angle_out - roll_angle_out;

    //正运动学
    arm_q[0] = yaw_motor->measure.position;
    arm_q[1] = pitch_motor->measure.position;
    arm_q[2] = roll_motor->measure.position;
    arm_q[3] = diff_pitch_angle;
    arm_q[4] = diff_roll_angle;
    fk_T = engineer_arm.fkine(arm_q);
    fk_p = robotics::t2p(fk_T);
    fk_rpy = robotics::t2rpy(fk_T);
    //逆运动学
    ik_q = engineer_arm.ikine_analytic(fk_T);

    // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
    // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
    switch (gimbal_cmd_recv.gimbal_mode)
    {
    // 停止
    case GIMBAL_ZERO_FORCE:
        DMMotorStop(yaw_motor);
        DMMotorStop(pitch_motor);
        DMMotorStop(roll_motor);
        DMMotorStop(diff_r_motor);
        DMMotorStop(diff_l_motor);

        break;
    // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
    case GIMBAL_GYRO_MODE: // 后续只保留此模式
        DMMotorEnable(yaw_motor);
        DMMotorEnable(pitch_motor);
        DMMotorEnable(roll_motor);
        DMMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch * DEGREE_2_RAD);
        DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD);
        DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD);

        DMMotorEnable(diff_r_motor);
        DMMotorEnable(diff_l_motor);
        DMMotorSetRef(diff_r_motor,r_speed_set);
        DMMotorSetRef(diff_l_motor,-l_speed_set);
        break;
    default:
        break;
    }

    // 在合适的地方添加pitch重力补偿前馈力矩
    // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
    // ...

    // 设置反馈数据,主要是imu和yaw的ecd
    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;

    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}