wheel_legged/application/chassis/chassis.c

/**
 * @file chassis.c
 * @author NeoZeng neozng1@hnu.edu.cn
 * @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出
 *        注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为x正方向;横向运动的右侧为y正方向
 *
 * @version 0.1
 * @date 2022-12-04
 *
 * @copyright Copyright (c) 2022
 *
 */

#include "chassis.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "HT04.h"
#include "LK9025.h"
#include "super_cap.h"
#include "message_center.h"
#include "referee_task.h"
#include "power_meter.h"
#include "ins_task.h"

#include "balance_old.h"
#include "observer.h"
#include "estimator.h"

#include "general_def.h"
#include "bsp_dwt.h"
#include "referee_UI.h"
#include "arm_math.h"
#include "vofa.h"
#include "user_lib.h"
/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长

/* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
#ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
#include "can_comm.h"
#include "ins_task.h"
static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
INS_t *Chassis_IMU_data;
#endif // CHASSIS_BOARD
#ifdef ONE_BOARD
static Publisher_t *chassis_pub;                    // 用于发布底盘的数据
static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控制命令
INS_t *Chassis_IMU_data;
#endif                                              // !ONE_BOARD
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据

static referee_info_t* referee_data; // 用于获取裁判系统的数据
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI

static SuperCapInstance *cap;                                       // 超级电容
static PowerMeterInstance *power_meter;                             //功率计

static HTMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
static LKMotorInstance *wheel_motor_r,*wheel_motor_l;
/* 用于自旋变速策略的时间变量 */

/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅

static LegInstance legInstance;

Balance_Control_t BalanceControl;

BalanceObserver Observer;

PIDInstance Turn_Loop_PID , Roll_Loop_PID;

Chassis_state_e ChassisState;

