diff --git a/Src/main.c b/Src/main.c
index 5367de8..d0436ed 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -120,6 +120,7 @@ int main(void)
     MX_CRC_Init();
     MX_DAC_Init();
     /* USER CODE BEGIN 2 */
+    HAL_Delay(1500); //delay 一段时间 等待关节电机驱动初始化
     RobotInit(); // 唯一的初始化函数
     LOGINFO("[main] SystemInit() and RobotInit() done");
     /* USER CODE END 2 */
diff --git a/application/balance/balance.c b/application/balance/balance.c
new file mode 100644
index 0000000..246c3d2
--- /dev/null
+++ b/application/balance/balance.c
@@ -0,0 +1,336 @@
+//
+// Created by SJQ on 2023/12/30.
+//
+
+#include "balance.h"
+
+static void mylqr_k(float L0, float K_mat[12]);
+
+void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
+{
+
+    balanceControl->target_L0 = 0.15f;
+    Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig);
+    Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig);
+
+    LQR_Init(&balanceControl->LQR_l,InitConfig.LQR_state_num,InitConfig.LQR_control_num);
+    LQR_Init(&balanceControl->LQR_r,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);
+
+    estimator_init(&balanceControl->v_estimator,0.001f,10.0f);
+    Observer_Init(&balanceControl->state_observer);
+    memset(balanceControl->state_r,0,6);
+    memset(balanceControl->target_state_r,0,6);
+    memset(balanceControl->state_l,0,6);
+    memset(balanceControl->target_state_l,0,6);
+}
+
+//解算地面支持力F
+float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last);
+
+
+// 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,const float x[2],const float x_dot[2],const float MotionAccel[3])
+{
+    LegInstance *leg_r = &balanceControl->leg_right;
+    LegInstance *leg_l = &balanceControl->leg_left;
+
+    Leg_State_update(leg_r,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
+    Leg_State_update(leg_l,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
+
+    float K_matrix_r[12]={0};
+    float K_matrix_l[12]={0};
+
+    //右腿
+    float alpha_r = PI/2 - leg_r->legState.phi0;
+    float theta_r = alpha_r - body_phi;
+    float theta_dot_r = - leg_r->legState.phi0_dot - body_phi_dot;
+
+    static uint32_t dot_cnt = 0;
+    float dot_dt = DWT_GetDeltaT(&dot_cnt);
+    float theta_dot2_r = (balanceControl->state_r[1] - theta_dot_r) / dot_dt;
+
+    //左腿
+    float alpha_l = PI/2 - leg_l->legState.phi0;
+    float theta_l = alpha_l - body_phi;
+    float theta_dot_l = - leg_l->legState.phi0_dot - body_phi_dot;
+    float theta_dot2_l = (balanceControl->state_l[1] - theta_dot_l) / dot_dt;
+
+    //计算机体速度
+    float xb_r = x[0] + leg_r->legState.L0 * arm_sin_f32(theta_r);
+    float xb_dot_r = x_dot[0] + (leg_r->legState.L0_dot* arm_sin_f32(theta_r) + leg_r->legState.L0 * theta_dot_r * arm_cos_f32(theta_r));
+
+    float xb_l = x[1] + leg_l->legState.L0 * arm_sin_f32(theta_l);
+    float xb_dot_l = x_dot[1] + (leg_l->legState.L0_dot* arm_sin_f32(theta_l) + leg_l->legState.L0 * theta_dot_l * arm_cos_f32(theta_l));
+
+    float xb = (xb_r + xb_l)/2;
+    float xb_dot = (xb_dot_r + xb_dot_l)/2;
+
+    estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[1]);
+
+    float x_est_r = balanceControl->v_estimator.Estimate_X_[0] - leg_r->legState.L0 * arm_sin_f32(theta_r);
+    float x_dot_est_r = balanceControl->v_estimator.Estimate_X_[1] - (leg_r->legState.L0_dot* arm_sin_f32(theta_r) + leg_r->legState.L0 * theta_dot_r * arm_cos_f32(theta_r));
+    float x_est_l = balanceControl->v_estimator.Estimate_X_[0] - leg_l->legState.L0 * arm_sin_f32(theta_l);
+    float x_dot_est_l = balanceControl->v_estimator.Estimate_X_[1] - (leg_l->legState.L0_dot* arm_sin_f32(theta_l) + leg_r->legState.L0 * theta_dot_l * arm_cos_f32(theta_l));
+
+
+    balanceControl->v_est[0] = x_est_r;
+    balanceControl->v_est[1] = x_dot_est_r;
+    balanceControl->v_est[2] = x_est_l;
+    balanceControl->v_est[3] = x_dot_est_l;
+
+    balanceControl->state_r[0] = theta_r;
+    balanceControl->state_r[1] = theta_dot_r;//(1.0f - 0.75f) * balanceControl->state_r[1] + 0.75f * theta_dot_r;
+    balanceControl->state_r[2] = x_est_r;//x[0];//balanceControl->v_estimator.Estimate_X_[0];
+    balanceControl->state_r[3] = x_dot_est_r;//x_dot[0];//balanceControl->v_estimator.Estimate_X_[1];
+    balanceControl->state_r[4] = body_phi;
+    balanceControl->state_r[5] = body_phi_dot;//(1.0f - 0.75f) * balanceControl->state_r[5] + 0.75f * body_phi_dot;
+
+    balanceControl->state_l[0] = theta_l;
+    balanceControl->state_l[1] = theta_dot_l;//(1.0f - 0.75f) * balanceControl->state_l[1] + 0.75f * theta_dot_l;
+    balanceControl->state_l[2] = x_est_l;//x[0];//balanceControl->v_estimator.Estimate_X_[0];
+    balanceControl->state_l[3] = x_dot_est_l;//x_dot[0];//balanceControl->v_estimator.Estimate_X_[1];
+    balanceControl->state_l[4] = body_phi;
+    balanceControl->state_l[5] = body_phi_dot;//(1.0f - 0.75f) * balanceControl->state_l[5] + 0.75f * body_phi_dot;
+
+    //地面支持力解算
+    float theta_state_r[3] = {theta_r,theta_dot_r,theta_dot2_r};
+    static uint32_t right_cnt = 0,left_cnt = 0;
+    balanceControl->FN_right = calculate_F(MotionAccel[2],theta_state_r,leg_r,&right_cnt);
+    float theta_state_l[3] = {theta_l,theta_dot_l,theta_dot2_l};
+    balanceControl->FN_left = calculate_F(MotionAccel[2],theta_state_l,leg_l,&left_cnt);
+
+
+    mylqr_k(leg_r->legState.L0,K_matrix_r);
+    mylqr_k(leg_l->legState.L0,K_matrix_l);
+    //离地情况
+    //注意电机输出反向力矩也会导致判断离地
+    if((balanceControl->FN_right <= 10.0f || leg_r->legFeedback.F <= 10.0f) ||
+        (balanceControl->jump_flag == 1 ||balanceControl->jump_flag == 2) )//&& leg_r->legState.L0>=0.15f
+    {
+        balanceControl->fly_flag_r = TRUE;
+        float lqr_K[12] = {0};
+        lqr_K[6] = K_matrix_r[6];
+        lqr_K[7] = K_matrix_r[7];
+        LQR_set_K(&balanceControl->LQR_r,lqr_K);
+    }
+    else
+    {
+        balanceControl->fly_flag_r = FALSE;
+        LQR_set_K(&balanceControl->LQR_r,K_matrix_r);
+    }
+    if((balanceControl->FN_left <= 10.0f || leg_l->legFeedback.F <= 10.0f)||
+       (balanceControl->jump_flag == 1 || balanceControl->jump_flag == 2) )//&& leg_l->legState.L0>=0.15f
+    {
+        balanceControl->fly_flag_l = TRUE;
+        float lqr_K[12] = {0};
+        lqr_K[6] = K_matrix_l[6];
+        lqr_K[7] = K_matrix_l[7];
+        LQR_set_K(&balanceControl->LQR_l,lqr_K);
+    }
+    else
+    {
+        balanceControl->fly_flag_l = FALSE;
+        LQR_set_K(&balanceControl->LQR_l,K_matrix_l);
+    }
+
+}
+
+#define F_FEEDFORWARD 30.0f //腿部支持力前馈
+#define L_MAX 0.40f
+#define L_MIN 0.13f    //腿长最大最小值
+
+void Balance_Control_Loop(Balance_Control_t* balanceControl,uint8_t jump_flag)
+{
+    LQR_update(&balanceControl->LQR_r,balanceControl->state_r,balanceControl->target_state_r);
+    LQR_update(&balanceControl->LQR_l,balanceControl->state_l,balanceControl->target_state_l);
+
+    balanceControl->LQR_r.control_vector[1] = - balanceControl->LQR_r.control_vector[1];
+    float Tp_r = balanceControl->LQR_r.control_vector[1];  //髋关节力矩
+    float T_r = balanceControl->LQR_r.control_vector[0];   //驱动轮力矩
+
+    balanceControl->LQR_l.control_vector[1] = - balanceControl->LQR_l.control_vector[1];
+    float Tp_l = balanceControl->LQR_l.control_vector[1];  //髋关节力矩
+    float T_l = balanceControl->LQR_l.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,balanceControl->target_L0);
+    //float F_pid_out_l =  PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
+
+    //跳跃控制
+    static float F_jump_l,F_jump_r;
+    float max_scale = 10.0f;
+    float K = (max_scale - 1) * F_FEEDFORWARD / (L_MIN - L_MAX);
+    F_jump_l = K*(leg_l->legState.L0-L_MIN) + max_scale * F_FEEDFORWARD;
+    F_jump_r = K*(leg_r->legState.L0-L_MIN) + max_scale * F_FEEDFORWARD;
+
+    //跳跃支持力
+    F_jump_l = float_constrain(F_jump_l,F_FEEDFORWARD,max_scale * F_FEEDFORWARD);
+    F_jump_r = float_constrain(F_jump_r,F_FEEDFORWARD,max_scale * F_FEEDFORWARD);
+
+
+    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 = 0;//PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0);
+
+    if(jump_flag == 1)//起跳阶段
+    {
+        Leg_Input_update(&leg_r->legInput,F_jump_r,Tp_r + cor_Tp);
+        Leg_Input_update(&leg_l->legInput,F_jump_l,Tp_l - cor_Tp);
+    }
+
+    else if(jump_flag == 2) //收腿阶段
+    {
+        Leg_Input_update(&leg_r->legInput,-100,Tp_r + cor_Tp);
+        Leg_Input_update(&leg_l->legInput,-100,Tp_l - cor_Tp);
+    }
+    else //非跳跃阶段
+    {
+        Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 40,Tp_r + cor_Tp);
+        Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 40,Tp_l - 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 add_x)
+{
+    balanceControl->target_state_r[3] = add_x;
+    balanceControl->target_state_l[3] = add_x;
+
+    if( fabsf(add_x) >= 0.01f)
+    {
+        balanceControl->target_state_r[2] = balanceControl->state_r[2];
+        balanceControl->target_state_l[2] = balanceControl->state_l[2];
+    }
+}
+
+void state_clear(Balance_Control_t* balanceControl)
+{
+    //清除位移状态量
+    balanceControl->target_state_r[2] = 0;
+    balanceControl->state_r[2] = 0;
+
+    balanceControl->target_state_l[2] = 0;
+    balanceControl->state_l[2] = 0;
+
+    //estimator_init(&balanceControl->v_estimator,0.001f,10.0f);
+}
+
+void target_L_set(Balance_Control_t* balanceControl,float add_L)
+{
+    balanceControl->target_L0 += add_L;
+    balanceControl->target_L0 = float_constrain(balanceControl->target_L0,L_MIN,L_MAX);
+}
+
+
+
+/*
+ * MYLQR_K  MATLAB生成
+ *     K = MYLQR_K(L0)
+ *
+ * Arguments    : float L0
+ *                float K[12]
+ * Return Type  : void
+ */
+void mylqr_k(float L0, float K_mat[12])
+{
+    float t2;
+    float t3;
+
+    float K[12];
+    /*     This function was generated by the Symbolic Math Toolbox version 9.2.
+     */
+
+
+    /*     06-May-2024 17:35:50 */
+    t2 = L0*L0;
+    t3 = t2*L0;
+    K[0] =
+            ((L0 * -282.545074F + t2 * 330.464752F) - t3 * 225.209259F) - 21.1259251F;
+    K[1] =
+            ((L0 * 191.247147F - t2 * 904.015686F) + t3 * 1025.96033F) + 44.8702393F;
+    K[2] =
+            ((L0 * -38.6851654F + t2 * 7.79979753F) - t3 * 7.8749814F) - 5.59480667F;
+    K[3] =
+            ((L0 * -10.0553722F - t2 * 60.9720421F) + t3 * 94.1053925F) + 17.016264F;
+    K[4] =
+            ((L0 * 8.41862774F - t2 * 39.7602615F) + t3 * 49.5394897F) - 9.96936703F;
+    K[5] =
+            ((L0 * 51.7300377F - t2 * 238.936829F) + t3 * 284.121246F) + 3.30276108F;
+    K[6] =
+            ((L0 * 58.7921486F - t2 * 213.884598F) + t3 * 239.798187F) - 32.5602646F;
+    K[7] =
+            ((L0 * 112.231422F - t2 * 583.832947F) + t3 * 718.246582F) + 13.6089172F;
+    K[8] =
+            ((L0 * -198.74147F + t2 * 196.315338F) - t3 * 42.9540405F) + 89.0673752F;
+    K[9] = ((L0 * 348.149323F - t2 * 531.783142F) + t3 * 252.826599F) + 70.4726F;
+    K[10] =
+            ((L0 * -30.6359673F + t2 * 40.8268318F) - t3 * 21.4240017F) + 15.1511059F;
+    K[11] =
+            ((L0 * 40.7163315F - t2 * 35.2042122F) - t3 * 12.030282F) + 9.66719341F;
+    //matlab赋值顺序不同
+    for (int i = 0; i < 6; ++i) {
+        K_mat[i] = K[2*i];
+    }
+    for (int i = 0; i < 6; ++i) {
+        K_mat[6+i] = K[2*i+1];
+    }
+}
+
+/*
+ * File trailer for mylqr_k.c
+ *
+ * [EOF]
+ */
+
+
+/**
+ * @brief  解算地面支持力 F
+ * @param  float theta[3] = {theta,theta_dot,theta_dot2}
+ * @retval none
+ * @attention
+ */
+float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last)
+{
+    //求导所用时间
+    //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 L = leg->legState.L0;
+    float L_dot = leg->legState.L0_dot;
+    float L_dot2 = leg->legState.L0_dot2;
+
+    float cos_theta = arm_cos_f32(theta[0]);
+    float sin_theta = arm_sin_f32(theta[0]);
+    float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta[1] * sin_theta
+                    + L * theta[2] * sin_theta + L * theta[1] * theta[1] * cos_theta;
+
+    float P = leg->legFeedback.F * cos_theta + leg->legFeedback.Tp * sin_theta/L;
+
+    float F_N = P + 0.8f * (zw_dot2 + 9.8f);
+
+    return F_N;
+
+}
diff --git a/application/balance/balance.cpp b/application/balance/balance.cpp
deleted file mode 100644
index be07ae2..0000000
--- a/application/balance/balance.cpp
+++ /dev/null
@@ -1,106 +0,0 @@
-//
-// Created by SJQ on 2024/3/2.
-//
-
-#include "balance.h"
-void get_lqr_k(float L0, float K[12]);
-
-balance::balance(Balance_Init_config_s *InitConfig):
-leg_r_(&InitConfig->legInitConfig),leg_l_(&InitConfig->legInitConfig)
-{
-    PIDInit(&leg_coordination_PID_,&InitConfig->leg_cor_PID_config);
-    PIDInit(&leg_length_PID_,&InitConfig->legInitConfig.length_PID_conf);
-    memset(state_,0,6);
-    memset(target_state_,0,6);
-}
-
-void balance::feedback_update(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_r_.state_update(leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
-    leg_l_.state_update(leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
-
-    Matrixf<4,1> leg_r_state = leg_r_.get_state();
-    Matrixf<4,1> leg_l_state = leg_l_.get_state();
-    float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2;
-    float phi_average = (leg_r_state[1][0] + leg_l_state[1][0])/2;
-    float phi_dot_average = (leg_r_state[3][0] + leg_l_state[3][0])/2;
-
-    float theta = PI/2 - phi_average - body_phi;
-    float theta_dot = -phi_dot_average - body_phi_dot;
-    state_[0] = theta;
-    state_[1] = theta_dot;
-    state_[2] = x; state_[3] = x_dot;
-    state_[4] = body_phi; state_[5] = body_phi_dot;
-
-    float  K_mat[12];
-    get_lqr_k(L_average,K_mat);
-    balance_LQR_.set_k(K_mat);
-}
-
-void balance::control_loop() {
-    balance_LQR_.update(state_,target_state_);
-
-    Matrixf<2,1> control = balance_LQR_.get_control();
-    Matrixf<4,1> leg_r_state = leg_r_.get_state();
-    Matrixf<4,1> leg_l_state = leg_l_.get_state();
-
-    float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2;
-    float F_pid_out=  PIDCalculate(&leg_length_PID_,L_average,target_length_);
-    float coordination_err = leg_r_state[1][0] - leg_l_state[1][0];
-    float cor_Tp = PIDCalculate(&leg_coordination_PID_,coordination_err,0);
-
-    leg_r_.input_update(F_pid_out + leg_r_.F_feedforward_ + 20,control[1][0] + cor_Tp);
-    leg_l_.input_update(F_pid_out + leg_l_.F_feedforward_ + 20,control[1][0] - cor_Tp);
-}
-
-void balance::target_set(float x) {
-    target_state_[2] = x;
-}
-
-Matrixf<2,1> balance::get_control() {
-    return balance_LQR_.get_control();
-}
-
-void get_lqr_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;
-    /*     24-Jan-2024 14:05:09 */
-    K_temp[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F;
-    K_temp[1] =
-            ((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F;
-    K_temp[2] =
-            ((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F;
-    K_temp[3] =
-            ((L0 * 2.06703424F - t2 * 7.27073288F) + t3 * 6.41747713F) + 0.598731041F;
-    K_temp[4] =
-            ((L0 * -3.48888779F + t2 * 5.95020676F) - t3 * 3.25835323F) - 9.27006531F;
-    K_temp[5] =
-            ((L0 * -19.3605652F + t2 * 5.27040243F) + t3 * 19.7889462F) + 8.40925217F;
-    K_temp[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) -
-                10.4576302F;
-    K_temp[7] =
-            ((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F;
-    K_temp[8] =
-            ((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F;
-    K_temp[9] =
-            ((L0 * 74.7469635F - t2 * 150.980896F) + t3 * 115.21946F) + 63.3836441F;
-    K_temp[10] =
-            ((L0 * -4.06481314F + t2 * 2.58104968F) + t3 * 0.92904079F) + 4.43995333F;
-    K_temp[11] =
-            ((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F;
-
-    //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];
-    }
-}
\ No newline at end of file
diff --git a/application/balance/balance.h b/application/balance/balance.h
index f448e58..1540f15 100644
--- a/application/balance/balance.h
+++ b/application/balance/balance.h
@@ -1,38 +1,62 @@
 //
-// Created by SJQ on 2024/3/2.
+// Created by SJQ on 2023/12/30.
 //
 
 #ifndef WHEEL_LEGGED_BALANCE_H
 #define WHEEL_LEGGED_BALANCE_H
 
 #include "leg.h"
-#include "LQR.h"
+#include "LQR_old.h"
+#include "controller.h"
+#include "kalman_filter.h"
+#include "estimator.h"
+#include "observer.h"
+typedef struct
+{
+    LegInstance leg_right;
+    LegInstance leg_left;
+    PIDInstance leg_coordination_PID;  //双腿协调PD控制
+
+    PIDInstance leg_length_PID; //平均腿长控制
+
+    float state_r[6];
+    float target_state_r[6];
+    float state_l[6];
+    float target_state_l[6];
+
+    float v_est[4];
+
+    float target_L0; //平均腿长目标值
+    float FN_left; //地面支持力
+    float FN_right;
+
+    //双腿LQR分开计算
+    LQRInstance LQR_l;
+    LQRInstance LQR_r;
+    estimator_t v_estimator;
+    BalanceObserver state_observer;
+
+    uint8_t jump_flag;//跳跃标志
+    uint8_t fly_flag_r; //右腿离地标志
+    uint8_t fly_flag_l; //左腿离地标志
+
+}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;
 
-class balance {
-public:
-    balance(Balance_Init_config_s* InitConfig);
-    void feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z);
-    void control_loop();
-    void target_set(float x);
-    Matrixf<2,1> get_control();
-
-private:
-    leg leg_r_;
-    leg leg_l_;
-    float target_length_; //平均腿长目标值
-    PIDInstance leg_coordination_PID_;  //双腿协调PD控制
-    PIDInstance leg_length_PID_; //平均腿长控制
-
-    float state_[6];
-    float target_state_[6];
-    LQR<6,2> balance_LQR_;
-};
-
+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,const float x[2],const float x_dot[2],const float MotionAccel[3]);
+void Balance_Control_Loop(Balance_Control_t* balanceControl,uint8_t jump_flag);
+void target_x_set(Balance_Control_t* balanceControl,float add_x);
+void target_L_set(Balance_Control_t* balanceControl,float add_L);
+void state_clear(Balance_Control_t* balanceControl);
 
 #endif //WHEEL_LEGGED_BALANCE_H
diff --git a/application/chassis/cal_phi0_dot.c b/application/balance/cal_phi0_dot.c
similarity index 100%
rename from application/chassis/cal_phi0_dot.c
rename to application/balance/cal_phi0_dot.c
diff --git a/application/chassis/cal_phi0_dot.h b/application/balance/cal_phi0_dot.h
similarity index 100%
rename from application/chassis/cal_phi0_dot.h
rename to application/balance/cal_phi0_dot.h
diff --git a/application/balance/estimator.c b/application/balance/estimator.c
index bdee988..1bdf84d 100644
--- a/application/balance/estimator.c
+++ b/application/balance/estimator.c
@@ -5,20 +5,20 @@
 #include "estimator.h"
 static const float dt = 1e-3f;
 //状态转移矩阵
-static float F[4] = {0,1,
-                     1,0};
+static float F[4] = {1,dt,
+                     0,1};
 //控制矩阵
-static float B[2] = {dt,
-                     1};
+static float B[2] = {0.5f*dt*dt,
+                     dt};
 //观测矩阵
 static float H[4] = {1,0,
                      0,1};
 //后验估计协方差初始值
-static float P[4] = {100, 0.1f,
-                     0.1f, 100};
-// R  Q矩阵初始值（其实这里设置多少都无所谓）
-static float Q[4] = {1.0f, 0,
-                     0, 1.0f};
+static float P[4] = {1, 0,
+                     0, 1};
+// R  Q矩阵初始值（其实这里设置多少都无所谓）  需要在调用初始化函数时设置
+static float Q[4] = {1e-3f, 0,
+                     0, 1e-3f};
 static float R[4] = {1000.0f, 0,
                      0, 1000.0f,};
 void estimator_init(estimator_t *est ,float process_noise, float measure_noise) {
@@ -49,8 +49,19 @@ void estimator_update(estimator_t *est ,float x,float x_dot,float ax)  {
     Kalman_Filter_Update(&est->EstimateKF_);
 
     // 提取估计值
+
     for (uint8_t i = 0; i < 2; i++)
     {
-        est->Estimate_X_[i] = est->EstimateKF_.FilteredValue[i];
+        if(isnanf(est->EstimateKF_.FilteredValue[i]))
+        {
+            memset(est->EstimateKF_.xhat_data,0,est->EstimateKF_.xhatSize * 4);
+            memset(est->EstimateKF_.xhatminus_data,0,est->EstimateKF_.xhatSize * 4);
+            memset(est->EstimateKF_.FilteredValue,0,est->EstimateKF_.xhatSize * 4);
+        }
+        else
+        {
+            est->Estimate_X_[i] = est->EstimateKF_.FilteredValue[i];
+        }
+
     }
 }
diff --git a/application/chassis/leg_old.c b/application/balance/leg.c
similarity index 97%
rename from application/chassis/leg_old.c
rename to application/balance/leg.c
index 86da0ef..068b533 100644
--- a/application/chassis/leg_old.c
+++ b/application/balance/leg.c
@@ -2,7 +2,7 @@
 // Created by SJQ on 2023/12/25.
 //
 
-#include "leg_old.h"
+#include "leg.h"
 #include "math.h"
 #include "main.h"
 #include "arm_math.h"
@@ -120,8 +120,11 @@ void calculate_leg_pos(Leg_State_t* leg_state)
     leg_state->L0 = L0;
     leg_state->phi0 = phi0;
     leg_state->phi2 = phi2;
-
     leg_state->phi3 = phi3;
+
+    leg_state->position[0] = xb; leg_state->position[1] = yb;
+    leg_state->position[2] = xc; leg_state->position[3] = yc;
+    leg_state->position[4] = xd; leg_state->position[5] = yd;
 }
 
 void VMC_getT(LegInstance* legInstance)
diff --git a/application/balance/leg.cpp b/application/balance/leg.cpp
deleted file mode 100644
index 411d519..0000000
--- a/application/balance/leg.cpp
+++ /dev/null
@@ -1,155 +0,0 @@
-//
-// Created by SJQ on 2024/3/1.
-//
-
-
-#include "leg.h"
-#include <cmath>
-
-
-static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f};
-
-leg::leg(Leg_init_config_s* legInitConfig)
-{
-    PIDInit(&Length_PID_,&legInitConfig->length_PID_conf);
-    PIDInit(&Phi0_PID_,&legInitConfig->phi0_PID_conf);
-
-    target_L0_ = legInitConfig->init_target_L0;
-    F_feedforward_ = legInitConfig->F_feedforward;
-}
-
-void leg::input_update(float F, float Tp)
-{
-    legInput_ = Matrixf<2,1>(new (float[2]){F,Tp});
-}
-
-void leg::state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot)
-{
-    if(phi1<=PI/2)
-        phi1 = PI/2;
-    if(phi4>=PI/2)
-        phi4 = PI/2;
-
-    legState_.phi1 = phi1;
-    legState_.phi4 = phi4;
-
-    //以机体中点建立坐标系
-    float xb = -l[4]/2+l[0]* arm_cos_f32(legState_.phi1);
-    float yb = l[0]* arm_sin_f32(legState_.phi1);
-
-    float xd = l[4]/2+l[3]* arm_cos_f32(legState_.phi4);
-    float yd = l[3]* arm_sin_f32(legState_.phi4);
-
-    float A0 = 2*l[1]*(xd-xb);
-    float B0 = 2*l[1]*(yd-yb);
-
-    float lbd2 = (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
-    float C0 = l[1]*l[1]+lbd2-l[2]*l[2];
-
-    float tanPhi = 0;
-
-    arm_sqrt_f32((A0*A0 + B0*B0 - C0*C0),&tanPhi);
-    //tanPhi = (B0+tanPhi)/(A0+C0);
-
-    float phi2 = 2* atan2f(B0+tanPhi,A0+C0);
-    float xc = xb + l[1] * arm_cos_f32(phi2);
-    float yc = yb + l[1] * arm_sin_f32(phi2);
-
-    float phi3 = acosf(xc-xd/l[2]);
-    float L0 = 0;
-    arm_sqrt_f32((xc*xc + yc*yc),&L0);
-    float phi0 = atan2f(yc,xc);
-
-    //赋值
-    legState_.L0 = L0;
-    legState_.phi0 = phi0;
-    legState_.phi2 = phi2;
-    legState_.phi3 = phi3;
-
-    get_dot(phi1_dot,phi4_dot);
-}
-
-void leg::calculate_output() {
-    float sinPhi23 = arm_sin_f32(legState_.phi2 - legState_.phi3);
-    float sinPhi12 = arm_sin_f32(legState_.phi1 - legState_.phi2);
-    float sinPhi34 = arm_sin_f32(legState_.phi3 - legState_.phi4);
-
-    float sinPhi03 = arm_sin_f32(legState_.phi0 - legState_.phi3);
-    float sinPhi02 = arm_sin_f32(legState_.phi0 - legState_.phi2);
-    float cosPhi03 = arm_cos_f32(legState_.phi0 - legState_.phi3);
-    float cosPhi02 = arm_cos_f32(legState_.phi0 - legState_.phi2);
-
-    float Jacobi_data[4] = {0};
-    Jacobi_data[0] = -(l[0]* sinPhi03*sinPhi12)/sinPhi23;
-    Jacobi_data[1] = -(l[0]* cosPhi03*sinPhi12)/(legState_.L0 * sinPhi23);
-
-    Jacobi_data[2] = -(l[3]* sinPhi02*sinPhi34)/sinPhi23;
-    Jacobi_data[3] = -(l[3]* cosPhi02*sinPhi34)/(legState_.L0 * sinPhi23);
-
-    VMC_Jacobi_ = Matrixf<2,2>(Jacobi_data);
-    legOutput_ = VMC_Jacobi_ * legInput_;
-}
-
-void leg::feedback_update(float T1_fb, float T2_fb) {
-    Matrixf<2,1> T12(new (float[2]){T1_fb,T2_fb});
-    legFeedback_ = matrixf::inv(VMC_Jacobi_) * T12;
-}
-
-void leg::get_dot(float phi1_dot,float phi4_dot)
-{
-    //多项式拟合方法获得雅可比矩阵  待测试
-    float phi_1 = legState_.phi1;
-    float phi_4 = legState_.phi4;
-
-    float phi1_sq = phi_1*phi_1;
-    float phi4_sq = phi_4*phi_4;
-    float pose_Jacobi[4] = {0};
-    float temp = sqrtf(powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2));
-
-    pose_Jacobi[0] =
-            (0.5f*(2.0f*(0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - 2.0f*(0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp;
-    pose_Jacobi[1] =
-            (0.5f*(2.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + 2.0f*(0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp;
-
-    float temp2 = (powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2)/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + 1.0f);
-
-    pose_Jacobi[2] =
-            ((0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + ((0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2))/temp2;
-    pose_Jacobi[3] =
-            -(1.0f*((0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - (1.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2)))/temp2;
-    Matrixf<2,2> pose_J(pose_Jacobi);
-
-    State_dot_ = pose_J * Matrixf<2,1>(new (float[2]){phi1_dot,phi4_dot});
-}
-
-Matrixf<4, 1> leg::get_state() {
-    return Matrixf<4, 1>(new (float[4]) {legState_.L0,legState_.phi0,*State_dot_[0],*State_dot_[1]});
-}
-
-float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) {
-    //求导所用时间
-    static uint32_t dot_cnt = 0;
-    static float last_theta_dot,last_L_dot;
-    float L = legState_.L0;
-    float L_dot = State_dot_[0][0];
-
-    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 = legFeedback_[0][0]*cos_theta + legFeedback_[1][0]*sin_theta/L;
-
-    float F_N = P + 0.8f * (zw_dot2 + 9.8f);
-
-    return F_N;
-}
-
-
-
-
-
diff --git a/application/balance/leg.h b/application/balance/leg.h
index ab3aa96..af11244 100644
--- a/application/balance/leg.h
+++ b/application/balance/leg.h
@@ -1,11 +1,9 @@
 //
-// Created by SJQ on 2024/3/1.
+// Created by SJQ on 2023/12/25.
 //
 
 #ifndef WHEEL_LEGGED_LEG_H
 #define WHEEL_LEGGED_LEG_H
-
-#include <matrix.h>
 #include "controller.h"
 
 typedef struct
@@ -21,13 +19,31 @@ typedef struct
 
     uint32_t DWT_CNT;
     float dt;        //用于计算时间并求导
-    float last_phi0;
+
+    float last_dPhi0;
     float phi0_dot;
+    float phi0_dot2;
 
+    float last_dL0;
     float L0_dot;
+    float L0_dot2;
 
+    float position[6]; //腿部关节位置  用于UI绘制
 }Leg_State_t;
 
+typedef struct
+{
+    float F;
+    float Tp;
+}Leg_Input_t;
+
+typedef struct
+{
+    float T1;
+    float T2;
+}Leg_Output_t;
+
+
 typedef struct
 {
     PID_Init_Config_s length_PID_conf;
@@ -35,35 +51,36 @@ typedef struct
     float init_target_L0;    //初始腿长目标值
     float F_feedforward;
 }Leg_init_config_s;
-
-class leg
+typedef struct
 {
-public:
-    leg(Leg_init_config_s* legInitConfig);
-    void state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot);
-    void input_update(float F,float Tp);
-    void feedback_update(float T1_fb,float T2_fb);
-    void calculate_output();
-    Matrixf<4,1> get_state();
-    float calculate_FN(float MotionAccel_z,float theta,float theta_dot);
+    Leg_State_t legState;
+    Leg_Input_t legInput;
+    Leg_Input_t legFeedback;
+    Leg_Output_t legOutput;
 
-    float F_feedforward_; //支持力前馈补偿
-private:
-    Leg_State_t legState_;
-    Matrixf<2,1> legInput_;
-    Matrixf<2,1> legOutput_;
-    Matrixf<2,1> legFeedback_;
+    float target_L0;
+    float target_phi0;
+    PIDInstance Length_PID;
+    PIDInstance Phi0_PID;
 
-    Matrixf<2,1> State_dot_; // L0 和 phi0 的导数
+    float F_feedforward; //支持力前馈补偿
 
-    float target_L0_;
-    float target_phi0_;
-    PIDInstance Length_PID_;
-    PIDInstance Phi0_PID_;
+    arm_matrix_instance_f32 VMC_Jacobi;
+    float VMC_Jacobi_data[4];
+    arm_matrix_instance_f32 inv_VMC_Jacobi;
+    float inv_VMC_Jacobi_data[4];
+}LegInstance;
 
-    Matrixf<2,2> VMC_Jacobi_;
 
-    void get_dot(float phi1_dot,float phi4_dot);
-};
+
+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_feedback_update(LegInstance* legInstance,float T1_fb,float T2_fb);
+
+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/chassis/observer.c b/application/balance/observer.c
similarity index 100%
rename from application/chassis/observer.c
rename to application/balance/observer.c
diff --git a/application/chassis/observer.h b/application/balance/observer.h
similarity index 100%
rename from application/chassis/observer.h
rename to application/balance/observer.h
diff --git a/application/chassis/balance_old.c b/application/chassis/balance_old.c
deleted file mode 100644
index 6691096..0000000
--- a/application/chassis/balance_old.c
+++ /dev/null
@@ -1,242 +0,0 @@
-//
-// Created by SJQ on 2023/12/30.
-//
-
-#include "balance_old.h"
-
-static void mylqr_k(float L0, float K_mat[12]);
-
-void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
-{
-
-    balanceControl->target_L0 = 0.15f;
-    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);
-
-    estimator_init(&balanceControl->v_estimator,1,0.5);
-    Observer_Init(&balanceControl->state_observer);
-    memset(balanceControl->state,0,6);
-    memset(balanceControl->target_state,0,6);
-}
-
-//解算地面支持力F
-float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last);
-
-
-// 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,const float MotionAccel[3])
-{
-    LegInstance *leg_r = &balanceControl->leg_right;
-    LegInstance *leg_l = &balanceControl->leg_left;
-
-    Leg_State_update(leg_r,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
-    Leg_State_update(leg_l,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
-
-    float K_matrix[12]={0};
-    float L_average = leg_r->legState.L0; //(leg_r->legState.L0 + leg_l->legState.L0)/2;
-        //(leg_r->legState.L0 + leg_l->legState.L0)/2;
-    float alpha = PI/2 - leg_r->legState.phi0;
-    float theta = alpha - body_phi;
-    float theta_dot = - leg_r->legState.phi0_dot - body_phi_dot;
-
-    static uint32_t dot_cnt = 0;
-    float dot_dt = DWT_GetDeltaT(&dot_cnt);
-    float theta_dot2 = (balanceControl->state[1] - theta_dot) / dot_dt;
-
-    //计算机体速度
-    float xb = x + 2 * leg_r->legState.L0 * arm_sin_f32(theta);
-    float xb_dot = x_dot + 2*(leg_r->legState.L0_dot* arm_sin_f32(theta) + leg_r->legState.L0 * theta_dot * arm_cos_f32(theta));
-
-    estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[0]);
-
-    balanceControl->state[0] = theta;
-    balanceControl->state[1] = (1.0f - 0.75f) * balanceControl->state[1] +
-                               0.75f * theta_dot;
-    //balanceControl->state[1] = theta_dot;
-    balanceControl->state[2] = x;
-    balanceControl->state[3] = x_dot;
-
-    balanceControl->state[4] = body_phi;
-    balanceControl->state[5] = (1.0f - 0.75f) * balanceControl->state[5] +
-                                0.75f * body_phi_dot;
-//    balanceControl->state[5] = body_phi_dot;
-
-    //Observer_Estimate(&balanceControl->state_observer,balanceControl->state,balanceControl->balance_LQR.control_vector,L_average);
-
-    //地面支持力解算
-    //balanceControl->FN_right = calculate_F(MotionAccel[2],theta,theta_dot,leg_r->legState.L0,leg_r->legState.L0_dot,leg_r->legFeedback.F,leg_r->legFeedback.Tp);
-    //balanceControl->FN_left = calculate_F(MotionAccel[2],theta,theta_dot,leg_l->legState.L0,leg_l->legState.L0_dot,leg_l->legFeedback.F,leg_l->legFeedback.Tp);
-    float theta_state[3] = {theta,theta_dot,theta_dot2};
-    static uint32_t right_cnt = 0,left_cnt = 0;
-    balanceControl->FN_right = calculate_F(MotionAccel[2],theta_state,leg_r,&right_cnt);
-    balanceControl->FN_left = calculate_F(MotionAccel[2],theta_state,leg_l,&left_cnt);
-
-    mylqr_k(L_average,K_matrix);
-
-
-    //离地情况
-    if(balanceControl->FN_right <= 20.0f && balanceControl->FN_left <= 20.0f)
-    {
-        float lqr_K[12] = {0};
-        lqr_K[6] = K_matrix[6];
-        lqr_K[7] = K_matrix[7];
-        LQR_set_K(&balanceControl->balance_LQR,lqr_K);
-    }
-    else
-    {
-        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,balanceControl->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 + 30,Tp + cor_Tp);
-    Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 30,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 add_x)
-{
-    balanceControl->target_state [2] += add_x;
-}
-
-void state_clear(Balance_Control_t* balanceControl)
-{
-    //清除位移状态量
-    balanceControl->target_state [2] = 0;
-    balanceControl->state[2] = 0;
-}
-
-void target_L_set(Balance_Control_t* balanceControl,float add_L)
-{
-    balanceControl->target_L0 += add_L;
-    float_constrain(balanceControl->target_L0,0.15f,0.30f);
-}
-
-
-
-/*
- * MYLQR_K  MATLAB生成
- *     K = MYLQR_K(L0)
- *
- * Arguments    : float L0
- *                float K[12]
- * Return Type  : void
- */
-void mylqr_k(float L0, float K_mat[12])
-{
-    float t2;
-    float t3;
-
-    float K[12];
-    /*     This function was generated by the Symbolic Math Toolbox version 9.2.
-     */
-    t2 = L0 * L0;
-    t3 = t2 * L0;
-    /*     11-Mar-2024 19:12:17 */
-    K[0] =
-            ((L0 * -201.705292F + t2 * 289.584625F) - t3 * 247.715317F) - 14.6884327F;
-    K[1] = ((L0 * 133.66568F - t2 * 384.819702F) + t3 * 354.437653F) + 9.50502F;
-    K[2] =
-            ((L0 * -20.6946182F - t2 * 2.9124589F) - t3 * 2.76665568F) - 3.57213783F;
-    K[3] =
-            ((L0 * 2.46232843F - t2 * 18.8637314F) + t3 * 18.9820461F) + 4.76390266F;
-    K[4] =
-            ((L0 * 16.4841461F - t2 * 49.2097549F) + t3 * 45.4498405F) - 23.5533714F;
-    K[5] = ((L0 * 182.920456F - t2 * 598.236755F) + t3 * 622.566F) - 10.9050007F;
-    K[6] =
-            ((L0 * 56.6551895F - t2 * 155.341293F) + t3 * 140.137665F) - 27.346283F;
-    K[7] =
-            ((L0 * 165.648911F - t2 * 564.416138F) + t3 * 604.892395F) - 9.52724552F;
-    K[8] =
-            ((L0 * -97.1309662F + t2 * 154.570053F) - t3 * 113.610588F) + 42.7205772F;
-    K[9] =
-            ((L0 * -20.722168F + t2 * 194.949692F) - t3 * 261.115692F) + 32.8661461F;
-    K[10] =
-            ((L0 * -16.3245182F + t2 * 33.2480202F) - t3 * 28.061224F) + 7.589993F;
-    K[11] =
-            ((L0 * -17.9091167F + t2 * 72.7530289F) - t3 * 82.8507385F) + 4.82622719F;
-    //matlab赋值顺序不同
-    for (int i = 0; i < 6; ++i) {
-        K_mat[i] = K[2*i];
-    }
-    for (int i = 0; i < 6; ++i) {
-        K_mat[6+i] = K[2*i+1];
-    }
-}
-
-/*
- * File trailer for mylqr_k.c
- *
- * [EOF]
- */
-
-
-/**
- * @brief  解算地面支持力 F
- * @param  float theta[3] = {theta,theta_dot,theta_dot2}
- * @retval none
- * @attention
- */
-float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last)
-{
-    //求导所用时间
-    //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 L = leg->legState.L0;
-    float L_dot = leg->legState.L0_dot;
-    float L_dot2 = leg->legState.L0_dot2;
-
-    float cos_theta = arm_cos_f32(theta[0]);
-    float sin_theta = arm_sin_f32(theta[0]);
-    float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta[1] * sin_theta
-                    + L * theta[2] * sin_theta + L * theta[1] * theta[1] * cos_theta;
-
-    float P = leg->legFeedback.F * cos_theta + leg->legFeedback.Tp * sin_theta/L;
-
-    float F_N = P + 0.8f * (zw_dot2 + 9.8f);
-
-    return F_N;
-
-}
diff --git a/application/chassis/balance_old.h b/application/chassis/balance_old.h
deleted file mode 100644
index 4128c73..0000000
--- a/application/chassis/balance_old.h
+++ /dev/null
@@ -1,51 +0,0 @@
-//
-// Created by SJQ on 2023/12/30.
-//
-
-#ifndef WHEEL_LEGGED_BALANCE_OLD_H
-#define WHEEL_LEGGED_BALANCE_OLD_H
-
-#include "leg_old.h"
-#include "LQR_old.h"
-#include "controller.h"
-#include "kalman_filter.h"
-#include "estimator.h"
-#include "observer.h"
-typedef struct
-{
-    LegInstance leg_right;
-    LegInstance leg_left;
-    PIDInstance leg_coordination_PID;  //双腿协调PD控制
-
-    PIDInstance leg_length_PID; //平均腿长控制
-
-    float state[6];
-    float target_state[6];
-    float target_L0; //平均腿长目标值
-    float FN_left; //地面支持力
-    float FN_right;
-
-    LQRInstance balance_LQR;
-    estimator_t v_estimator;
-    BalanceObserver state_observer;
-
-}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,const float MotionAccel[3]);
-void Balance_Control_Loop(Balance_Control_t* balanceControl);
-void target_x_set(Balance_Control_t* balanceControl,float add_x);
-void target_L_set(Balance_Control_t* balanceControl,float add_L);
-void state_clear(Balance_Control_t* balanceControl);
-
-#endif //WHEEL_LEGGED_BALANCE_OLD_H
diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c
index 8541929..37caaa4 100644
--- a/application/chassis/chassis.c
+++ b/application/chassis/chassis.c
@@ -22,7 +22,7 @@
 #include "power_meter.h"
 #include "ins_task.h"
 
-#include "balance_old.h"
+#include "balance.h"
 #include "observer.h"
 #include "estimator.h"
 
@@ -74,7 +74,13 @@ BalanceObserver Observer;
 
 PIDInstance Turn_Loop_PID , Roll_Loop_PID;
 
-Chassis_state_e ChassisState;
+Chassis_state_e ChassisState,last_ChassisState;
+chassis_mode_e last_chassis_mode;
+
+first_order_filter_type_t wheel_r_filter,wheel_l_filter;
+
+first_order_filter_type_t ht_rf_filter,ht_rb_filter,ht_lf_filter,ht_lb_filter;
+first_order_filter_type_t vx_filter;
 
 void ChassisInit()
 {
@@ -115,7 +121,6 @@ void ChassisInit()
     //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);
@@ -125,6 +130,7 @@ void ChassisInit()
     //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);
@@ -142,13 +148,16 @@ void ChassisInit()
 
     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在其他人看来是反的
-        }};
+            .can_config = {
+                    .can_handle = &hcan1,
+                    .tx_id = 0x210,
+                    .rx_id = 0x211,
+            }};
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
+    SuperCapSetPower(cap,70);  // 超级电容限制功率
+
     PowerMeter_Init_Config_s power_conf = {
             .can_config = {
                     .can_handle = &hcan1,
@@ -161,8 +170,8 @@ void ChassisInit()
     Balance_Init_config_s balanceInitConfig = {
             .legInitConfig = {
                     .length_PID_conf = {
-                            .Kp = 500,//180 ,//50,
-                            .Ki = 100,//5,
+                            .Kp = 300,//500,//180 ,//50,
+                            .Ki = 0,//5,
                             .Kd = 20,//10,//6,//6,
                             .MaxOut = 100,
                             .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
@@ -199,10 +208,11 @@ void ChassisInit()
     Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
     //转向环
     PID_Init_Config_s turn_pid_cfg = {
-            .Kp = 10.0f,//10.0f,//1,
+            .Kp = 12.0f,//10.0f,//1,
             .Ki = 0,
-            .Kd = 0.0f,
-            .MaxOut = 2,
+            .Kd = 0.8f,
+            .MaxOut = 2.0f,
+
             .Improve =  PID_Derivative_On_Measurement|PID_DerivativeFilter,
             .Derivative_LPF_RC = 0.001f
     };
@@ -210,7 +220,7 @@ void ChassisInit()
 
     //Roll轴平衡
     PID_Init_Config_s roll_pid_cfg = {
-            .Kp = 8.0f,
+            .Kp = 12.0f,
             .Ki = 0,
             .Kd = 0.5f,
             .MaxOut = 100,
@@ -240,6 +250,17 @@ void ChassisInit()
     chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
     chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
 #endif // ONE_BOARD
+    const float a = 0.03f;
+    first_order_filter_init(&wheel_r_filter,1e-3f,&a);
+    first_order_filter_init(&wheel_l_filter,1e-3f,&a);
+
+    const float ht_a = 0.05f;
+    first_order_filter_init(&ht_rf_filter,1e-3f,&ht_a);
+    first_order_filter_init(&ht_rb_filter,1e-3f,&ht_a);
+    first_order_filter_init(&ht_lf_filter,1e-3f,&ht_a);
+    first_order_filter_init(&ht_lb_filter,1e-3f,&ht_a);
+    const float vx_a = 0.6f;
+    first_order_filter_init(&vx_filter,1e-3f,&vx_a);
 }
 
 
@@ -247,14 +268,16 @@ void ChassisInit()
  * @brief 获取数据并计算状态量
  *
  */
+
+#define HIP_OFFSET 0.49899f//0.23719f //0.119031f
 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 - HIP_OFFSET);
+    leg_phi[1] = -(motor_rb->measure.total_angle + HIP_OFFSET);
+    leg_phi[2] = PI-(-motor_lf->measure.total_angle - HIP_OFFSET);
+    leg_phi[3] = -(-motor_lb->measure.total_angle + HIP_OFFSET);
 
 //最高点上电
 //    leg_phi[0] = PI-(motor_rf->measure.total_angle + 0.81007f);
@@ -268,7 +291,7 @@ static void Chassis_State_update()
     leg_phi_dot[3] = motor_lb->measure.speed_rads;
 
 
-    float body_phi = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
+    float body_phi = (Chassis_IMU_data->Pitch) * DEGREE_2_RAD; //+ 2
     float body_phi_dot = Chassis_IMU_data->Gyro[X];
 
     //陀螺仪积分获取位移和速度
@@ -290,6 +313,7 @@ static void Chassis_State_update()
     //倒地需要清零总位移数据
     if(chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
     {
+        ChassisState = FAIL;
         left_offset = wheel_motor_l->measure.total_angle;
         right_offset = wheel_motor_r->measure.total_angle;
     }
@@ -305,14 +329,26 @@ static void Chassis_State_update()
     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);
+    float separate_x[2] = { - x_r * DEGREE_2_RAD * RADIUS_WHEEL , x_l * DEGREE_2_RAD * RADIUS_WHEEL};
+    float separate_x_dot[2] = { - wheel_motor_r->measure.speed_rads * RADIUS_WHEEL , wheel_motor_l->measure.speed_rads * RADIUS_WHEEL};
+//    float separate_x[2] = { x,x};
+//    float separate_x_dot[2] = { x_dot,x_dot};
 
+
+    //Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n);
+    Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,separate_x,separate_x_dot,Chassis_IMU_data->MotionAccel_n);
     if( ChassisState == FAIL )
     {
         if(abs(body_phi) <= 0.05 && abs(body_phi) <= 0.005 )
             ChassisState = STAND;
     }
+    if(chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE && last_chassis_mode != CHASSIS_ZERO_FORCE)
+    {
+        //倒地重新初始化卡尔曼滤波器一次
+        //estimator_init(&BalanceControl.v_estimator,0.001f,10.0f);
+    }
 
+    last_chassis_mode = chassis_cmd_recv.chassis_mode;
 }
 
 /* 机器人底盘控制核心任务 */
@@ -327,6 +363,15 @@ void ChassisTask()
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
 #endif // CHASSIS_BOARD
 
+    //死亡情况下 关闭电机
+    if(referee_data->GameRobotState.power_management_chassis_output == 0)
+        chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
+
+    //设置超电上限功率
+    float power_limit = referee_data->GameRobotState.chassis_power_limit;
+    power_limit = float_constrain(power_limit,40,120);
+    SuperCapSetPower(cap,power_limit);
+
     if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
     { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
         HTMotorStop(motor_lf);
@@ -343,6 +388,7 @@ void ChassisTask()
         PIDClear(&BalanceControl.leg_coordination_PID);
         //清除状态量
         state_clear(&BalanceControl);
+        //estimator_init(&BalanceControl.v_estimator,0.001f,10.0f);
     }
     else
     { // 正常工作
@@ -356,6 +402,8 @@ void ChassisTask()
 
 
     static int8_t init_flag = FALSE;
+    static uint8_t jump_flag = 0,last_jump_flag = 0;
+    static float jump_time = 0,shrink_time = 0;
     // 根据控制模式设定旋转速度
     switch (chassis_cmd_recv.chassis_mode)
     {
@@ -364,11 +412,36 @@ void ChassisTask()
         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);
+        chassis_cmd_recv.wz = 0.08f * chassis_cmd_recv.offset_angle;
+        jump_flag = 0;
+        break;
+    case CHASSIS_LATERAL: //侧向对敌
+        chassis_cmd_recv.wz = 0.08f * loop_float_constrain(chassis_cmd_recv.offset_angle + 90,-180,180);
         break;
     case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
+    {   //chassis_cmd_recv.wz = 1.0f;
+        if(last_jump_flag == 0)
+        {
+            jump_flag = 1;
+            jump_time = DWT_GetTimeline_ms();
+        }
+        else if(last_jump_flag == 1)
+        {
+            if (jump_time + 150 <= DWT_GetTimeline_ms()) {
+                jump_flag = 2;
+                shrink_time = DWT_GetTimeline_ms();
+            }
+        }
+        else if(last_jump_flag == 2)
+        {
+            if (shrink_time + 200 <= DWT_GetTimeline_ms()) {
+                jump_flag = 3;
+            }
+        }
+        last_jump_flag = jump_flag;
+    }
         //chassis_cmd_recv.wz = 4000;
-        chassis_cmd_recv.wz = 2.0f;
+        //chassis_cmd_recv.wz = 2.0f;
         break;
     default:
         break;
@@ -379,9 +452,12 @@ void ChassisTask()
     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;
+//    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;
+    first_order_filter_cali(&vx_filter,chassis_cmd_recv.vx/5280 * 3);
+    chassis_vx = vx_filter.out;
+
 
     // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
     // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
@@ -392,74 +468,64 @@ void ChassisTask()
 
     //获取数据并计算状态量
     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_x_set(&BalanceControl,chassis_vx);
         target_L_set(&BalanceControl,chassis_cmd_recv.vy/5280 * 0.0003f);
-        Balance_Control_Loop(&BalanceControl);
+
+        float turn_T = 0;
+        //离地情况下把转向环和roll轴补偿关掉
+        if(BalanceControl.fly_flag_r == FALSE || BalanceControl.fly_flag_l == FALSE)
+        {
+            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;
+            turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);
+        }
+        else
+        {
+            turn_T = 0;
+            BalanceControl.leg_right.F_feedforward = 0;
+            BalanceControl.leg_left.F_feedforward = 0;
+        }
+
+        Balance_Control_Loop(&BalanceControl,jump_flag);
 
         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);
+        wz_feedback =  Chassis_IMU_data->Gyro[Z];
 
-        static float T,wheel_current_r,wheel_current_l;
+        static float wheel_current_r,wheel_current_l,Tr,Tl;
 
-        T = BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
+        //轮电机滤波并输出
+        Tr = BalanceControl.LQR_r.control_vector[0];
+        Tl = BalanceControl.LQR_l.control_vector[0];
 
-        //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;
+        first_order_filter_cali(&wheel_r_filter,Tr+turn_T);
+        first_order_filter_cali(&wheel_l_filter,Tl-turn_T);
+        wheel_current_r = (wheel_r_filter.out)*LK_TORQUE_2_CMD;//(T+turn_T) * LK_TORQUE_2_CMD;
+        wheel_current_l = (wheel_l_filter.out)*LK_TORQUE_2_CMD;//(T-turn_T) * LK_TORQUE_2_CMD;
 
         float_constrain(wheel_current_r,-4000,4000);
         float_constrain(wheel_current_l,-4000,4000);
+        //关节电机滤波并输出
+        first_order_filter_cali(&ht_rf_filter,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
+        first_order_filter_cali(&ht_rb_filter,-BalanceControl.leg_right.legOutput.T2 * HT_CMD_COEF);
 
-//        if (ChassisState == FAIL) // 倒地 未起身
-//        {
+        first_order_filter_cali(&ht_lf_filter,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF);
+        first_order_filter_cali(&ht_lb_filter,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF);
+
+        HTMotorSetRef(motor_rf,ht_rf_filter.out);
+        HTMotorSetRef(motor_rb,ht_rb_filter.out);
+
+        HTMotorSetRef(motor_lf,ht_lf_filter.out);
+        HTMotorSetRef(motor_lb,ht_lb_filter.out);
 //            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);
@@ -477,11 +543,31 @@ void ChassisTask()
         PIDClear(&BalanceControl.leg_coordination_PID);
     }
 
+    static uint8_t last_UI_refresh = 0 ;
+    if(chassis_cmd_recv.UI_refresh != last_UI_refresh)
+        MyUIInit();
+    last_UI_refresh = chassis_cmd_recv.UI_refresh;
+    //推送ui显示界面
+    ui_data.relative_angle = chassis_cmd_recv.offset_angle;
 
+    for (int i = 0; i < 6; ++i) {
+        ui_data.leg_pos_r[i] = BalanceControl.leg_right.legState.position[i];
+        ui_data.leg_pos_l[i] = BalanceControl.leg_left.legState.position[i];
+    }
+
+    ui_data.chassis_mode = chassis_cmd_recv.chassis_mode;
+    ui_data.shoot_mode = chassis_cmd_recv.shoot_mode;
+    ui_data.friction_mode = chassis_cmd_recv.friction_mode;
+    ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit;
+    ui_data.Chassis_Power_Data.cap_vol = (float)cap->cap_msg.cap_vol/100.0f;
+    ui_data.Chassis_Power_Data.input_vol = (float)cap->cap_msg.input_vol/100.0f;
 
 
 
     // 推送反馈消息
+    chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
+    chassis_feedback_data.real_wz = Chassis_IMU_data->Gyro[Z];
+    chassis_feedback_data.power_management_chassis_output = referee_data->GameRobotState.power_management_chassis_output;
 #ifdef ONE_BOARD
     PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
 #endif
diff --git a/application/chassis/leg_old.h b/application/chassis/leg_old.h
deleted file mode 100644
index ed03ffa..0000000
--- a/application/chassis/leg_old.h
+++ /dev/null
@@ -1,85 +0,0 @@
-//
-// Created by SJQ on 2023/12/25.
-//
-
-#ifndef WHEEL_LEGGED_LEG_OLD_H
-#define WHEEL_LEGGED_LEG_OLD_H
-#include "controller.h"
-
-typedef struct
-{
-    float L0;
-    float phi0;
-
-    float phi1;
-    float phi4;
-
-    float phi2;
-    float phi3;
-
-    uint32_t DWT_CNT;
-    float dt;        //用于计算时间并求导
-
-    float last_dPhi0;
-    float phi0_dot;
-    float phi0_dot2;
-
-    float last_dL0;
-    float L0_dot;
-    float L0_dot2;
-
-}Leg_State_t;
-
-typedef struct
-{
-    float F;
-    float Tp;
-}Leg_Input_t;
-
-typedef struct
-{
-    float T1;
-    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;
-    Leg_Input_t legInput;
-    Leg_Input_t legFeedback;
-    Leg_Output_t legOutput;
-
-    float target_L0;
-    float target_phi0;
-    PIDInstance Length_PID;
-    PIDInstance Phi0_PID;
-
-    float F_feedforward; //支持力前馈补偿
-
-    arm_matrix_instance_f32 VMC_Jacobi;
-    float VMC_Jacobi_data[4];
-    arm_matrix_instance_f32 inv_VMC_Jacobi;
-    float inv_VMC_Jacobi_data[4];
-}LegInstance;
-
-
-
-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_feedback_update(LegInstance* legInstance,float T1_fb,float T2_fb);
-
-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_OLD_H
diff --git a/application/robot_def.h b/application/robot_def.h
index 087ba3b..3ef03e7 100644
--- a/application/robot_def.h
+++ b/application/robot_def.h
@@ -86,6 +86,7 @@ typedef enum
     CHASSIS_ROTATE,            // 小陀螺模式
     CHASSIS_NO_FOLLOW,         // 不跟随，允许全向平移
     CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
+    CHASSIS_LATERAL,           // 侧向对敌
 } chassis_mode_e;
 
 // 云台模式设置
@@ -127,6 +128,8 @@ typedef enum
 typedef struct
 { // 功率控制
     float chassis_power_mx;
+    float cap_vol;
+    float input_vol;
 } Chassis_Power_Data_s;
 
 /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
@@ -142,10 +145,15 @@ typedef struct
     float vy;           // 横移方向速度
     float wz;           // 旋转速度
     float offset_angle; // 底盘和归中位置的夹角
+    float leg_length;   // 腿长
     chassis_mode_e chassis_mode;
     int chassis_speed_buff;
     // UI部分
     //  ...
+    shoot_mode_e shoot_mode;				 // 发射模式设置
+    friction_mode_e friction_mode;			 // 摩擦轮关闭
+
+    uint8_t UI_refresh;
 
 } Chassis_Ctrl_Cmd_s;
 
@@ -185,12 +193,11 @@ typedef struct
     // 后续增加底盘的真实速度
     // float real_vx;
     // float real_vy;
-    // float real_wz;
+    // 底盘旋转速度
+     float real_wz;
 
-    uint8_t rest_heat;           // 剩余枪口热量
-    Bullet_Speed_e bullet_speed; // 弹速限制
     Enemy_Color_e enemy_color;   // 0 for blue, 1 for red
-
+    uint8_t power_management_chassis_output;
 } Chassis_Upload_Data_s;
 
 
diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c
index f22a4bb..fe2122b 100644
--- a/modules/algorithm/user_lib.c
+++ b/modules/algorithm/user_lib.c
@@ -253,3 +253,34 @@ void ramp_calc(ramp_function_source_t *ramp_source_type, float input)
         ramp_source_type->out = ramp_source_type->min_value;
     }
 }
+
+/**
+  * @brief          一阶低通滤波初始化
+  * @author         RM
+  * @param[in]      一阶低通滤波结构体
+  * @param[in]      间隔的时间，单位 s
+  * @param[in]      滤波参数
+  * @retval         返回空
+  */
+void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1])
+{
+    first_order_filter_type->frame_period = frame_period;
+    first_order_filter_type->num[0] = num[0];
+    first_order_filter_type->input = 0.0f;
+    first_order_filter_type->out = 0.0f;
+}
+
+/**
+  * @brief          一阶低通滤波计算
+  * @author         RM
+  * @param[in]      一阶低通滤波结构体
+  * @param[in]      间隔的时间，单位 s
+  * @retval         返回空
+  */
+void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input)
+{
+    first_order_filter_type->input = input;
+    first_order_filter_type->out =
+            first_order_filter_type->num[0] / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->out + first_order_filter_type->frame_period / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->input;
+}
+
diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h
index e93a5d8..6b924f2 100644
--- a/modules/algorithm/user_lib.h
+++ b/modules/algorithm/user_lib.h
@@ -58,6 +58,20 @@ typedef  struct
     float frame_period; //时间间隔
 } ramp_function_source_t;
 
