sentry_left/application/gimbal/gimbal.c

#include "gimbal.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "DM4310.h"
#include "ECmotor/ECA8210.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"

#include "bmi088.h"
#include "vofa.h"

#include "power_meter.h"
#include "user_lib.h"


static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor, *pitch_motor;

static DMMotorInstance *big_yaw_motor;

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 float current_feedforward = 0.0f;
static float yaw_speed = 0.0f; //yaw轴旋转角速度需要通过pitch解算确定
static float big_yaw_angle = 0.0f; //大yaw轴绝对角度  通过小云台陀螺仪yaw-yaw轴编码器角度确定
static float big_yaw_speed = 0.0f;//大yaw轴绝对角速度
sin_input_generate_t sinInputGenerate;

static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控制命令
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令


void GimbalInit()
{
    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
    // YAW
    Motor_Init_Config_s yaw_config = {

        .can_init_config = {
            .can_handle = &hcan1,
            .tx_id = 1,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.5f, // 8
                .Ki = 0,
                .Kd = 0,
                .DeadBand = 0.1,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 100,

                .MaxOut = 500,
            },
            .speed_PID = {
                .Kp = 6000,  // 50
                .Ki = 500,//5000, // 200
                .Kd = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 5000,
                .MaxOut = 16000,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
            .other_speed_feedback_ptr = &yaw_speed,//&gimba_IMU_data->Gyro[2],
        },
        .controller_setting_init_config = {
            .angle_feedback_source = OTHER_FEED,
            .speed_feedback_source = OTHER_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = GM6020,
        .motor_control_type = CURRENT_CONTROL
    };
    // PITCH
    Motor_Init_Config_s pitch_config = {
        .can_init_config = {
            .can_handle = &hcan1,
            .tx_id = 2,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = -1.8f,//4.0f,//2.0f
                .Ki = -1.5f,//1,//0
                .Kd = 0.0f,//0.0f
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 100,
                .MaxOut = 500,
            },
            .speed_PID = {
                .Kp = -4000,//-4500,//6000,//800
                .Ki = -10000, //-600,//500,//100
                .Kd = 0,//0
                .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 10000,
                .MaxOut = 30000,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->Roll,//&gimba_IMU_data->Pitch,
            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
            .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[1]),//(&gimba_IMU_data->Gyro[0]),
            .current_feedforward_ptr = &current_feedforward,

        },
        .controller_setting_init_config = {
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
            .feedforward_flag = CURRENT_FEEDFORWARD,
        },
        .motor_type = GM6020,
        .motor_control_type = CURRENT_CONTROL,//CURRENT_CONTROL
    };
    //大YAW
    Motor_Init_Config_s big_yaw_config = {
            .can_init_config = {
                    .can_handle = &hcan2,
                    .tx_id = 1,
                    .rx_id = 1,
            },
            .controller_param_init_config = {
                    .angle_PID = {
                            .Kp = 0.15f,
                            .Ki = 0.0f,
                            .Kd = 0.005f,
                            .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement |PID_DerivativeFilter,
                            .IntegralLimit = 100,
                            .MaxOut = 1000,
                            .DeadBand = 0,
                    },
                    .speed_PID = {
                            .Kp = 3.5f,//150,//150,//这里KP尽可能小  防止震荡脱齿
                            .Ki = 0.0f,//200, // 350
                            .Kd = 0,   // 0
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 10000,
                            .MaxOut = 1000,
                    },
                    .other_angle_feedback_ptr = &big_yaw_angle,
                    .other_speed_feedback_ptr = &big_yaw_speed,
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = OTHER_FEED,
                    .speed_feedback_source = OTHER_FEED,
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type = ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP,
                    .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
            },
            .motor_type = DM4310,
    };


    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
    yaw_motor = DJIMotorInit(&yaw_config);
    pitch_motor = DJIMotorInit(&pitch_config);

    big_yaw_motor = DMMotorInit(&big_yaw_config);

    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

    chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));

    //sin_input_frequency_init(&sinInputGenerate);
}

/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
    // 获取云台控制数据
    // 后续增加未收到数据的处理
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
    SubGetMessage(chassis_sub, &chassis_cmd_recv);
//    if(gimbal_cmd_recv.enable_motor > 0){
//        LOGINFO("enabling DMmoter");
//        DMMotorSetMode(DM_CMD_MOTOR_MODE, big_yaw_motor);
//    }

    //gimbal_cmd_recv.big_yaw = 0;

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

        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);

        DMMotorSetRef(big_yaw_motor,gimbal_cmd_recv.big_yaw);
        break;
    // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
    case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
        DJIMotorEnable(yaw_motor);
        DJIMotorEnable(pitch_motor);
        DMMotorEnable(big_yaw_motor);
        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