void ChassisInit()
{
    // 四个关节电机的参数,改tx_id和反转标志位即可
    Motor_Init_Config_s chassis_motor_config = {
        .can_init_config.can_handle = &hcan2,
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = OPEN_LOOP,
            .close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
        },
        .motor_type = HT04,
    };

    Motor_Init_Config_s wheel_motor_config = {
            .can_init_config.can_handle = &hcan1,
            .controller_param_init_config ={
                    .speed_PID = {
                            .Kp = -100,  // 50
                            .Ki = 0, // 200
                            .Kd = 0,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                            .IntegralLimit = 3000,
                            .MaxOut = 20000,
                    },
            },
            .controller_setting_init_config = {
                    .angle_feedback_source = MOTOR_FEED,
                    .speed_feedback_source = MOTOR_FEED,
                    .outer_loop_type = OPEN_LOOP,
                    .close_loop_type = OPEN_LOOP,
            },
            .motor_type = LK9025,
    };
    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
    chassis_motor_config.can_init_config.tx_id = 1;
    //chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_rf = HTMotorInit(&chassis_motor_config);


    chassis_motor_config.can_init_config.tx_id = 2;
    //chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_rb = HTMotorInit(&chassis_motor_config);


    chassis_motor_config.can_init_config.tx_id = 3;
    //chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    motor_lf = HTMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 4;
    //chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_lb = HTMotorInit(&chassis_motor_config);

    //@todo:瓴控驱动代码有些问题  1号电机必须先初始化

    wheel_motor_config.can_init_config.tx_id = 1;
    wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
    wheel_motor_r = LKMotorInit(&wheel_motor_config);

    wheel_motor_config.can_init_config.tx_id = 2;
    wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    wheel_motor_l = LKMotorInit(&wheel_motor_config);



    referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI

    SuperCap_Init_Config_s cap_conf = {
        .can_config = {
            .can_handle = &hcan2,
            .tx_id = 0x302, // 超级电容默认接收id
            .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
        }};
    cap = SuperCapInit(&cap_conf); // 超级电容初始化
    PowerMeter_Init_Config_s power_conf = {
            .can_config = {
                    .can_handle = &hcan1,
                    .rx_id = 0x212,
            }
    };
    //功率計初始化
    power_meter = PowerMeterInit(&power_conf);

    Balance_Init_config_s balanceInitConfig = {
            .legInitConfig = {
                    .length_PID_conf = {
                            .Kp = 500,//180 ,//50,
                            .Ki = 100,//5,
                            .Kd = 20,//10,//6,//6,
                            .MaxOut = 100,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
                            .Derivative_LPF_RC = 0.001f,//1/(2*PI*10),
                            .IntegralLimit = 100,
                    },
                    .phi0_PID_conf = {
                            .Kp = 80,
                            .Ki = 0,
                            .Kd = 5,
                            .MaxOut = 5,
                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement| PID_DerivativeFilter,
                            .Derivative_LPF_RC = 1/(2*PI*10),
                            .IntegralLimit = 100,
                    },
                    .init_target_L0 = 0.15f,
                    .F_feedforward = 0,
            },
            .leg_cor_PID_config = {
                    .Kp = 25.0f,//25,//25,
                    .Ki = 0,
                    .Kd = 3.0f,//3,
                    .MaxOut = 3,
                    .Improve =  PID_Derivative_On_Measurement| PID_DerivativeFilter,
                    .Derivative_LPF_RC = 0.01f,
            },
            .LQR_state_num = 6,
            .LQR_control_num = 2,

    };

    //Leg_Init(&legInstance,&leg_r_init_conf);
    Balance_Control_Init(&BalanceControl,balanceInitConfig);
    Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
    //转向环
    PID_Init_Config_s turn_pid_cfg = {
            .Kp = 10.0f,//10.0f,//1,
            .Ki = 0,
            .Kd = 0.0f,
            .MaxOut = 2,
            .Improve =  PID_Derivative_On_Measurement|PID_DerivativeFilter,
            .Derivative_LPF_RC = 0.001f
    };
    PIDInit(&Turn_Loop_PID,&turn_pid_cfg);

    //Roll轴平衡
    PID_Init_Config_s roll_pid_cfg = {
            .Kp = 8.0f,
            .Ki = 0,
            .Kd = 0.5f,
            .MaxOut = 100,
            .Improve =  PID_Derivative_On_Measurement,
    };
    PIDInit(&Roll_Loop_PID,&roll_pid_cfg);

    Observer_Init(&Observer);

    // 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD
    Chassis_IMU_data = INS_Init(); // 底盘IMU初始化

    CANComm_Init_Config_s comm_conf = {
        .can_config = {
            .can_handle = &hcan1,
            .tx_id = 0x311,
            .rx_id = 0x312,
        },
        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
        .send_data_len = sizeof(Chassis_Upload_Data_s),
    };
    chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化
#endif                                          // CHASSIS_BOARD

#ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息
    chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
    chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
#endif // ONE_BOARD
}


/**
 * @brief 获取数据并计算状态量
 *
 */
