diff --git a/application/chassis/balance.c b/application/chassis/balance.c
new file mode 100644
index 0000000..c1dd908
--- /dev/null
+++ b/application/chassis/balance.c
@@ -0,0 +1,136 @@
+//
+// Created by SJQ on 2023/12/30.
+//
+
+#include "balance.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);
+    memset(balanceControl->state,0,6);
+    memset(balanceControl->target_state,0,6);
+}
+// 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)
+{
+    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_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);
+    float K_matrix_fix[12];
+    for (int i = 0; i < 6; ++i) {
+        K_matrix_fix[i] = K_matrix[2*i];
+    }
+    for (int i = 0; i < 6; ++i) {
+        K_matrix_fix[6+i] = K_matrix[2*i+1];
+    }
+
+    LQR_set_K(&balanceControl->balance_LQR,K_matrix_fix);
+}
+
+void Balance_Control_Loop(Balance_Control_t* balanceControl)
+{
+    LQR_update(&balanceControl->balance_LQR,balanceControl->state,balanceControl->target_state);
+
+    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 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_r+leg_r->F_feedforward,Tp + cor_Tp);
+    Leg_Input_update(&leg_l->legInput,F_pid_out_l+leg_l->F_feedforward,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;
+    /*     This function was generated by the Symbolic Math Toolbox version 9.2.
+     */
+    /*     10-Jan-2024 15:16:33 */
+    t2 = L0 * L0;
+    t3 = t2 * L0;
+    K[0] = ((L0 * -79.173111F - t2 * 4.61935F) + t3 * 53.7748F) - 1.68270767F;
+    K[1] =
+            ((L0 * 163.861588F - t2 * 454.401855F) + t3 * 423.311035F) + 0.115447611F;
+    K[2] = ((L0 * -5.08536911F - t2 * 30.8126068F) + t3 * 29.5321121F) -
+           0.590248764F;
+    K[3] =
+            ((L0 * 14.4037905F - t2 * 24.0141125F) + t3 * 13.8384714F) + 0.648175836F;
+    K[4] =
+            ((L0 * 10.8009977F - t2 * 72.3231735F) + t3 * 103.75676F) - 5.87192822F;
+    K[5] =
+            ((L0 * 48.2422218F - t2 * 312.532166F) + t3 * 420.993896F) + 12.3104877F;
+    K[6] =
+            ((L0 * 22.1635437F - t2 * 126.369873F) + t3 * 170.583176F) - 7.8162446F;
+    K[7] = ((L0 * 26.8571F - t2 * 228.328079F) + t3 * 316.050446F) + 14.7130146F;
+    K[8] =
+            ((L0 * 15.8044338F - t2 * 196.455856F) + t3 * 276.879242F) + 20.4759655F;
+    K[9] =
+            ((L0 * -80.1292953F + t2 * 698.451477F) - t3 * 1040.30969F) + 51.8307953F;
+    K[10] =
+            ((L0 * -2.62232423F - t2 * 4.29178047F) + t3 * 7.45553255F) + 4.33007193F;
+    K[11] =
+            ((L0 * -20.6260967F + t2 * 144.286728F) - t3 * 203.665939F) + 2.42666435F;
+}
+
+/*
+ * File trailer for mylqr_k.c
+ *
+ * [EOF]
+ */
diff --git a/application/chassis/balance.h b/application/chassis/balance.h
new file mode 100644
index 0000000..f0ca8b4
--- /dev/null
+++ b/application/chassis/balance.h
@@ -0,0 +1,39 @@
+//
+// Created by SJQ on 2023/12/30.
+//
+
+#ifndef WHEEL_LEGGED_BALANCE_H
+#define WHEEL_LEGGED_BALANCE_H
+
+#include "leg.h"
+#include "LQR.h"
+#include "controller.h"
+typedef struct
+{
+    LegInstance leg_right;
+    LegInstance leg_left;
+    PIDInstance leg_coordination_PID;  //双腿协调PD控制
+
+    float state[6];
+    float target_state[6];
+
+    LQRInstance balance_LQR;
+}Balance_Control_t;
+
+typedef struct
+{
+    Leg_init_config_s legInitConfig;
+    PID_Init_Config_s leg_cor_PID_config;
+
+    int LQR_state_num;
+    int LQR_control_num;
+}Balance_Init_config_s;
+
+void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig);
+//void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float body_phi,float body_phi_dot,float x,float x_dot);
+
+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);
+void Balance_Control_Loop(Balance_Control_t* balanceControl);
+void target_x_set(Balance_Control_t* balanceControl,float x);
+
+#endif //WHEEL_LEGGED_BALANCE_H
diff --git a/application/chassis/cal_phi0_dot.c b/application/chassis/cal_phi0_dot.c
new file mode 100644
index 0000000..722c125
--- /dev/null
+++ b/application/chassis/cal_phi0_dot.c
@@ -0,0 +1,626 @@
+//
+// Created by SJQ on 2024/1/3.
+//
+
+#include "controller.h"
+#include "cal_phi0_dot.h"
+#include "arm_math.h"
+/* Function Definitions */
+/*
+ * Arguments    : float phi_1
+ *                float phi_4
+ *                float d_phi1
+ *                float d_phi4
+ * Return Type  : float
+ */
+float get_dphi0(float phi_1, float phi_4, float d_phi1, float d_phi4)
+{
+    float a;
+    float a_tmp;
+    float ab_a;
+    float ac_a;
+    float ad_a;
+    float b_a;
+    float b_a_tmp;
+    float b_d_phi0_tmp;
+    float b_d_phi0_tmp_tmp;
+    float b_out;
+    float bb_a;
+    float bc_a;
+    float bd_a;
+    float c_a;
+    float c_d_phi0_tmp;
+    float c_d_phi0_tmp_tmp;
+    float c_out;
+    float cb_a;
+    float cc_a;
+    float cd_a;
+    float d_a;
+    float d_d_phi0_tmp;
+    float d_d_phi0_tmp_tmp;
+    float d_out;
+    float d_phi0_tmp;
+    float d_phi0_tmp_tmp;
+    float db_a;
+    float dc_a;
+    float dd_a;
+    float e_a;
+    float e_d_phi0_tmp;
+    float e_d_phi0_tmp_tmp;
+    float e_out;
+    float eb_a;
+    float ec_a;
+    float ed_a;
+    float f_a;
+    float f_d_phi0_tmp;
+    float f_out;
+    float fb_a;
+    float fc_a;
+    float fd_a;
+    float g_a;
+    float g_d_phi0_tmp;
+    float g_out;
+    float gb_a;
+    float gc_a;
+    float gd_a;
+    float h_a;
+    float h_d_phi0_tmp;
+    float h_out;
+    float hb_a;
+    float hc_a;
+    float i_a;
+    float i_d_phi0_tmp;
+    float i_out;
+    float ib_a;
+    float ic_a;
+    float j_a;
+    float j_d_phi0_tmp;
+    float j_out;
+    float jb_a;
+    float jc_a;
+    float k_a;
+    float k_d_phi0_tmp;
+    float k_out;
+    float kb_a;
+    float kc_a;
+    float l_a;
+    float l_d_phi0_tmp;
+    float l_out;
+    float lb_a;
+    float lc_a;
+    float m_a;
+    float m_out;
+    float mb_a;
+    float mc_a;
+    float n_a;
+    float n_out;
+    float nb_a;
+    float nc_a;
+    float o_a;
+    float o_out;
+    float ob_a;
+    float oc_a;
+    float out;
+    float p_a;
+    float p_out;
+    float pb_a;
+    float pc_a;
+    float q_a;
+    float q_out;
+    float qb_a;
+    float qc_a;
+    float r_a;
+    float rb_a;
+    float rc_a;
+    float s_a;
+    float sb_a;
+    float sc_a;
+    float t_a;
+    float tb_a;
+    float tc_a;
+    float u_a;
+    float ub_a;
+    float uc_a;
+    float v_a;
+    float vb_a;
+    float vc_a;
+    float w_a;
+    float wb_a;
+    float wc_a;
+    float x_a;
+    float xb_a;
+    float xc_a;
+    float y_a;
+    float yb_a;
+    float yc_a;
+    d_phi0_tmp = arm_cos_f32(phi_1);
+    d_phi0_tmp_tmp = arm_sin_f32(phi_1);
+    b_d_phi0_tmp_tmp = 0.15F * d_phi0_tmp_tmp;
+    c_d_phi0_tmp_tmp = arm_sin_f32(phi_4);
+    b_d_phi0_tmp = b_d_phi0_tmp_tmp - 0.15F * c_d_phi0_tmp_tmp;
+    d_d_phi0_tmp_tmp = 0.15F * d_phi0_tmp;
+    e_d_phi0_tmp_tmp = arm_cos_f32(phi_4);
+    c_d_phi0_tmp = (0.15F - d_d_phi0_tmp_tmp) + 0.15F * e_d_phi0_tmp_tmp;
+    d_d_phi0_tmp = 0.54F * c_d_phi0_tmp;
+    e_d_phi0_tmp = 0.54F * b_d_phi0_tmp;
+    f_d_phi0_tmp = 0.3F * d_phi0_tmp * b_d_phi0_tmp;
+    g_d_phi0_tmp = 0.3F * d_phi0_tmp_tmp * c_d_phi0_tmp;
+    h_d_phi0_tmp = 2.0F * (f_d_phi0_tmp + g_d_phi0_tmp);
+    i_d_phi0_tmp = 0.0874800086F * d_phi0_tmp * b_d_phi0_tmp;
+    j_d_phi0_tmp = 0.0874800086F * d_phi0_tmp_tmp * c_d_phi0_tmp;
+    d_phi0_tmp *= 0.0810000077F;
+    f_d_phi0_tmp = (0.0810000077F * d_phi0_tmp_tmp + f_d_phi0_tmp) + g_d_phi0_tmp;
+    g_d_phi0_tmp = 0.3F * e_d_phi0_tmp_tmp * b_d_phi0_tmp;
+    d_phi0_tmp_tmp = 0.3F * c_d_phi0_tmp_tmp * c_d_phi0_tmp;
+    k_d_phi0_tmp = 2.0F * (g_d_phi0_tmp + d_phi0_tmp_tmp);
+    b_d_phi0_tmp *= 0.0874800086F * e_d_phi0_tmp_tmp;
+    c_d_phi0_tmp *= 0.0874800086F * c_d_phi0_tmp_tmp;
+    l_d_phi0_tmp = 0.0810000077F * e_d_phi0_tmp_tmp;
+    g_d_phi0_tmp =
+            (0.0810000077F * c_d_phi0_tmp_tmp + g_d_phi0_tmp) + d_phi0_tmp_tmp;
+    d_phi0_tmp_tmp = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    e_d_phi0_tmp_tmp = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    c_d_phi0_tmp_tmp = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    out = (e_d_phi0_tmp_tmp * e_d_phi0_tmp_tmp +
+           c_d_phi0_tmp_tmp * c_d_phi0_tmp_tmp) +
+          0.0729000047F;
+    e_d_phi0_tmp_tmp = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    c_d_phi0_tmp_tmp = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    b_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    c_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    d_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    e_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    f_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    b_out = (e_a * e_a + f_a * f_a) + 0.0729000047F;
+    e_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    f_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    g_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    h_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    i_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    j_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    c_out = (i_a * i_a + j_a * j_a) + 0.0729000047F;
+    i_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    j_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    k_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    a_tmp = 0.54F * ((0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4));
+    j_a = (((j_a * j_a + k_a * k_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
+    k_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    l_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    m_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    d_out = (l_a * l_a + m_a * m_a) + 0.0729000047F;
+    l_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    b_a_tmp = 0.54F * (0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4));
+    k_a = sqrtf((0.291600019F * (k_a * k_a) -
+                 (d_out - 0.0729000047F) * (d_out - 0.0729000047F)) +
+                0.291600019F * (l_a * l_a)) -
+          b_a_tmp;
+    l_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    m_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    l_a = (((l_a * l_a + m_a * m_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
+    m_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    n_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    o_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    d_out = (n_a * n_a + o_a * o_a) + 0.0729000047F;
+    n_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    o_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    p_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    q_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    r_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    s_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    e_out = (r_a * r_a + s_a * s_a) + 0.0729000047F;
+    r_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    s_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    t_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    u_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    v_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    w_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    f_out = (v_a * v_a + w_a * w_a) + 0.0729000047F;
+    v_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    w_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    x_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    y_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ab_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    bb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    cb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    db_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    g_out = (cb_a * cb_a + db_a * db_a) + 0.0729000047F;
+    cb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    db_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    eb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    fb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    gb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    hb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    h_out = (gb_a * gb_a + hb_a * hb_a) + 0.0729000047F;
+    gb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    hb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ib_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    hb_a =
+            (((hb_a * hb_a + ib_a * ib_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
+    ib_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    jb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    kb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    i_out = (jb_a * jb_a + kb_a * kb_a) + 0.0729000047F;
+    jb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    ib_a = sqrtf((0.291600019F * (ib_a * ib_a) -
+                  (i_out - 0.0729000047F) * (i_out - 0.0729000047F)) +
+                 0.291600019F * (jb_a * jb_a)) -
+           b_a_tmp;
+    jb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    kb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    jb_a =
+            (((jb_a * jb_a + kb_a * kb_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
+    kb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    lb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    mb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    i_out = (lb_a * lb_a + mb_a * mb_a) + 0.0729000047F;
+    lb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    mb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    nb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    kb_a = (0.27F * arm_cos_f32(2.0F *
+                         atanf((sqrtf((0.291600019F * (kb_a * kb_a) -
+                                       (i_out - 0.0729000047F) *
+                                       (i_out - 0.0729000047F)) +
+                                      0.291600019F * (lb_a * lb_a)) -
+                                b_a_tmp) /
+                               ((((mb_a * mb_a + nb_a * nb_a) + 0.0729000047F) -
+                                 0.0729000047F) +
+                                a_tmp))) -
+            0.075F) +
+           0.15F * arm_cos_f32(phi_1);
+    lb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    mb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    nb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    i_out = (mb_a * mb_a + nb_a * nb_a) + 0.0729000047F;
+    mb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    nb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ob_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    lb_a = 0.27F *
+           arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (lb_a * lb_a) -
+                                     (i_out - 0.0729000047F) *
+                                     (i_out - 0.0729000047F)) +
+                                    0.291600019F * (mb_a * mb_a)) -
+                              b_a_tmp) /
+                             ((((nb_a * nb_a + ob_a * ob_a) + 0.0729000047F) -
+                               0.0729000047F) +
+                              a_tmp))) +
+           0.15F * arm_sin_f32(phi_1);
+    mb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    nb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ob_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    i_out = (nb_a * nb_a + ob_a * ob_a) + 0.0729000047F;
+    nb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    ob_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    pb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    mb_a = (0.27F * arm_cos_f32(2.0F *
+                         atanf((sqrtf((0.291600019F * (mb_a * mb_a) -
+                                       (i_out - 0.0729000047F) *
+                                       (i_out - 0.0729000047F)) +
+                                      0.291600019F * (nb_a * nb_a)) -
+                                b_a_tmp) /
+                               ((((ob_a * ob_a + pb_a * pb_a) + 0.0729000047F) -
+                                 0.0729000047F) +
+                                a_tmp))) -
+            0.075F) +
+           0.15F * arm_cos_f32(phi_1);
+    nb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ob_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    pb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    i_out = (ob_a * ob_a + pb_a * pb_a) + 0.0729000047F;
+    ob_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    pb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    qb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    rb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    sb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    tb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ub_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    vb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    j_out = (ub_a * ub_a + vb_a * vb_a) + 0.0729000047F;
+    ub_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    vb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    wb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    xb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    yb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ac_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    k_out = (yb_a * yb_a + ac_a * ac_a) + 0.0729000047F;
+    yb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    ac_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    bc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    ac_a =
+            (((ac_a * ac_a + bc_a * bc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
+    bc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    cc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    dc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    l_out = (cc_a * cc_a + dc_a * dc_a) + 0.0729000047F;
+    cc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    bc_a = sqrtf((0.291600019F * (bc_a * bc_a) -
+                  (l_out - 0.0729000047F) * (l_out - 0.0729000047F)) +
+                 0.291600019F * (cc_a * cc_a)) -
+           b_a_tmp;
+    cc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    dc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    cc_a =
+            (((cc_a * cc_a + dc_a * dc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
+    dc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ec_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    fc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    l_out = (ec_a * ec_a + fc_a * fc_a) + 0.0729000047F;
+    ec_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    fc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    gc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    hc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ic_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    jc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    m_out = (ic_a * ic_a + jc_a * jc_a) + 0.0729000047F;
+    ic_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    jc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    kc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    lc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    mc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    nc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    n_out = (mc_a * mc_a + nc_a * nc_a) + 0.0729000047F;
+    mc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    nc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    oc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    pc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    qc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    rc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    sc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    tc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    o_out = (sc_a * sc_a + tc_a * tc_a) + 0.0729000047F;
+    sc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    tc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    uc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    vc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    wc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    xc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    p_out = (wc_a * wc_a + xc_a * xc_a) + 0.0729000047F;
+    wc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    xc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    yc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    xc_a =
+            (((xc_a * xc_a + yc_a * yc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
+    yc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ad_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    bd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    q_out = (ad_a * ad_a + bd_a * bd_a) + 0.0729000047F;
+    ad_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    yc_a = sqrtf((0.291600019F * (yc_a * yc_a) -
+                  (q_out - 0.0729000047F) * (q_out - 0.0729000047F)) +
+                 0.291600019F * (ad_a * ad_a)) -
+           b_a_tmp;
+    ad_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    bd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    ad_a =
+            (((ad_a * ad_a + bd_a * bd_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
+    bd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    cd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    dd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    q_out = (cd_a * cd_a + dd_a * dd_a) + 0.0729000047F;
+    cd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    dd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ed_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    bd_a = (0.27F * arm_cos_f32(2.0F *
+                         atanf((sqrtf((0.291600019F * (bd_a * bd_a) -
+                                       (q_out - 0.0729000047F) *
+                                       (q_out - 0.0729000047F)) +
+                                      0.291600019F * (cd_a * cd_a)) -
+                                b_a_tmp) /
+                               ((((dd_a * dd_a + ed_a * ed_a) + 0.0729000047F) -
+                                 0.0729000047F) +
+                                a_tmp))) -
+            0.075F) +
+           0.15F * arm_cos_f32(phi_1);
+    cd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    dd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ed_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    q_out = (dd_a * dd_a + ed_a * ed_a) + 0.0729000047F;
+    dd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    ed_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    fd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    cd_a = 0.27F *
+           arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (cd_a * cd_a) -
+                                     (q_out - 0.0729000047F) *
+                                     (q_out - 0.0729000047F)) +
+                                    0.291600019F * (dd_a * dd_a)) -
+                              b_a_tmp) /
+                             ((((ed_a * ed_a + fd_a * fd_a) + 0.0729000047F) -
+                               0.0729000047F) +
+                              a_tmp))) +
+           0.15F * arm_sin_f32(phi_1);
+    dd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    ed_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    fd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    q_out = (ed_a * ed_a + fd_a * fd_a) + 0.0729000047F;
+    ed_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    fd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
+    gd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
+    dd_a = (0.27F * arm_cos_f32(2.0F *
+                         atanf((sqrtf((0.291600019F * (dd_a * dd_a) -
+                                       (q_out - 0.0729000047F) *
+                                       (q_out - 0.0729000047F)) +
+                                      0.291600019F * (ed_a * ed_a)) -
+                                b_a_tmp) /
+                               ((((fd_a * fd_a + gd_a * gd_a) + 0.0729000047F) -
+                                 0.0729000047F) +
+                                a_tmp))) -
+            0.075F) +
+           0.15F * arm_cos_f32(phi_1);
+    return d_phi1 *
+           ((d_d_phi0_tmp_tmp +
+             0.54F *
+             arm_cos_f32(2.0F *
+                  atanf((sqrtf((0.291600019F *
+                                (d_phi0_tmp_tmp * d_phi0_tmp_tmp) -
+                                (out - 0.0729000047F) *
+                                (out - 0.0729000047F)) +
+                               0.291600019F * (e_d_phi0_tmp_tmp *
+                                               e_d_phi0_tmp_tmp)) -
+                         e_d_phi0_tmp) /
+                        ((((c_d_phi0_tmp_tmp * c_d_phi0_tmp_tmp + a * a) +
+                           0.0729000047F) -
+                          0.0729000047F) +
+                         d_d_phi0_tmp))) *
+             ((((i_d_phi0_tmp - h_d_phi0_tmp * (((b_a * b_a + c_a * c_a) +
+                                                 0.0729000047F) -
+                                                0.0729000047F)) +
+                j_d_phi0_tmp) /
+               (2.0F * sqrtf((0.291600019F * (d_a * d_a) -
+                              (b_out - 0.0729000047F) *
+                              (b_out - 0.0729000047F)) +
+                             0.291600019F * (e_a * e_a))) -
+               d_phi0_tmp) /
+              ((((f_a * f_a + g_a * g_a) + 0.0729000047F) -
+                0.0729000047F) +
+               d_d_phi0_tmp) -
+              (sqrtf((0.291600019F * (h_a * h_a) -
+                      (c_out - 0.0729000047F) * (c_out - 0.0729000047F)) +
+                     0.291600019F * (i_a * i_a)) -
+               e_d_phi0_tmp) *
+              f_d_phi0_tmp / (j_a * j_a)) /
+             (k_a * k_a / (l_a * l_a) + 1.0F)) /
+            ((0.27F *
+              arm_cos_f32(2.0F *
+                   atanf((sqrtf((0.291600019F * (m_a * m_a) -
+                                 (d_out - 0.0729000047F) *
+                                 (d_out - 0.0729000047F)) +
+                                0.291600019F * (n_a * n_a)) -
+                          e_d_phi0_tmp) /
+                         ((((o_a * o_a + p_a * p_a) + 0.0729000047F) -
+                           0.0729000047F) +
+                          d_d_phi0_tmp))) -
+              0.075F) +
+             d_d_phi0_tmp_tmp) +
+            (0.27F * arm_sin_f32(2.0F *
+                          atanf((sqrtf((0.291600019F * (q_a * q_a) -
+                                        (e_out - 0.0729000047F) *
+                                        (e_out - 0.0729000047F)) +
+                                       0.291600019F * (r_a * r_a)) -
+                                 e_d_phi0_tmp) /
+                                ((((s_a * s_a + t_a * t_a) + 0.0729000047F) -
+                                  0.0729000047F) +
+                                 d_d_phi0_tmp))) +
+             b_d_phi0_tmp_tmp) *
+            (b_d_phi0_tmp_tmp +
+             0.54F *
+             arm_sin_f32(2.0F *
+                  atanf((sqrtf((0.291600019F * (u_a * u_a) -
+                                (f_out - 0.0729000047F) *
+                                (f_out - 0.0729000047F)) +
+                               0.291600019F * (v_a * v_a)) -
+                         e_d_phi0_tmp) /
+                        ((((w_a * w_a + x_a * x_a) + 0.0729000047F) -
+                          0.0729000047F) +
+                         d_d_phi0_tmp))) *
+             ((((i_d_phi0_tmp -
+                 h_d_phi0_tmp *
+                 (((y_a * y_a + ab_a * ab_a) + 0.0729000047F) -
+                  0.0729000047F)) +
+                j_d_phi0_tmp) /
+               (2.0F * sqrtf((0.291600019F * (bb_a * bb_a) -
+                              (g_out - 0.0729000047F) *
+                              (g_out - 0.0729000047F)) +
+                             0.291600019F * (cb_a * cb_a))) -
+               d_phi0_tmp) /
+              ((((db_a * db_a + eb_a * eb_a) + 0.0729000047F) -
+                0.0729000047F) +
+               d_d_phi0_tmp) -
+              (sqrtf((0.291600019F * (fb_a * fb_a) -
+                      (h_out - 0.0729000047F) *
+                      (h_out - 0.0729000047F)) +
+                     0.291600019F * (gb_a * gb_a)) -
+               e_d_phi0_tmp) *
+              f_d_phi0_tmp / (hb_a * hb_a)) /
+             (ib_a * ib_a / (jb_a * jb_a) + 1.0F)) /
+            (kb_a * kb_a)) /
+           (lb_a * lb_a / (mb_a * mb_a) + 1.0F) -
+           d_phi4 *
+           (0.54F *
+            arm_cos_f32(2.0F *
+                 atanf((sqrtf((0.291600019F * (nb_a * nb_a) -
+                               (i_out - 0.0729000047F) *
+                               (i_out - 0.0729000047F)) +
+                              0.291600019F * (ob_a * ob_a)) -
+                        e_d_phi0_tmp) /
+                       ((((pb_a * pb_a + qb_a * qb_a) + 0.0729000047F) -
+                         0.0729000047F) +
+                        d_d_phi0_tmp))) *
+            ((((b_d_phi0_tmp -
+                k_d_phi0_tmp *
+                (((rb_a * rb_a + sb_a * sb_a) + 0.0729000047F) -
+                 0.0729000047F)) +
+               c_d_phi0_tmp) /
+              (2.0F * sqrtf((0.291600019F * (tb_a * tb_a) -
+                             (j_out - 0.0729000047F) *
+                             (j_out - 0.0729000047F)) +
+                            0.291600019F * (ub_a * ub_a))) -
+              l_d_phi0_tmp) /
+             ((((vb_a * vb_a + wb_a * wb_a) + 0.0729000047F) -
+               0.0729000047F) +
+              d_d_phi0_tmp) -
+             (sqrtf((0.291600019F * (xb_a * xb_a) -
+                     (k_out - 0.0729000047F) * (k_out - 0.0729000047F)) +
+                    0.291600019F * (yb_a * yb_a)) -
+              e_d_phi0_tmp) *
+             g_d_phi0_tmp / (ac_a * ac_a)) /
+            ((bc_a * bc_a / (cc_a * cc_a) + 1.0F) *
+             ((0.27F * arm_cos_f32(2.0F *
+                            atanf((sqrtf((0.291600019F * (dc_a * dc_a) -
+                                          (l_out - 0.0729000047F) *
+                                          (l_out - 0.0729000047F)) +
+                                         0.291600019F * (ec_a * ec_a)) -
+                                   e_d_phi0_tmp) /
+                                  ((((fc_a * fc_a + gc_a * gc_a) +
+                                     0.0729000047F) -
+                                    0.0729000047F) +
+                                   d_d_phi0_tmp))) -
+               0.075F) +
+              d_d_phi0_tmp_tmp)) +
+            0.54F *
+            arm_sin_f32(2.0F *
+                 atanf((sqrtf((0.291600019F * (hc_a * hc_a) -
+                               (m_out - 0.0729000047F) *
+                               (m_out - 0.0729000047F)) +
+                              0.291600019F * (ic_a * ic_a)) -
+                        e_d_phi0_tmp) /
+                       ((((jc_a * jc_a + kc_a * kc_a) + 0.0729000047F) -
+                         0.0729000047F) +
+                        d_d_phi0_tmp))) *
+            (0.27F *
+             arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (lc_a * lc_a) -
+                                       (n_out - 0.0729000047F) *
+                                       (n_out - 0.0729000047F)) +
+                                      0.291600019F * (mc_a * mc_a)) -
+                                e_d_phi0_tmp) /
+                               ((((nc_a * nc_a + oc_a * oc_a) +
+                                  0.0729000047F) -
+                                 0.0729000047F) +
+                                d_d_phi0_tmp))) +
+             b_d_phi0_tmp_tmp) *
+            ((((b_d_phi0_tmp -
+                k_d_phi0_tmp *
+                (((pc_a * pc_a + qc_a * qc_a) + 0.0729000047F) -
+                 0.0729000047F)) +
+               c_d_phi0_tmp) /
+              (2.0F * sqrtf((0.291600019F * (rc_a * rc_a) -
+                             (o_out - 0.0729000047F) *
+                             (o_out - 0.0729000047F)) +
+                            0.291600019F * (sc_a * sc_a))) -
+              l_d_phi0_tmp) /
+             ((((tc_a * tc_a + uc_a * uc_a) + 0.0729000047F) -
+               0.0729000047F) +
+              d_d_phi0_tmp) -
+             (sqrtf((0.291600019F * (vc_a * vc_a) -
+                     (p_out - 0.0729000047F) * (p_out - 0.0729000047F)) +
+                    0.291600019F * (wc_a * wc_a)) -
+              e_d_phi0_tmp) *
+             g_d_phi0_tmp / (xc_a * xc_a)) /
+            ((yc_a * yc_a / (ad_a * ad_a) + 1.0F) * (bd_a * bd_a))) /
+           (cd_a * cd_a / (dd_a * dd_a) + 1.0F);
+}
+
+/*
+ * File trailer for get_dphi0.c
+ *
+ * [EOF]
+ */
diff --git a/application/chassis/cal_phi0_dot.h b/application/chassis/cal_phi0_dot.h
new file mode 100644
index 0000000..a5dec13
--- /dev/null
+++ b/application/chassis/cal_phi0_dot.h
@@ -0,0 +1,10 @@
+//
+// Created by SJQ on 2024/1/3.
+//
+
+#ifndef WHEEL_LEGGED_CAL_PHI0_DOT_H
+#define WHEEL_LEGGED_CAL_PHI0_DOT_H
+
+float get_dphi0(float phi_1, float phi_4, float d_phi1, float d_phi4);
+
+#endif //WHEEL_LEGGED_CAL_PHI0_DOT_H
diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c
index 88d4abb..5a1b4ad 100644
--- a/application/chassis/chassis.c
+++ b/application/chassis/chassis.c
@@ -15,18 +15,21 @@
 #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 "leg.h"
+#include "balance.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)   // 半轮距
@@ -51,24 +54,26 @@ static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据
 
 static SuperCapInstance *cap;                                       // 超级电容
 static PowerMeterInstance *power_meter;                             //功率计
-//static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
+
 static HTMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
+static LKMotorInstance *wheel_motor_r,*wheel_motor_l;
 /* 用于自旋变速策略的时间变量 */
-// static float t;
 
 /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
 
-//static Leg_State_t legState;
-//static Leg_Input_t legInput;
+attitude_t *Chassis_IMU_data;
+
 static LegInstance legInstance;
 
+Balance_Control_t BalanceControl;
+
 void ChassisInit()
 {
-    // 四个轮子的参数一样,改tx_id和反转标志位即可
+    // 四个关节电机的参数,改tx_id和反转标志位即可
     Motor_Init_Config_s chassis_motor_config = {
-        .can_init_config.can_handle = &hcan1,
+        .can_init_config.can_handle = &hcan2,
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
@@ -77,6 +82,17 @@ void ChassisInit()
         },
         .motor_type = HT04,
     };
+
+    Motor_Init_Config_s wheel_motor_config = {
+            .can_init_config.can_handle = &hcan1,
+            .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 = 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;
@@ -96,6 +112,17 @@ void ChassisInit()
     //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_l = 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_REVERSE;
+    wheel_motor_r = LKMotorInit(&wheel_motor_config);
+
+
+
     referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
 
     SuperCap_Init_Config_s cap_conf = {
@@ -111,24 +138,48 @@ void ChassisInit()
                     .rx_id = 0x211,
             }
     };
-
+    //功率計初始化
     power_meter = PowerMeterInit(&power_conf);
 
-    PID_Init_Config_s leg_pid_config = {
-            .Kp = 50,
-            .Ki = 10,
-            .Kd = 0,
-            .MaxOut = 50,
+    Balance_Init_config_s balanceInitConfig = {
+            .legInitConfig = {
+                    .length_PID_conf = {
+                            .Kp = 150,//180 ,//50,
+                            .Ki = 20,//5,
+                            .Kd = 10,//10,//6,//6,
+                            .MaxOut = 100,
+                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
+                            .Derivative_LPF_RC = 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 = 8,
+                    .Ki = 0,
+                    .Kd = 1,
+                    .MaxOut = 3,
+                    .Improve =  PID_Derivative_On_Measurement| PID_DerivativeFilter,
+                    .Derivative_LPF_RC = 1/(2*PI*10),
+            },
+            .LQR_state_num = 6,
+            .LQR_control_num = 2,
 
     };
-    PID_Init_Config_s phi_pid_config = {
-            .Kp = 2,
-            .Ki = 0,
-            .Kd = 0,
-            .MaxOut = 5,
-    };
-    Leg_Init(&legInstance,leg_pid_config,phi_pid_config);
 
+    //Leg_Init(&legInstance,&leg_r_init_conf);
+    Balance_Control_Init(&BalanceControl,balanceInitConfig);
+    Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
 
     // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
@@ -222,21 +273,27 @@ void ChassisTask()
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
 #endif // CHASSIS_BOARD
 
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_NO_FOLLOW)
     { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
-//        HTMotorStop(motor_lf);
-//        HTMotorStop(motor_rf);
-//        HTMotorStop(motor_lb);
-//        HTMotorStop(motor_rb);
+        HTMotorStop(motor_lf);
+        HTMotorStop(motor_rf);
+        HTMotorStop(motor_lb);
+        HTMotorStop(motor_rb);
+        LKMotorStop(wheel_motor_r);
+        LKMotorStop(wheel_motor_l);
     }
     else
     { // 正常工作
-//        HTMotorEnable(motor_lf);
-//        HTMotorEnable(motor_rf);
-//        HTMotorEnable(motor_lb);
-//        HTMotorEnable(motor_rb);
+        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)
     {
@@ -263,15 +320,10 @@ void ChassisTask()
     chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
 
     // 根据控制模式进行正运动学解算,计算底盘输出
-    //MecanumCalculate();
     OmniCalculate();
 
     // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
     LimitChassisOutput();
-//    float vofa_send_data[2];
-//    vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
-//    vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
-//    vofa_justfloat_output(vofa_send_data,8,&huart1);
 
     // 根据电机的反馈速度和IMU(如果有)计算真实速度
     EstimateSpeed();
@@ -283,38 +335,86 @@ void ChassisTask()
     // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
     // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
 
-    HTMotorEnable(motor_lf);
-    HTMotorEnable(motor_rf);
-    HTMotorEnable(motor_lb);
-    HTMotorEnable(motor_rb);
 
-//    HTMotorSetRef(motor_rf,0);
-//    HTMotorSetRef(motor_rb,0);
-    HTMotorSetRef(motor_lf,0);
-    HTMotorSetRef(motor_lb,0);
+    float vofa_send_data[7];
 
-    float vofa_send_data[4];
+    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);
 
-//    vofa_send_data[0] = PI-(motor_rf->measure.total_angle+0.8f);
-////
-////    // phi1 = PI/2-motor_rf->measure.total_angle;
-//    vofa_send_data[1] = -(motor_rb->measure.total_angle-0.8f);
-//    // phi4 = PI/2-motor_rb->measure.total_angle;
+    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];
+
+    float x = - (wheel_motor_r->measure.total_angle * DEGREE_2_RAD ) * 0.135f/2;
+    float x_dot = - wheel_motor_r->measure.speed_rads * 0.135f/2;
 
 
-    Leg_State_update(&legInstance.legState,PI-(motor_rf->measure.total_angle+0.8f),-(motor_rb->measure.total_angle-0.8f));
-    legInstance.target_L0 = 0.3f;
-    legInstance.target_phi0 = PI/2;
-    Leg_length_control_loop(&legInstance);
+    Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot);
+
+    BalanceControl.leg_right.target_L0 += chassis_cmd_recv.vx/5280 * 0.0001f;
+    BalanceControl.leg_right.target_L0 = float_constrain(BalanceControl.leg_right.target_L0,0.15f,0.30f);
+    BalanceControl.leg_right.target_phi0 = PI/2;
+
+    BalanceControl.leg_left.target_L0 = BalanceControl.leg_right.target_L0;
+    BalanceControl.leg_left.target_L0 = float_constrain(BalanceControl.leg_left.target_L0,0.15f,0.30f);
+    BalanceControl.leg_left.target_phi0 = PI/2;
+
+
+    vofa_send_data[0] = BalanceControl.leg_right.legState.phi0;
+    vofa_send_data[1] = BalanceControl.leg_left.legState.phi0;
+    vofa_send_data[2] = BalanceControl.leg_coordination_PID.Output;
+    vofa_send_data[3] = BalanceControl.leg_coordination_PID.Measure;
+    vofa_send_data[4] = BalanceControl.leg_right.legInput.F;
+
+
+    vofa_justfloat_output(vofa_send_data,28,&huart1);
+
+    static float target_x = 0;
+
+    if(chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE)  // 右侧开关状态[下],底盘跟随云台
+    {
+        target_x += chassis_cmd_recv.vy/5280 * 0.001f;
+        target_x_set(&BalanceControl,target_x);
+
+        Balance_Control_Loop(&BalanceControl);
+
+        float T = float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
+
+        float wheel_current = T / 0.00512f;
+
+        float_constrain(wheel_current,-2000,2000);
+
+
+        HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * 0.5142857f);
+        HTMotorSetRef(motor_rb,-BalanceControl.leg_right.legOutput.T2 * 0.5142857f);
+
+        HTMotorSetRef(motor_lf,BalanceControl.leg_left.legOutput.T1 * 0.5142857f);
+        HTMotorSetRef(motor_lb,BalanceControl.leg_left.legOutput.T2 * 0.5142857f);
+
+
+        LKMotorSetRef(wheel_motor_r,wheel_current);
+        LKMotorSetRef(wheel_motor_l,wheel_current);
+    }
+    else{
+//        HTMotorSetRef(motor_rf,0);
+//        HTMotorSetRef(motor_rb,0);
+//        HTMotorSetRef(motor_lf,0);
+//        HTMotorSetRef(motor_lb,0);
+        //LKMotorSetRef(wheel_motor_r,0);
+//        LKMotorSetRef(wheel_motor_l,0);
+    }
+
 
-    vofa_send_data[0] = legInstance.Length_PID.Output;
 
-    vofa_send_data[2] = legInstance.legState.L0;
-    vofa_send_data[3] = legInstance.legState.phi0;
-    vofa_justfloat_output(vofa_send_data,16,&huart1);
 
-    HTMotorSetRef(motor_rf,-legInstance.legOutput.T1);
-    HTMotorSetRef(motor_rb,-legInstance.legOutput.T2);
 
     // 推送反馈消息
 #ifdef ONE_BOARD
diff --git a/application/chassis/leg.c b/application/chassis/leg.c
index 7c200d3..f83a0d4 100644
--- a/application/chassis/leg.c
+++ b/application/chassis/leg.c
@@ -7,6 +7,8 @@
 #include "main.h"
 #include "arm_math.h"
 
+#include "cal_phi0_dot.h"
+
 
 static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f};
 static void calculate_leg_pos(Leg_State_t* leg_state);
@@ -14,10 +16,15 @@ static void VMC_jacobian(float L0, float phi0, float phi1, float phi2, float phi
                             float phi4, float F, float Tp, float T12[2]);
 void VMC_getT(LegInstance* legInstance);
 
-void Leg_Init(LegInstance* legInstance,PID_Init_Config_s lengthConfig,PID_Init_Config_s phiConfig)
+void Leg_Init(LegInstance* legInstance, Leg_init_config_s* legInitConfig)
 {
-    PIDInit(&legInstance->Length_PID,&lengthConfig);
-    PIDInit(&legInstance->Phi0_PID,&phiConfig);
+    PIDInit(&legInstance->Length_PID,&legInitConfig->length_PID_conf);
+    PIDInit(&legInstance->Phi0_PID,&legInitConfig->phi0_PID_conf);
+    legInstance->target_L0 = legInitConfig->init_target_L0;
+    legInstance->F_feedforward = legInitConfig->F_feedforward;
+
+    // set rest of memory to 0
+    DWT_GetDeltaT(&legInstance->legState.DWT_CNT);
 }
 
 void Leg_length_control_loop(LegInstance* legInstance)
@@ -28,9 +35,9 @@ void Leg_length_control_loop(LegInstance* legInstance)
     //Leg_calc_output(&legInstance->legState,&legInstance->legInput,&legInstance->legOutput);
     VMC_getT(legInstance);
 }
-void Leg_State_update(Leg_State_t* leg_state, float phi1,float phi4)
+void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi1_dot,float phi4_dot)
 {
-    //@todo:输入要限制在限幅以内
+    Leg_State_t* leg_state = &legInstance->legState;
     if(phi1<=PI/2)
         phi1 = PI/2;
     if(phi4>=PI/2)
@@ -42,10 +49,16 @@ void Leg_State_update(Leg_State_t* leg_state, float phi1,float phi4)
 
     //calculate_leg_pos(leg_state->phi1,leg_state->phi4,leg_pos);
     calculate_leg_pos(leg_state);
-//    leg_state->L0 = leg_pos[0];
-//    leg_state->phi0 = leg_pos[1];
-//    leg_state->phi2 = leg_pos[2];
-//    leg_state->phi3 = leg_pos[3];
+
+
+    //对phi0求导
+//    leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
+//    if(leg_state->last_phi0>=0)
+//        leg_state->phi0_dot = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
+//    else
+//        leg_state->phi0_dot = 0;
+//    leg_state->last_phi0 = leg_state->phi0;
+    leg_state->phi0_dot = get_dphi0(phi1,phi4,phi1_dot,phi4_dot);
 }
 
 void Leg_Input_update(Leg_Input_t* leg_Input, float F,float Tp)
@@ -68,7 +81,7 @@ void Leg_calc_output(Leg_State_t* leg_state,Leg_Input_t* leg_input,Leg_Output_t*
 void calculate_leg_pos(Leg_State_t* leg_state)
 {
     //以机体中点建立坐标系
-    float xb = -l[4]/2+l[0]* arm_sin_f32(leg_state->phi1);
+    float xb = -l[4]/2+l[0]* arm_cos_f32(leg_state->phi1);
     float yb = l[0]* arm_sin_f32(leg_state->phi1);
 
     float xd = l[4]/2+l[3]* arm_cos_f32(leg_state->phi4);
diff --git a/application/chassis/leg.h b/application/chassis/leg.h
index cf0d903..d416570 100644
--- a/application/chassis/leg.h
+++ b/application/chassis/leg.h
@@ -16,6 +16,12 @@ typedef struct
 
     float phi2;
     float phi3;
+
+    uint32_t DWT_CNT;
+    float dt;        //用于计算时间并求导
+    float last_phi0;
+    float phi0_dot;
+
 }Leg_State_t;
 
 typedef struct
@@ -30,6 +36,14 @@ typedef struct
     float T2;
 }Leg_Output_t;
 
+
+typedef struct
+{
+    PID_Init_Config_s length_PID_conf;
+    PID_Init_Config_s phi0_PID_conf;
+    float init_target_L0;    //初始腿长目标值
+    float F_feedforward;
+}Leg_init_config_s;
 typedef struct
 {
     Leg_State_t legState;
@@ -41,14 +55,18 @@ typedef struct
     PIDInstance Length_PID;
     PIDInstance Phi0_PID;
 
+    float F_feedforward; //支持力前馈补偿
 }LegInstance;
 
-void Leg_State_update(Leg_State_t* leg_state, float phi1,float phi4);
+
+
+void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi1_dot,float phi4_dot);
 void Leg_calc_output(Leg_State_t* leg_state,Leg_Input_t* leg_input,Leg_Output_t* leg_output);
 void Leg_Input_update(Leg_Input_t* leg_Input, float F,float Tp);
 
-void Leg_Init(LegInstance* legInstance,PID_Init_Config_s lengthConfig,PID_Init_Config_s phiConfig);
+void Leg_Init(LegInstance* legInstance, Leg_init_config_s* legInitConfig);
 void Leg_length_control_loop(LegInstance* legInstance);
+void VMC_getT(LegInstance* legInstance);
 
 
 #endif //WHEEL_LEGGED_LEG_H
diff --git a/application/robot.c b/application/robot.c
index e06dee4..23fd7bc 100644
--- a/application/robot.c
+++ b/application/robot.c
@@ -32,12 +32,13 @@ void RobotInit()
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDInit();
     //暂时注释云台和发射任务  调试底盘
-    GimbalInit();
-//    ShootInit();
+    //GimbalInit();
+    //ShootInit();
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
     ChassisInit();
+
 #endif
 
     OSTaskInit(); // 创建基础任务
@@ -52,7 +53,7 @@ void RobotTask()
     RobotCMDTask();
     //暂时注释云台和发射任务  调试底盘
     //GimbalTask();
-//    ShootTask();
+    //ShootTask();
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
diff --git a/application/robot_def.h b/application/robot_def.h
index e66ac8f..235f480 100644
--- a/application/robot_def.h
+++ b/application/robot_def.h
@@ -18,7 +18,7 @@
 
 /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
 #define ONE_BOARD // 单板控制整车
-// #define CHASSIS_BOARD //底盘板
+//#define CHASSIS_BOARD //底盘板
 // #define GIMBAL_BOARD  //云台板
 
 #define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c
index e19af5c..cde05da 100644
--- a/application/shoot/shoot.c
+++ b/application/shoot/shoot.c
@@ -5,6 +5,7 @@
 #include "message_center.h"
 #include "bsp_dwt.h"
 #include "general_def.h"
+#include "vofa.h"
 
 /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
 static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
@@ -62,8 +63,8 @@ void ShootInit()
     // 拨盘电机
     Motor_Init_Config_s loader_config = {
         .can_init_config = {
-            .can_handle = &hcan2,
-            .tx_id = 3,
+            .can_handle = &hcan1,
+            .tx_id = 1,
         },
         .controller_param_init_config = {
             .angle_PID = {
@@ -93,7 +94,7 @@ void ShootInit()
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
             .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
-            .close_loop_type = CURRENT_LOOP | SPEED_LOOP,
+            .close_loop_type = SPEED_LOOP,
             .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
         },
         .motor_type = M2006 // 英雄使用m3508
@@ -108,21 +109,21 @@ void ShootInit()
 void ShootTask()
 {
     // 从cmd获取控制数据
-    SubGetMessage(shoot_sub, &shoot_cmd_recv);
+    //SubGetMessage(shoot_sub, &shoot_cmd_recv);
 
     // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
-    if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
-    {
-        DJIMotorStop(friction_l);
-        DJIMotorStop(friction_r);
-        DJIMotorStop(loader);
-    }
-    else // 恢复运行
-    {
-        DJIMotorEnable(friction_l);
-        DJIMotorEnable(friction_r);
-        DJIMotorEnable(loader);
-    }
+//    if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
+//    {
+//        DJIMotorStop(friction_l);
+//        DJIMotorStop(friction_r);
+//        DJIMotorStop(loader);
+//    }
+//    else // 恢复运行
+//    {
+//        DJIMotorEnable(friction_l);
+//        DJIMotorEnable(friction_r);
+//        DJIMotorEnable(loader);
+//    }
 
     // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
     // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
@@ -130,43 +131,43 @@ void ShootTask()
     //     return;
 
     // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
-    switch (shoot_cmd_recv.load_mode)
-    {
-    // 停止拨盘
-    case LOAD_STOP:
-        DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
-        DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
-        break;
-    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
-    case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
-        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
-        DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
-        hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
-        dead_time = 150;                                                                    // 完成1发弹丸发射的时间
-        break;
-    // 三连发,如果不需要后续可能删除
-    case LOAD_3_BULLET:
-        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                                  // 切换到速度环
-        DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
-        hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
-        dead_time = 300;                                                                        // 完成3发弹丸发射的时间
-        break;
-    // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
-    case LOAD_BURSTFIRE:
-        DJIMotorOuterLoop(loader, SPEED_LOOP);
-        DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
-        // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
-        break;
-    // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
-    // 也有可能需要从switch-case中独立出来
-    case LOAD_REVERSE:
-        DJIMotorOuterLoop(loader, SPEED_LOOP);
-        // ...
-        break;
-    default:
-        while (1)
-            ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
-    }
+//    switch (shoot_cmd_recv.load_mode)
+//    {
+//    // 停止拨盘
+//    case LOAD_STOP:
+//        DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
+//        DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
+//        break;
+//    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
+//    case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
+//        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
+//        DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
+//        hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
+//        dead_time = 150;                                                                    // 完成1发弹丸发射的时间
+//        break;
+//    // 三连发,如果不需要后续可能删除
+//    case LOAD_3_BULLET:
+//        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                                  // 切换到速度环
+//        DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
+//        hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
+//        dead_time = 300;                                                                        // 完成3发弹丸发射的时间
+//        break;
+//    // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
+//    case LOAD_BURSTFIRE:
+//        DJIMotorOuterLoop(loader, SPEED_LOOP);
+//        DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
+//        // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
+//        break;
+//    // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
+//    // 也有可能需要从switch-case中独立出来
+//    case LOAD_REVERSE:
+//        DJIMotorOuterLoop(loader, SPEED_LOOP);
+//        // ...
+//        break;
+//    default:
+//        while (1)
+//            ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
+//    }
 
     // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
     if (shoot_cmd_recv.friction_mode == FRICTION_ON)
@@ -210,4 +211,20 @@ void ShootTask()
 
     // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
     PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
+
+    DJIMotorEnable(loader);
+    DJIMotorSetRef(loader,1);
+
+//    float vofa_send_data[7];
+//
+//    vofa_send_data[0] = loader->motor_controller.pid_ref;
+//    vofa_send_data[1] = loader->measure.speed_aps;
+//    vofa_send_data[2] = loader->motor_controller.speed_PID.Output;
+////    vofa_send_data[2] = BalanceControl.leg_right.target_L0;
+////    vofa_send_data[3] = BalanceControl.leg_right.legState.L0;
+////    vofa_send_data[4] = BalanceControl.leg_right.legInput.F;
+//
+//
+//    vofa_justfloat_output(vofa_send_data,28,&huart1);
+
 }
\ No newline at end of file
diff --git a/modules/algorithm/LQR.c b/modules/algorithm/LQR.c
new file mode 100644
index 0000000..cdda5f2
--- /dev/null
+++ b/modules/algorithm/LQR.c
@@ -0,0 +1,51 @@
+//
+// Created by SJQ on 2023/12/29.
+//
+
+#include "LQR.h"
+
+void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt)
+{
+    lqrInstance->state_num = state_cnt;
+    lqrInstance->control_num = control_cnt;
+
+    lqrInstance->state_vector = (float *)zmalloc(state_cnt *4);
+    lqrInstance->state_cmd = (float *)zmalloc(state_cnt *4);
+
+    lqrInstance->control_vector = (float *)zmalloc(control_cnt *4);
+
+    MatInit(&lqrInstance->K_matrix,control_cnt,state_cnt);
+    MatInit(&lqrInstance->state_err,state_cnt,1);
+    MatInit(&lqrInstance->state_matrix,state_cnt,1);
+    MatInit(&lqrInstance->control_matrix,control_cnt,1);
+    MatInit(&lqrInstance->state_cmd_matrix,state_cnt,1);
+
+}
+
+void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state)
+{
+    memcpy(lqrInstance->state_vector,new_state,lqrInstance->state_num * 4);
+    memcpy(lqrInstance->state_matrix.pData,new_state,lqrInstance->state_num * 4);
+
+    memcpy(lqrInstance->state_cmd,target_state,lqrInstance->state_num* 4);
+    memcpy(lqrInstance->state_cmd_matrix.pData,target_state,lqrInstance->state_num* 4);
+
+    arm_mat_sub_f32(&lqrInstance->state_cmd_matrix,&lqrInstance->state_matrix,&lqrInstance->state_err);
+
+    arm_mat_mult_f32(&lqrInstance->K_matrix,&lqrInstance->state_err,&lqrInstance->control_matrix);
+
+    memcpy(lqrInstance->control_vector,lqrInstance->control_matrix.pData,lqrInstance->control_num* 4);
+
+//    arm_mat_init_f32(&lqrInstance->state_matrix,lqrInstance->state_num,1,new_state);
+//    arm_mat_init_f32(&lqrInstance->state_cmd_matrix,lqrInstance->state_num,1,target_state);
+//
+//    arm_mat_sub_f32(&lqrInstance->state_cmd_matrix,&lqrInstance->state_matrix,&lqrInstance->state_err);
+//    arm_mat_mult_f32(&lqrInstance->K_matrix,&lqrInstance->state_err,&lqrInstance->control_matrix);
+}
+
+void LQR_set_K(LQRInstance* lqrInstance, float *K_value)
+{
+    int K_cnt = lqrInstance->K_matrix.numCols * lqrInstance->K_matrix.numRows;
+    memcpy(lqrInstance->K_matrix.pData,K_value,K_cnt *4);
+    //arm_mat_init_f32(&lqrInstance->K_matrix,lqrInstance->control_num,lqrInstance->state_num,K_value);
+}
\ No newline at end of file
diff --git a/modules/algorithm/LQR.h b/modules/algorithm/LQR.h
new file mode 100644
index 0000000..5dacaab
--- /dev/null
+++ b/modules/algorithm/LQR.h
@@ -0,0 +1,30 @@
+//
+// Created by SJQ on 2023/12/29.
+//
+
+#ifndef WHEEL_LEGGED_LQR_H
+#define WHEEL_LEGGED_LQR_H
+#include "user_lib.h"
+typedef struct
+{
+    float* state_vector;
+    float* control_vector;
+
+    float* state_cmd;
+
+    int state_num;    //状态量维数
+    int control_num;  //控制量维数
+
+    mat control_matrix;  //其实是向量 定义成矩阵方便计算  @todo：更新DSP库 新版本库支持矩阵直接乘数组
+    mat state_matrix;
+    mat state_cmd_matrix;
+
+    mat state_err;
+    mat K_matrix;
+}LQRInstance;
+
+void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt);
+void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state);
+void LQR_set_K(LQRInstance* lqrInstance, float *K_value);
+
+#endif //WHEEL_LEGGED_LQR_H
diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c
index 5029109..f22a4bb 100644
--- a/modules/algorithm/user_lib.c
+++ b/modules/algorithm/user_lib.c
@@ -212,3 +212,44 @@ void MatInit(mat *m, uint8_t row, uint8_t col)
     m->numRows = row;
     m->pData = (float *)zmalloc(row * col * sizeof(float));
 }