+typedef struct
+{
+    float input;        //输入数据
+    float out;          //滤波输出的数据
+    float num[1];       //滤波参数
+    float frame_period; //滤波的时间间隔 单位 s
+} first_order_filter_type_t;
+
+//一阶低通滤波初始化
+void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1]);
+//一阶低通滤波计算
+void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input);
+
+
 /* circumference ratio */
 #ifndef PI
 #define PI 3.14159265354f
diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c
index a5e8b27..b8f051f 100644
--- a/modules/motor/HTmotor/HT04.c
+++ b/modules/motor/HTmotor/HT04.c
@@ -71,7 +71,7 @@ static void HTMotorLostCallback(void *motor_ptr)
 {
     HTMotorInstance *motor = (HTMotorInstance *)motor_ptr;
     LOGWARNING("[ht_motor] motor %d lost\n", motor->motor_can_instace->tx_id);
-    if (++motor->lost_cnt % 10 != 0)
+    if (++motor->lost_cnt % 100 != 0)
         HTMotorSetMode(CMD_MOTOR_MODE, motor); // 尝试重新让电机进入控制模式
 }
 
diff --git a/modules/referee/referee_UI.c b/modules/referee/referee_UI.c
index 55ce3c6..a087879 100644
--- a/modules/referee/referee_UI.c
+++ b/modules/referee/referee_UI.c
@@ -198,7 +198,7 @@ void UIOvalDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate,
 
 void UIArcDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
 			   uint32_t Graph_StartAngle, uint32_t Graph_EndAngle, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y,