//        big_yaw_angle = chassis_cmd_recv.offset_angle;
//        big_yaw_speed = big_yaw_motor->measure.speed_rads;
//
//        ECMotorSetRef(big_yaw_motor,0);
//            big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - 44);
//            big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD;

            DMMotorSetRef(big_yaw_motor,gimbal_cmd_recv.big_yaw);
        break;
    default:
        break;
    }

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

    float input = sin_input_generate(&sinInputGenerate);

    //ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0);
    float theta = gimba_IMU_data->Roll/180*PI;//(pitch_motor->measure.angle_single_round - 5505 * ECD_ANGLE_COEF_DJI)/180*PI;
    yaw_speed = gimba_IMU_data->Gyro[2] * arm_cos_f32(theta) - gimba_IMU_data->Gyro[0] * arm_sin_f32(theta);
    //big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - 44);
    //big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD;


    big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - (-80));
    //big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD;

    big_yaw_speed = (yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD);


//    big_yaw_angle = chassis_cmd_recv.offset_angle;
//    //loop_float_constrain(big_yaw_motor->measure.total_angle * (4.0f/3.0f),0,2*PI) * RAD_2_DEGREE - 340.0f;
//    big_yaw_speed = big_yaw_motor->measure.speed_rads;
    //yaw轴角速度

    //重力补偿力矩
    current_feedforward = (1500)*arm_cos_f32(theta);
    //current_feedforward = 0;
    float vofa_send_data[6];
    vofa_send_data[0] = pitch_motor->measure.speed_aps;
    vofa_send_data[1] = gimba_IMU_data->Pitch;
    vofa_send_data[2] = gimba_IMU_data->Gyro[0];
    vofa_send_data[3] = pitch_motor->motor_controller.speed_PID.Ref;
    vofa_send_data[4] = pitch_motor->motor_controller.speed_PID.Measure;
    vofa_send_data[5] = pitch_motor->measure.angle_single_round;

    vofa_justfloat_output(vofa_send_data,24,&huart1);



    // 设置反馈数据,主要是imu和yaw的ecd
    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;

    static float big_yaw_relative_angle;
    
    big_yaw_relative_angle = big_yaw_motor->measure.total_angle ;

    gimbal_feedback_data.yaw_motor_single_round_angle = big_yaw_motor->measure.angle_single_round;
            //loop_float_constrain(big_yaw_relative_angle,0,2*PI) * RAD_2_DEGREE;
    gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.total_angle;
    gimbal_feedback_data.big_yaw_online = DMMotorIsOnline(big_yaw_motor);
    gimbal_feedback_data.big_yaw_angle = big_yaw_angle;
    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}

//以下代码为测试系统辨识时使用
//void sin_input_generate(float frequency, int count)
//{
//    static uint32_t cnt;
//    static float time;
//    while(time>=count*(1/frequency))
//    {
//        float deltaT=DWT_GetDeltaT(&cnt);
//        time += deltaT;
//
//        float input = arm_sin_f32(2*PI*frequency*time);
//        DJIMotorSetRef(yaw_motor,input);
//    }
//}

void sin_input_frequency_init(sin_input_generate_t* InputGenerate)
{
    for(int i=0;i<43;i++)
    {
        InputGenerate->frequency[i] = 1.0 + 0.5*i;
    }
    for(int i=0;i<9;i++)
    {
        InputGenerate->frequency[i+43] = 24.0 + 2.0*i;
    }
    for(int i=0;i<8;i++)
    {
        InputGenerate->frequency[i+43+9] = 50 + 10*i;
    }
    InputGenerate->frequency[60] = 200;
    InputGenerate->frequency[61] = 250;
    InputGenerate->frequency[62] = 333;
    InputGenerate->frequency[63] = 500;
}

float sin_input_generate(sin_input_generate_t* InputGenerate)
{
    InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt);
    InputGenerate->time += InputGenerate->DeltaT;
    if(InputGenerate->time >= 20*(1/InputGenerate->frequency[InputGenerate->frequency_index]))
    {
        InputGenerate->time = 0;
        InputGenerate->frequency_index += 1;
    }
    if(InputGenerate->frequency_index >= 64)
    {
        InputGenerate->input = 0;
    }
    else
        InputGenerate->input = arm_sin_f32(2*PI*InputGenerate->frequency[InputGenerate->frequency_index]*InputGenerate->time);
    //float input = arm_sin_f32(2*PI*frequency*time);
    InputGenerate->input *= 2000;

    return InputGenerate->input;
}

float step_input_generate(sin_input_generate_t* InputGenerate)
{
    static int8_t forward_flag = 1;
    InputGenerate->DeltaT = DWT_GetDeltaT(&InputGenerate->cnt);
    InputGenerate->time += InputGenerate->DeltaT;
    if(InputGenerate->time >= 3)
    {
        if(forward_flag ==1) forward_flag = -1;
        else if (forward_flag == -1) forward_flag = 1;

        InputGenerate->time = 0;
    }
   return 60*forward_flag;
}