+
+
+/**
+  * @brief          斜波函数初始化
+  * @author         RM
+  * @param[in]      斜波函数结构体
+  * @param[in]      间隔的时间，单位 s
+  * @param[in]      最大值
+  * @param[in]      最小值
+  * @retval         返回空
+  */
+void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min)
+{
+    ramp_source_type->frame_period = frame_period;
+    ramp_source_type->max_value = max;
+    ramp_source_type->min_value = min;
+    ramp_source_type->input = 0.0f;
+    ramp_source_type->out = 0.0f;
+}
+
+/**
+  * @brief          斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
+  * @author         RM
+  * @param[in]      斜波函数结构体
+  * @param[in]      输入值
+  * @param[in]      滤波参数
+  * @retval         返回空
+  */
+void ramp_calc(ramp_function_source_t *ramp_source_type, float input)
+{
+    ramp_source_type->input = input;
+    ramp_source_type->out += ramp_source_type->input * ramp_source_type->frame_period;
+    if (ramp_source_type->out > ramp_source_type->max_value)
+    {
+        ramp_source_type->out = ramp_source_type->max_value;
+    }
+    else if (ramp_source_type->out < ramp_source_type->min_value)
+    {
+        ramp_source_type->out = ramp_source_type->min_value;
+    }
+}
diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h
index 9a15097..e93a5d8 100644
--- a/modules/algorithm/user_lib.h
+++ b/modules/algorithm/user_lib.h
@@ -49,6 +49,15 @@ void MatInit(mat *m, uint8_t row, uint8_t col);
 #define FALSE 0 /**< boolean fails */
 #endif
 