-			   uint32_t end_x, uint32_t end_y)
+			   uint32_t x_Length, uint32_t y_Length)
 {
 	int i;
 	for (i = 0; i < 3 && graphname[i] != '\0'; i++)
@@ -217,8 +217,8 @@ void UIArcDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, u
 	graph->start_x = Start_x;
 	graph->start_y = Start_y;
 	graph->radius = 0;
-	graph->end_x = end_x;
-	graph->end_y = end_y;
+	graph->end_x = x_Length;
+	graph->end_y = y_Length;
 }
 
 /************************************************绘制浮点型数据*************************************************
diff --git a/modules/referee/referee_UI.h b/modules/referee/referee_UI.h
index 4fba98b..d967270 100644
--- a/modules/referee/referee_UI.h
+++ b/modules/referee/referee_UI.h
@@ -54,7 +54,7 @@ void UIOvalDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate,
 
 void UIArcDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
                uint32_t Graph_StartAngle, uint32_t Graph_EndAngle, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y,
-               uint32_t end_x, uint32_t end_y);
+               uint32_t x_Length, uint32_t y_Length);
 
 void UIFloatDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
                  uint32_t Graph_Size, uint32_t Graph_Digit, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, int32_t Graph_Float);
diff --git a/modules/referee/referee_protocol.h b/modules/referee/referee_protocol.h
index 22416b3..6a9eda0 100644
--- a/modules/referee/referee_protocol.h
+++ b/modules/referee/referee_protocol.h
@@ -83,6 +83,8 @@ typedef enum
 	ID_robot_hurt = 0x0206,				   // 伤害状态数据
 	ID_shoot_data = 0x0207,				   // 实时射击数据
 	ID_student_interactive = 0x0301,	   // 机器人间交互数据
+    ID_custom_robot = 0x302,               // 自定义控制器数据（图传链路）
+    ID_remote_control = 0x304,             // 键鼠遥控数据（图传链路）
 } CmdID_e;
 
 /* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */
@@ -102,6 +104,8 @@ typedef enum
 	LEN_shoot_data = 7,							 // 0x0207
 	LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301
 
+    LEN_remote_control = 12,                     // 0x0304
+
 } JudgeDataLength_e;
 
 /****************************接收数据的详细说明****************************/
@@ -157,37 +161,31 @@ typedef struct
 	uint8_t supply_projectile_num;
 } ext_supply_projectile_action_t;
 
-/* ID: 0X0201  Byte: 27    机器人状态数据 */
+/* ID: 0X0201  Byte: 13    机器人性能体系数据 */
 typedef struct
 {
-	uint8_t robot_id;
-	uint8_t robot_level;
-	uint16_t remain_HP;
-	uint16_t max_HP;
-	uint16_t shooter_id1_17mm_cooling_rate;
-	uint16_t shooter_id1_17mm_cooling_limit;
-	uint16_t shooter_id1_17mm_speed_limit;
-	uint16_t shooter_id2_17mm_cooling_rate;
-	uint16_t shooter_id2_17mm_cooling_limit;
-	uint16_t shooter_id2_17mm_speed_limit;
-	uint16_t shooter_id1_42mm_cooling_rate;
-	uint16_t shooter_id1_42mm_cooling_limit;
-	uint16_t shooter_id1_42mm_speed_limit;
-	uint16_t chassis_power_limit;
-	uint8_t mains_power_gimbal_output : 1;
-	uint8_t mains_power_chassis_output : 1;
-	uint8_t mains_power_shooter_output : 1;
+    uint8_t robot_id;
+    uint8_t robot_level;
+    uint16_t current_HP;
+    uint16_t maximum_HP;
+    uint16_t shooter_barrel_cooling_value;
+    uint16_t shooter_barrel_heat_limit;
+    uint16_t chassis_power_limit;
+    uint8_t power_management_gimbal_output : 1;
+    uint8_t power_management_chassis_output : 1;
+    uint8_t power_management_shooter_output : 1;
 } ext_game_robot_state_t;
 
