wheel_legged/application/chassis/balance_old.c

//
// Created by SJQ on 2023/12/30.
//

#include "balance_old.h"

static void mylqr_k(float L0, float K[12]);

void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
{
    Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig);
    Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig);

    LQR_Init(&balanceControl->balance_LQR,InitConfig.LQR_state_num,InitConfig.LQR_control_num);
    PIDInit(&balanceControl->leg_coordination_PID,&InitConfig.leg_cor_PID_config);

    PIDInit(&balanceControl->leg_length_PID,&InitConfig.legInitConfig.length_PID_conf);
    memset(balanceControl->state,0,6);
    memset(balanceControl->target_state,0,6);
}

//解算地面支持力F
float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb);

// leg_phi[4] = {phi1_r,phi4_r,phi1_l,phi4_l}
void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z)
{
    Leg_State_update(&balanceControl->leg_right,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
    Leg_State_update(&balanceControl->leg_left,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);

    float K_matrix[12]={0};
    float L_average = (balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2;
        //(balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2;
    float alpha = PI/2 - balanceControl->leg_right.legState.phi0;

    float theta = alpha - body_phi;

//    static uint32_t dot_cnt = 0;
//    float dot_dt = DWT_GetDeltaT(&dot_cnt);
//    float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;

    float theta_dot = - balanceControl->leg_right.legState.phi0_dot - body_phi_dot;

    balanceControl->state[0] = theta;
    balanceControl->state[1] = theta_dot;
    balanceControl->state[2] = x; balanceControl->state[3] = x_dot;
    balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_dot;

    mylqr_k(L_average,K_matrix);
    LQR_set_K(&balanceControl->balance_LQR,K_matrix);
}



void Balance_Control_Loop(Balance_Control_t* balanceControl)
{
    LQR_update(&balanceControl->balance_LQR,balanceControl->state,balanceControl->target_state);

    balanceControl->balance_LQR.control_vector[1] = - balanceControl->balance_LQR.control_vector[1];
    float Tp = balanceControl->balance_LQR.control_vector[1];  //髋关节力矩
    float T = balanceControl->balance_LQR.control_vector[0];   //驱动轮力矩

    LegInstance *leg_r,*leg_l;
    leg_r = &balanceControl->leg_right;
    leg_l = &balanceControl->leg_left;
    //两条腿独立控制腿长
    float F_pid_out_r =  PIDCalculate(&leg_r->Length_PID,leg_r->legState.L0,leg_r->target_L0);
    float F_pid_out_l =  PIDCalculate(&leg_l->Length_PID,leg_l->legState.L0,leg_l->target_L0);
    //控制平均腿长
    float average_length = (leg_r->legState.L0 + leg_l->legState.L0)/2;
    float F_pid_out=  PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
    //float F_pid_out_l =  PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);

    float Tp_pid_out_r = PIDCalculate(&leg_r->Phi0_PID,leg_r->legState.phi0,leg_r->target_phi0);
    float Tp_pid_out_l = PIDCalculate(&leg_l->Phi0_PID,leg_l->legState.phi0,leg_l->target_phi0);

    float coordination_err = leg_r->legState.phi0 - leg_l->legState.phi0;

    float cor_Tp = PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0);

    Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 20,Tp + cor_Tp);
    Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 20,Tp - cor_Tp);

//    Leg_Input_update(&leg_r->legInput,F_pid_out_r + 20 ,Tp + cor_Tp);
//    Leg_Input_update(&leg_l->legInput,F_pid_out_l + 20 ,Tp - cor_Tp);

    VMC_getT(leg_r);
    VMC_getT(leg_l);
}

void target_x_set(Balance_Control_t* balanceControl,float x)
{
    balanceControl->target_state [2] = x;
}



/*
 * MYLQR_K  MATLAB生成
 *     K = MYLQR_K(L0)
 *
 * Arguments    : float L0
 *                float K[12]
 * Return Type  : void
 */
void mylqr_k(float L0, float K[12])
{
    float t2;
    float t3;

    float K_temp[12];
    /*     This function was generated by the Symbolic Math Toolbox version 9.2.
     */
    t2 = L0 * L0;
    t3 = t2 * L0;
    /*     06-Mar-2024 21:40:29 */
    K_temp[0] =
            ((L0 * -161.752151F + t2 * 164.793869F) - t3 * 97.0541687F) - 1.77457297F;
    K_temp[1] =
            ((L0 * 96.1845703F - t2 * 284.733704F) + t3 * 279.889618F) + 3.29777265F;
    K_temp[2] =
            ((L0 * -15.8959455F - t2 * 17.009819F) + t3 * 16.7305775F) - 0.452344805F;
    K_temp[3] =
            ((L0 * 5.66845322F - t2 * 16.5131245F) + t3 * 14.656208F) + 0.677895188F;
    K_temp[4] = ((L0 * -7.06628323F + t2 * 13.4040155F) - t3 * 8.83661747F) - 8.61461F;
    K_temp[5] =
            ((L0 * -28.798315F + t2 * 17.1446743F) + t3 * 15.3877935F) + 11.4884624F;
    K_temp[6] =
            ((L0 * -5.75727034F - t2 * 3.09837651F) + t3 * 10.1307449F) - 9.7942028F;
    K_temp[7] =
            ((L0 * -33.38451F + t2 * 35.8943558F) - t3 * 7.07260895F) + 11.7050676F;
    K_temp[8] =
            ((L0 * -83.7100143F + t2 * 109.333092F) - t3 * 54.9969864F) + 36.5699463F;
    K_temp[9] =
            ((L0 * 124.852882F - t2 * 253.387085F) + t3 * 193.295883F) + 55.7350769F;
    K_temp[10] = ((L0 * -6.44499636F + t2 * 5.48124027F) - t3 * 0.516800761F) +
            5.52106953F;
    K_temp[11] =
            ((L0 * 12.3147469F - t2 * 13.2560177F) + t3 * 1.4054811F) + 2.85080624F;

    //matlab赋值顺序不同
    for (int i = 0; i < 6; ++i) {
        K[i] = K_temp[2*i];
    }
    for (int i = 0; i < 6; ++i) {
        K[6+i] = K_temp[2*i+1];
    }
}

/*
 * File trailer for mylqr_k.c
 *
 * [EOF]
 */

//解算地面支持力F
float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb)
{
    //求导所用时间
    static uint32_t dot_cnt = 0;
    static float last_theta_dot,last_L_dot;
    float dot_dt = DWT_GetDeltaT(&dot_cnt);
    float theta_dot2 = (last_theta_dot - theta_dot) / dot_dt;
    float L_dot2 = (last_L_dot - L_dot) / dot_dt;

    float cos_theta = arm_cos_f32(theta);
    float sin_theta = arm_sin_f32(theta);
    float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta_dot * sin_theta
                    + L * theta_dot2 * sin_theta + L * theta_dot * theta_dot * cos_theta;

    float P = F_fb*cos_theta + Tp_fb*sin_theta/L;

    float F_N = P + 0.8f * (zw_dot2 + 9.8f);

    return F_N;

}