+typedef  struct
+{
+    float input;        //输入数据
+    float out;          //输出数据
+    float min_value;    //限幅最小值
+    float max_value;    //限幅最大值
+    float frame_period; //时间间隔
+} ramp_function_source_t;
+
 /* circumference ratio */
 #ifndef PI
 #define PI 3.14159265354f
@@ -121,6 +130,11 @@ float Dot3d(float *v1, float *v2);
 
 float AverageFilter(float new_data, float *buf, uint8_t len);
 
+
+
+
 #define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
 
+
+
 #endif
diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c
index efdcced..e53ede4 100644
--- a/modules/imu/ins_task.c
+++ b/modules/imu/ins_task.c
@@ -20,19 +20,13 @@
 #include "general_def.h"
 #include "master_process.h"
 
-#include "vofa.h"
-
 static INS_t INS;
 static IMU_Param_t IMU_Param;
 static PIDInstance TempCtrl = {0};
 
-//const float xb[3] = {1, 0, 0};
-//const float yb[3] = {0, 1, 0};
-//const float zb[3] = {0, 0, 1};
-
-const float xb[3] = {0, 0, 1};
+const float xb[3] = {1, 0, 0};
 const float yb[3] = {0, 1, 0};
-const float zb[3] = {-1, 0, 0};
+const float zb[3] = {0, 0, 1};
 
 // 用于获取两次采样之间的时间间隔
 static uint32_t INS_DWT_Count = 0;