-/* ID: 0X0202  Byte: 14    实时功率热量数据 */
+/* ID: 0X0202  Byte: 16    实时功率热量数据 */
 typedef struct
 {
-	uint16_t chassis_volt;
-	uint16_t chassis_current;
-	float chassis_power;		   // 瞬时功率
-	uint16_t chassis_power_buffer; // 60焦耳缓冲能量
-	uint16_t shooter_heat0;		   // 17mm
-	uint16_t shooter_heat1;
+    uint16_t chassis_voltage;
+    uint16_t chassis_current;
+    float chassis_power;
+    uint16_t buffer_energy;
+    uint16_t shooter_17mm_1_barrel_heat;
+    uint16_t shooter_17mm_2_barrel_heat;
+    uint16_t shooter_42mm_barrel_heat;
 } ext_power_heat_data_t;
 
 /* ID: 0x0203  Byte: 16    机器人位置数据 */
@@ -227,6 +225,19 @@ typedef struct
 	float bullet_speed;
 } ext_shoot_data_t;
 
+/****************************图传链路数据****************************/
+/* ID: 0x0304  Byte:  7    图传链路键鼠遥控数据 */
+typedef struct
+{
+    int16_t mouse_x;
+    int16_t mouse_y;
+    int16_t mouse_z;
+    int8_t left_button_down;
+    int8_t right_button_down;
+    uint16_t keyboard_value;
+    uint16_t reserved;
+}remote_control_t;
+/****************************图传链路数据****************************/
 /****************************机器人交互数据****************************/
 /****************************机器人交互数据****************************/
 /* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超，数据段最多30个，也不会超*/
diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c
index 7971265..1eedd25 100644
--- a/modules/referee/referee_task.c
+++ b/modules/referee/referee_task.c
@@ -14,6 +14,7 @@
 #include "referee_UI.h"
 #include "string.h"
 #include "cmsis_os.h"
+#include "user_lib.h"
 
 static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
 static referee_info_t *referee_recv_info;            // 接收到的裁判系统数据
@@ -39,6 +40,14 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
 static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测
 static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化
 
+static void DrawArmorPose(float relative_angle);
+void DrawLegPose(Referee_Interactive_info_t *_Interactive_data,int x_offset,int y_offset,int scale);
+
+//腿部姿态绘制位置、大小
+#define X_OFFSET 1560
+#define Y_OFFSET 440
+#define SCALE 400
+
 referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data)
 {
     referee_recv_info = RefereeInit(referee_usart_handle); // 初始化裁判系统的串口,并返回裁判系统反馈数据指针
@@ -49,20 +58,28 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int
 
 void UITask()
 {
-    RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
+    //RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
     MyUIRefresh(referee_recv_info, Interactive_data);
+    DrawArmorPose(Interactive_data->relative_angle);
+    DrawLegPose(Interactive_data,X_OFFSET,Y_OFFSET,SCALE);
 }
 
 static Graph_Data_t UI_shoot_line[10]; // 射击准线
-static Graph_Data_t UI_Energy[3];      // 电容能量条
-static String_Data_t UI_State_sta[6];  // 机器人状态,静态只需画一次
+static Graph_Data_t UI_Energy[5];      // 电容能量条
+
+static Graph_Data_t UI_ArmorPose[3];   // 前后装甲板位置
+static Graph_Data_t UI_LegPose_r[5];     // 腿部姿态
+static Graph_Data_t UI_LegPose_l[5];     // 腿部姿态
+
+static String_Data_t UI_State_sta[7];  // 机器人状态,静态只需画一次
 static String_Data_t UI_State_dyn[6];  // 机器人状态,动态先add才能change
 static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
 
+
 void MyUIInit()
 {
-    if (!referee_recv_info->init_flag)
-        vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
+//    if (!referee_recv_info->init_flag)
+//        vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
     while (referee_recv_info->GameRobotState.robot_id == 0)
         osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
 
@@ -103,8 +120,13 @@ void MyUIInit()
     UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
 
     // 底盘功率显示，静态
-    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 620, 230, "Power:");
+    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power:");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
+
+    // 电容电压显示，动态
+    UICharDraw(&UI_State_sta[6], "v_cap_str", UI_Graph_ADD, 8, UI_Color_Main, 18, 2, 1000, 230, "V cap:");
+    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]);
+
     // 能量条框
     UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
     UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
