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"
#include "bsp_log.h"
#include "referee_VT.h"
#ifdef __cplusplus
}
#endif
#include "matrix.h"
#include "robotics.h"


static attitude_t *gimba_IMU_data; // 云台IMU数据
static DMMotorInstance *yaw_motor, *pitch_motor_l,*pitch_motor_r, *roll_motor;
static DMMotorInstance *diff_r_motor,*diff_l_motor;
//pitch轴双6006 双环pid算出力矩 一人一半
static PIDInstance pitch_spd_loop,pitch_angle_loop;

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 = 6.0;

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的控制信息

static gimbal_mode_e last_gimbal_mode;     //上次模式  用于模式切换数据过渡

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, 1> arm_q;  //正运动学  末端关节位置


Matrixf<4, 4> T_cmd; //拟运动学  期望末端变换矩阵
Matrixf<5, 2> ik_q;    //逆运动学  关节位置
Matrixf<5, 1> ik_q_cmd;//逆运动学 期望关节位置
Matrixf<3, 1> cmd_xyz; //逆运动学  末端期望位置
Matrixf<3, 3> cmd_R; //逆运动学  末端期望姿态
Matrixf<4, 1> cmd_quat; //逆运动学  末端期望姿态
Matrixf<3, 2> ik_q3;    //逆运动学  球形手腕关节位置
Matrixf<3, 1> ik_q3_cmd;//逆运动学  球形手腕期望关节位置
const float l1 = 0.151 ,l3 = 0.350, l4 = 0;//0.139;
void Arm_Init()
{
    link[0] = robotics::Link(0,l1,0,PI/2);
    link[1] = robotics::Link(0,0,l3,PI/2);
    link[2] = robotics::Link(0,0,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;
    engineer_arm.ikine_analytic_check = robotics::check_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.5f,//5,
                            .Ki = 0.0f,
                            .Kd = 0.02f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>(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,
            },
            .controller_setting_init_config = {
//                    .outer_loop_type = ANGLE_LOOP,
//                    .close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
                    .outer_loop_type = OPEN_LOOP,
                    .close_loop_type = OPEN_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 = 0x03;
    pitch_motor_config.can_init_config.rx_id = 0x04;
    pitch_motor_l = DMMotorInit(&pitch_motor_config);

    PID_Init_Config_s pitch_spd_config= {
            .Kp = 2.5f,
            .Ki = 0.8f,
            .Kd = 0.02f,
            .MaxOut = 10,
            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
            .IntegralLimit = 100,
    };
    PID_Init_Config_s pitch_angle_config= {
            .Kp = 12.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(&pitch_spd_loop,&pitch_spd_config);
    PIDInit(&pitch_angle_loop,&pitch_angle_config);

    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.2f,//0.6f,
                            .Ki = 0,//0.1f,
                            .Kd = 0.02,//0.02f,
                            .MaxOut = 10,
                            .Improve = static_cast<PID_Improvement_e>( PID_Integral_Limit | PID_Derivative_On_Measurement),
                            .IntegralLimit = 10.0F,
                            .Output_LPF_RC = 0.02F,
                    },
            },
            .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.05f;
    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 = 20.0F,//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);

    pitch_motor_config.can_init_config.tx_id = 11; //按顺序初始化！！！
    pitch_motor_config.can_init_config.rx_id = 12;
    pitch_motor_r = DMMotorInit(&pitch_motor_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控制,不再需要电机的反馈 */

//float debug_diff_kp = 0.1f,debug_diff_ki=0,debug_diff_kd = 0;
void GimbalTask()
{
//    diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
//    diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
//    diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
//
//    diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
//    diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
//    diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
    // 获取云台控制数据
    // 后续增加未收到数据的处理
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

    /* 控制参数计算 ------------------------------------------------------------------------*/
    //大臂重力补偿力矩
    arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor_l->measure.position);
    //电机速度滤波
    first_order_filter_cali(&pitch_spd_filter,pitch_motor_l->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;

    /* 控制参数计算 ------------------------------------------------------------------------*/

    //正运动学
    arm_q[0][0] = yaw_motor->measure.position;
    arm_q[1][0] = -pitch_motor_l->measure.position;
    arm_q[2][0] = roll_motor->measure.position;
    arm_q[3][0] = diff_pitch_angle;
    arm_q[4][0] = 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);
//
//    ik_q_cmd = ik_q.block<5,1>(0,0);

//    ik_q3 = robotics::spherical_wrist_ikine(robotics::t2r(fk_T),arm_q[0][0],arm_q[1][0]);

    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
    {
        DMMotorStop(pitch_motor_l);DMMotorStop(pitch_motor_r);
        DMMotorStop(yaw_motor);DMMotorStop(roll_motor);
        DMMotorStop(diff_r_motor);DMMotorStop(diff_l_motor);
    }
    else
    {
        DMMotorEnable(pitch_motor_l);DMMotorEnable(pitch_motor_r);
        DMMotorEnable(yaw_motor);DMMotorEnable(roll_motor);
        DMMotorEnable(diff_r_motor);DMMotorEnable(diff_l_motor);
    }

    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_GYRO_MODE) //各关节分开控制
    {
        float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD);
        float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, gimbal_cmd_recv.diff_roll * DEGREE_2_RAD);

        float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out;
        float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out;

        //pitch轴双环PID
        float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,gimbal_cmd_recv.pitch * DEGREE_2_RAD);
        float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward;

        DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2);
        DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2);
        DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD);
        DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD);

        DMMotorSetRef(diff_r_motor,r_speed_set);
        DMMotorSetRef(diff_l_motor,-l_speed_set);
    }
    if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE)
    {
        static float q_set[5] ; //记录五个关节的目标值
        if(last_gimbal_mode == GIMBAL_GYRO_MODE)
        {
            //切换至逆解模式时，目标值设置为当前值
            cmd_R = robotics::t2r( fk_T);
        }
        for (int i = 0; i < 4; ++i) {
            cmd_quat[i][0] = gimbal_cmd_recv.quat[i];
        }
        float trans_rpy_data[3] = {-PI,0,0};
        Matrixf<3,1> trans_rpy(trans_rpy_data);
            cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans_rpy); //转到基座坐标系下的姿态