@@ -43,7 +37,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
 
 static void IMUPWMSet(uint16_t pwm)
 {
-    __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
+            __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
 }
 
 /**
@@ -66,9 +60,9 @@ static void InitQuaternion(float *init_q4)
     for (uint8_t i = 0; i < 100; ++i)
     {
         BMI088_Read(&BMI088);
-        acc_init[X] += -BMI088.Accel[2];
+        acc_init[X] += BMI088.Accel[X];
         acc_init[Y] += BMI088.Accel[Y];
-        acc_init[Z] += BMI088.Accel[0];
+        acc_init[Z] += BMI088.Accel[Z];
         DWT_Delay(0.001);
     }
     for (uint8_t i = 0; i < 3; ++i)
@@ -78,9 +72,6 @@ static void InitQuaternion(float *init_q4)
     float angle = acosf(Dot3d(acc_init, gravity_norm));
     Cross3d(acc_init, gravity_norm, axis_rot);
     Norm3d(axis_rot);
-
-    //q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2))
-    //其中a表示旋转角度，(x,y,z)表示旋转轴  计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
     init_q4[0] = cosf(angle / 2.0f);
     for (uint8_t i = 0; i < 2; ++i)
         init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
@@ -106,16 +97,16 @@ attitude_t *INS_Init(void)
     IMU_Param.flag = 1;
 
     float init_quaternion[4] = {0};
-    InitQuaternion(init_quaternion);//计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
+    InitQuaternion(init_quaternion);
     IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
     // imu heat init
     PID_Init_Config_s config = {.MaxOut = 2000,
-                                .IntegralLimit = 300,
-                                .DeadBand = 0,
-                                .Kp = 1000,
-                                .Ki = 20,
-                                .Kd = 0,
-                                .Improve = 0x01}; // enable integratiaon limit
+            .IntegralLimit = 300,
+            .DeadBand = 0,
+            .Kp = 1000,
+            .Ki = 20,
+            .Kd = 0,
+            .Improve = 0x01}; // enable integratiaon limit
     PIDInit(&TempCtrl, &config);
 
     // noise of accel is relatively big and of high freq,thus lpf is used
@@ -138,12 +129,12 @@ void INS_Task(void)
     {
         BMI088_Read(&BMI088);
 
-        INS.Accel[X] = -BMI088.Accel[2];
-        INS.Accel[Y] = BMI088.Accel[1];
-        INS.Accel[Z] = BMI088.Accel[0];
-        INS.Gyro[X] = -BMI088.Gyro[2];
-        INS.Gyro[Y] = BMI088.Gyro[1];
-        INS.Gyro[Z] = BMI088.Gyro[0];
+        INS.Accel[X] = BMI088.Accel[X];
+        INS.Accel[Y] = BMI088.Accel[Y];
+        INS.Accel[Z] = BMI088.Accel[Z];
+        INS.Gyro[X] = BMI088.Gyro[X];
+        INS.Gyro[Y] = BMI088.Gyro[Y];
+        INS.Gyro[Z] = BMI088.Gyro[Z];
 
         // demo function,用于修正安装误差,可以不管,本demo暂时没用
         IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
@@ -171,35 +162,11 @@ void INS_Task(void)
         }
         BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
 
-
         INS.Yaw = QEKF_INS.Yaw;
         INS.Pitch = QEKF_INS.Pitch;
         INS.Roll = QEKF_INS.Roll;
-
-//        // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
-//        if (INS.Yaw - INS.YawAngleLast > 180.0f)
-//        {
-//            INS.YawRoundCount--;
-//        }
-//        else if (INS.Yaw - INS.YawAngleLast < -180.0f)
-//        {
-//            INS.YawRoundCount++;
-//        }
-//        INS.YawTotalAngle = 360.0f * INS.YawRoundCount + INS.Yaw;
         INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
 
-//        float vofa_send_data[9];
-//        vofa_send_data[0]=INS.Yaw;
-//        vofa_send_data[1]=INS.Pitch;
-//        vofa_send_data[2]=INS.Roll;
-//        vofa_send_data[3]=INS.Gyro[X];
-//        vofa_send_data[4]=INS.Gyro[Y];
-//        vofa_send_data[5]=INS.Gyro[Z];
-//        vofa_send_data[6]=BMI088.Accel[X];
-//        vofa_send_data[7]=BMI088.Accel[Y];
-//        vofa_send_data[8]=BMI088.Accel[Z];
-//        vofa_justfloat_output(vofa_send_data,36,&huart1);
-
         VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
     }
 
diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h
index f42decf..4acac19 100644
--- a/modules/imu/ins_task.h
+++ b/modules/imu/ins_task.h
@@ -23,10 +23,6 @@
 #define Y 1
 #define Z 2
 
-//#define X 2
-//#define Y 1
-//#define Z 0
-
 #define INS_TASK_PERIOD 1
 
 typedef struct
@@ -38,7 +34,6 @@ typedef struct
     float Pitch;
     float Yaw;
     float YawTotalAngle;
-
 } attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
 
 typedef struct
@@ -68,9 +63,6 @@ typedef struct
     float Yaw;
     float YawTotalAngle;
 
-    float YawAngleLast;
-    float YawRoundCount;
-
     uint8_t init;
 } INS_t;
 
diff --git a/modules/motor/HTmotor/HT04.h b/modules/motor/HTmotor/HT04.h
index b33be20..1a0162c 100644
--- a/modules/motor/HTmotor/HT04.h
+++ b/modules/motor/HTmotor/HT04.h
@@ -16,8 +16,8 @@
 #define P_MAX 95.5f
 #define V_MIN -45.0f // Rad/s
 #define V_MAX 45.0f
-#define T_MIN -5.0f // N·m
-#define T_MAX 5.0f
+#define T_MIN -18.0f // N·m  这里是用于发送扭矩命令时映射的，不能改  严格按照例程说明的
+#define T_MAX 18.0f
 #define KP_MIN 0.0f // N-m/rad
 #define KP_MAX 500.0f
 #define KD_MIN 0.0f // N-m/rad/s
diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c
index c9fadbc..75f9047 100644
--- a/modules/motor/LKmotor/LK9025.c
+++ b/modules/motor/LKmotor/LK9025.c
@@ -10,6 +10,16 @@ static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
 static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
 // 后续考虑兼容单电机和多电机指令.
 
+#define MULTI_MOTOR  //多电机指令
+//#define SINGLE_MOTOR //单电机指令
+
+//将-32~32 映射为-2000到2000
+static int16_t float_to_int16(float x, float x_abs_max,float out_abs_max)
+{
+    int16_t output = (int16_t)(x/x_abs_max * out_abs_max);
+    return output;
+}
+
 /**
  * @brief 电机反馈报文解析
  *
@@ -24,8 +34,14 @@ static void LKMotorDecode(CANInstance *_instance)
     DaemonReload(motor->daemon); // 喂狗
     measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
 
+    if(measure->init_flag == 0)
+    {
+        measure->offset_ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
+        measure->init_flag = 1;
+    }
+
     measure->last_ecd = measure->ecd;
-    measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
+    measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]) - measure->offset_ecd; //上电位置为ecd 0点
 
     measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
 
@@ -65,11 +81,20 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
 
     config->can_init_config.id = motor;
     config->can_init_config.can_module_callback = LKMotorDecode;
+
+#ifdef MULTI_MOTOR //多电机指令
     config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
     config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280
+#endif
+#ifdef SINGLE_MOTOR //单电机指令
+    config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
+    config->can_init_config.tx_id = 0x140 + config->can_init_config.tx_id;
+#endif
+
     motor->motor_can_ins = CANRegister(&config->can_init_config);
 
-    if (idx == 0) // 用第一个电机的can instance发送数据
+
+    if (idx == 0) // 多电机指令用第一个电机的can instance发送数据
     {
         sender_instance = motor->motor_can_ins;
         sender_instance->tx_id = 0x280; //  修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用
@@ -132,20 +157,31 @@ void LKMotorControl()
             pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
         }
 
-        set = pid_ref;
+        set = (int16_t)pid_ref;
         if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
             set *= -1;
+
+        //int16_t tmp = float_to_int16(set,I_MAX,COMMAND_MAX);
         // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
-        memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
+        memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(int16_t));
+//        motor->motor_can_ins->tx_buff[0] = 0xA2;  //A1转矩闭环 A2速度闭环
+//        int32_t tmp = -1080*100;//(int32_t)set;
+//        memcpy(motor->motor_can_ins->tx_buff + 4 , &tmp, sizeof(int32_t));
 
         if (motor->stop_flag == MOTOR_STOP)
         { // 若该电机处于停止状态,直接将发送buff置零
-            memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t));
+            memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(int16_t));
+            //memset( motor->motor_can_ins->tx_buff + 4 , 0, 4);
         }
+#ifdef SINGLE_MOTOR
+        CANTransmit(motor->motor_can_ins, 0.2);
+#endif
     }
-
+#ifdef MULTI_MOTOR
     if (idx) // 如果有电机注册了
         CANTransmit(sender_instance, 0.2);
+#endif
+
 }
 
 void LKMotorStop(LKMotorInstance *motor)
diff --git a/modules/motor/LKmotor/LK9025.h b/modules/motor/LKmotor/LK9025.h
index a5cef5a..9a62b1f 100644
--- a/modules/motor/LKmotor/LK9025.h
+++ b/modules/motor/LKmotor/LK9025.h
@@ -9,8 +9,13 @@
 
 #define LK_MOTOR_MX_CNT 4 // 最多允许4个LK电机使用多电机指令,挂载在一条总线上
 
-#define I_MIN -2000
-#define I_MAX 2000
+#define I_MIN -32.0f
+#define I_MAX 32.0f
+
+#define COMMAND_MAX 2000.0f
+
+#define TORQUE_2_I (1.0f / 0.32f)  //16T版本 扭矩常数 0.32Nm/A
+
 #define CURRENT_SMOOTH_COEF 0.9f
 #define SPEED_SMOOTH_COEF 0.85f
 #define REDUCTION_RATIO_DRIVEN 1
@@ -19,6 +24,9 @@
 
 typedef struct // 9025
 {
+    uint16_t offset_ecd;      //上电时刻的编码器值
+    uint8_t init_flag;        //是否首次上电的标志位
+
     uint16_t last_ecd;        // 上一次读取的编码器值
     uint16_t ecd;             // 当前编码器值
     float angle_single_round; // 单圈角度
diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c
index 9a8f874..e8beb09 100644
--- a/modules/motor/motor_task.c
+++ b/modules/motor/motor_task.c
@@ -15,14 +15,14 @@ void MotorControlTask()
 
     /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
     LKMotorControl();
-    ECMotorControl();
+    //ECMotorControl();
     // legacy support
     // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
     // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
     // HTMotorControl();
     // 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
 
-    ServeoMotorControl();
+    //ServeoMotorControl();
 
     // StepMotorControl();
 }