static void Chassis_State_update()
{
    float leg_phi[4]={0};
    float leg_phi_dot[4] = {0};
    leg_phi[0] = PI-(motor_rf->measure.total_angle - 0.119031f);
    leg_phi[1] = -(motor_rb->measure.total_angle + 0.119031f);
    leg_phi[2] = PI-(-motor_lf->measure.total_angle - 0.119031f);
    leg_phi[3] = -(-motor_lb->measure.total_angle + 0.119031f);

//最高点上电
//    leg_phi[0] = PI-(motor_rf->measure.total_angle + 0.81007f);
//    leg_phi[1] = -(motor_rb->measure.total_angle - 0.81007f);
//    leg_phi[2] = PI-(-motor_lf->measure.total_angle + 0.81007f);
//    leg_phi[3] = -(-motor_lb->measure.total_angle - 0.81007f);

    leg_phi_dot[0] = -motor_rf->measure.speed_rads;
    leg_phi_dot[1] = -motor_rb->measure.speed_rads;
    leg_phi_dot[2] = motor_lf->measure.speed_rads;
    leg_phi_dot[3] = motor_lb->measure.speed_rads;


    float body_phi = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
    float body_phi_dot = Chassis_IMU_data->Gyro[X];

    //陀螺仪积分获取位移和速度
    static uint32_t integral_cnt = 0;
    static float imu_v,imu_x;
    float integral_dt = DWT_GetDeltaT(&integral_cnt);

    if(fabsf(Chassis_IMU_data->MotionAccel_n[X])>=0.02)
    {
        imu_v += Chassis_IMU_data->MotionAccel_n[X] * integral_dt;

    }
        imu_x += imu_v * integral_dt;


    //float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
    static float left_offset = 0;
    static float right_offset = 0;
    //倒地需要清零总位移数据
    if(chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
    {
        left_offset = wheel_motor_l->measure.total_angle;
        right_offset = wheel_motor_r->measure.total_angle;
    }

    float x_l = wheel_motor_l->measure.total_angle - left_offset;
    float x_r = wheel_motor_r->measure.total_angle - right_offset;
    //驱动轮位移与速度
//    float total_angle = (- wheel_motor_r->measure.total_angle + wheel_motor_l->measure.total_angle)/2;
//    float x =  (total_angle * DEGREE_2_RAD ) * RADIUS_WHEEL;
//    float speed = (- wheel_motor_r->measure.speed_rads + wheel_motor_l->measure.speed_rads)/2;
//    float x_dot = speed * RADIUS_WHEEL;

    float total_angle = (- x_r + x_l)/2;
    float x =  (total_angle * DEGREE_2_RAD ) * RADIUS_WHEEL;
    float speed = (- wheel_motor_r->measure.speed_rads + wheel_motor_l->measure.speed_rads)/2;
    float x_dot = speed * RADIUS_WHEEL;

//    Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current / HT_CMD_COEF,motor_rb->measure.real_current / HT_CMD_COEF);
//    Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current / HT_CMD_COEF,motor_lb->measure.real_current / HT_CMD_COEF);

    Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n);

    if( ChassisState == FAIL )
    {
        if(abs(body_phi) <= 0.05 && abs(body_phi) <= 0.005 )
            ChassisState = STAND;
    }

}

/* 机器人底盘控制核心任务 */
void ChassisTask()
{
    // 后续增加没收到消息的处理(双板的情况)
    // 获取新的控制信息
#ifdef ONE_BOARD
    SubGetMessage(chassis_sub, &chassis_cmd_recv);
#endif
#ifdef CHASSIS_BOARD
    chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
#endif // CHASSIS_BOARD

    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
        HTMotorStop(motor_lf);
        HTMotorStop(motor_rf);
        HTMotorStop(motor_lb);
        HTMotorStop(motor_rb);
        LKMotorStop(wheel_motor_r);
        LKMotorStop(wheel_motor_l);

        //清除相关pid控制量
        PIDClear(&BalanceControl.leg_length_PID);
        PIDClear(&Turn_Loop_PID);
        PIDClear(&Roll_Loop_PID);
        PIDClear(&BalanceControl.leg_coordination_PID);
        //清除状态量
        state_clear(&BalanceControl);
    }
    else
    { // 正常工作
        HTMotorEnable(motor_lf);
        HTMotorEnable(motor_rf);
        HTMotorEnable(motor_lb);
        HTMotorEnable(motor_rb);
        LKMotorEnable(wheel_motor_r);
        LKMotorEnable(wheel_motor_l);
    }


    static int8_t init_flag = FALSE;
    // 根据控制模式设定旋转速度
    switch (chassis_cmd_recv.chassis_mode)
    {
    case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
        //chassis_cmd_recv.wz = 0;
        break;
    case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
        //chassis_cmd_recv.wz = 1.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
        chassis_cmd_recv.wz = 0.01f * chassis_cmd_recv.offset_angle* abs(chassis_cmd_recv.offset_angle);
        break;
    case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
        //chassis_cmd_recv.wz = 4000;
        chassis_cmd_recv.wz = 2.0f;
        break;
    default:
        break;
    }

    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
    static float sin_theta, cos_theta;
//    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
//    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
    sin_theta = 0; cos_theta = 1;
    chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
    chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;

    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
    // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
    // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
    // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
    // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
    // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;

    //获取数据并计算状态量
    Chassis_State_update();

    float vofa_send_data[12];

