engineering/application/to_stretch/to_stretch.c

#include "to_stretch.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "message_center.h"
#include "referee_task.h"
#include "general_def.h"
#include "bsp_dwt.h"
#include "referee_UI.h"
#include "arm_math.h"
#include "user_lib.h"

//to_stretch

static Publisher_t *to_stretch_pub;                    // 用于发布伸缩的数据
static Subscriber_t *to_stretch_sub;                   // 用于订阅伸缩的控制命令
static To_stretch_Ctrl_Cmd_s to_stretch_cmd_recv;         // 伸缩接收到的控制命令
static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反馈数据

static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd, *tuchuan;

float gravity_feedforward = 3000;
float l_protract = 0,r_protract = 0;
float protract_x = 0;  //前伸距离
float l_lift = 0,r_lift = 0;
float lift_z = 0;  //抬升高度
float ld_offset = 0,rd_offset = 0;
float lu_offset = 0,ru_offset = 0;

PIDInstance protract_position_loop;//前伸位置环
PIDInstance lift_position_loop;//抬升位置环

void To_stretchInit() {
    // 上下
    Motor_Init_Config_s ud_config = {
            .can_init_config = {
                    .can_handle = &hcan1,
            },
            .controller_param_init_config = {
                    .angle_PID = {
                            .Kp = 0.0f,
                            .Ki = 0,
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 100,
                            .MaxOut = 500,
                    },
                    .speed_PID = {
                            .Kp = 2,
                            .Ki = 1,
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 2500,
                            .MaxOut = 5000,
                    },
                    .current_feedforward_ptr = &gravity_feedforward,
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = MOTOR_FEED,
                    .outer_loop_type = SPEED_LOOP,//ANGLE_LOOP,
                    .close_loop_type = SPEED_LOOP,//SPEED_LOOP | ANGLE_LOOP,
                    .feedforward_flag = CURRENT_FEEDFORWARD,
            },
            .motor_type = M3508,

    };
    // 前后
    Motor_Init_Config_s fb_config = {
            .can_init_config = {
                    .can_handle = &hcan1,
            },
            .controller_param_init_config = {
                    .angle_PID = {
                            .Kp = 10.0f,
                            .Ki = 0,
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 100,
                            .MaxOut = 5000,
                    },
                    .speed_PID = {
                            .Kp = 2,
                            .Ki = 0,
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 2500,
                            .MaxOut = 5000,
                    },
                    .other_angle_feedback_ptr = &protract_x,
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = OTHER_FEED,
                    .speed_feedback_source = MOTOR_FEED,
                    .outer_loop_type = SPEED_LOOP,
                    .close_loop_type = SPEED_LOOP,
            },
            .motor_type = M3508,
    };
    //上下
    ud_config.can_init_config.tx_id = 5;
    ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    ud_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL;
    motor_lu = DJIMotorInit(&ud_config);

    ud_config.can_init_config.tx_id = 6;
    ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    ud_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE;
    motor_ru = DJIMotorInit(&ud_config);
    //前后
    fb_config.can_init_config.tx_id = 7;
    fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    fb_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL;
    motor_ld = DJIMotorInit(&fb_config);

    fb_config.can_init_config.tx_id = 8;
    fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    fb_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE;
    motor_rd = DJIMotorInit(&fb_config);

    PID_Init_Config_s protract_pid_config = {
            .Kp = 2500.0f,
            .Ki = 0,
            .Kd = 0,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .IntegralLimit = 100,
            .MaxOut = 20000,
    };
    PIDInit(&protract_position_loop,&protract_pid_config);

    PID_Init_Config_s lift_pid_config = {
            .Kp = 2500.0f,
            .Ki = 0,
            .Kd = 0,
            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
            .IntegralLimit = 100,
            .MaxOut = 20000,
    };
    PIDInit(&lift_position_loop,&lift_pid_config);

    // 图传电机
    Motor_Init_Config_s tuchuan_config = {
            .can_init_config = {
                    .can_handle = &hcan2,
                    .tx_id = 1,
            },
            .controller_param_init_config = {
                    .angle_PID = {
                            .Kp = 20,
                            .Ki = 10,
                            .Kd = 0,
                            .MaxOut = 15000,
                    },
                    .speed_PID = {
                            .Kp = 2.0f,
                            .Ki = 1.0f,
                            .Kd = 0,
                            .Improve = PID_Integral_Limit,
                            .IntegralLimit = 5000,
                            .MaxOut = 30000,
                    },
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = MOTOR_FEED,
                    .outer_loop_type = ANGLE_LOOP,
                    .close_loop_type =  ANGLE_LOOP | SPEED_LOOP,
                    .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
            },
            .motor_type = M2006
    };
    tuchuan = DJIMotorInit(&tuchuan_config);

    to_stretch_sub = SubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s));
    to_stretch_pub = PubRegister("to_stretch_feed", sizeof(To_stretch_Upload_Data_s));

}