@@ -114,6 +136,43 @@ void MyUIInit()
     // 能量条初始状态
     UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
     UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
+    // 电容电压显示,动态
+    UIFloatDraw(&UI_Energy[3], "v_cap", UI_Graph_ADD, 8, UI_Color_Main, 18, 2, 2, 1200, 230, 24000);
+    UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[3]);
+
+    // 装甲板姿态基准线 静态
+//    UICircleDraw(&UI_ArmorPose[0],"armor",UI_Graph_ADD,8,UI_Color_Main,5,960,540,350);
+//    UIGraphRefresh(&referee_recv_info->referee_id, 1, &UI_ArmorPose[0]);
+    //装甲板相对位置 动态
+    UIArcDraw(&UI_ArmorPose[1],"front",UI_Graph_ADD,7,UI_Color_Yellow,60,120,10,960,540,350,350);
+    UIArcDraw(&UI_ArmorPose[2],"back",UI_Graph_ADD,7,UI_Color_Yellow,240,300,10,960,540,350,350);
+    UIGraphRefresh(&referee_recv_info->referee_id,2,UI_ArmorPose[1],UI_ArmorPose[2]);
+
+    UILineDraw(&UI_LegPose_r[0], "AE", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UILineDraw(&UI_LegPose_r[1], "AB", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UILineDraw(&UI_LegPose_r[2], "BC", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UILineDraw(&UI_LegPose_r[3], "CD", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UILineDraw(&UI_LegPose_r[4], "ED", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_LegPose_r[0], UI_LegPose_r[1], UI_LegPose_r[2], UI_LegPose_r[3], UI_LegPose_r[4]);
+
+
+    UILineDraw(&UI_LegPose_l[0], "AE_l", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UILineDraw(&UI_LegPose_l[1], "AB_l", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UILineDraw(&UI_LegPose_l[2], "BC_l", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UILineDraw(&UI_LegPose_l[3], "CD_l", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UILineDraw(&UI_LegPose_l[4], "ED_l", UI_Graph_ADD, 7, UI_Color_Main,
+               3, 960-75, 540, 960+75, 540);
+    UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_LegPose_l[0], UI_LegPose_l[1], UI_LegPose_l[2], UI_LegPose_l[3], UI_LegPose_l[4]);
+
 }
 
 // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
@@ -247,10 +306,81 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
     if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
     {
         UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 750, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
-        UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
-        UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
+        //UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
+        UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[1]);
         _Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
     }
+
+    //电容电压始终刷新
+    // 电容电压显示,动态
+    UIFloatDraw(&UI_Energy[3], "v_cap", UI_Graph_Change, 8, UI_Color_Main, 18, 2, 2, 1200, 230, _Interactive_data->Chassis_Power_Data.cap_vol * 1000);
+    //UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[3]);
+    //电容能量条  720~1220
+    float percent = (_Interactive_data->Chassis_Power_Data.cap_vol - 12)/(_Interactive_data->Chassis_Power_Data.input_vol - 12);
+    float length = percent * (1220 - 720);
+    UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (int)(720+length), 160);
+    UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[2], UI_Energy[3]);
+}
+
+void DrawArmorPose(float relative_angle)
+{
+    float start_angle_b = loop_float_constrain(relative_angle - 30,0,360);
+    float end_angle_b = loop_float_constrain(relative_angle + 30,0,360);
+
+    float start_angle_f = loop_float_constrain(start_angle_b + 180,0,360);
+    float end_angle_f = loop_float_constrain(end_angle_b + 180,0,360);
+
+    UIArcDraw(&UI_ArmorPose[1],"front",UI_Graph_Change,7,UI_Color_Yellow,
+              start_angle_f,end_angle_f,10,960,540,350,350);
+    UIArcDraw(&UI_ArmorPose[2],"back",UI_Graph_Change,7,UI_Color_Yellow,
+              start_angle_b,end_angle_b,10,960,540,350,350);
+    UIGraphRefresh(&referee_recv_info->referee_id,2,UI_ArmorPose[1],UI_ArmorPose[2]);
+}
+
+
+void DrawLegPose(Referee_Interactive_info_t *_Interactive_data,int x_offset,int y_offset,int scale)
+{
+    //float *leg_position = _Interactive_data->leg_pos;
+    float xa = 0.075f * scale + x_offset,xe = -0.075f * scale + x_offset;
+    float ya = 0 + y_offset,ye = 0 + y_offset;
+    float leg_position[6] = {0};
+    for (int i = 0; i < 6; i+=2) {
+        leg_position[i] = -_Interactive_data->leg_pos_r[i] * scale + x_offset;
+        leg_position[i+1] = -_Interactive_data->leg_pos_r[i+1] * scale + y_offset;
+    }
+    UILineDraw(&UI_LegPose_r[0], "AE", UI_Graph_Change, 7, UI_Color_Main,
+               3, xa, ya, xe, ye);
+    UILineDraw(&UI_LegPose_r[1], "AB", UI_Graph_Change, 7, UI_Color_Main,
+               3, xa, ya, leg_position[0], leg_position[1]);
+    UILineDraw(&UI_LegPose_r[2], "BC", UI_Graph_Change, 7, UI_Color_Main,
+               3, leg_position[0], leg_position[1], leg_position[2], leg_position[3]);
+    UILineDraw(&UI_LegPose_r[3], "CD", UI_Graph_Change, 7, UI_Color_Main,
+               3, leg_position[2], leg_position[3], leg_position[4], leg_position[5]);
+    UILineDraw(&UI_LegPose_r[4], "ED", UI_Graph_Change, 7, UI_Color_Main,
+               3, xe, ye, leg_position[4], leg_position[5]);
+    UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_LegPose_r[0], UI_LegPose_r[1], UI_LegPose_r[2], UI_LegPose_r[3], UI_LegPose_r[4]);
+
+    x_offset += 0.5 * scale;
+    xa = 0.075f * scale + x_offset,xe = -0.075f * scale + x_offset;
+    ya = 0 + y_offset,ye = 0 + y_offset;
+
+    for (int i = 0; i < 6; i+=2) {
+        leg_position[i] = -_Interactive_data->leg_pos_r[i] * scale + x_offset;
+        leg_position[i+1] = -_Interactive_data->leg_pos_r[i+1] * scale + y_offset;
+    }
+    UILineDraw(&UI_LegPose_l[0], "AE_l", UI_Graph_Change, 7, UI_Color_Main,
+               3, xa, ya, xe, ye);
+    UILineDraw(&UI_LegPose_l[1], "AB_l", UI_Graph_Change, 7, UI_Color_Main,
+               3, xa, ya, leg_position[0], leg_position[1]);
+    UILineDraw(&UI_LegPose_l[2], "BC_l", UI_Graph_Change, 7, UI_Color_Main,
+               3, leg_position[0], leg_position[1], leg_position[2], leg_position[3]);
+    UILineDraw(&UI_LegPose_l[3], "CD_l", UI_Graph_Change, 7, UI_Color_Main,
+               3, leg_position[2], leg_position[3], leg_position[4], leg_position[5]);
+    UILineDraw(&UI_LegPose_l[4], "ED_l", UI_Graph_Change, 7, UI_Color_Main,
+               3, xe, ye, leg_position[4], leg_position[5]);
+    UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_LegPose_l[0], UI_LegPose_l[1], UI_LegPose_l[2], UI_LegPose_l[3], UI_LegPose_l[4]);
+
+
 }
 
 /**
diff --git a/modules/referee/rm_referee.c b/modules/referee/rm_referee.c
index 2743d7d..71041b7 100644
--- a/modules/referee/rm_referee.c
+++ b/modules/referee/rm_referee.c
@@ -95,6 +95,9 @@ static void JudgeReadData(uint8_t *buff)
 				case ID_student_interactive: // 0x0301   syhtodo接收代码未测试
 					memcpy(&referee_info.ReceiveData, (buff + DATA_Offset), LEN_receive_data);
 					break;
+                case ID_remote_control: // 0x0302
+                    memcpy(&referee_info.RemoteControl, (buff + DATA_Offset), LEN_remote_control);
+                    break;
 				}
 			}
 		}
diff --git a/modules/referee/rm_referee.h b/modules/referee/rm_referee.h
index e935e94..c8784ef 100644
--- a/modules/referee/rm_referee.h
+++ b/modules/referee/rm_referee.h
@@ -38,6 +38,8 @@ typedef struct
 	ext_robot_hurt_t RobotHurt;							   // 0x0206
 	ext_shoot_data_t ShootData;							   // 0x0207
 
+    remote_control_t RemoteControl;                        // 0x304
+
 	// 自定义交互数据的接收
 	Communicate_ReceiveData_t ReceiveData;
 
@@ -68,6 +70,10 @@ typedef struct
 	lid_mode_e lid_mode;					 // 弹舱盖打开
 	Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
 
+    float relative_angle;  //云台相对角度
+    float leg_pos_r[6];      //腿部姿态
+    float leg_pos_l[6];      //腿部姿态
+
 	// 上一次的模式，用于flag判断
 	chassis_mode_e chassis_last_mode;
 	gimbal_mode_e gimbal_last_mode;
diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c
index eb86749..a75d507 100644
--- a/modules/super_cap/super_cap.c
+++ b/modules/super_cap/super_cap.c
@@ -1,14 +1,8 @@
-/*
- * @Descripttion:
- * @version:
- * @Author: Chenfu
- * @Date: 2022-12-02 21:32:47
- * @LastEditTime: 2022-12-05 15:29:49
- */
 #include "super_cap.h"
 #include "memory.h"
 #include "stdlib.h"
 