//    for(int i = 0; i<6;i++)
//        vofa_send_data[i] = BalanceControl.state[i];
//    for(int i = 0; i<6;i++)
//        vofa_send_data[i+6] = BalanceControl.state_observer.Estimate_X[i];
    vofa_justfloat_output(vofa_send_data,12*4,&huart1);

    static float target_x = 0;
    static float target_yaw = 0;
    if(chassis_cmd_recv.chassis_mode != CHASSIS_ZERO_FORCE)  // 右侧开关状态[下],底盘跟随云台
    {
        float Roll_pid_out = PIDCalculate(&Roll_Loop_PID,-Chassis_IMU_data->Roll,0);
        //将roll轴自平衡PID输出直接叠加到前馈支持力上
        BalanceControl.leg_right.F_feedforward = Roll_pid_out;
        BalanceControl.leg_left.F_feedforward = -Roll_pid_out;

        //target_x += chassis_cmd_recv.vx/5280 * 0.003f;
        target_x_set(&BalanceControl,chassis_cmd_recv.vx/5280 * 0.003f);
        target_L_set(&BalanceControl,chassis_cmd_recv.vy/5280 * 0.0003f);
        Balance_Control_Loop(&BalanceControl);

        static float wz_feedback;
        wz_feedback = (1.0f - 0.75f) * wz_feedback + 0.75f * Chassis_IMU_data->Gyro[Z];
        float turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);

        static float T,wheel_current_r,wheel_current_l;

        T = BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);

        //T = (1.0f - 0.75f) * T + 0.75f * BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);

//        wheel_current_r = (1.0f - 0.75f) * wheel_current_r + 0.75f * (T+turn_T) * LK_TORQUE_2_CMD;//+turn_T
//        wheel_current_l = (1.0f - 0.75f) * wheel_current_l + 0.75f * (T-turn_T) * LK_TORQUE_2_CMD;//-turn_T

        wheel_current_r = (T+turn_T) * LK_TORQUE_2_CMD;
        wheel_current_l = (T-turn_T) * LK_TORQUE_2_CMD;

        float_constrain(wheel_current_r,-4000,4000);
        float_constrain(wheel_current_l,-4000,4000);

//        if (ChassisState == FAIL) // 倒地 未起身
//        {
//            HTMotorSetRef(motor_rf,0);
//            HTMotorSetRef(motor_rb,0);
//
//            HTMotorSetRef(motor_lf,0);
//            HTMotorSetRef(motor_lb,0);
//
//            PIDClear(&BalanceControl.leg_length_PID);
//            PIDClear(&Turn_Loop_PID);
//            PIDClear(&Roll_Loop_PID);
//            PIDClear(&BalanceControl.leg_coordination_PID);
//        }
//        else
        {
            HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
            HTMotorSetRef(motor_rb,-BalanceControl.leg_right.legOutput.T2 * HT_CMD_COEF);

            HTMotorSetRef(motor_lf,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF);
            HTMotorSetRef(motor_lb,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF);
//            HTMotorSetRef(motor_rf,0);
//            HTMotorSetRef(motor_rb,0);
//
//            HTMotorSetRef(motor_lf,0);
//            HTMotorSetRef(motor_lb,0);
        }

        LKMotorSetRef(wheel_motor_r,wheel_current_r);
        LKMotorSetRef(wheel_motor_l,wheel_current_l);
//        LKMotorSetRef(wheel_motor_r,0);
//        LKMotorSetRef(wheel_motor_l,0);

//        Observer_Estimate(&Observer,BalanceControl.state,BalanceControl.balance_LQR.control_vector, BalanceControl.leg_right.legState.L0);
//        Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current/HT_CMD_COEF,motor_rb->measure.real_current/HT_CMD_COEF);
//        Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current/HT_CMD_COEF,motor_lb->measure.real_current/HT_CMD_COEF);
    }
    else{
        PIDClear(&BalanceControl.leg_length_PID);
        PIDClear(&Turn_Loop_PID);
        PIDClear(&Roll_Loop_PID);
        PIDClear(&BalanceControl.leg_coordination_PID);
    }





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