/* 机器人伸缩控制核心任务 */
void To_stretchTask()
{
    static uint8_t init_flag = FALSE;
    if(init_flag == FALSE)
    {
        ld_offset = motor_ld->measure.total_angle ;
        rd_offset = motor_rd->measure.total_angle ;

        lu_offset = motor_lu->measure.total_angle ;
        ru_offset = motor_ru->measure.total_angle ;

        init_flag = TRUE;
    }
    l_protract = -(motor_ld->measure.total_angle - ld_offset) / 19.0f * DEGREE_2_RAD * 20.0f; //单位mm
    r_protract = (motor_rd->measure.total_angle - rd_offset) / 19.0f * DEGREE_2_RAD * 20.0f;

    l_lift = (motor_lu->measure.total_angle - lu_offset) / 19.0f * DEGREE_2_RAD * 20.0f; //单位mm
    r_lift = -(motor_ru->measure.total_angle - ru_offset) / 19.0f * DEGREE_2_RAD * 20.0f;

    protract_x = (l_protract+r_protract)/2;
    lift_z = (l_lift+r_lift)/2;

    SubGetMessage(to_stretch_sub, &to_stretch_cmd_recv);

    float fb_speed_set = PIDCalculate(&protract_position_loop,protract_x,to_stretch_cmd_recv.fb);
    float ud_speed_set = PIDCalculate(&lift_position_loop,lift_z,to_stretch_cmd_recv.ud);

    if (to_stretch_cmd_recv.to_stretch_mode == TO_STRETCH_ZERO_FORCE)
    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
        DJIMotorStop(motor_lu);
        DJIMotorStop(motor_ru);
        DJIMotorStop(motor_ld);
        DJIMotorStop(motor_rd);
        DJIMotorStop(tuchuan);
    }
    else
    { // 正常工作
        DJIMotorEnable(motor_lu);
        DJIMotorEnable(motor_ru);
        DJIMotorEnable(motor_ld);
        DJIMotorEnable(motor_rd);
        DJIMotorEnable(tuchuan);
    }

    // 根据模式设定伸缩+图传运动形式
    switch (to_stretch_cmd_recv.to_stretch_mode)
    {
        case TO_STRETCH_ZERO_FORCE:
            DJIMotorStop(motor_lu);
            DJIMotorStop(motor_ru);
            DJIMotorStop(motor_ld);
            DJIMotorStop(motor_rd);
            DJIMotorStop(tuchuan);
            break;
        case TO_STRETCH_MODE:
            DJIMotorEnable(motor_lu);
            DJIMotorEnable(motor_ru);
            DJIMotorEnable(motor_ld);
            DJIMotorEnable(motor_rd);
            DJIMotorEnable(tuchuan);

            DJIMotorSetRef(motor_lu, ud_speed_set);
            DJIMotorSetRef(motor_ru, -ud_speed_set);
            DJIMotorSetRef(motor_ld, -fb_speed_set);
            DJIMotorSetRef(motor_rd, fb_speed_set);
            DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
            break;
        case TO_STRETCH_KEEP:
            DJIMotorEnable(motor_lu);
            DJIMotorEnable(motor_ru);
            DJIMotorEnable(motor_ld);
            DJIMotorEnable(motor_rd);
            DJIMotorEnable(tuchuan);
            break;
        default:
            break;
    }

    // 推送反馈消息
#ifdef ONE_BOARD
    PubPushMessage(to_stretch_pub, (void *)&to_stretch_feedback_data);
#endif
#ifdef CHASSIS_BOARD
    CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
#endif // CHASSIS_BOARD
}