+
 static SuperCapInstance *super_cap_instance = NULL; // 可以由app保存此指针
 
 static void SuperCapRxCallback(CANInstance *_instance)
@@ -17,16 +11,17 @@ static void SuperCapRxCallback(CANInstance *_instance)
     SuperCap_Msg_s *Msg;
     rxbuff = _instance->rx_buff;
     Msg = &super_cap_instance->cap_msg;
-    Msg->vol = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
-    Msg->current = (uint16_t)(rxbuff[2] << 8 | rxbuff[3]);
-    Msg->power = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
+    Msg->input_vol = (int16_t)(rxbuff[1] << 8 | rxbuff[0]);
+    Msg->cap_vol = (int16_t)(rxbuff[3] << 8 | rxbuff[2]);
+    Msg->input_cur = (int16_t)(rxbuff[5] << 8 | rxbuff[4]);
+    Msg->power_set = (int16_t)(rxbuff[7] << 8 | rxbuff[6]);
 }
 
 SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
 {
     super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance));
     memset(super_cap_instance, 0, sizeof(SuperCapInstance));
-    
+
     supercap_config->can_config.can_module_callback = SuperCapRxCallback;
     super_cap_instance->can_ins = CANRegister(&supercap_config->can_config);
     return super_cap_instance;
@@ -38,6 +33,15 @@ void SuperCapSend(SuperCapInstance *instance, uint8_t *data)
     CANTransmit(instance->can_ins,1);
 }
 
+void SuperCapSetPower(SuperCapInstance *instance, float power_set)
+{
+    uint16_t tmpPower = (uint16_t)(power_set * 100);
+    uint8_t data[8] = {0};
+    data[0] = tmpPower >> 8;
+    data[1] = tmpPower;
+    SuperCapSend(instance,data);
+}
+
 SuperCap_Msg_s SuperCapGet(SuperCapInstance *instance)
 {
     return instance->cap_msg;
diff --git a/modules/super_cap/super_cap.h b/modules/super_cap/super_cap.h
index 81c5f8e..171c851 100644
--- a/modules/super_cap/super_cap.h
+++ b/modules/super_cap/super_cap.h
@@ -1,10 +1,3 @@
-/*
- * @Descripttion:
- * @version:
- * @Author: Chenfu
- * @Date: 2022-12-02 21:32:47
- * @LastEditTime: 2022-12-05 15:25:46
- */
 #ifndef SUPER_CAP_H
 #define SUPER_CAP_H
 
@@ -13,9 +6,10 @@
 #pragma pack(1)
 typedef struct
 {
-    uint16_t vol; // 电压
-    uint16_t current; // 电流
-    uint16_t power; // 功率
+    int16_t input_vol; // 输入电压
+    int16_t cap_vol;   // 电容电压
+    int16_t input_cur; // 输入电流
+    int16_t power_set; // 设定功率
 } SuperCap_Msg_s;
 #pragma pack()
 
@@ -34,7 +28,7 @@ typedef struct
 
 /**
  * @brief 初始化超级电容
- * 
+ *
  * @param supercap_config 超级电容初始化配置
  * @return SuperCapInstance* 超级电容实例指针
  */
@@ -42,10 +36,18 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
 
 /**
  * @brief 发送超级电容控制信息
- * 
+ *
  * @param instance 超级电容实例
  * @param data 超级电容控制信息
  */
 void SuperCapSend(SuperCapInstance *instance, uint8_t *data);
 
+/**
+ * @brief 发送超级电容目标功率
+ *
+ * @param instance 超级电容实例
+ * @param power_set 超级电容目标功率
+ */
+void SuperCapSetPower(SuperCapInstance *instance, float power_set);
+
 #endif // !SUPER_CAP_Hd