//        T_cmd = robotics::p2t(cmd_xyz);

        ik_q3 = robotics::spherical_wrist_ikine(cmd_R,arm_q[0][0],arm_q[1][0]);

            //后三关节的误差 选用误差小的一组解
            float err[2] = {0};
            Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0);
            for (int i = 0; i < 2; ++i) {
                Matrixf<3,1> ik = ik_q3.block<3,1>(0,i);
                err[i]  = (ik - arm_q3).norm();
            }

//            if (err[1] >= err[0])
//                ik_q3_cmd = ik_q3.block<3,1>(0,0);
//            else
//                ik_q3_cmd = ik_q3.block<3,1>(0,1);

//            //选用 roll误差小的一组解
            if (abs(ik_q3[0][0] - arm_q3[0][0]) <= abs(ik_q3[0][1]- arm_q3[0][0]))
                ik_q3_cmd = ik_q3.block<3,1>(0,0);
            else
                ik_q3_cmd = ik_q3.block<3,1>(0,1);

            ik_q3_cmd[0][0] = float_constrain(ik_q3_cmd[0][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);

            //ik_q3_cmd[1][0] = float_constrain(ik_q3_cmd[1][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
            ik_q3_cmd[2][0] = float_constrain(ik_q3_cmd[2][0],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);

            for (int i = 0; i < 3; ++i) {
                q_set[i] = ik_q3_cmd[i][0];
            }



        float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[1]);
        float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, q_set[2]);

        float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out;
        float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out;

        //pitch轴双环PID pitch yaw先遥控器控制
        float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,gimbal_cmd_recv.pitch * DEGREE_2_RAD);
        float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward;

        DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2);
        DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2);
        DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD);

        DMMotorSetRef(roll_motor,q_set[0]);

        DMMotorSetRef(diff_r_motor,r_speed_set);
        DMMotorSetRef(diff_l_motor,-l_speed_set);

    }
    //保存上次模式
    last_gimbal_mode = gimbal_cmd_recv.gimbal_mode;






    // 在合适的地方添加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);
}