diff --git a/Chassis_Task.c b/Chassis_Task.c
new file mode 100644
index 0000000..101c9d9
--- /dev/null
+++ b/Chassis_Task.c
@@ -0,0 +1,1358 @@
+#include "Chassis_Task.h"
+#include "bsp_buzzer.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "detect_task.h"
+#include "remote_control.h"
+#include "referee.h"
+#include "can.h"
+#include "bsp_can.h"
+#include "arm_math.h"
+#include "string.h"
+#include "Kalman_Mix.h"
+#include "INS_Task.h"
+#include "math.h"
+#include "VMC.h"
+#include "mpc.h"
+#include "lqr.h"
+#include "user_lib.h"
+
+
+#ifndef PI
+#define PI 3.14159265358979f
+#endif
+#ifndef FOLLOW_DEADBAND
+#define FOLLOW_DEADBAND 0.05f
+#endif
+#ifndef min
+#define min(a,b) ((a)<(b)?(a):(b))
+#endif
+#ifndef square
+#define square(x) ((x)*(x))
+#endif
+
+#define ABS(x) (((x) > 0) ? (x) : (-(x)))
+#define LimitMax(input, max)   \
+{                              \
+	if ((input) > max)       \
+	{                      \
+			input = max;   \
+	}                      \
+	else if ((input) < -max) \
+	{                      \
+			input = -max;  \
+	}                      \
+}
+#define LimitOutput(input, min, max )   \
+{										\
+	if( input < min )					\
+		input = min;					\
+	else if( input > max )				\
+		input = max;					\
+}
+
+#define SIGN(x) ((x)>0 ? 1 : ((x)<0? -1: 0))
+
+
+uint8_t use_mpc = 0, abnormal_debug = 0;
+chassis_control_t chassis_control;
+kalman2_state Body_Speed_KF;
+// ------------------ PID info ------------------ 
+fp32 roll_PD[2]        = {100.0, 20.0}; // 200, 45
+fp32 coordinate_PD[2]  = { 10.0f, 0.5f }; //15.0f,1.0f
+fp32 yaw_PD_test[2]    = { 20.0f, 180.0f };
+fp32 stand_PD[2]       = { 200.0f, 50.0f };
+fp32 jump_stand_PD[2]       = { 10000.0f, 150.0f };
+fp32 suspend_stand_PD[2] = { 100.0f, 30.0f };
+// fp32 stand_PD[2]={150.0f, 20.0f};//1N/cm
+fp32 suspend_foot_speed_p = 10.0f;
+// fp32 High_Offset = 0.20;
+fp32 High_Offset = 0.06;
+fp32 Moving_High_Offset = 0.0f;
+uint8_t last_is_update;
+fp32 IDEAL_PREPARING_STAND_JUMPING_ANGLE = 0.6f;
+
+extern fp32 LQR[4][10];
+
+fp32 suspend_LQR[2][6] = {	
+	20.0f,5.0f,	0.0f,	0.0f,	0.0f,	0.0f,
+	0,	0,  0,  0,	  0,	0
+};
+fp32 x_set[10];
+fp32 debug_2 = 1,upper_bound, stepp = 0.2;
+fp32 debug_flag, last_debug_flag;
+fp32 HIGH_HIGH = 0.35f;
+
+fp32 SIT_HIGH = 0.11f;
+fp32 NORMAL_HIGH = 0.18f; //16
+
+fp32 lower_dec_speed = 1.0f;
+uint8_t mpc_cnt = 0, mpc_period = 9;
+float alpha_dx = 0.95f;
+fp32 rollP, rollD, roll_angle_deadband = 0.0f, roll_gyro_deadband = 0.0f, leg_dlength_deadband = 0.0f;
+fp32 rotate_move_threshold = 45.0f, rotate_speed = 12.0f, fake_sign = 1.0f, rc_sign = 1.0f, rotate_move_scale = 1.0f,normal_move_scale = 2.0f, offset_k = 0.31f, fly_speed = 2.5f;
+fp32 rc_angle, rc_angle_temp, X_speed, Y_speed, delta_theta, delta_theta_temp, rotate_move_offset, normalized_speed, temp_max_spd, acc_step = 1.3f, dec_step = 0.1f;
+fp32 move_scale_list[11] =     { 0.0, 1.0, 1.5, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0};
+fp32 cap_move_scale_list[11] = { 0.0, 1.5, 2.0, 2.5, 2.5, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0};
+fp32 rotate_speed_list[11] = {0.0, 5.0, 5.3, 5.6, 6.0, 6.0, 7.0, 8.0, 9.0, 10.0, 12.0}; 
+fp32 rotate_move_scale_list[11] = {0.0, 1.0, 1.2, 1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8};
+int speed_sign = 0, target_speed_sign = 0;
+uint8_t robot_level = 1;
+ext_robot_status_t *robot;
+fp32 TK_x_p = -10.0f, TK_y_p = 10.0f, TK_y_d = 3.0f, reducing_p = 120.0f;
+fp32 stablize_foot_speed_threshold = 1.2f, stablize_yaw_speed_threshold = 1.5f;
+uint8_t no_follow_flag, follow_angle;
+
+enum Chassis_Mode chassis_mode;
+
+
+// ------------------ function definition ------------------ 
+void Chassis_Init( chassis_control_t *init );
+void Chassis_Data_Update( chassis_control_t *fdb );
+void Chassis_Status_Detect( chassis_control_t *detect );
+void Chassis_Mode_Set( chassis_control_t *mode_set );
+void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change );
+void Target_Value_Set( chassis_control_t *target_value_set );
+void Chassis_Torque_Calculation( chassis_control_t *bl_ctrl );
+void Chassis_Torque_Combine( chassis_control_t *bl_ctrl );
+void Motor_CMD_Send(chassis_control_t *CMD_Send);
+void Joint_Motor_to_Init_Pos(void);
+void Motor_Zero_CMD_Send(void);
+void HT_Motor_zero_set(void);
+
+int16_t set1, set2;
+uint16_t launching_time = 0;
+
+void Chassis_Task(void const * argument)
+{
+	vTaskDelay(CHASSIS_TASK_INIT_TIME);
+	Chassis_Init(&chassis_control);
+	vTaskDelay(100);
+	while(1)
+	{
+		// ------------------ Update Data ------------------
+		Chassis_Data_Update(&chassis_control);
+
+		// ------------------ Chassis Control ------------------
+		Chassis_Status_Detect(&chassis_control);
+
+		// ------------------ Mode Set ------------------
+		Chassis_Mode_Set(&chassis_control);
+
+		Chassis_Mode_Change_Control_Transit(&chassis_control);
+
+		Target_Value_Set(&chassis_control);
+
+		Chassis_Torque_Calculation(&chassis_control);
+
+		Chassis_Torque_Combine(&chassis_control);
+
+		Motor_CMD_Send(&chassis_control);
+
+		vTaskDelay(1);
+	}
+}
+
+
+void Chassis_Init( chassis_control_t *init )
+{
+	// ------------------ Set HT Zero Point ------------------ 
+	buzzer_on(75, 60);
+	vTaskDelay(100);
+	HT_Motor_zero_set();
+	buzzer_off();
+
+	Motor_Zero_CMD_Send();
+
+	kalman_second_order_init(&Body_Speed_KF);
+
+	// ------------------ Ptr Set ------------------ 
+	init->joint_motor_1.motor_measure = get_HT_motor_measure_point(0);
+	init->joint_motor_2.motor_measure = get_HT_motor_measure_point(1);
+	init->joint_motor_3.motor_measure = get_HT_motor_measure_point(2);
+	init->joint_motor_4.motor_measure = get_HT_motor_measure_point(3);
+	// init->foot_motor_L.motor_measure = get_BMmotor_measure_point(0);
+	// init->foot_motor_R.motor_measure = get_BMmotor_measure_point(1);
+	init->chassis_rc_ctrl = get_Gimabl_control_point();
+	init->chassis_posture_info.chassis_INS_angle_point = get_INS_angle_point();
+	init->chassis_posture_info.chassis_INS_gyro_point = get_gyro_data_point();
+	init->chassis_posture_info.chassis_INS_accel_point = get_acc_data_point();
+
+	/*
+	first_order_filter_init(&rc_filter, 0.002f, rc_filt_numb);
+	first_order_filter_init(&tl_filter, 0.002f, tl_numb);
+	first_order_filter_init(&tl_run_filter, 0.002f, tl_numb);
+    OLS_Init(&OLS_S0_L,2);
+	OLS_Init(&OLS_S0_R,2);
+	*/
+
+	// ------------------ Mode Set ------------------ 
+	init->mode.chassis_mode = init->mode.last_chassis_mode = DISABLE_CHASSIS;
+	init->mode.chassis_balancing_mode = init->mode.last_chassis_balancing_mode = NO_FORCE;
+	init->mode.sport_mode = init->mode.last_sport_mode = NORMAL_MOVING_MODE;
+	init->joint_motor_1.motor_mode = init->joint_motor_1.last_motor_mode = MOTOR_NO_FORCE;
+	init->joint_motor_2.motor_mode = init->joint_motor_2.last_motor_mode = MOTOR_NO_FORCE;
+	init->joint_motor_3.motor_mode = init->joint_motor_3.last_motor_mode = MOTOR_NO_FORCE;
+	init->joint_motor_4.motor_mode = init->joint_motor_4.last_motor_mode = MOTOR_NO_FORCE;
+	init->foot_motor_L.motor_mode = init->foot_motor_L.last_motor_mode = MOTOR_NO_FORCE;
+	init->foot_motor_R.motor_mode = init->foot_motor_R.last_motor_mode = MOTOR_NO_FORCE;
+
+	// ------------------ Initialize HT Motor ------------------ 
+	init->joint_motor_1.position_offset = init->joint_motor_1.motor_measure->ecd;
+	init->joint_motor_2.position_offset = init->joint_motor_2.motor_measure->ecd;
+	init->joint_motor_3.position_offset = init->joint_motor_3.motor_measure->ecd;
+	init->joint_motor_4.position_offset = init->joint_motor_4.motor_measure->ecd;
+	init->joint_motor_1.position = (init->joint_motor_1.motor_measure->ecd -init->joint_motor_1.position_offset) + LEG_OFFSET;
+	init->joint_motor_2.position = (init->joint_motor_2.motor_measure->ecd -init->joint_motor_2.position_offset) - LEG_OFFSET;
+	init->joint_motor_3.position = (init->joint_motor_3.motor_measure->ecd -init->joint_motor_3.position_offset) + LEG_OFFSET;
+	init->joint_motor_4.position = (init->joint_motor_4.motor_measure->ecd -init->joint_motor_4.position_offset) - LEG_OFFSET;
+
+	init->flag_info.init_flag = 1;
+	Chassis_Data_Update(init);
+	init->flag_info.init_flag = 0;
+
+	init->foot_motor_L.distance_offset = init->foot_motor_L.distance;
+	init->foot_motor_R.distance_offset = init->foot_motor_R.distance;
+	
+	Power_Init(init);
+
+	// ------------------ MPC init ------------------
+	MPC_Init();
+}
+
+void Chassis_Data_Update( chassis_control_t *fdb )
+{
+	robot = get_robot_status_point();
+	robot_level = robot->robot_level;
+	// robot_level = 1;
+	// -------------------- Update foot measure --------------------
+	fdb->foot_motor_L.motor_measure.ecd = (uint16_t)motor_BM[0].ecd;
+	fdb->foot_motor_L.motor_measure.last_ecd = (uint16_t)motor_BM[0].last_ecd;
+	fdb->foot_motor_L.motor_measure.speed_rpm = (int16_t)motor_BM[0].speed_rpm;
+	fdb->foot_motor_L.torque_get = motor_BM[0].given_current*55.0f/32767.0f;
+
+	fdb->foot_motor_R.motor_measure.ecd = (uint16_t)motor_BM[1].ecd;
+	fdb->foot_motor_R.motor_measure.last_ecd = (uint16_t)motor_BM[1].last_ecd;
+	fdb->foot_motor_R.motor_measure.speed_rpm = (int16_t)motor_BM[1].speed_rpm;
+	fdb->foot_motor_R.torque_get = motor_BM[1].given_current*55.0f/32767.0f;
+
+	//  ------------------ Update mode info ------------------ 
+	fdb->mode.last_chassis_mode                = fdb->mode.chassis_mode;
+	fdb->mode.last_chassis_balancing_mode      = fdb->mode.chassis_balancing_mode;
+	fdb->mode.last_sport_mode                  = fdb->mode.sport_mode;
+	fdb->mode.last_jumping_mode                = fdb->mode.jumping_mode;
+	fdb->mode.last_jumping_stage               = fdb->mode.jumping_stage;
+	fdb->mode.last_chassis_high_mode           = fdb->mode.chassis_high_mode;
+	fdb->flag_info.last_static_flag            = fdb->flag_info.static_flag;
+	fdb->flag_info.last_moving_flag            = fdb->flag_info.moving_flag;
+	last_debug_flag                            = debug_flag;
+	fdb->flag_info.last_overpower_warning_flag = fdb->flag_info.overpower_warning_flag;
+	fdb->flag_info.last_stablize_high_flag     = fdb->flag_info.stablize_high_flag;
+
+	fdb->flag_info.last_suspend_flag_L         = fdb->flag_info.suspend_flag_L;
+	fdb->flag_info.last_suspend_flag_R         = fdb->flag_info.suspend_flag_R;
+
+	fdb->joint_motor_1.last_motor_mode = fdb->joint_motor_1.motor_mode;
+	fdb->joint_motor_2.last_motor_mode = fdb->joint_motor_2.motor_mode;
+	fdb->joint_motor_3.last_motor_mode = fdb->joint_motor_3.motor_mode;
+	fdb->joint_motor_4.last_motor_mode = fdb->joint_motor_4.motor_mode;
+	fdb->foot_motor_L.last_motor_mode  = fdb->foot_motor_L.motor_mode;
+	fdb->foot_motor_R.last_motor_mode  = fdb->foot_motor_R.motor_mode;
+
+	// ------------------ Update IMU info ------------------ 
+	fdb->chassis_posture_info.pitch_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point  + INS_PITCH_ADDRESS_OFFSET);
+	fdb->chassis_posture_info.roll_angle  = *(fdb->chassis_posture_info.chassis_INS_angle_point  + INS_ROLL_ADDRESS_OFFSET);
+	fdb->chassis_posture_info.yaw_angle   = *(fdb->chassis_posture_info.chassis_INS_angle_point  + INS_YAW_ADDRESS_OFFSET) + yaw_count * PI2;
+	fdb->chassis_posture_info.pitch_gyro  = *(fdb->chassis_posture_info.chassis_INS_gyro_point  + INS_PITCH_ADDRESS_OFFSET);
+	fdb->chassis_posture_info.roll_gyro =   *(fdb->chassis_posture_info.chassis_INS_gyro_point  + INS_ROLL_ADDRESS_OFFSET);
+  	fdb->chassis_posture_info.yaw_gyro    = *(fdb->chassis_posture_info.chassis_INS_gyro_point  + INS_YAW_ADDRESS_OFFSET);
+
+	// ------------------ Update HT Motor info ------------------ 
+	fdb->joint_motor_1.position = (fdb->joint_motor_1.motor_measure->ecd - fdb->joint_motor_1.position_offset) + LEG_OFFSET;
+	fdb->joint_motor_2.position = (fdb->joint_motor_2.motor_measure->ecd - fdb->joint_motor_2.position_offset) - LEG_OFFSET;
+	fdb->joint_motor_3.position = (fdb->joint_motor_3.motor_measure->ecd - fdb->joint_motor_3.position_offset) + LEG_OFFSET;
+	fdb->joint_motor_4.position = (fdb->joint_motor_4.motor_measure->ecd - fdb->joint_motor_4.position_offset) - LEG_OFFSET;
+
+	fdb->joint_motor_1.velocity = fdb->joint_motor_1.motor_measure->speed_rpm;
+	fdb->joint_motor_2.velocity = fdb->joint_motor_2.motor_measure->speed_rpm;
+	fdb->joint_motor_3.velocity = fdb->joint_motor_3.motor_measure->speed_rpm;
+	fdb->joint_motor_4.velocity = fdb->joint_motor_4.motor_measure->speed_rpm;
+
+	fdb->joint_motor_1.torque_get = 0.95f * fdb->joint_motor_1.torque_get + 0.05f * fdb->joint_motor_1.motor_measure->real_torque;
+	fdb->joint_motor_2.torque_get = 0.95f * fdb->joint_motor_2.torque_get + 0.05f * fdb->joint_motor_2.motor_measure->real_torque;
+	fdb->joint_motor_3.torque_get = 0.95f * fdb->joint_motor_3.torque_get + 0.05f * fdb->joint_motor_3.motor_measure->real_torque;
+	fdb->joint_motor_4.torque_get = 0.95f * fdb->joint_motor_4.torque_get + 0.05f * fdb->joint_motor_4.motor_measure->real_torque;
+
+	// ------------------ Update LK Motor info ------------------ 
+	fdb->foot_motor_L.last_position = fdb->foot_motor_L.position;
+	fdb->foot_motor_R.last_position = fdb->foot_motor_R.position;
+	fdb->foot_motor_L.position = fdb->foot_motor_L.motor_measure.ecd * MOTOR_ECD_TO_RAD;// Angle (rad)
+	fdb->foot_motor_R.position = fdb->foot_motor_R.motor_measure.ecd * MOTOR_ECD_TO_RAD;
+
+	if( fdb->flag_info.init_flag != 1 ) // Init_flag = 0: after Init fdb 如果不是在init里面调用就�?�圈
+	{
+		if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) > HALF_POSITION_RANGE )
+			fdb->foot_motor_L.turns++;		
+		else if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) < -HALF_POSITION_RANGE )
+			fdb->foot_motor_L.turns--;	
+		if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) > HALF_POSITION_RANGE )
+			fdb->foot_motor_R.turns--;
+		else if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) < -HALF_POSITION_RANGE )
+			fdb->foot_motor_R.turns++;
+	}
+	fdb->foot_motor_L.distance	= ( fdb->foot_motor_L.position/PI2 + fdb->foot_motor_L.turns ) * WHEEL_PERIMETER - fdb->foot_motor_L.distance_offset;
+	fdb->foot_motor_R.distance  = ( -fdb->foot_motor_R.position/PI2 + fdb->foot_motor_R.turns ) * WHEEL_PERIMETER - fdb->foot_motor_R.distance_offset;
+	fdb->chassis_posture_info.foot_distance = ( fdb->foot_motor_L.distance + fdb->foot_motor_R.distance ) /2.0f;
+	
+	fdb->foot_motor_L.speed = fdb->foot_motor_L.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f; // rpm -> m/s
+	fdb->foot_motor_R.speed = -fdb->foot_motor_R.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f;
+	fdb->chassis_posture_info.foot_speed  = ( fdb->foot_motor_L.speed + fdb->foot_motor_R.speed ) / 2.0f;
+
+	// fdb->foot_motor_L.torque_get = fdb->foot_motor_L.motor_measure.given_current;
+	// fdb->foot_motor_R.torque_get = fdb->foot_motor_R.motor_measure.given_current;
+	
+	// ------------------ Kalman Filter ------------------ 
+
+	kalman_second_order_update(&Body_Speed_KF, fdb->chassis_posture_info.foot_distance, fdb->chassis_posture_info.foot_speed, temp_a);
+	fdb->chassis_posture_info.foot_distance_K = Body_Speed_KF.x[0];
+	fdb->chassis_posture_info.foot_speed_K    =  Body_Speed_KF.x[1];
+
+	// ------------------ Five_Bars_to_Pendulum ------------------ 
+	Forward_kinematic_solution(fdb,-fdb->joint_motor_2.position,-fdb->joint_motor_2.velocity,
+		-fdb->joint_motor_1.position,-fdb->joint_motor_1.velocity,1);
+	Forward_kinematic_solution(fdb,fdb->joint_motor_3.position,fdb->joint_motor_3.velocity,
+		fdb->joint_motor_4.position, fdb->joint_motor_4.velocity,0);
+
+	// ------------------ Huanzhi information update ------------------ 
+	fdb->chassis_posture_info.leg_angle_L -= PI_2;
+	fdb->chassis_posture_info.leg_angle_L -= fdb->chassis_posture_info.pitch_angle;
+	// fdb->chassis_posture_info.leg_gyro_L  -= fdb->chassis_posture_info.pitch_gyro;
+	fp32 temp_v_L = ( fdb->chassis_posture_info.leg_angle_L - fdb->chassis_posture_info.last_leg_angle_L ) / TASK_RUN_TIME;
+	fdb->chassis_posture_info.leg_gyro_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_L;
+	fdb->chassis_posture_info .last_leg_angle_L = fdb->chassis_posture_info .leg_angle_L;
+
+	fdb->chassis_posture_info.leg_angle_R -= PI_2;
+	fdb->chassis_posture_info.leg_angle_R -= fdb->chassis_posture_info.pitch_angle;
+	// fdb->chassis_posture_info.leg_gyro_R  -= fdb->chassis_posture_info.pitch_gyro;
+	fp32 temp_v_R = ( fdb->chassis_posture_info.leg_angle_R - fdb->chassis_posture_info.last_leg_angle_R ) / TASK_RUN_TIME;
+	fdb->chassis_posture_info.leg_gyro_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_R;
+	fdb->chassis_posture_info.last_leg_angle_R = fdb->chassis_posture_info .leg_angle_R;
+
+	temp_v_L = ( fdb->chassis_posture_info.leg_length_L - fdb->chassis_posture_info.last_leg_length_L ) / TASK_RUN_TIME;
+	fdb->chassis_posture_info.leg_dlength_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_L;
+	fdb->chassis_posture_info.last_leg_length_L = fdb->chassis_posture_info.leg_length_L;
+
+	temp_v_R = ( fdb->chassis_posture_info.leg_length_R - fdb->chassis_posture_info.last_leg_length_R ) / TASK_RUN_TIME;
+	fdb->chassis_posture_info.leg_dlength_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_R;
+	fdb->chassis_posture_info.last_leg_length_R = fdb->chassis_posture_info.leg_length_R;
+
+	// ---------- Rotate and Move Info update ------------
+	rotate_move_offset = offset_k * rotate_speed_list[robot_level];
+	rc_sign = 1.0f;
+	X_speed = fdb->chassis_rc_ctrl->X_speed;
+	Y_speed = fdb->chassis_rc_ctrl->Y_speed;
+	// Original rc angle
+	if (X_speed == 0){
+		if (Y_speed > 0)
+			rc_angle_temp = PI/2;
+		else if (Y_speed < 0)
+			rc_angle_temp = -PI/2;
+		else
+			rc_angle_temp = 0;
+	}
+	else{
+		Y_speed = fdb->chassis_rc_ctrl->Y_speed;
+		rc_angle_temp = atan_tl(Y_speed/X_speed);
+	}
+	// Normalized speed
+	if (abs(X_speed) < 0.1 ||abs(Y_speed) <0.1)
+		temp_max_spd = 1.0f;
+	else if (X_speed<Y_speed){
+		temp_max_spd = sqrt(square((1/Y_speed)*X_speed)+1);
+	}
+	else{
+		temp_max_spd = sqrt(square((1/X_speed)*Y_speed)+1);
+	}
+	normalized_speed = sqrt(X_speed*X_speed+Y_speed*Y_speed) / temp_max_spd;
+	// Ref angle and sign
+	if (X_speed<0) rc_sign *= -1.0f;
+	rc_angle_temp += rotate_move_offset;
+	if (rc_angle_temp>PI/2||rc_angle_temp<-PI/2){
+		rc_sign *= -1.0f;
+	}
+	while (rc_angle_temp>PI/2) {
+		rc_angle_temp -= PI;
+	}
+	while (rc_angle_temp<-PI/2){
+		rc_angle_temp += PI;
+	} 
+	// if (X_speed < 0) rc_sign = -1.0f; 	
+	rc_angle = rc_angle_temp * 180.0f / PI;
+	if (fdb->chassis_rc_ctrl->fake_flag == 0) fake_sign = -1.0f;
+	else fake_sign = 1.0f;
+}
+
+void Chassis_Status_Detect( chassis_control_t *detect )
+{
+	// ------------------ Off Ground Detect ------------------ 
+
+
+	if( detect->mode.chassis_balancing_mode == BALANCING_READY && detect->mode.sport_mode!=TK_MODE)
+	{
+		if( ABS(detect->chassis_posture_info.pitch_angle) >= DANGER_PITCH_ANGLE )
+			detect->flag_info.abnormal_flag = 1;
+		else if( ABS(detect->chassis_posture_info.foot_speed_K) < MOVE_LOWER_BOUND && 
+			ABS(detect->chassis_posture_info.pitch_angle)        < 0.1f &&
+				detect->flag_info.abnormal_flag )
+				detect->flag_info.abnormal_flag = 0;
+	}
+	if (abnormal_debug){
+		detect->flag_info.abnormal_flag = 0;
+	}
+
+	Supportive_Force_Cal(detect,  detect->joint_motor_1.position, detect->joint_motor_2.position, 1.0f );	
+	Supportive_Force_Cal(detect,  detect->joint_motor_3.position,  detect->joint_motor_4.position, 0.0f );
+	
+	if( detect->mode.jumping_stage == CONSTACTING_LEGS )
+		detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND;
+	else
+	{
+		if( detect->mode.sport_mode == JUMPING_MODE )
+		{
+			if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE_FOR_JUMP &&
+				detect->chassis_posture_info.leg_length_R > 0.13f )
+				detect->flag_info.suspend_flag_R = OFF_GROUND;
+			else
+				detect->flag_info.suspend_flag_L = ON_GROUND;
+			if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE_FOR_JUMP &&
+				detect->chassis_posture_info.leg_length_L > 0.13f )
+				detect->flag_info.suspend_flag_L = OFF_GROUND;
+			else 
+				detect->flag_info.suspend_flag_R = ON_GROUND;
+		}
+		else
+		{
+			if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE &&
+				detect->chassis_posture_info.leg_length_R > 0.13f )
+				detect->flag_info.suspend_flag_R = OFF_GROUND;
+			else
+				detect->flag_info.suspend_flag_L = ON_GROUND;
+			if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE &&
+				detect->chassis_posture_info.leg_length_L > 0.13f )
+				detect->flag_info.suspend_flag_L = OFF_GROUND;
+			else 
+				detect->flag_info.suspend_flag_R = ON_GROUND;
+		}
+	} 
+
+
+	if( detect->flag_info.abnormal_flag == 1 &&
+		( detect->flag_info.last_suspend_flag_L == ON_GROUND || detect->flag_info.last_suspend_flag_R == ON_GROUND ) &&
+		( detect->flag_info.suspend_flag_L == OFF_GROUND || detect->flag_info.suspend_flag_R  == OFF_GROUND ) )
+			detect->flag_info.Ignore_Off_Ground = 1;
+	else if( detect->flag_info.abnormal_flag != 1 )
+		detect->flag_info.Ignore_Off_Ground = 0;
+	if( detect->flag_info.Ignore_Off_Ground )
+	{
+		detect->flag_info.suspend_flag_R = ON_GROUND;
+		detect->flag_info.suspend_flag_L = ON_GROUND;
+	}
+	// detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND;
+	//Moving_High_Offset
+
+	if( ABS(detect->chassis_posture_info.foot_speed_K) > stablize_foot_speed_threshold &&
+		ABS(detect->chassis_posture_info.yaw_gyro ) > stablize_yaw_speed_threshold )
+		detect->flag_info.stablize_high_flag = 1;
+
+}
+
+void Chassis_Mode_Set( chassis_control_t *mode_set )
+{
+	// ----------------- Chassis Mode Update ----------------- 
+	if( mode_set->chassis_rc_ctrl->mode_R == 0 || toe_is_error(GIMBAL_TOE)|| toe_is_error(BM1_TOE)||toe_is_error(BM2_TOE))
+		mode_set->mode.chassis_mode = DISABLE_CHASSIS;
+	else 
+		mode_set->mode.chassis_mode = ENABLE_CHASSIS;
+	
+	if( mode_set->mode.chassis_mode == ENABLE_CHASSIS )
+	{
+		mode_set->joint_motor_1.motor_mode = MOTOR_FORCE;
+		mode_set->joint_motor_2.motor_mode = MOTOR_FORCE;
+		mode_set->joint_motor_3.motor_mode = MOTOR_FORCE;
+		mode_set->joint_motor_4.motor_mode = MOTOR_FORCE;
+		mode_set->foot_motor_L.motor_mode  = MOTOR_FORCE;
+		mode_set->foot_motor_R.motor_mode  = MOTOR_FORCE;
+	}
+	else
+	{
+		if( mode_set->mode.chassis_balancing_mode == JOINT_REDUCING )
+		{
+			mode_set->joint_motor_1.motor_mode = MOTOR_FORCE;
+			mode_set->joint_motor_2.motor_mode = MOTOR_FORCE;
+			mode_set->joint_motor_3.motor_mode = MOTOR_FORCE;
+			mode_set->joint_motor_4.motor_mode = MOTOR_FORCE;
+			mode_set->foot_motor_L.motor_mode  = MOTOR_NO_FORCE;
+			mode_set->foot_motor_R.motor_mode  = MOTOR_NO_FORCE;
+		}
+		else
+		{
+			mode_set->joint_motor_1.motor_mode = MOTOR_NO_FORCE;
+			mode_set->joint_motor_2.motor_mode = MOTOR_NO_FORCE;
+			mode_set->joint_motor_3.motor_mode = MOTOR_NO_FORCE;
+			mode_set->joint_motor_4.motor_mode = MOTOR_NO_FORCE;
+			mode_set->foot_motor_L.motor_mode  = MOTOR_NO_FORCE;
+			mode_set->foot_motor_R.motor_mode  = MOTOR_NO_FORCE;
+		}
+
+	}
+
+	// ----------------- Sport Mode Update ----------------- 
+	if( mode_set->mode.chassis_balancing_mode == BALANCING_READY )
+	{
+		if( mode_set->chassis_rc_ctrl->tk_flag )
+			mode_set->mode.sport_mode = TK_MODE;
+		else if( mode_set->flag_info.abnormal_flag )
+			mode_set->mode.sport_mode = ABNORMAL_MOVING_MODE;
+		else if( mode_set->mode.sport_mode == JUMPING_MODE && mode_set->mode.jumping_stage != FINISHED )
+			mode_set->mode.sport_mode = JUMPING_MODE;
+		else if( mode_set->chassis_rc_ctrl->jump_flag )
+			mode_set->mode.sport_mode = JUMPING_MODE;
+		else if( mode_set->chassis_rc_ctrl->cap_flag )
+			mode_set->mode.sport_mode = CAP_MODE;
+		else if( mode_set->chassis_rc_ctrl->fly_flag )
+			mode_set->mode.sport_mode = FLY_MODE;
+		else 
+			mode_set->mode.sport_mode = NORMAL_MOVING_MODE;
+	}
+	else
+		mode_set->mode.sport_mode = NONE;
+
+	// ----------------- Rotation Flag --------------------
+	static uint8_t last_rotation_flag = 0;
+	last_rotation_flag = mode_set->flag_info.rotation_flag;
+	mode_set->flag_info.rotation_flag = mode_set->chassis_rc_ctrl->mode_R == 4; //Rotate
+	if (!last_rotation_flag && mode_set->flag_info.rotation_flag){
+		for (int i = 0; i < 11; ++i)
+			rotate_speed_list[i] = - rotate_speed_list[i];
+	}
+
+	// ----------------- No Follow Flag ----------------------
+	if (toe_is_error(GIMBAL_TOE)) no_follow_flag = 1;
+	else no_follow_flag = mode_set->chassis_rc_ctrl->mode_R == 3; //No Follow
+	
+	// ---------------- Moving Flag ----------------------------
+	if( mode_set->chassis_rc_ctrl->X_speed != 0.0f )
+	{
+		mode_set->flag_info.controlling_flag = 1;
+		mode_set->flag_info.set_pos_after_moving = 1;
+	}	
+	else 
+		mode_set->flag_info.controlling_flag = 0;
+		
+	if( ABS(mode_set->chassis_posture_info.foot_speed_K) <= 0.05f &&
+		!mode_set->flag_info.controlling_flag &&
+		mode_set->flag_info.set_pos_after_moving )  					// maybe bug
+		{
+			mode_set->flag_info.moving_flag = 0;
+			mode_set->flag_info.set_pos_after_moving = 0;
+		}
+	else 
+		mode_set->flag_info.moving_flag = 1;
+
+
+}
+
+uint8_t reduce_flag = 0;
+fp32 reduce_high, high_offset = 0.2f;
+fp32 debug_1 = 0.995;
+void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change )
+{
+	if( chassis_mode_change->mode.chassis_mode == ENABLE_CHASSIS && chassis_mode_change->mode.last_chassis_mode == DISABLE_CHASSIS )
+		chassis_mode_change->mode.chassis_balancing_mode = FOOT_LAUNCHING;
+
+	if( chassis_mode_change->mode.chassis_balancing_mode == FOOT_LAUNCHING && 
+		ABS(chassis_mode_change->chassis_posture_info.pitch_angle) < EXIT_PITCH_ANGLE )
+		chassis_mode_change->mode.chassis_balancing_mode = JOINT_LAUNCHING;
+	else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_LAUNCHING && (
+		(NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_L) < 0.03f ||
+		(NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_R) < 0.03f) )
+		chassis_mode_change->mode.chassis_balancing_mode = BALANCING_READY;
+	else if( chassis_mode_change->mode.chassis_balancing_mode == BALANCING_READY &&
+		chassis_mode_change->mode.chassis_mode == DISABLE_CHASSIS ){
+			if (!reduce_flag) {
+				reduce_high = chassis_mode_change->chassis_posture_info.ideal_high + high_offset;
+				reduce_flag = 1;
+			}
+			chassis_mode_change->mode.chassis_balancing_mode = JOINT_REDUCING;
+		}
+	else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_REDUCING && (
+		ABS( chassis_mode_change->chassis_posture_info.leg_length_L - SIT_HIGH ) < 0.001f ||
+		ABS( chassis_mode_change->chassis_posture_info.leg_length_R - SIT_HIGH ) < 0.001f ) ){
+			chassis_mode_change->mode.chassis_balancing_mode = NO_FORCE;
+			reduce_flag = 0;
+		}
+
+	if( chassis_mode_change->mode.sport_mode == JUMPING_MODE && chassis_mode_change->mode.last_sport_mode != JUMPING_MODE )
+	{
+		chassis_mode_change->mode.jumping_mode = STANDING_JUMP;
+	}
+	else if( chassis_mode_change->mode.sport_mode != JUMPING_MODE )
+		chassis_mode_change->mode.jumping_mode = NOT_DEFINE;
+	
+
+	if( chassis_mode_change->mode.jumping_mode == MOVING_JUMP )
+	{
+		if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP )
+			chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS;
+		else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS &&
+			chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f )
+				chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS;
+		else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS  && 
+			chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f )
+				chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2;
+		else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2  && 
+			chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f )
+				chassis_mode_change->mode.jumping_stage = PREPARING_LANDING;
+		else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING && 
+			chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND &&
+			chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND )
+				chassis_mode_change->mode.jumping_stage = FINISHED;
+		else if( chassis_mode_change->mode.jumping_stage == FINISHED )
+				chassis_mode_change->mode.jumping_stage = READY_TO_JUMP;
+	}
+	else if( chassis_mode_change->mode.jumping_mode == STANDING_JUMP )
+	{
+		if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP )
+			chassis_mode_change->mode.jumping_stage = PREPARING_STAND_JUMPING;
+		else if(chassis_mode_change->mode.jumping_stage == PREPARING_STAND_JUMPING &&
+			ABS(chassis_mode_change->chassis_posture_info.leg_angle_L + IDEAL_PREPARING_STAND_JUMPING_ANGLE ) < stepp )
+			chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS;
+		else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS  && 
+			chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f )
+				chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2;
+		else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2  && 
+			chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f )
+				chassis_mode_change->mode.jumping_stage = PREPARING_LANDING;
+		else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING && 
+			chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND &&
+			chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND )
+				chassis_mode_change->mode.jumping_stage = FINISHED;
+		else if( chassis_mode_change->mode.jumping_stage == FINISHED )
+				chassis_mode_change->mode.jumping_stage = READY_TO_JUMP;
+	}
+	else
+		chassis_mode_change->mode.jumping_stage = FINISHED; 
+
+
+	if( chassis_mode_change->flag_info.stablize_high_flag == 1 &&
+		Moving_High_Offset >= 0.2 &&
+		chassis_mode_change->chassis_posture_info.yaw_gyro <= ( stablize_yaw_speed_threshold - 0.5f ) )
+			chassis_mode_change->flag_info.stablize_high_flag = 0;
+}
+
+fp32 HIGH_SWITCH = 36.0f;
+
+void Target_Value_Set( chassis_control_t *target_value_set )
+{
+	// ------------------------------------
+	if( target_value_set->flag_info.stablize_high_flag == 1 )
+		if( Moving_High_Offset < 0.2 )
+			Moving_High_Offset += 0.001f;
+
+	if( target_value_set->flag_info.stablize_high_flag == 0 )
+		Moving_High_Offset = 0.0f;
+
+	// --------- X Speed Set ----------
+	if( target_value_set->mode.sport_mode             != NONE                 && 
+		target_value_set->flag_info.suspend_flag_L     == ON_GROUND &&
+		target_value_set->flag_info.suspend_flag_R     == ON_GROUND )
+	{
+		if (!target_value_set->flag_info.rotation_flag) {// Normal move
+			if(toe_is_error(REFEREE_TOE)){
+				target_value_set->chassis_posture_info.foot_speed_set = fake_sign * target_value_set->chassis_rc_ctrl->X_speed * normal_move_scale ;
+
+			} else {
+				// target_speed_sign = (fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]) > 0 ? 1: -1;
+				target_speed_sign = SIGN(fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]);					
+				target_value_set->chassis_posture_info.foot_speed_set = target_value_set->chassis_posture_info.foot_speed_K + target_speed_sign * acc_step;
+
+				if (target_value_set->mode.sport_mode == FLY_MODE) 
+					target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * fly_speed),ABS(target_value_set->chassis_posture_info.foot_speed_set));
+				else if (target_value_set->mode.sport_mode == CAP_MODE)
+					target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * cap_move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set));
+				else
+					target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set));
+
+				// if( target_value_set->chassis_posture_info.leg_length_L > 0.22f || target_value_set->chassis_posture_info.leg_length_R > 0.22f )
+				// {
+				// 	target_value_set->chassis_posture_info.foot_speed_set /= 2.0f;
+				// 	debug_2++;
+				// }	
+			}
+		
+		}
+		else // Rotate and move
+		{
+			delta_theta_temp = target_value_set->chassis_rc_ctrl->W_angle - rc_angle;
+			if (delta_theta_temp>PI/2) delta_theta_temp -= PI;
+			else if (delta_theta_temp<-PI/2) delta_theta_temp += PI;
+			delta_theta = abs(delta_theta_temp);
+			if (delta_theta<rotate_move_threshold)
+				target_value_set->chassis_posture_info.foot_speed_set
+				= ((rotate_move_threshold-delta_theta)/rotate_move_threshold)
+				 * rc_sign * fake_sign * normalized_speed * rotate_move_scale_list[robot_level];
+			else target_value_set->chassis_posture_info.foot_speed_set = 0;
+		}
+
+	}
+	else
+		target_value_set->chassis_posture_info.foot_speed_set = 0;
+
+	// --------- Distance Set ---------
+	if( target_value_set->mode.chassis_balancing_mode == NO_FORCE || target_value_set->mode.sport_mode == TK_MODE)
+		target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K;
+	else if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE )
+		target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_set;
+	else if( target_value_set->flag_info.suspend_flag_R == OFF_GROUND ||
+		target_value_set->flag_info.suspend_flag_L == OFF_GROUND )
+		target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K;
+	else if( target_value_set->flag_info.controlling_flag )
+		target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K;
+	else if( !target_value_set->flag_info.moving_flag && 
+			target_value_set->mode.chassis_balancing_mode == BALANCING_READY &&
+			target_value_set->flag_info.last_moving_flag )
+		target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K;
+
+
+
+	// --------- Y Speed & Angle Set --------- 
+	if( target_value_set->mode.sport_mode             != NONE    && 
+		target_value_set->flag_info.suspend_flag_L      == ON_GROUND &&
+		target_value_set->flag_info.suspend_flag_R      == ON_GROUND )
+	{
+	 	if( target_value_set->flag_info.rotation_flag == 1 )
+	 	{
+	 	 	target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle;
+	 	 	target_value_set->chassis_posture_info.yaw_gyro_set = rotate_speed_list[robot_level];
+	 	} 
+	 	else
+	 	{
+			if (target_value_set->chassis_rc_ctrl->W_angle<-FOLLOW_DEADBAND) 
+				target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle+FOLLOW_DEADBAND) / 180.0f * PI;
+			else if (target_value_set->chassis_rc_ctrl->W_angle>FOLLOW_DEADBAND) 
+				target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle-FOLLOW_DEADBAND) / 180.0f * PI;
+			else 
+				target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle;
+			target_value_set->chassis_posture_info.yaw_gyro_set = 0.0f;
+		}
+	}
+	else
+	{
+	 	target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle;
+	 	target_value_set->chassis_posture_info.yaw_gyro_set = target_value_set->chassis_posture_info.yaw_gyro;
+	}
+
+
+
+	// --------- Side Angle Set ---------
+	// if( target_value_set->mode.sport_mode         != NONE                 &&
+	// 	target_value_set->mode.sport_mode         != ABNORMAL_MOVING_MODE && 
+	// 	target_value_set->flag_info.suspend_flag_R   == ON_GROUND &&
+	// 	target_value_set->flag_info.suspend_flag_L   == ON_GROUND )
+	// {
+	// 	if( target_value_set->mode.sport_mode == SIDE_MODE )
+	// 		target_value_set->chassis_posture_info.roll_angle_set = (fp32)(5.0 / 180.0 * PI) * 50.0f;
+	// 	// else if( target_value_set->chassis_rc_ctrl->side_flag == -1 )
+	// 	// 	target_value_set->chassis_posture_info.roll_angle_set = -(fp32)(5.0 / 180.0 * PI) * 50.0f;
+	// 	else	
+	// 		target_value_set->chassis_posture_info.roll_angle_set = 0.0f;
+	// }
+	// else
+		target_value_set->chassis_posture_info.roll_angle_set = 0.0f;
+
+	// ---------------- Leg Angle Set ----------
+	if( target_value_set->mode.jumping_stage == PREPARING_STAND_JUMPING )
+	{
+		target_value_set->chassis_posture_info.leg_angle_L_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE;
+		target_value_set->chassis_posture_info.leg_angle_R_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE;
+	}
+	else
+	{
+		target_value_set->chassis_posture_info.leg_angle_L_set = 0.0f;
+		target_value_set->chassis_posture_info.leg_angle_R_set = 0.0f;
+	}
+	
+
+	// ----------------- Chassis High Mode Update ----------------- 
+	if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE || target_value_set->mode.sport_mode == TK_MODE)
+		target_value_set->mode.chassis_high_mode = SIT_MODE;
+	else if( target_value_set->mode.chassis_balancing_mode == FOOT_LAUNCHING )
+		target_value_set->mode.chassis_high_mode = SIT_MODE;
+	else if( target_value_set->mode.chassis_balancing_mode == JOINT_LAUNCHING )
+		target_value_set->mode.chassis_high_mode = NORMAL_MODE;
+	else if( target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING )
+		target_value_set->mode.chassis_high_mode = CHANGING_HIGH;
+	else if( target_value_set->chassis_rc_ctrl->sit_flag )
+		target_value_set->mode.chassis_high_mode = SIT_MODE;
+	else if( target_value_set->chassis_rc_ctrl->high_flag )
+		target_value_set->mode.chassis_high_mode = HIGH_MODE;
+	else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS )
+		target_value_set->mode.chassis_high_mode = SIT_MODE;
+	else if( target_value_set->mode.jumping_stage == EXTENDING_LEGS )
+		target_value_set->mode.chassis_high_mode = HIGH_MODE;
+	else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS_2 )
+		target_value_set->mode.chassis_high_mode = SIT_MODE;
+	else if( target_value_set->mode.jumping_stage == PREPARING_LANDING )
+		target_value_set->mode.chassis_high_mode = NORMAL_MODE;
+	else
+		target_value_set->mode.chassis_high_mode = NORMAL_MODE;
+
+	// --------- Leg Length Set --------- 
+	if( target_value_set->mode.chassis_high_mode == SIT_MODE )
+		target_value_set->chassis_posture_info.ideal_high = SIT_HIGH;
+	else if( target_value_set->mode.chassis_high_mode == NORMAL_MODE )
+		target_value_set->chassis_posture_info.ideal_high = NORMAL_HIGH + High_Offset - Moving_High_Offset;
+	else if( target_value_set->mode.chassis_high_mode == HIGH_MODE )
+		target_value_set->chassis_posture_info.ideal_high = HIGH_HIGH + High_Offset - Moving_High_Offset;
+	else if( target_value_set->mode.chassis_high_mode == CHANGING_HIGH )
+	{
+		reduce_high = reduce_high * debug_1;
+		target_value_set->chassis_posture_info.ideal_high = min(reduce_high,target_value_set->chassis_posture_info.ideal_high);
+			// target_value_set->chassis_posture_info.ideal_high = 
+			// 	debug_1 * target_value_set->chassis_posture_info.ideal_high;
+	}
+
+	if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE ||
+		( target_value_set->flag_info.suspend_flag_L == 1 && target_value_set->flag_info.suspend_flag_R == 1 ) ||
+		target_value_set->chassis_posture_info.ideal_high == SIT_HIGH ||
+		target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING )
+	{
+		target_value_set->chassis_posture_info.leg_length_L_set = target_value_set->chassis_posture_info.ideal_high;
+		target_value_set->chassis_posture_info.leg_length_R_set = target_value_set->chassis_posture_info.ideal_high;
+	}
+	else
+	{
+		target_value_set->chassis_posture_info.foot_roll_angle = 
+			target_value_set->chassis_posture_info.roll_angle +
+			atan(( target_value_set->chassis_posture_info.leg_length_L - target_value_set->chassis_posture_info.leg_length_R ) / 0.50f );
+		
+		target_value_set->chassis_posture_info.leg_length_L_set = 
+			target_value_set->chassis_posture_info .ideal_high 
+			+ 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle );
+		target_value_set->chassis_posture_info.leg_length_R_set = 
+			target_value_set->chassis_posture_info .ideal_high
+			- 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle );
+	}
+		
+}
+
+void Chassis_Torque_Calculation(chassis_control_t *bl_ctrl)
+{
+
+	LQR_Data_Update(bl_ctrl);
+	// -----Roll Balance-----
+	if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 ||
+		bl_ctrl->mode.chassis_high_mode == SIT_MODE )
+	{
+		bl_ctrl->torque_info.joint_roll_torque_R = 0.0f;
+		bl_ctrl->torque_info.joint_roll_torque_L = 0.0f;
+	}
+	else
+	{
+		if (bl_ctrl->chassis_posture_info.roll_angle < -roll_angle_deadband) {
+			rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle+roll_angle_deadband));
+		} 
+		else if (bl_ctrl->chassis_posture_info.roll_angle > roll_angle_deadband){
+			rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle-roll_angle_deadband));
+		}
+		else {
+			rollP = 0.0f;
+		}
+
+		if (bl_ctrl->chassis_posture_info.roll_gyro < -roll_gyro_deadband) {
+			rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro + roll_gyro_deadband);
+		} 
+		else if (bl_ctrl->chassis_posture_info.roll_gyro > roll_gyro_deadband){
+			rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro - roll_gyro_deadband);
+		}
+		else {
+			rollD = 0.0f;
+		}
+		bl_ctrl->torque_info.joint_roll_torque_R = rollP + rollD;
+		bl_ctrl->torque_info.joint_roll_torque_L = -bl_ctrl->torque_info.joint_roll_torque_R;
+	}
+	
+
+	// -----During turns: prevent displacement of two legs----- 
+	bl_ctrl->torque_info.joint_prevent_splits_torque_L = 
+		coordinate_PD[0] * (bl_ctrl->chassis_posture_info.leg_angle_L - bl_ctrl->chassis_posture_info.leg_angle_R)
+		+ coordinate_PD[1] * (bl_ctrl->chassis_posture_info.leg_gyro_L - bl_ctrl->chassis_posture_info.leg_gyro_R);
+
+	bl_ctrl->torque_info.joint_prevent_splits_torque_R = -bl_ctrl->torque_info.joint_prevent_splits_torque_L;
+
+	if( bl_ctrl->mode.jumping_stage == EXTENDING_LEGS ||
+		bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 )
+	{
+		bl_ctrl->torque_info.joint_stand_torque_L = 
+			+ jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) 
+			+ jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L );
+	
+		bl_ctrl->torque_info.joint_stand_torque_R = 
+			+ jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) 
+			+ jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R );
+	}
+	else if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE )
+	{
+		bl_ctrl->torque_info.joint_stand_torque_L = 
+				+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) 
+				+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L );
+		bl_ctrl->torque_info.joint_stand_torque_R = 
+				+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) 
+				+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R );
+	}
+	else if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 )
+	{
+		if( bl_ctrl->flag_info.suspend_flag_L == 1 )
+		{
+			bl_ctrl->torque_info.joint_stand_torque_L = 
+				+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) 
+				+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L );
+		}
+		else{
+			bl_ctrl->torque_info.joint_stand_torque_L = 
+				FEED_f
+				+ stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) 
+				+ stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L );
+		}
+
+		if( bl_ctrl->flag_info.suspend_flag_R == 1 )
+		{
+			bl_ctrl->torque_info.joint_stand_torque_R = 
+				+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) 
+				+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R );
+		} else {
+			bl_ctrl->torque_info.joint_stand_torque_R = 
+				FEED_f
+				+ stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) 
+				+ stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R );
+		}
+	}
+	else if( bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING )
+	{
+
+		bl_ctrl->torque_info.joint_stand_torque_L = 
+			+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) 
+			+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L );
+	
+		bl_ctrl->torque_info.joint_stand_torque_R = 
+			+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) 
+			+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R );
+	}
+	else
+	{
+		bl_ctrl->torque_info.joint_stand_torque_L = FEED_f;
+		bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L );
+		if (bl_ctrl->chassis_posture_info.leg_dlength_L > leg_dlength_deadband){
+			bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L-leg_dlength_deadband) );
+		}
+		else if (bl_ctrl->chassis_posture_info.leg_dlength_L < -leg_dlength_deadband){
+			bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L+leg_dlength_deadband));
+		}
+		else {
+			bl_ctrl->torque_info.joint_stand_torque_L += 0.0f;
+		}
+		bl_ctrl->torque_info.joint_stand_torque_R = FEED_f;
+		bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R );
+		if (bl_ctrl->chassis_posture_info.leg_dlength_R > leg_dlength_deadband){
+			bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R-leg_dlength_deadband) );
+		}
+		else if (bl_ctrl->chassis_posture_info.leg_dlength_R < -leg_dlength_deadband){
+			bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R+leg_dlength_deadband));
+		}
+		else {
+			bl_ctrl->torque_info.joint_stand_torque_R += 0.0f;
+		}
+		
+	}
+	// --------------------- joint motor calucaltion ---------------------
+
+
+	// TODDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDO
+	if( bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 )
+	{
+		bl_ctrl->torque_info.joint_balancing_torque_L = 
+			-suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_L ) 
+			-suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L );
+		bl_ctrl->torque_info.joint_balancing_torque_R = 
+			-suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_R )
+			-suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R );
+
+		bl_ctrl->torque_info.joint_moving_torque_L = 0.0f;
+		bl_ctrl->torque_info.joint_moving_torque_R = 0.0f;
+	}
+	else 
+	{
+		if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE || bl_ctrl->mode.sport_mode == TK_MODE )
+		{
+			bl_ctrl->torque_info.joint_balancing_torque_L = 
+			bl_ctrl->torque_info.joint_moving_torque_L    =
+			bl_ctrl->torque_info.joint_balancing_torque_R =
+			bl_ctrl->torque_info.joint_moving_torque_R    = 0;
+		}
+		else 
+		{
+			if( bl_ctrl->mode.chassis_high_mode == SIT_MODE ||
+				bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING )
+			{
+				bl_ctrl->torque_info.joint_balancing_torque_L = 
+				bl_ctrl->torque_info.joint_moving_torque_L    =
+				bl_ctrl->torque_info.joint_balancing_torque_R = 
+				bl_ctrl->torque_info.joint_moving_torque_R    = 0;
+			}
+			else
+			{
+				
+				bl_ctrl->torque_info.joint_balancing_torque_L = (
+					-LQR[2][4] * ( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L )
+					-LQR[2][5] * (                  0.0f                         - bl_ctrl->chassis_posture_info.leg_gyro_L )
+					-LQR[2][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle ) 
+					-LQR[2][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set  - bl_ctrl->chassis_posture_info.pitch_gyro ) );
+				bl_ctrl->torque_info.joint_moving_torque_L    = ( 
+					+LQR[2][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET)
+					+LQR[2][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set    - bl_ctrl->chassis_posture_info.foot_speed_K )
+					+LQR[2][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett    - bl_ctrl->chassis_posture_info.yaw_angle )
+					+LQR[2][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set      - bl_ctrl->chassis_posture_info.yaw_gyro  )
+					);
+
+				bl_ctrl->torque_info.joint_balancing_torque_R = (
+					-LQR[3][6] * ( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R ) 
+					-LQR[3][7] * (                  0.0f                         - bl_ctrl->chassis_posture_info.leg_gyro_R )
+					-LQR[3][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle ) 
+					-LQR[3][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set  - bl_ctrl->chassis_posture_info.pitch_gyro ) );
+				bl_ctrl->torque_info.joint_moving_torque_R    = ( 
+					+LQR[3][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET)
+					+LQR[3][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set    - bl_ctrl->chassis_posture_info.foot_speed_K )
+					+LQR[3][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett    - bl_ctrl->chassis_posture_info.yaw_angle )
+					+LQR[3][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set      - bl_ctrl->chassis_posture_info.yaw_gyro  )
+					);
+			}
+		}
+	}
+
+	// --------------------- Foot motor LQR ---------------------
+	if ( bl_ctrl->mode.sport_mode == TK_MODE ) {
+		bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f;
+		bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f;
+		bl_ctrl->torque_info.foot_moving_torque_L = (int) (
+			-TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K ) 
+			-TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set    - bl_ctrl->chassis_posture_info.yaw_angle )
+			-TK_y_d*( 0.0f		                                      - bl_ctrl->chassis_posture_info.yaw_gyro )
+		)* TORQ_K;
+		bl_ctrl->torque_info.foot_moving_torque_R = (int)-(
+			-TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K ) 
+			+TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set    - bl_ctrl->chassis_posture_info.yaw_angle )
+			+TK_y_d*( 0.0f		                                      - bl_ctrl->chassis_posture_info.yaw_gyro )
+		)* TORQ_K;
+	}
+	else {
+		bl_ctrl->torque_info.foot_balancing_torque_L = (int) ( 
+			-LQR[0][4]*( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L) 
+			-LQR[0][5]*(                  0.0f                         - bl_ctrl->chassis_posture_info.leg_gyro_L)
+			+LQR[0][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle) 
+			+LQR[0][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro) 
+		)* TORQ_K;
+
+		bl_ctrl->torque_info.foot_moving_torque_L = (int) ( 
+			-LQR[0][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET)
+			-LQR[0][1]*( bl_ctrl->chassis_posture_info.foot_speed_set    - bl_ctrl->chassis_posture_info.foot_speed_K) 
+			-LQR[0][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett    - bl_ctrl->chassis_posture_info.yaw_angle)
+			-LQR[0][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set      - bl_ctrl->chassis_posture_info.yaw_gyro  )
+		)* TORQ_K;
+
+		bl_ctrl->torque_info.foot_balancing_torque_R = (int) -( 
+			-LQR[1][6]*( bl_ctrl->chassis_posture_info.leg_angle_R_set  - bl_ctrl->chassis_posture_info.leg_angle_R) 
+			-LQR[1][7]*(                  0.0f                         - bl_ctrl->chassis_posture_info.leg_gyro_R)  
+			+LQR[1][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set  - bl_ctrl->chassis_posture_info.pitch_angle) 
+			+LQR[1][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro) 
+		)* TORQ_K;
+
+		bl_ctrl->torque_info.foot_moving_torque_R = (int) -( 
+			-LQR[1][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET)
+			-LQR[1][1]*( bl_ctrl->chassis_posture_info.foot_speed_set    - bl_ctrl->chassis_posture_info.foot_speed_K) 
+			+LQR[1][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett    - bl_ctrl->chassis_posture_info.yaw_angle)
+			+LQR[1][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set      - bl_ctrl->chassis_posture_info.yaw_gyro  )
+		)* TORQ_K;
+	}
+
+	if( bl_ctrl->flag_info.suspend_flag_R == 1 )
+	{
+		bl_ctrl->torque_info.joint_balancing_torque_R = 
+			-suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_R )
+			-suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R );
+
+		bl_ctrl->torque_info.foot_moving_torque_R = 
+			-suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_R.speed );	
+
+		bl_ctrl->torque_info.joint_moving_torque_R   = 0.0f;
+		bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f;
+	}
+	if( bl_ctrl->flag_info.suspend_flag_L == 1 )
+	{
+		bl_ctrl->torque_info.joint_balancing_torque_L = 
+			-suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_L ) 
+			-suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L );
+		bl_ctrl->torque_info.foot_moving_torque_L = 
+			+suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_L.speed );
+
+		bl_ctrl->torque_info.joint_moving_torque_L   = 0.0f;
+		bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f;
+	}
+
+
+	// LimitMax( bl_ctrl->torque_info.foot_moving_torque_L,  MAX_ACCL );
+	// LimitMax( bl_ctrl->torque_info.foot_moving_torque_R,  MAX_ACCL );
+	LimitMax( bl_ctrl->torque_info.joint_moving_torque_L, MAX_ACCL_JOINT );
+	LimitMax( bl_ctrl->torque_info.joint_moving_torque_R, MAX_ACCL_JOINT );
+
+}
+
+void Chassis_Torque_Combine(chassis_control_t *bl_ctrl)
+{
+	bl_ctrl->mapping_info .J1_L = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L );
+	bl_ctrl->mapping_info .J2_L = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L);  
+	bl_ctrl->mapping_info .J3_L = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L);
+	bl_ctrl->mapping_info .J4_L = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L );
+	bl_ctrl->mapping_info .J1_R = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R );
+	bl_ctrl->mapping_info .J2_R = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R);  
+	bl_ctrl->mapping_info .J3_R = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R);
+	bl_ctrl->mapping_info .J4_R = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R );
+
+	bl_ctrl->torque_info.foot_horizontal_torque_L = 
+		bl_ctrl->torque_info.foot_balancing_torque_L + bl_ctrl->torque_info.foot_moving_torque_L;
+	bl_ctrl->torque_info.foot_horizontal_torque_R = 
+		bl_ctrl->torque_info.foot_balancing_torque_R + bl_ctrl->torque_info.foot_moving_torque_R;
+
+	bl_ctrl->foot_motor_L.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_L;
+	bl_ctrl->foot_motor_R.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_R;
+
+	LimitMax(bl_ctrl->foot_motor_L.torque_out,16383);
+	LimitMax(bl_ctrl->foot_motor_R.torque_out,16383);
+
+	bl_ctrl->torque_info.joint_horizontal_torque_L = 
+		bl_ctrl->torque_info.joint_balancing_torque_L + bl_ctrl->torque_info.joint_moving_torque_L + bl_ctrl->torque_info.joint_prevent_splits_torque_L;
+	bl_ctrl->torque_info.joint_horizontal_torque_R =
+		bl_ctrl->torque_info.joint_balancing_torque_R + bl_ctrl->torque_info.joint_moving_torque_R + bl_ctrl->torque_info.joint_prevent_splits_torque_R;
+
+	bl_ctrl->torque_info.joint_vertical_torque_L = 
+		bl_ctrl->torque_info.joint_stand_torque_L + bl_ctrl->torque_info.joint_roll_torque_L;
+	bl_ctrl->torque_info.joint_vertical_torque_R = 
+		bl_ctrl->torque_info.joint_stand_torque_R + bl_ctrl->torque_info.joint_roll_torque_R;
+
+	bl_ctrl->torque_info.joint_horizontal_torque_temp1_L = 
+		(bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J3_L ;
+	bl_ctrl->torque_info.joint_horizontal_torque_temp2_L = 
+		(bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J4_L ;
+	bl_ctrl->torque_info.joint_horizontal_torque_temp1_R = 
+		(bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J3_R ;
+	bl_ctrl->torque_info.joint_horizontal_torque_temp2_R = 
+		(bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J4_R ;
+	
+	bl_ctrl->torque_info.joint_vertical_torque_temp1_L = 
+		(bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J1_L;
+	bl_ctrl->torque_info.joint_vertical_torque_temp2_L = 
+		(bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J2_L;
+	bl_ctrl->torque_info.joint_vertical_torque_temp1_R = 
+		(bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J1_R;
+	bl_ctrl->torque_info.joint_vertical_torque_temp2_R = 
+		(bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J2_R;
+
+	/****************************************/
+
+	fp32 MAX_balance = 2000.0f;
+
+	LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_L,MAX_balance);
+	LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_L,MAX_balance);
+	LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_R,MAX_balance);
+	LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_R,MAX_balance);
+	LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_L,15);
+	LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_L,15);
+	LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_R,15);
+	LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_R,15);
+
+	// 分配到四�?关节电机
+	bl_ctrl->joint_motor_1.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp1_L + bl_ctrl->torque_info.joint_vertical_torque_temp1_L;
+	bl_ctrl->joint_motor_2.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp2_L + bl_ctrl->torque_info.joint_vertical_torque_temp2_L;
+	bl_ctrl->joint_motor_3.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp1_R + bl_ctrl->torque_info.joint_vertical_torque_temp1_R;
+	bl_ctrl->joint_motor_4.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp2_R + bl_ctrl->torque_info.joint_vertical_torque_temp2_R;
+
+	if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND )
+	{ 
+		bl_ctrl->joint_motor_1.max_torque = LIMITED_TORQUE;
+		bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE;
+	}
+	else if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND )
+	{
+		bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE;
+		bl_ctrl->joint_motor_1.min_torque = -1.0f * LIMITED_TORQUE;
+	}
+	else
+	{
+		bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE;
+		bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE;
+	}
+	
+	if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND )
+	{
+		bl_ctrl->joint_motor_3.max_torque = LIMITED_TORQUE;
+		bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE;
+	}
+	else if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND )
+	{
+		bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE;
+		bl_ctrl->joint_motor_3.min_torque = -1.0f * LIMITED_TORQUE;
+	}
+	else
+	{
+		bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE;
+		bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE;
+	}
+
+	if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND )
+	{
+		bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE;
+		bl_ctrl->joint_motor_2.min_torque = -1.0f * LIMITED_TORQUE;
+	}
+	else if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND )
+	{
+		bl_ctrl->joint_motor_2.max_torque = LIMITED_TORQUE;
+		bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE;
+	}
+	else
+	{
+		bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE;
+		bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE;
+	}
+
+	if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND )
+	{
+		bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE;
+		bl_ctrl->joint_motor_4.min_torque = -1.0f * LIMITED_TORQUE;
+	}
+	else if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND )
+	{
+		bl_ctrl->joint_motor_4.max_torque = LIMITED_TORQUE;
+		bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE;
+	}
+	else
+	{
+		bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE;
+		bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE;
+	}
+
+	LimitOutput( bl_ctrl->joint_motor_1.torque_out, bl_ctrl->joint_motor_1.min_torque, bl_ctrl->joint_motor_1.max_torque);
+	LimitOutput( bl_ctrl->joint_motor_2.torque_out, bl_ctrl->joint_motor_2.min_torque, bl_ctrl->joint_motor_2.max_torque);
+	LimitOutput( bl_ctrl->joint_motor_3.torque_out, bl_ctrl->joint_motor_3.min_torque, bl_ctrl->joint_motor_3.max_torque);
+	LimitOutput( bl_ctrl->joint_motor_4.torque_out, bl_ctrl->joint_motor_4.min_torque, bl_ctrl->joint_motor_4.max_torque);
+}
+
+void Motor_CMD_Send(chassis_control_t *CMD_Send)
+{
+	if( CMD_Send->joint_motor_1.motor_mode == MOTOR_FORCE )
+		CAN_HT_CMD( 0x01, CMD_Send->joint_motor_1.torque_out );
+	else 
+		CAN_HT_CMD( 0x01, 0.0 );
+	if( CMD_Send->joint_motor_2.motor_mode == MOTOR_FORCE )
+		CAN_HT_CMD( 0x02, CMD_Send->joint_motor_2.torque_out );
+	else 
+		CAN_HT_CMD( 0x02, 0.0 );
+
+
+	if( CMD_Send->foot_motor_R.motor_mode != MOTOR_FORCE )
+		CMD_Send->foot_motor_R .torque_out = 0.0f;
+	if( CMD_Send->foot_motor_L.motor_mode != MOTOR_FORCE )
+		CMD_Send->foot_motor_L .torque_out = 0.0f;
+	CAN_BM_CONTROL_CMD( CMD_Send->foot_motor_L.torque_out, CMD_Send->foot_motor_R.torque_out );
+
+	vTaskDelay(1);
+
+	if( CMD_Send->joint_motor_3.motor_mode == MOTOR_FORCE )
+		CAN_HT_CMD( 0x03, CMD_Send->joint_motor_3.torque_out );
+	else 
+		CAN_HT_CMD( 0x03, 0.0 );
+	
+
+	if( CMD_Send->joint_motor_4.motor_mode == MOTOR_FORCE )
+		CAN_HT_CMD( 0x04, CMD_Send->joint_motor_4.torque_out );
+	else 
+		CAN_HT_CMD( 0x04, 0.0 );
+}
+
+void Joint_Motor_to_Init_Pos()
+{
+	int Init_Time = 0;
+	while( Init_Time < 300 )
+	{
+		CAN_HT_CMD( 0x01, 1.0 );
+		CAN_HT_CMD( 0x02, -1.0 );
+		CAN_HT_CMD( 0x03, 1.0 );
+		vTaskDelay(1);
+		CAN_HT_CMD( 0x04, -1.0 );
+		vTaskDelay(1);
+		Init_Time++;
+	}
+}
+
+void HT_Motor_zero_set(void)
+{
+	uint8_t tx_buff[8];
+	for( int i = 0; i < 7; i++ )
+		tx_buff[i] = 0xFF;
+	tx_buff[7] = 0xfc;
+
+	// ENABLE_CHASSIS
+	CAN_CMD_HT_Enable( 0x01, tx_buff );
+	vTaskDelay(50);
+	CAN_CMD_HT_Enable( 0x02, tx_buff );
+	vTaskDelay(50);
+	CAN_CMD_HT_Enable( 0x03, tx_buff );
+	vTaskDelay(50);
+	CAN_CMD_HT_Enable( 0x04, tx_buff );
+	vTaskDelay(50);
+	// �?到限�?
+	Joint_Motor_to_Init_Pos();
+	// Set zero init point
+	tx_buff[7] = 0xfe;
+
+	CAN_CMD_HT_Enable( 0x01, tx_buff );
+	vTaskDelay(50);
+	CAN_CMD_HT_Enable( 0x02, tx_buff );
+	vTaskDelay(50);
+	CAN_CMD_HT_Enable( 0x03, tx_buff );
+	vTaskDelay(50);
+	CAN_CMD_HT_Enable( 0x04, tx_buff );
+
+	vTaskDelay(50);
+
+}
+
+void Motor_Zero_CMD_Send(void)
+{
+	CAN_HT_CMD( 0x01, 0.0 );
+	vTaskDelay(1);
+	CAN_HT_CMD( 0x02, 0.0 );
+	vTaskDelay(1);
+	CAN_HT_CMD( 0x03, 0.0 );
+	vTaskDelay(1);
+	CAN_HT_CMD( 0x04, 0.0 );
+	vTaskDelay(1);
+
+	// CAN_BM_ENABLE_CMD();
+	// vTaskDelay(1);
+
+	// CAN_BM_MODE_SET_CMD();
+	// vTaskDelay(2);
+
+	// CAN_BM_CURRENT_MODE_CMD();
+	// vTaskDelay(1);
+
+}
diff --git a/Chassis_Task.h b/Chassis_Task.h
new file mode 100644
index 0000000..2d33123
--- /dev/null
+++ b/Chassis_Task.h
@@ -0,0 +1,299 @@
+#ifndef _CHASSIS_TASK
+#define _CHASSIS_TASK
+
+#include "main.h"
+#include "struct_typedef.h"
+#include "pid.h"
+#include "bsp_can.h"
+// ------------- Limit info ------------- 
+#define MAX_ACCL 13000.0f
+#define MAX_ACCL_JOINT 15.0f
+#define MAX_FOOT_OUTPUT 2048
+
+//  ------------- Target value info ------------- 
+#define SIT_MODE_HEIGHT_SET            0.18f
+#define NORMAL_MODE_HEIGHT_SET         0.10f
+#define HIGH_MODE_HEIGHT_SET           0.27f
+#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f
+
+// ------------- Mech info ------------- 
+#define L1 0.15f
+#define L2 0.25f
+#define L3 0.25f
+#define L4 0.15f
+#define L5 0.1f
+
+#define WHEEL_PERIMETER 0.56547
+#define WHEEL_RADIUS 0.09f
+#define LEG_OFFSET       0.3790855135f // 水平位置到上限位的夹角
+#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f
+#define LOWER_SUPPORT_FORCE 0.0f
+#define MOVE_LOWER_BOUND 0.3f
+#define EXIT_PITCH_ANGLE 0.1f
+#define DANGER_PITCH_ANGLE 0.25f
+
+#define FEED_f 7.5f
+#define FEED_f_1 3.5f
+
+#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f
+
+#define MOTOR_POS_UPPER_BOUND 0.05f
+#define MOTOR_POS_LOWER_BOUND 1.4f
+#define LIMITED_TORQUE 0.5f
+#define UNLIMITED_TORQUE 200.0f
+
+// ------------- Time info ------------- 
+#define CHASSIS_TASK_INIT_TIME  500
+#define TASK_RUN_TIME           0.002f
+
+// ------------- Transfer info ------------- 
+#define MOTOR_ECD_TO_RAD     0.00019174779      // 2*PI / 32767
+#define HALF_ECD_RANGE                14383
+#define HALF_POSITION_RANGE 3.0f
+// #define CC                          0.00512f 
+// #define CC                          1/494.0f
+#define TORQ_K                      494.483818182
+// ------------- Math info ------------- 
+#define PI2					  6.28318530717959f
+#define PI					  3.14159265358979f
+#define PI_2				  1.57079632679489f
+#define PI_4				  0.78539816339744f
+
+
+
+typedef enum
+{
+    ENABLE_CHASSIS = 0,
+    DISABLE_CHASSIS,
+} chassis_mode_e;
+
+typedef enum
+{
+    NO_FORCE,
+    FOOT_LAUNCHING,
+    JOINT_LAUNCHING,
+    BALANCING_READY,
+    JOINT_REDUCING,
+} chassis_balancing_mode_e;
+
+typedef enum
+{
+    NONE,
+    NORMAL_MOVING_MODE,
+    ABNORMAL_MOVING_MODE,
+    JUMPING_MODE,
+    CAP_MODE,
+    FLY_MODE,
+    TK_MODE,
+} sport_mode_e;
+
+typedef enum
+{
+    READY_TO_JUMP,
+    PREPARING_LANDING,
+    PREPARING_STAND_JUMPING,
+    CONSTACTING_LEGS,
+    EXTENDING_LEGS,
+    CONSTACTING_LEGS_2,
+    FINISHED,
+} jumping_stage_e;
+
+typedef enum
+{
+    NOT_DEFINE,
+    STANDING_JUMP,
+    MOVING_JUMP,
+} jumping_mode_e;
+
+typedef enum
+{
+    SIT_MODE = 0,
+    NORMAL_MODE,
+    HIGH_MODE,
+    EXTREMELY_HIGH_MODE,
+    CHANGING_HIGH,
+} chassis_high_mode_e;
+
+typedef enum
+{
+    MOTOR_NO_FORCE = 0,
+    MOTOR_FORCE,
+} chassis_motor_mode_e;
+
+typedef enum
+{
+	ON_GROUND = 0,
+	OFF_GROUND = 1,
+} suspend_flag_e;
+
+typedef struct
+{
+    chassis_mode_e           chassis_mode, last_chassis_mode;
+    chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode;
+    sport_mode_e             sport_mode, last_sport_mode; 
+    
+    jumping_mode_e           jumping_mode, last_jumping_mode;
+    jumping_stage_e          jumping_stage, last_jumping_stage;
+
+    chassis_high_mode_e       chassis_high_mode, last_chassis_high_mode;
+
+} mode_t;
+
+typedef struct
+{
+    const fp32 *chassis_INS_angle_point;
+  	const fp32 *chassis_INS_gyro_point;
+    const fp32 *chassis_INS_accel_point;
+    fp32 yaw_angle, pitch_angle, roll_angle;
+    fp32 yaw_gyro, pitch_gyro, roll_gyro;
+    fp32 yaw_accel, pitch_accel, roll_accel;
+
+    fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set;
+    fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set;
+
+    fp32 ideal_high;
+    fp32 leg_length_L, last_leg_length_L, leg_length_L_set;
+    fp32 leg_length_R, last_leg_length_R, leg_length_R_set;
+    fp32 leg_dlength_L;
+    fp32 leg_dlength_R;
+
+    fp32 foot_roll_angle;
+    fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set;
+    fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set;
+    fp32 leg_gyro_L, leg_gyro_R;
+
+    fp32 foot_distance, foot_distance_K, foot_distance_set;
+    fp32 foot_speed, foot_speed_K, foot_speed_set;
+
+    fp32 supportive_force_L, supportive_force_R;
+
+} chassis_posture_info_t;
+
+typedef struct
+{
+    // -------- horizontal force -------- 
+    fp32 joint_balancing_torque_L, joint_balancing_torque_R;
+    fp32 foot_balancing_torque_L,  foot_balancing_torque_R;
+	
+	fp32 foot_moving_torque_L,  foot_moving_torque_R;
+    fp32 joint_moving_torque_L, joint_moving_torque_R;
+
+    fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R;
+
+    fp32 joint_horizontal_torque_L, joint_horizontal_torque_R;
+    fp32 foot_horizontal_torque_L,  foot_horizontal_torque_R;
+
+    fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R;
+    fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L;
+
+    fp32 yaw_torque;
+
+    // -------- vertical force -------- 
+    fp32 joint_roll_torque_L,  joint_roll_torque_R;
+    fp32 joint_stand_torque_L, joint_stand_torque_R;
+
+    fp32 joint_vertical_torque_L,      joint_vertical_torque_R;
+    fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R;
+
+    fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R;
+    fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L;
+
+} torque_info_t;
+
+typedef struct 
+{
+    fp32 J1_L,J2_L;
+	fp32 J3_L,J4_L;
+	fp32 J1_R,J2_R;
+	fp32 J3_R,J4_R;
+	fp32 invJ1_L,invJ2_L;
+	fp32 invJ3_L,invJ4_L;
+	fp32 invJ1_R,invJ2_R;
+	fp32 invJ3_R,invJ4_R;
+
+} mapping_info_t;
+
+typedef struct
+{
+    const HT_motor_measure_t *motor_measure;
+    chassis_motor_mode_e motor_mode, last_motor_mode;
+
+    bool_t offline_flag;
+
+    fp32 position;
+    fp32 init_position;
+    fp32 position_offset;
+
+    fp32 velocity;
+    fp32 torque_out, torque_get;
+    fp32 max_torque, min_torque;
+} joint_motor_t;
+
+typedef struct
+{
+    motor_measure_t motor_measure;
+    chassis_motor_mode_e motor_mode, last_motor_mode;
+
+    bool_t offline_flag;
+
+    fp32 distance, distance_offset, last_position, position, turns;
+    fp32 speed;
+    fp32 torque_out, torque_get;
+
+} foot_motor_t;
+
+typedef struct
+{
+    bool_t init_flag;
+	suspend_flag_e suspend_flag_L, last_suspend_flag_L;
+    suspend_flag_e suspend_flag_R, last_suspend_flag_R;
+    bool_t Ignore_Off_Ground;
+    bool_t abnormal_flag;
+    bool_t static_flag, last_static_flag;
+    bool_t moving_flag, last_moving_flag;
+    bool_t rotation_flag;
+    bool_t controlling_flag;
+    bool_t set_pos_after_moving;
+    bool_t overpower_warning_flag;
+    bool_t last_overpower_warning_flag;
+    bool_t stablize_high_flag;
+    bool_t last_stablize_high_flag;
+} flag_info_t;
+
+typedef struct 
+{
+	pid_type_def buffer_control_pid;
+	pid_type_def cap_pid;
+	pid_type_def scale_down_pid;
+} pid_info_t;
+
+typedef struct
+{
+    mode_t                  mode;
+    chassis_posture_info_t  chassis_posture_info;
+    torque_info_t           torque_info;
+    mapping_info_t          mapping_info;
+    flag_info_t             flag_info;
+	pid_info_t              pid_info;
+    const Gimbal_ctrl_t     *chassis_rc_ctrl;
+
+    joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4;
+    foot_motor_t foot_motor_L, foot_motor_R;
+
+}chassis_control_t;
+
+enum Chassis_Mode
+{
+  Chassis_No_Force = 0,
+  Follow_Gimbal,
+  Follow_Gimbal_90Deg,
+  No_Follow,
+  Rotate,
+//   TK_MODE,
+};
+
+extern enum Chassis_Mode chassis_mode;
+extern chassis_control_t chassis_control;
+extern fp32 roll_PD[2];
+
+#endif
diff --git a/User/component/lqr.c b/User/component/lqr.c
new file mode 100644
index 0000000..8816a42
--- /dev/null
+++ b/User/component/lqr.c
@@ -0,0 +1,144 @@
+#include "lqr.h"
+#include <string.h>
+
+// 默认LQR增益矩阵 (需要根据实际系统调整)
+static const float DEFAULT_K[LQR_INPUT_DIM][LQR_STATE_DIM] = {
+    // K矩阵行: [T_L, T_R]
+    // K矩阵列: [x, x_dot, theta, theta_dot, phi_L, phi_R]
+    {-50.0f, -20.0f, 800.0f, 100.0f, -200.0f, 0.0f},    // 左腿力矩
+    {-50.0f, -20.0f, 800.0f, 100.0f, 0.0f, -200.0f}     // 右腿力矩
+};
+
+/**
+ * @brief 初始化LQR控制器
+ */
+int8_t LQR_Init(LQR_Controller_t *lqr, const LQR_Params_t *params) {
+    if (lqr == NULL) return -1;
+    
+    // 清零结构体
+    memset(lqr, 0, sizeof(LQR_Controller_t));
+    
+    // 设置参数
+    if (params != NULL) {
+        memcpy(&lqr->params, params, sizeof(LQR_Params_t));
+    } else {
+        // 使用默认参数
+        memset(&lqr->params.Q, 0, sizeof(lqr->params.Q));
+        memset(&lqr->params.R, 0, sizeof(lqr->params.R));
+        
+        // 设置默认权重矩阵对角元素
+        lqr->params.Q[STATE_POSITION][STATE_POSITION] = 100.0f;     // 位置权重
+        lqr->params.Q[STATE_VELOCITY][STATE_VELOCITY] = 10.0f;      // 速度权重
+        lqr->params.Q[STATE_PITCH][STATE_PITCH] = 1000.0f;          // 俯仰角权重
+        lqr->params.Q[STATE_PITCH_RATE][STATE_PITCH_RATE] = 100.0f; // 俯仰角速度权重
+        lqr->params.Q[STATE_LEG_L][STATE_LEG_L] = 50.0f;            // 左腿角度权重
+        lqr->params.Q[STATE_LEG_R][STATE_LEG_R] = 50.0f;            // 右腿角度权重
+        
+        lqr->params.R[INPUT_TORQUE_L][INPUT_TORQUE_L] = 1.0f;       // 左腿力矩权重
+        lqr->params.R[INPUT_TORQUE_R][INPUT_TORQUE_R] = 1.0f;       // 右腿力矩权重
+        
+        lqr->params.max_torque = 50.0f;         // 最大力矩50Nm
+        lqr->params.deadband_position = 0.01f;  // 位置死区1cm
+        lqr->params.deadband_angle = 0.02f;     // 角度死区约1度
+    }
+    
+    // 设置默认增益矩阵
+    memcpy(lqr->params.K, DEFAULT_K, sizeof(DEFAULT_K));
+    
+    lqr->initialized = 1;
+    return 0;
+}
+
+/**
+ * @brief 设置LQR增益矩阵
+ */
+int8_t LQR_SetGains(LQR_Controller_t *lqr, float K[LQR_INPUT_DIM][LQR_STATE_DIM]) {
+    if (lqr == NULL || K == NULL) return -1;
+    
+    memcpy(lqr->params.K, K, sizeof(lqr->params.K));
+    return 0;
+}
+
+/**
+ * @brief 更新系统状态
+ */
+int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
+    if (lqr == NULL || state == NULL || !lqr->initialized) return -1;
+    
+    memcpy(&lqr->current_state, state, sizeof(LQR_State_t));
+    return 0;
+}
+
+/**
+ * @brief 设置参考状态
+ */
+int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_Reference_t *reference) {
+    if (lqr == NULL || reference == NULL || !lqr->initialized) return -1;
+    
+    memcpy(&lqr->reference, reference, sizeof(LQR_Reference_t));
+    return 0;
+}
+
+/**
+ * @brief LQR控制计算
+ */
+int8_t LQR_Calculate(LQR_Controller_t *lqr) {
+    if (lqr == NULL || !lqr->initialized) return -1;
+    
+    // 计算状态误差
+    lqr->state_error[STATE_POSITION] = lqr->reference.position_ref - lqr->current_state.position;
+    lqr->state_error[STATE_VELOCITY] = lqr->reference.velocity_ref - lqr->current_state.velocity;
+    lqr->state_error[STATE_PITCH] = lqr->reference.pitch_ref - lqr->current_state.pitch_angle;
+    lqr->state_error[STATE_PITCH_RATE] = 0.0f - lqr->current_state.pitch_rate; // 俯仰角速度期望为0
+    lqr->state_error[STATE_LEG_L] = lqr->reference.leg_angle_L_ref - lqr->current_state.leg_angle_L;
+    lqr->state_error[STATE_LEG_R] = lqr->reference.leg_angle_R_ref - lqr->current_state.leg_angle_R;
+    
+    // 应用死区
+    lqr->state_error[STATE_POSITION] = LQR_Deadband(lqr->state_error[STATE_POSITION], 
+                                                    lqr->params.deadband_position);
+    lqr->state_error[STATE_PITCH] = LQR_Deadband(lqr->state_error[STATE_PITCH], 
+                                                 lqr->params.deadband_angle);
+    lqr->state_error[STATE_LEG_L] = LQR_Deadband(lqr->state_error[STATE_LEG_L], 
+                                                 lqr->params.deadband_angle);
+    lqr->state_error[STATE_LEG_R] = LQR_Deadband(lqr->state_error[STATE_LEG_R], 
+                                                 lqr->params.deadband_angle);
+    
+    // LQR控制律: u = -K * x_error
+    for (int i = 0; i < LQR_INPUT_DIM; i++) {
+        lqr->control_output[i] = 0.0f;
+        for (int j = 0; j < LQR_STATE_DIM; j++) {
+            lqr->control_output[i] -= lqr->params.K[i][j] * lqr->state_error[j];
+        }
+        // 限制输出
+        lqr->control_output[i] = LQR_Limit(lqr->control_output[i], lqr->params.max_torque);
+    }
+    
+    return 0;
+}
+
+/**
+ * @brief 获取控制输出
+ */
+int8_t LQR_GetOutput(LQR_Controller_t *lqr, float *torque_L, float *torque_R) {
+    if (lqr == NULL || torque_L == NULL || torque_R == NULL || !lqr->initialized) return -1;
+    
+    *torque_L = lqr->control_output[INPUT_TORQUE_L];
+    *torque_R = lqr->control_output[INPUT_TORQUE_R];
+    
+    return 0;
+}
+
+/**
+ * @brief 重置LQR控制器
+ */
+int8_t LQR_Reset(LQR_Controller_t *lqr) {
+    if (lqr == NULL || !lqr->initialized) return -1;
+    
+    // 清零状态和输出
+    memset(&lqr->current_state, 0, sizeof(LQR_State_t));
+    memset(&lqr->reference, 0, sizeof(LQR_Reference_t));
+    memset(lqr->control_output, 0, sizeof(lqr->control_output));
+    memset(lqr->state_error, 0, sizeof(lqr->state_error));
+    
+    return 0;
+}
\ No newline at end of file
diff --git a/User/component/lqr.h b/User/component/lqr.h
new file mode 100644
index 0000000..b5daf02
--- /dev/null
+++ b/User/component/lqr.h
@@ -0,0 +1,151 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <math.h>
+
+// LQR状态向量维度定义
+#define LQR_STATE_DIM 6    // 状态向量维度 [x, x_dot, theta, theta_dot, phi_L, phi_R]
+#define LQR_INPUT_DIM 2    // 输入向量维度 [T_L, T_R] (左右腿力矩)
+
+// 系统状态索引
+typedef enum {
+    STATE_POSITION = 0,     // 机体位置 x
+    STATE_VELOCITY = 1,     // 机体速度 x_dot
+    STATE_PITCH = 2,        // 俯仰角 theta
+    STATE_PITCH_RATE = 3,   // 俯仰角速度 theta_dot
+    STATE_LEG_L = 4,        // 左腿角度 phi_L
+    STATE_LEG_R = 5         // 右腿角度 phi_R
+} LQR_State_Index_t;
+
+// 控制输入索引
+typedef enum {
+    INPUT_TORQUE_L = 0,     // 左腿力矩
+    INPUT_TORQUE_R = 1      // 右腿力矩
+} LQR_Input_Index_t;
+
+// LQR参数结构体
+typedef struct {
+    float Q[LQR_STATE_DIM][LQR_STATE_DIM];  // 状态权重矩阵
+    float R[LQR_INPUT_DIM][LQR_INPUT_DIM];  // 输入权重矩阵
+    float K[LQR_INPUT_DIM][LQR_STATE_DIM];  // LQR增益矩阵
+    float max_torque;                        // 最大输出力矩限制
+    float deadband_position;                 // 位置死区
+    float deadband_angle;                    // 角度死区
+} LQR_Params_t;
+
+// 系统状态结构体
+typedef struct {
+    float position;          // 机体位置 (m)
+    float velocity;          // 机体速度 (m/s)
+    float pitch_angle;       // 俯仰角 (rad)
+    float pitch_rate;        // 俯仰角速度 (rad/s)
+    float leg_angle_L;       // 左腿角度 (rad)
+    float leg_angle_R;       // 右腿角度 (rad)
+    float leg_length_L;      // 左腿长度 (m)
+    float leg_length_R;      // 右腿长度 (m)
+} LQR_State_t;
+
+// 目标状态结构体
+typedef struct {
+    float position_ref;      // 目标位置
+    float velocity_ref;      // 目标速度
+    float pitch_ref;         // 目标俯仰角
+    float leg_angle_L_ref;   // 左腿目标角度
+    float leg_angle_R_ref;   // 右腿目标角度
+} LQR_Reference_t;
+
+// LQR控制器结构体
+typedef struct {
+    LQR_Params_t params;         // LQR参数
+    LQR_State_t current_state;   // 当前状态
+    LQR_Reference_t reference;   // 参考状态
+    float control_output[LQR_INPUT_DIM];  // 控制输出
+    float state_error[LQR_STATE_DIM];     // 状态误差
+    uint8_t initialized;         // 初始化标志
+} LQR_Controller_t;
+
+/**
+ * @brief 初始化LQR控制器
+ * @param lqr LQR控制器指针
+ * @param params LQR参数指针
+ * @return 0: 成功, -1: 失败
+ */
+int8_t LQR_Init(LQR_Controller_t *lqr, const LQR_Params_t *params);
+
+/**
+ * @brief 设置LQR增益矩阵
+ * @param lqr LQR控制器指针
+ * @param K 增益矩阵 [INPUT_DIM][STATE_DIM]
+ * @return 0: 成功, -1: 失败
+ */
+int8_t LQR_SetGains(LQR_Controller_t *lqr, float K[LQR_INPUT_DIM][LQR_STATE_DIM]);
+
+/**
+ * @brief 更新系统状态
+ * @param lqr LQR控制器指针
+ * @param state 当前状态指针
+ * @return 0: 成功, -1: 失败
+ */
+int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
+
+/**
+ * @brief 设置参考状态
+ * @param lqr LQR控制器指针
+ * @param reference 参考状态指针
+ * @return 0: 成功, -1: 失败
+ */
+int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_Reference_t *reference);
+
+/**
+ * @brief LQR控制计算
+ * @param lqr LQR控制器指针
+ * @return 0: 成功, -1: 失败
+ */
+int8_t LQR_Calculate(LQR_Controller_t *lqr);
+
+/**
+ * @brief 获取控制输出
+ * @param lqr LQR控制器指针
+ * @param torque_L 左腿力矩输出指针
+ * @param torque_R 右腿力矩输出指针
+ * @return 0: 成功, -1: 失败
+ */
+int8_t LQR_GetOutput(LQR_Controller_t *lqr, float *torque_L, float *torque_R);
+
+/**
+ * @brief 重置LQR控制器
+ * @param lqr LQR控制器指针
+ * @return 0: 成功, -1: 失败
+ */
+int8_t LQR_Reset(LQR_Controller_t *lqr);
+
+/**
+ * @brief 限制输出幅值
+ * @param value 输入值
+ * @param limit 限制值
+ * @return 限制后的值
+ */
+static inline float LQR_Limit(float value, float limit) {
+    if (value > limit) return limit;
+    if (value < -limit) return -limit;
+    return value;
+}
+
+/**
+ * @brief 死区处理
+ * @param value 输入值
+ * @param deadband 死区大小
+ * @return 处理后的值
+ */
+static inline float LQR_Deadband(float value, float deadband) {
+    if (fabs(value) < deadband) return 0.0f;
+    return value > 0 ? (value - deadband) : (value + deadband);
+}
+
+#ifdef __cplusplus
+}
+#endif
\ No newline at end of file
diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index ae0024b..d2890c8 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -10,12 +10,6 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
   if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
 
-  // 如果从轮子平衡模式切换出去，恢复PID参数的原始range设置
-  if (c->mode == CHASSIS_MODE_WHELL_BALANCE) {
-    ((KPID_Params_t*)c->pid.left_wheel.param)->range = 0.0f;
-    ((KPID_Params_t*)c->pid.right_wheel.param)->range = 0.0f;
-  }
-
   PID_Reset(&c->pid.left_wheel);
   PID_Reset(&c->pid.right_wheel);
   PID_Reset(&c->pid.follow);
@@ -49,11 +43,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
     PID_Init(&c->pid.left_wheel, KPID_MODE_CALC_D, target_freq, &param->motor_pid_param);
     PID_Init(&c->pid.right_wheel, KPID_MODE_CALC_D, target_freq, &param->motor_pid_param);
     PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, &param->follow_pid_param);
-    PID_Init(&c->pid.balance, KPID_MODE_CALC_D, target_freq, &param->motor_pid_param);
+    PID_Init(&c->pid.balance, KPID_MODE_CALC_D, target_freq, &param->balance_pid_param);
 
     // 初始化设定点
-    c->setpoint.left_wheel = 0.0f;
-    c->setpoint.right_wheel = 0.0f;
+    // c->setpoint.left_wheel = 0.0f;
+    // c->setpoint.right_wheel = 0.0f;
 
     return CHASSIS_OK;
 }
@@ -170,59 +164,91 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
         
         case CHASSIS_MODE_WHELL_BALANCE:
         {   
-            // 定义腿部运动学参数（假设单位为mm转换为m）
-            KIN_SerialLeg_Param_t left_leg_param = {
-                .thigh_length = 0.215f, 
-                .calf_length = 0.258f    
-            };
-            KIN_SerialLeg_Param_t right_leg_param = {
-                .thigh_length = 0.215f, 
-                .calf_length = 0.258f    
-            };
+            // 轮腿平衡模式：不修改关节电机，只控制轮毂电机进行平衡
+            switch (c->state) {
+                case 0:
+                    // 初始化状态：使能轮毂电机和关节电机
+                    for (int i = 0; i < 2; i++) {
+                        MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
+                    }
+                    for (int i = 0; i < 4; i++) {
+                        MOTOR_LZ_Enable(&c->param->joint_motors[i]);
+                    }
+                    
+                    c->state = 1;
+                    c->height = 0.16f; 
+                    c->setpoint.chassis.rol = 0.0f;
+                    c->setpoint.chassis.pit = 0.0f;
+                    c->setpoint.chassis.yaw = c->feedback.imu.euler.yaw;
+                    break;
+                    
+                case 1: {
+                    KIN_SerialLeg_Param_t leg_param = {
+                        .thigh_length = 0.215f, 
+                        .calf_length = 0.258f    
+                    };
 
-            float angle = 1.2f;
-            float height = 0.16f;
+                    float angle = 1.35f;
+                    // float height = 0.16f;
+                    c->height += c_cmd->height * 0.000001f;
+                    c->setpoint.chassis.yaw += c_cmd->move_vec.wz * -0.0005f; // 目标yaw角随输入变化
+                    if (c->height < 0.16f) c->height = 0.16f;
+                    if (c->height > 0.35f) c->height = 0.35f;
 
-            for (int i = 0; i < 4; i++) {
-                c->output.joint[i].torque = 0.0f;
-                c->output.joint[i].target_velocity = 0.0f;
-                c->output.joint[i].kp = 50.0f;
-                c->output.joint[i].kd = 1.0f;
-            }
-
-            KIN_SerialLeg_IK(&left_leg_param, &angle, &height, 
+                    KIN_SerialLeg_IK(&leg_param, 
+                                        &angle, 
+                                        &c->height, 
                                         &c->output.joint[0].target_angle, 
                                         &c->output.joint[1].target_angle);
-            c->output.joint[2].target_angle = c->output.joint[1].target_angle;
-            c->output.joint[3].target_angle = c->output.joint[0].target_angle;
+                    c->output.joint[3].target_angle = c->output.joint[0].target_angle;
+                    c->output.joint[2].target_angle = c->output.joint[1].target_angle;
 
-            for (int i = 0; i < 4; i++) {
-                MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &c->output.joint[i]);
-            }
+                    for (int i = 0; i < 4; i++) {
+                        c->output.joint[i].torque = 0.0f;
+                        c->output.joint[i].target_velocity = 0.0f;
+                        c->output.joint[i].kp = 50.0f;
+                        c->output.joint[i].kd = 1.0f;
+                        MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &c->output.joint[i]);
+                    }
+
+
+                    // 轮毂电机平衡控制 - 双环控制结构
+                    // 外环：角度控制器（平衡控制器）
+                    // 内环：速度控制器（轮子电机控制器）
+                    float pitch_angle = c->feedback.imu.euler.pit;  // 机体俯仰角
+                    float pitch_rate = c->feedback.imu.gyro.y;      // 俯仰角速度
+
+
+                    
+                    // 外环：平衡控制器，目标俯仰角为0，输出目标速度
+                    // float target_speed = PID_Calc(&c->pid.balance, 0.0f, pitch_angle, 0.0, c->dt);
+                    float target_speed = PID_Calc(&c->pid.balance, c_cmd->move_vec.vx/10.0f, pitch_angle, 0.0, c->dt);
+                    // 内环：速度控制器，控制轮子速度跟踪目标速度
+                    float left_wheel_speed = c->feedback.wheel[0].rotor_speed/2000;   // 当前左轮速度
+                    float right_wheel_speed = c->feedback.wheel[1].rotor_speed/2000;  // 当前右轮速度
+                    
+
+                    float target_yaw = c->setpoint.chassis.yaw; // 目标 yaw 角度
+                    float current_yaw = c->feedback.imu.euler.yaw; // 当前 yaw 角度
+                    float target_yaw_rate = PID_Calc(&c->pid.follow, target_yaw, current_yaw, 0.0f, c->dt);
+
+
+                    float left_speed_output = PID_Calc(&c->pid.left_wheel, target_speed - target_yaw_rate, left_wheel_speed, 0.0f, c->dt);
+                    float right_speed_output = PID_Calc(&c->pid.right_wheel, target_speed + target_yaw_rate, right_wheel_speed, 0.0f, c->dt);
+                    
+                    // 输出到轮毂电机
+                    c->output.wheel[0] = left_speed_output;
+                    c->output.wheel[1] = right_speed_output;
+                    // c->output.wheel[0] = target_speed;
+                    // c->output.wheel[1] = target_speed;
+                    MOTOR_LK_SetOutput(&c->param->wheel_motors[0], c->output.wheel[0]);
+                    MOTOR_LK_SetOutput(&c->param->wheel_motors[1], c->output.wheel[1]);
+                    
+                    break;
+                }
 
-            // 轮子控制：保持位置或进行平衡控制
-            float left_wheel_angle = c->feedback.wheel[0].rotor_abs_angle;
-            float right_wheel_angle = c->feedback.wheel[1].rotor_abs_angle;
-            
-            // 使用setpoint作为目标位置
-            if (c->state == 0) {
-                // 首次进入模式时，将当前位置设为目标位置
-                c->setpoint.left_wheel = left_wheel_angle;
-                c->setpoint.right_wheel = right_wheel_angle;
-                c->state = 1;
             }
             
-            float left_wheel_output = PID_Calc(&c->pid.left_wheel, c->setpoint.left_wheel, left_wheel_angle, 0.0f, c->dt);
-            float right_wheel_output = PID_Calc(&c->pid.right_wheel, c->setpoint.right_wheel, right_wheel_angle, 0.0f, c->dt);
-            
-            // 限制输出范围
-            left_wheel_output = AbsClip(left_wheel_output, 1.0f);
-            right_wheel_output = AbsClip(right_wheel_output, 1.0f);
-            
-            // 输出到轮毂电机
-            MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_wheel_output);
-            MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_wheel_output);
-            
             break;
         }        
         case CHASSIS_MODE_WHELL_LEG_BALANCE:
diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h
index de77412..06b7730 100644
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@@ -69,6 +69,7 @@ typedef struct {
 typedef struct {
   KPID_Params_t motor_pid_param;  /* 轮子控制PID的参数 */
   KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */
+  KPID_Params_t balance_pid_param; /* 平衡PID的参数 */
 
   MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
   MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
@@ -110,8 +111,7 @@ typedef struct {
 
   /* PID计算的目标值 */
   struct {
-    float left_wheel;
-    float right_wheel;
+    AHRS_Eulr_t chassis;
   } setpoint;
 
   /* 反馈控制用的PID */
diff --git a/User/module/config.c b/User/module/config.c
index 404f41a..c26edb1 100644
--- a/User/module/config.c
+++ b/User/module/config.c
@@ -69,24 +69,35 @@ Config_RobotParam_t robot_config = {
     .chassis_param = {
         .follow_pid_param = {
             .k = 1.0f,
-            .p = 20.0f,
+            .p = 5.0f,
             .i = 0.0f,
-            .d = 0.0f,
+            .d = 0.1f,
             .i_limit = 0.0f,
-            .out_limit = 500.0f,
-            .d_cutoff_freq = 30.0f,
-            .range = 0.0f,
+            .out_limit = 1.0f,
+            .d_cutoff_freq = -1.0f,
+            .range = -1.0f,
         },
         .motor_pid_param = {
-            .k = 0.1f,
-            .p = 10.0f,
-            .i = 0.2f,
-            .d = 0.2f,
-            .i_limit = 0.3f,
-            .out_limit = -1.0f,
-            .d_cutoff_freq = 0.0f,
-            .range = M_2PI,
+            .k = 0.8f,
+            .p = 2.0f,    // 速度环PID参数，通常P比位置环小
+            .i = 0.0f,    // 增加积分项，消除稳态误差
+            .d = 0.0f,   // 减小微分项，避免噪声放大
+            .i_limit = 0.0f,
+            .out_limit = 1.0f,  // 限制在[-1, 1]范围内
+            .d_cutoff_freq = -1.0f, // 增加微分项滤波频率
+            .range = -1.0f,  // 速度控制不需要循环范围
         },
+        .balance_pid_param = {
+            .k = -1.0f,
+            .p = 5.0f,   // 增大比例项，提高响应速度  
+            .i = 0.2f,    // 增加积分项，消除稳态误差
+            .d = 0.2f,    // 增加微分项，提高稳定性
+            .i_limit = 1.0f,  // 限制积分饱和
+            .out_limit = 1.0f,  // 输出目标速度，单位可能是rpm或rad/s
+            .d_cutoff_freq = -1.0f, // 微分项滤波
+            .range = -1.0f,  // 角度控制不需要循环范围（这里是pitch角度）
+        },
+        
         .low_pass_cutoff_freq = {
             .in = 30.0f,
             .out = 30.0f,
diff --git a/User/module/mod_wheelleg_chassis.cpp b/User/module/mod_wheelleg_chassis.cpp
new file mode 100644
index 0000000..8f82c6c
--- /dev/null
+++ b/User/module/mod_wheelleg_chassis.cpp
@@ -0,0 +1,662 @@
+#include "mod_wheelleg_chassis.hpp"
+
+#include <cmath>
+#include <tuple>
+
+using namespace Module;
+Wheelleg::Wheelleg(Param& param)
+    : param_(param),
+      roll_pid_(param.roll_pid_param, 333.0f),
+      yaw_pid_(param.yaw_pid_param, 333.0f),
+      yaccl_pid_(param.yaccl_pid_param, 333.0f),
+      lowfilter_(333.0f, 50.0f),
+      acclfilter_(333.0f, 30.0f),
+      manfilter_(param.manfilter_param),
+
+      ctrl_lock_(true) {
+  memset(&(this->cmd_), 0, sizeof(this->cmd_));
+
+  this->hip_motor_.at(0) =
+      new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
+  this->hip_motor_.at(1) =
+      new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
+  this->hip_motor_.at(2) =
+      new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
+  this->hip_motor_.at(3) =
+      new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
+
+  this->wheel_motor_.at(0) =
+      new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
+  this->wheel_motor_.at(1) =
+      new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
+
+  this->leglength_pid_.at(0) =
+      new Component::PID(param.leglength_pid_param.at(0), 333.0f);
+  this->leglength_pid_.at(1) =
+      new Component::PID(param.leglength_pid_param.at(1), 333.0f);
+
+  this->theta_pid_.at(0) =
+      new Component::PID(param.theta_pid_param.at(0), 333.0f);
+  this->theta_pid_.at(1) =
+      new Component::PID(param.theta_pid_param.at(1), 333.0f);
+
+  this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
+  this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
+
+  this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
+  this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
+
+  auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
+    chassis->ctrl_lock_.Wait(UINT32_MAX);
+    switch (event) {
+      case SET_MODE_RELAX:
+        chassis->SetMode(RELAX);
+        break;
+      case SET_MODE_STAND:
+        chassis->SetMode(STAND);
+        break;
+      case SET_MODE_ROTOR:
+        chassis->SetMode(ROTOR);
+        break;
+      case SET_MODE_RESET:
+        chassis->SetMode(RESET);
+        break;
+      default:
+        break;
+    }
+    chassis->ctrl_lock_.Post();
+  };
+
+  Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
+      event_callback, this, this->param_.EVENT_MAP);
+
+  auto chassis_thread = [](Wheelleg* chassis) {
+    auto cmd_sub =
+        Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
+    auto eulr_sub =
+        Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
+    auto gyro_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
+
+    auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+
+    auto accl_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
+    // auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+    uint32_t last_online_time = bsp_time_get_ms();
+
+    while (1) {
+      eulr_sub.DumpData(chassis->eulr_); /* imu */
+      gyro_sub.DumpData(chassis->gyro_); /* imu */
+      accl_sub.DumpData(chassis->accl_);
+
+      yaw_sub.DumpData(chassis->yaw_);
+      cmd_sub.DumpData(chassis->cmd_);
+      // yaw_sub.DumpData(chassis->yaw_);
+
+      chassis->ctrl_lock_.Wait(UINT32_MAX);
+      chassis->MotorSetAble();
+      chassis->UpdateFeedback();
+      chassis->Calculate();
+      chassis->Control();
+      chassis->ctrl_lock_.Post();
+
+      /* 运行结束，等待下一次唤醒 */
+      chassis->thread_.SleepUntil(3, last_online_time);
+    }
+  };
+  this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
+                       System::Thread::MEDIUM);
+}
+
+void Wheelleg::MotorSetAble() {
+  if (this->hip_motor_flag_ == 0) {
+    this->hip_motor_[0]->Relax();
+    this->hip_motor_[1]->Relax();
+    this->hip_motor_[2]->Relax();
+    this->hip_motor_[3]->Relax();
+    this->dm_motor_flag_ = 0;
+  }
+
+  else {
+    if (this->dm_motor_flag_ == 0) {
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[0]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[1]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[2]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[3]->Enable();
+      }
+
+      this->dm_motor_flag_ = 1;
+    }
+  };
+}
+
+void Wheelleg::UpdateFeedback() {
+  this->now_ = bsp_time_get();
+  this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
+  this->last_wakeup_ = this->now_;
+
+  this->wheel_motor_[0]->Update();
+  this->wheel_motor_[1]->Update();
+
+  this->leg_argu_[0].phi4_ =
+      this->hip_motor_[0]->GetAngle() -
+      this->param_.wheel_param.mech_zero[0];  // 前关节角度
+  this->leg_argu_[0].phi1_ =
+      this->hip_motor_[1]->GetAngle() -
+      this->param_.wheel_param.mech_zero[1];  // 后关节角度
+
+  this->leg_argu_[1].phi4_ =
+      (-this->hip_motor_[2]->GetAngle() +
+       this->param_.wheel_param.mech_zero[3]);  // 前关节角度
+  if (leg_argu_[1].phi4_ > M_PI) {
+    this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
+  }
+
+  this->leg_argu_[1].phi1_ =
+      (-this->hip_motor_[3]->GetAngle() +
+       this->param_.wheel_param.mech_zero[2]);  // 后关节角度
+  if (leg_argu_[1].phi1_ > M_PI) {
+    this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
+  }
+
+  this->pit_ = this->eulr_.pit;
+  if (this->pit_ > M_PI) {
+    this->pit_ = this->eulr_.pit - 2 * M_PI;
+  }
+
+  /*  注意极性 */
+  std::tuple<float, float, float, float> result0 =
+      this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[0].L0 = std::get<0>(result0);
+  this->leg_argu_[0].d_L0 = std::get<1>(result0);
+  this->leg_argu_[0].theta = -std::get<2>(result0);
+  this->leg_argu_[0].d_theta = -std::get<3>(result0);
+
+  if (leg_argu_[0].theta > M_PI) {
+    leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
+  }
+  if (leg_argu_[0].theta < -M_PI) {
+    leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
+  }
+
+  std::tuple<float, float, float, float> result1 =
+      this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[1].L0 = std::get<0>(result1);
+  this->leg_argu_[1].d_L0 = std::get<1>(result1);
+  this->leg_argu_[1].theta = -std::get<2>(result1);
+  this->leg_argu_[1].d_theta = -std::get<3>(result1);
+
+  if (leg_argu_[1].theta > M_PI) {
+    leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
+  }
+  if (leg_argu_[1].theta < -M_PI) {
+    leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
+  }
+
+  /* 轮子转速近似于编码器速度 由此推测机体速度 */
+  this->leg_argu_[0].single_x_dot =
+      -wheel_motor_[0]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+      leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
+
+  this->leg_argu_[1].single_x_dot =
+      wheel_motor_[1]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+      leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
+
+  this->move_argu_.last_x_dot = this->move_argu_.x_dot;
+  this->move_argu_.x_dot =
+
+      (this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
+  this->move_argu_.x_dot =
+      (-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
+      this->param_.wheel_param.wheel_radius / 60.f / 15.765;
+  this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
+
+  this->move_argu_.delta_speed =
+      lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
+
+  this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
+
+  if (now_ > 1000000) {
+    this->move_argu_.x_dot_hat =
+        manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
+                               this->move_argu_.last_x_dot, accl_.y * 9.8f,
+                               dt_) -
+        ((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+          leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
+         (leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+          leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
+            2;
+    this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
+  }
+}
+
+void Wheelleg::Calculate() {
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+      // if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
+      //   if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
+      //     move_argu_.target_dot_x -= 0.004;
+      //   }
+      //   if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
+      //     move_argu_.target_dot_x += 0.004;
+      //   }
+      // } else {
+      //   move_argu_.target_dot_x = cmd_.y * 1.5f;
+      // }
+      // move_argu_.target_x = move_argu_.target_dot_x * dt_ +
+      // move_argu_.target_x;
+      move_argu_.target_x = 0;
+      move_argu_.target_dot_x = 0;
+      break;
+    case Wheelleg::STAND:
+
+      /* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
+      if (fabs(move_argu_.target_dot_x -
+               cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
+        if (move_argu_.target_dot_x >
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x -= 0.004;
+        }
+        if (move_argu_.target_dot_x <
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x += 0.004;
+        }
+      } else {
+        move_argu_.target_dot_x =
+            cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+      }
+      move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+
+      this->move_argu_.target_yaw = 0.0f;
+
+      /*双零点*/
+      if (this->yaw_ > M_PI_2) {
+        this->move_argu_.target_yaw = 3.14158f;
+      }
+      if (this->yaw_ < -M_PI_2) {
+        this->move_argu_.target_yaw = 3.14158f;
+      }
+      break;
+
+    case Wheelleg::ROTOR:
+      if (fabs(move_argu_.target_dot_x -
+               cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
+        if (move_argu_.target_dot_x >
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x -= 0.004;
+        }
+        if (move_argu_.target_dot_x <
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x += 0.004;
+        }
+      } else {
+        move_argu_.target_dot_x =
+            cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+      }
+      move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+      this->move_argu_.target_yaw = this->yaw_ + 0.01;
+
+      break;
+      // move_argu_.target_dot_x = cmd_.x;
+      // move_argu_.target_x =
+      //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+      // // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
+      // cmd_.y);
+      // // move_argu_.x_dot =
+      // //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+      // // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
+      // break;
+
+    case Wheelleg::RESET:
+      this->move_argu_.target_dot_x = 0;
+      move_argu_.target_x = 0;
+      this->move_argu_.target_yaw = this->eulr_.yaw;
+      this->move_argu_.xhat = 0;
+
+      //     move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
+      // if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
+      //   this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
+      // }
+      break;
+
+    default:
+      XB_ASSERT(false);
+      return;
+  }
+
+  leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
+  onground0_flag_ = false;
+  if (leg_argu_[0].Fn > 30) {
+    onground0_flag_ = true;
+  }
+  leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
+  onground1_flag_ = false;
+  if (leg_argu_[1].Fn > 30) {
+    onground1_flag_ = true;
+  }
+  std::tuple<float, float> result3;
+  std::tuple<float, float> result4;
+
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+    case Wheelleg::ROTOR:
+    case Wheelleg::STAND:
+
+      for (int i = 0; i < 12; i++) {
+        leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
+            &this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
+            leg_argu_[0].L0);
+      }
+
+      for (int i = 0; i < 12; i++) {
+        leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
+            &this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
+            leg_argu_[1].L0);
+      }
+      if (now_ > 1000000)
+        if (fabs(move_argu_.target_dot_x) > 0.5 ||
+            fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
+            ((!onground0_flag_) & (!onground1_flag_))) {
+          leg_argu_[0].LQR_K[2] = 0;
+          leg_argu_[1].LQR_K[2] = 0;
+          this->move_argu_.xhat = 0;
+          move_argu_.target_x = 0;
+        }
+
+      if (onground0_flag_) {
+        leg_argu_[0].Tw =
+            (leg_argu_[0].LQR_K[0] *
+                 (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+             leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
+             leg_argu_[0].LQR_K[2] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[0].LQR_K[3] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
+             leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
+        leg_argu_[0].Tp =
+            (leg_argu_[0].LQR_K[6] *
+                 (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+             leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
+             leg_argu_[0].LQR_K[8] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[0].LQR_K[9] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
+             leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
+      } else {
+        leg_argu_[0].Tw = 0;
+
+        leg_argu_[0].Tp =
+            (leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
+             leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
+      }
+      if (onground1_flag_) {
+        leg_argu_[1].Tw =
+            (leg_argu_[1].LQR_K[0] *
+                 (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+             leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
+             leg_argu_[1].LQR_K[2] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[1].LQR_K[3] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
+             leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
+        leg_argu_[1].Tp =
+            (leg_argu_[1].LQR_K[6] *
+                 (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+             leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
+             leg_argu_[1].LQR_K[8] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[1].LQR_K[9] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
+             leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
+      } else {
+        leg_argu_[1].Tw = 0;
+        leg_argu_[1].Tp =
+            (leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
+             leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
+      }
+
+      this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                    this->param_.wheel_param.static_L0[0] +
+                                    +roll_pid_.Calculate(0, eulr_.rol, dt_);
+      this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                    this->param_.wheel_param.static_L0[1] -
+                                    roll_pid_.Calculate(0, eulr_.rol, dt_);
+
+      leg_argu_[0].F0 =
+          leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
+          this->param_.wheel_param.static_F0[0] +
+          leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
+                                          this->leg_argu_[0].L0, this->dt_);
+      leg_argu_[1].F0 =
+          leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
+          this->param_.wheel_param.static_F0[1] +
+          leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
+                                          this->leg_argu_[1].L0, this->dt_);
+
+      this->leg_argu_[0].Delta_Tp =
+          leg_argu_[0].L0 * 10.0f *
+          tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                   this->leg_argu_[0].theta, this->dt_);
+      this->leg_argu_[1].Delta_Tp =
+          -leg_argu_[1].L0 * 10.0f *
+          tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                   this->leg_argu_[0].theta, this->dt_);
+
+      result3 = leg_[0]->VMCinserve(
+          -leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+          -leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
+      this->leg_argu_[0].T1 = std::get<0>(result3);
+      this->leg_argu_[0].T2 = std::get<1>(result3);
+
+      result4 = leg_[1]->VMCinserve(
+          -leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+          -leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
+      this->leg_argu_[1].T1 = -std::get<0>(result4);
+      this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+      if (onground0_flag_ & onground1_flag_) {
+        move_argu_.yaw_force =
+            -this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
+      } else {
+        move_argu_.yaw_force = 0;
+      }
+      /*3508不带减速箱是Tw*3.2f*/
+      /*2006带减速是Tw/1.8f*/
+      /* 3508带15.7减速箱是Tw/4.9256 */
+
+      this->wheel_motor_out_[0] =
+          this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
+
+      this->wheel_motor_out_[1] =
+          this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
+
+      this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+      this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+      this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+      this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+      this->hip_motor_flag_ = 1;
+      break;
+
+    case Wheelleg::RESET:
+      if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
+          fabs(leg_argu_[1].theta) > 1.57) {
+        leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+
+        if (fabs(pit_) > M_PI / 2) {
+          if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
+            leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
+          }
+          if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
+            leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
+          }
+          leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
+                                     0.65f, this->leg_argu_[0].L0, this->dt_);
+          leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
+                                     0.65f, this->leg_argu_[1].L0, this->dt_);
+        }
+        if (fabs(leg_argu_[0].theta) < 1.57) {
+          leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
+          leg_argu_[0].target_theta = leg_argu_[0].theta;
+        }
+
+        if (fabs(leg_argu_[1].theta) < 1.57) {
+          leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+          leg_argu_[1].target_theta = leg_argu_[1].theta;
+        }
+
+        if (leg_argu_[0].target_theta > M_PI) {
+          leg_argu_[0].target_theta -= 2 * M_PI;
+        }
+        if (leg_argu_[0].target_theta < -M_PI) {
+          leg_argu_[0].target_theta += 2 * M_PI;
+        }
+        if (leg_argu_[1].target_theta > M_PI) {
+          leg_argu_[1].target_theta -= 2 * M_PI;
+        }
+        if (leg_argu_[1].target_theta < -M_PI) {
+          leg_argu_[1].target_theta += 2 * M_PI;
+        }
+        leg_argu_[0].Tp =
+            500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
+                                                 leg_argu_[0].theta, dt_);
+        leg_argu_[1].Tp =
+            500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
+                                                 leg_argu_[1].theta, dt_);
+
+      } else {
+        leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
+                                  0.10f, this->leg_argu_[0].L0, this->dt_);
+        leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
+                                  0.10f, this->leg_argu_[1].L0, this->dt_);
+
+        if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
+          leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
+                                      0.1, leg_argu_[0].theta, dt_);
+          leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
+                                      0.1, leg_argu_[1].theta, dt_);
+          clampf(&leg_argu_[0].Tp, -10, 10);
+          clampf(&leg_argu_[1].Tp, -10, 10);
+        } else {
+          leg_argu_[0].Tp = 0;
+          leg_argu_[1].Tp = 0;
+        }
+      }
+
+      result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                                    -leg_argu_[0].Tp, leg_argu_[0].F0);
+      this->leg_argu_[0].T1 = std::get<0>(result3);
+      this->leg_argu_[0].T2 = std::get<1>(result3);
+
+      result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                                    -leg_argu_[1].Tp, leg_argu_[1].F0);
+      this->leg_argu_[1].T1 = -std::get<0>(result4);
+      this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+      this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+      this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+      this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+      this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+      this->hip_motor_flag_ = 1;
+      break;
+  }
+}
+
+void Wheelleg::Control() {
+  clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
+  clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
+  clampf(&hip_motor_out_[0], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[1], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[2], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[3], -20.0f, 20.0f);
+
+  // if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
+  //     fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
+  //   wheel_motor_out_[0] = 0;
+  //   wheel_motor_out_[1] = 0;
+
+  //   if (fabs(this->pit_) > 0.5f) {
+  //     this->hip_motor_flag_ = 0;
+  //   }
+  // }
+
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+
+      this->wheel_motor_[0]->Relax();
+      this->wheel_motor_[1]->Relax();
+      hip_motor_flag_ = 0;
+      hip_motor_[0]->SetMit(0.0f);
+      hip_motor_[1]->SetMit(0.0f);
+      hip_motor_[2]->SetMit(0.0f);
+      hip_motor_[3]->SetMit(0.0f);
+      break;
+
+    case Wheelleg::STAND:
+    case Wheelleg::ROTOR:
+      // this->wheel_motor_[0]->Relax();
+      // this->wheel_motor_[1]->Relax();
+      this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
+      this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
+      hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+      hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+      hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+      // hip_motor_[0]->SetMit(0.0f);
+      // hip_motor_[1]->SetMit(0.0f);
+      // hip_motor_[2]->SetMit(0.0f);
+      // hip_motor_[3]->SetMit(0.0f);
+      break;
+
+    case Wheelleg::RESET:
+
+      this->wheel_motor_[0]->Relax();
+      this->wheel_motor_[1]->Relax();
+
+      hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+      hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+
+      hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+
+      break;
+  }
+}
+
+void Wheelleg::SetMode(Wheelleg::Mode mode) {
+  if (mode == this->mode_) {
+    return;
+  }
+
+  this->leg_[0]->Reset();
+  this->leg_[1]->Reset();
+  move_argu_.x = 0;
+  move_argu_.x_dot = 0;
+  move_argu_.last_x_dot = 0;
+  move_argu_.target_x = move_argu_.xhat;
+  move_argu_.target_yaw = this->eulr_.yaw;
+  move_argu_.target_dot_x = 0;
+  move_argu_.xhat = 0;
+  move_argu_.x_dot_hat = 0;
+  this->manfilter_.reset_comp();
+  this->mode_ = mode;
+}
diff --git a/User/task/rc.c b/User/task/rc.c
index 89b1705..bcbc569 100644
--- a/User/task/rc.c
+++ b/User/task/rc.c
@@ -60,7 +60,7 @@ void Task_rc(void *argument) {
           // 将遥控器通道值从[364, 1684]映射到[-1.0, 1.0]
           float ch_l_y_norm = (float)(dr16.data.ch_l_y - 1024) / 660.0f; // 前后
           float ch_l_x_norm = (float)(dr16.data.ch_l_x - 1024) / 660.0f; // 左右
-          float ch_r_x_norm = (float)(dr16.data.ch_r_x - 1024) / 660.0f; // 旋转
+          float ch_r_x_norm = (float)(dr16.data.ch_r_y - 1024) / 660.0f; // 旋转
           
           // 设置运动向量（根据需要调整增益）
           cmd_to_chassis.move_vec.vx = ch_l_y_norm * 2.0f;  // 前后运动，增益可调
@@ -68,7 +68,7 @@ void Task_rc(void *argument) {
           cmd_to_chassis.move_vec.wz = ch_r_x_norm * 3.0f;  // 旋转运动，增益可调
           
           // 设置目标高度（可根据右拨杆或其他输入调整）
-          cmd_to_chassis.height = 0.0f;
+          cmd_to_chassis.height = dr16.data.res-1024; // 目标高度，范围[-1024, 1024]，可根据需要调整比例
           
           // 发送命令到底盘控制任务
           osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_to_chassis, 0, 0);
diff --git a/some_functions.c b/some_functions.c
index 5dbbd5b..8af2891 100644
--- a/some_functions.c
+++ b/some_functions.c
@@ -20,81 +20,10 @@
 	(ptr)->real_torque = uint_to_float((uint16_t)(((data[4] & 0x0f) << 8) | (data)[5]), -18, +18, 12); \
 }	
 
-#define get_BM_motor_measure(ptr, data)                           	\
-{                                                                   \
-	(ptr)->last_ecd = (ptr)->ecd;                                   \
-	(ptr)->ecd = (uint16_t)((data)[4] << 8 | (data)[5]);            \
-	(ptr)->speed_rpm = (int16_t)((data)[0] << 8 | (data)[1]);      \
-	(ptr)->given_current = (int16_t)((data)[2] << 8 | (data)[3]);  \
-	(ptr)->error = (data)[6]; \
-	(ptr)->mode =  (data)[7];                                   \
-}
 
-void CAN_BM_ENABLE_CMD(void)
-{
-	CAN_TxHeaderTypeDef	bm_tx_measure;
-	uint8_t				bm_can_send_data[8];
 
-	uint32_t send_mail_box;
-	bm_tx_measure.StdId = 0x105; // 120
-	bm_tx_measure.IDE = CAN_ID_STD;
-	bm_tx_measure.RTR = CAN_RTR_DATA;
-	bm_tx_measure.DLC = 0x08;
-	bm_can_send_data[0] = 0x0A;
-	bm_can_send_data[1] = 0x0A;
-	bm_can_send_data[2] = 0x00;
-	bm_can_send_data[3] = 0x00;
-	bm_can_send_data[4] = 0x00;
-	bm_can_send_data[5] = 0x00;
-	bm_can_send_data[6] = 0x00;
-	bm_can_send_data[7] = 0x00;
 
-	HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
-}
 
-void CAN_BM_MODE_SET_CMD(void)
-{
-	CAN_TxHeaderTypeDef	bm_tx_measure;
-	uint8_t				bm_can_send_data[8];
-
-	uint32_t send_mail_box;
-	bm_tx_measure.StdId = 0x106; // 120
-	bm_tx_measure.IDE = CAN_ID_STD;
-	bm_tx_measure.RTR = CAN_RTR_DATA;
-	bm_tx_measure.DLC = 0x08;
-	bm_can_send_data[0] = 0x01;
-	bm_can_send_data[1] = 0x01;
-	bm_can_send_data[2] = 0x00;
-	bm_can_send_data[3] = 0x00;
-	bm_can_send_data[4] = 0x00;
-	bm_can_send_data[5] = 0x00;
-	bm_can_send_data[6] = 0x00;
-	bm_can_send_data[7] = 0x00;
-
-	HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
-}
-
-void CAN_BM_CONTROL_CMD( int16_t motor1, int16_t motor2 )
-{
-	CAN_TxHeaderTypeDef	bm_tx_measure;
-	uint8_t				bm_can_send_data[8];
-
-	uint32_t send_mail_box;
-	bm_tx_measure.StdId = 0x32; // 120
-	bm_tx_measure.IDE = CAN_ID_STD;
-	bm_tx_measure.RTR = CAN_RTR_DATA;
-	bm_tx_measure.DLC = 0x08;
-	bm_can_send_data[0] = motor1 >> 8;
-	bm_can_send_data[1] = motor1;
-	bm_can_send_data[2] = motor2 >> 8;
-	bm_can_send_data[3] = motor2;
-	bm_can_send_data[4] = 0x00;
-	bm_can_send_data[5] = 0x00;
-	bm_can_send_data[6] = 0x00;
-	bm_can_send_data[7] = 0x00;
-
-	HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
-}
 
 #define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
 #define P_MIN -95.5f// Radians
@@ -108,68 +37,6 @@ void CAN_BM_CONTROL_CMD( int16_t motor1, int16_t motor2 )
 #define T_MIN -18.0f
 #define T_MAX 18.0f
 
-void CAN_HT_CMD( uint8_t id, fp32 f_t )
-{
-    uint32_t canTxMailbox = CAN_TX_MAILBOX0;
-
-    fp32 f_p = 0.0f, f_v = 0.0f, f_kp = 0.0f, f_kd = 0.0f;
-    uint16_t p, v, kp, kd, t;
-    uint8_t buf[8];
-    LIMIT_MIN_MAX(f_p,  P_MIN,  P_MAX);
-    LIMIT_MIN_MAX(f_v,  V_MIN,  V_MAX);
-    LIMIT_MIN_MAX(f_kp, KP_MIN, KP_MAX);
-    LIMIT_MIN_MAX(f_kd, KD_MIN, KD_MAX);
-    LIMIT_MIN_MAX(f_t,  T_MIN,  T_MAX);
-    
-    p = float_to_uint(f_p,      P_MIN,  P_MAX,  16);            
-    v = float_to_uint(f_v,      V_MIN,  V_MAX,  12);
-    kp = float_to_uint(f_kp,    KP_MIN, KP_MAX, 12);
-    kd = float_to_uint(f_kd,    KD_MIN, KD_MAX, 12);
-    t = float_to_uint(f_t,      T_MIN,  T_MAX,  12);
-    
-    buf[0] = p>>8;
-    buf[1] = p&0xFF;
-    buf[2] = v>>4;
-    buf[3] = ((v&0xF)<<4)|(kp>>8);
-    buf[4] = kp&0xFF;
-    buf[5] = kd>>4;
-    buf[6] = ((kd&0xF)<<4)|(t>>8);
-    buf[7] = t&0xff;
-
-    chassis_tx_message.StdId = id;
-    chassis_tx_message.IDE = CAN_ID_STD;
-    chassis_tx_message.RTR = CAN_RTR_DATA;
-    chassis_tx_message.DLC = 0x08;
-
-		// while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
-		if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET) 
-		{canTxMailbox = CAN_TX_MAILBOX0;} 
-		else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET) 
-		{canTxMailbox = CAN_TX_MAILBOX1;} 
-		else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET) 
-		{canTxMailbox = CAN_TX_MAILBOX2;}
-		
-	if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, buf, (uint32_t *)&canTxMailbox)==HAL_OK){};
-}
-void CAN_CMD_HT_Enable(uint8_t id, uint8_t unterleib_motor_send_data[8] )
-{
-    uint32_t canTxMailbox= CAN_TX_MAILBOX0;
-
-    chassis_tx_message.StdId = id;
-    chassis_tx_message.IDE = CAN_ID_STD;
-    chassis_tx_message.RTR = CAN_RTR_DATA;
-    chassis_tx_message.DLC = 0x08;
-
-	// 	while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
-		if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET) 
-		{canTxMailbox = CAN_TX_MAILBOX0;} 
-		else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET) 
-		{canTxMailbox = CAN_TX_MAILBOX1;} 
-		else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET) 
-		{canTxMailbox = CAN_TX_MAILBOX2;}
-		
-	if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, unterleib_motor_send_data, (uint32_t *)&canTxMailbox)==HAL_OK){};
-}
 
 void Forward_kinematic_solution(chassis_control_t *feedback_update,
 											fp32 Q1,fp32 S1,fp32 Q4,fp32 S4, uint8_t ce)
diff --git a/utils/lqr.m b/utils/lqr.m
index 6dd76ae..6bb4ef9 100644
--- a/utils/lqr.m
+++ b/utils/lqr.m
@@ -105,27 +105,27 @@ g_ac = 9.81;
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-R_w_ac = 0.9;                           % 驱动轮半径                  （单位：m）
-R_l_ac = 0.25;                            % 两个驱动轮之间距离/2         （单位：m）
-l_c_ac = 0.037;                            % 机体质心到腿部关节中心点距离  （单位：m）
-m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542;    % 驱动轮质量 腿部质量 机体质量  （单位：kg）
-I_w_ac = (3510000)*10^(-7);                            % 驱动轮转动惯量               （单位：kg m^2）
-I_b_ac = 0.260;                            % 机体转动惯量(自然坐标系法向)  （单位：kg m^2）
-I_z_ac = 0.226;                            % 机器人z轴转动惯量            （单位：kg m^2）
+R_w_ac = 0.75;                           % 驱动轮半径                  （单位：m）
+R_l_ac = 0.215;                            % 两个驱动轮之间距离/2         （单位：m）
+l_c_ac = 0.025;                            % 机体质心到腿部关节中心点距离  （单位：m）
+m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 14.542;    % 驱动轮质量 腿部质量 机体质量  （单位：kg）
+I_w_ac = (7630000)*10^(-7);                            % 驱动轮转动惯量               （单位：kg m^2）
+I_b_ac = 0.3470;                            % 机体转动惯量(自然坐标系法向)  （单位：kg m^2）
+I_z_ac = 0.322;                            % 机器人z轴转动惯量            （单位：kg m^2）
 
 %%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
 
 % 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
 % 或者点击左侧数字"224"让程序只运行行之前的内容并停止
 
-l_l_ac = 0.18;        % 左腿摆杆长度                      （左腿对应数据）  （单位：m）
-l_wl_ac = 0.05;       % 左驱动轮质心到左腿摆杆质心距离                      （单位：m）
-l_bl_ac = 0.13;       % 机体转轴到左腿摆杆质心距离                          （单位：m）
-I_ll_ac = 0.02054500;      % 左腿摆杆转动惯量                                   （单位：kg m^2）
-l_r_ac = 0.18;        % 右腿摆杆长度                      （右腿对应数据）  （单位：m）
-l_wr_ac = 0.05;       % 右驱动轮质心到右腿摆杆质心距离                      （单位：m）
-l_br_ac = 0.13;       % 机体转轴到右腿摆杆质心距离                          （单位：m）
-I_lr_ac = 0.02054500;      % 右腿摆杆转动惯量                                   （单位：kg m^2）
+l_l_ac = 0.16;        % 左腿摆杆长度                      （左腿对应数据）  （单位：m）
+l_wl_ac = 0.10;       % 左驱动轮质心到左腿摆杆质心距离                      （单位：m）
+l_bl_ac = 0.8;       % 机体转轴到左腿摆杆质心距离                          （单位：m）
+I_ll_ac = 0.01186;      % 左腿摆杆转动惯量                                   （单位：kg m^2）
+l_r_ac = 0.16;        % 右腿摆杆长度                      （右腿对应数据）  （单位：m）
+l_wr_ac = 0.10;       % 右驱动轮质心到右腿摆杆质心距离                      （单位：m）
+l_br_ac = 0.8;       % 机体转轴到右腿摆杆质心距离                          （单位：m）
+I_lr_ac = 0.01186;      % 右腿摆杆转动惯量                                   （单位：kg m^2）
 
 % 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
 % 两侧两个关节电机之间的中间点相连所形成的轴
diff --git a/utils/matlab/LQR_calc.m b/utils/matlab/LQR_calc.m
new file mode 100644
index 0000000..0a56cef
--- /dev/null
+++ b/utils/matlab/LQR_calc.m
@@ -0,0 +1,86 @@
+clear;
+clc;
+
+syms x xd xdd     T Tp    thetadd thetad theta    phidd phid phi    P N PM NM    L LM;
+
+%% 参数设定
+% 均为标准单位制
+g = 9.81;  
+
+% 驱动轮
+R = 0.075;          %轮子半径
+mw = 0.58;          %轮子质量
+Iw = 0.00823;       %轮子转动惯量
+
+% 大腿
+l_active_leg = 0.14;
+m_active_leg = 0.174;
+% 小腿
+l_slave_leg = 0.24;
+m_slave_leg = 0.180;
+% 关节间距
+joint_distance = 0.11;
+% 摆杆
+mp = (m_active_leg + m_slave_leg)*2 + 0.728;  % 需要加上定子质量
+Ip = mp*L^2/3;  %摆杆转动惯量
+
+% 机体
+M = 12;        %机体重量
+l = -0.014;    %机体质心到关节电机转轴的距离
+IM = 0.124;
+
+%% 经典力学方程
+fu1=N-NM==mp*(xdd+L*(thetadd*cos(theta)-thetad*thetad*sin(theta)));
+fu2=P-PM-mp*g==mp*L*(-thetadd*sin(theta)-thetad*thetad*cos(theta));
+fu3=NM==M*(xdd+(L+LM)*(thetadd*cos(theta)-thetad*thetad*sin(theta))-l*(phidd*cos(phi)-phid*phid*sin(phi)));
+fu4=PM-M*g==M*((L+LM)*(-thetadd*sin(theta)-thetad*thetad*cos(theta))+l*(-phidd*sin(phi)-phid*phid*cos(phi)));
+
+%% 不同部件之间的力求解
+[N,NM,P,PM]=solve(fu1,fu2,fu3,fu4,N,NM,P,PM);
+f1=xdd==(T-N*R)/(Iw/R+mw*R);
+f2=Ip*thetadd==(P*L+PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+f3=IM*phidd==Tp+NM*l*cos(phi)+PM*l*sin(phi);
+[xdd,thetadd,phidd]=solve(f1,f2,f3,xdd,thetadd,phidd);
+
+%% 计算雅可比矩阵A and B
+func=[thetad,thetadd,xd,xdd,phid,phidd];
+A_lin_model=jacobian(func,[theta,thetad,x,xd,phi,phid]);
+temp_A=subs(A_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
+final_A=simplify(temp_A);
+
+B_lin_model=jacobian(func,[T Tp]);
+temp_B=subs(B_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
+final_B=simplify(temp_B);
+
+%% 计算不同腿长下LQR增益K
+L_var = 0.1;   % 腿质心到机体转轴距离
+
+Q_mat = diag([1,1,500,100,5000,1]);
+R_mat = diag([1,0.25]);
+K = zeros(20,12);
+leg_len = zeros(20,1);
+
+for i=1:20
+    L_var = L_var + 0.005;
+    leg_len(i) = L_var*2;
+    A = double(subs(final_A, [L LM], [L_var L_var]));
+    B = double(subs(final_B, [L LM], [L_var L_var]));
+    KK = lqrd(A, B, Q_mat, R_mat, 0.001);
+    KK_t=KK.';
+    K(i,:)=KK_t(:);
+end
+
+%% 不同腿长下二项式拟合K
+K_cons=zeros(12,3);  
+
+for i=1:12
+    res=fit(leg_len,K(:,i),"poly2");
+    K_cons(i,:)=[res.p1, res.p2, res.p3];
+end
+
+for j=1:12
+    for i=1:3
+        fprintf("%f,",K_cons(j,i));
+    end
+    fprintf("\n");
+end
\ No newline at end of file
diff --git a/utils/matlab/UseBodyVel.m b/utils/matlab/UseBodyVel.m
new file mode 100644
index 0000000..0d7df2f
--- /dev/null
+++ b/utils/matlab/UseBodyVel.m
@@ -0,0 +1,103 @@
+clear;
+clc;
+syms theta phi L x x_b N N_f T T_p  M N_M P_M L_M
+syms theta_dot x_dot phi_dot theta_ddot x_ddot phi_ddot
+syms x_b_dot x_b_ddot
+
+%% 参数设定
+% 均为标准单位制
+g = 9.81;           %重力加速度
+
+% 驱动轮
+mw = 0.58;          %轮子质量
+R = 0.075;          %轮子半径
+Iw = 0.00823;       %轮子转动惯量
+
+% 大腿
+l_active_leg = 0.14;
+m_active_leg = 0.174;
+% 小腿
+l_slave_leg = 0.24;
+m_slave_leg = 0.180;
+% 关节间距
+joint_distance = 0.11;
+% 摆杆
+mp = (m_active_leg + m_slave_leg)*2 + 0.728;
+Ip = mp*L^2/3;   % 摆杆转动惯量
+
+% 机体
+M = 12;         %机体重量
+IM = 0.124;     %机体惯量,绕质心
+l = -0.014;     %机体质心到电机转轴的距离
+
+% QR设置为相同的权重
+Q_cost = diag([1,1,500,100,5000,1]);
+R_cost = diag([1,0.25]);
+useBodyVelocity = 1;
+
+%% 经典力学方程
+if useBodyVelocity
+    x_ddot = x_b_ddot - (L+L_M)*cos(theta)*theta_ddot+ (L+L_M)*sin(theta)*theta_dot^2;
+end
+N_M = M*(x_ddot+(L+L_M)*theta_ddot*cos(theta)-(L+L_M)*theta_dot^2*sin(theta)-l*phi_ddot*cos(phi)+l*phi_dot^2*sin(phi));
+P_M = M*(g-(L+L_M)*theta_ddot*sin(theta)-(L+L_M)*theta_dot^2*cos(theta)-l*phi_ddot*sin(phi)-l*phi_dot^2*cos(phi));
+N = mp*(x_ddot+L*theta_ddot*cos(theta)-L*theta_dot^2*sin(theta))+N_M;
+P = mp*(g-L*theta_dot^2*cos(theta)-L*theta_ddot*sin(theta))+P_M;
+
+eqA = x_ddot == (T-N*R)/(Iw/R+mw*R);
+eqB = Ip*theta_ddot == (P*L+P_M*L_M)*sin(theta)-(N*L+N_M*L_M)*cos(theta) - T + T_p;
+eqC = IM*phi_ddot == T_p + N_M*l*cos(phi) + P_M*l*sin(phi);
+
+%% 计算雅可比矩阵
+U = [T T_p].';
+
+if useBodyVelocity
+    model_sol = solve([eqA eqB eqC],[theta_ddot,x_b_ddot,phi_ddot]);
+    X = [theta,theta_dot,x_b,x_b_dot,phi,phi_dot].';
+    dX = [theta_dot,simplify(model_sol.theta_ddot),...
+        x_b_dot,simplify(model_sol.x_b_ddot),...
+        phi_dot,simplify(model_sol.phi_ddot)].';
+    A_sym = subs(jacobian(dX,X),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
+    B_sym = subs(jacobian(dX,U),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
+else
+    model_sol = solve([eqA eqB eqC],[theta_ddot,x_ddot,phi_ddot]);
+    X = [theta,theta_dot,x,x_dot,phi,phi_dot].';
+    dX = [theta_dot,simplify(model_sol.theta_ddot),...
+        x_dot,simplify(model_sol.x_ddot),...
+        phi_dot,simplify(model_sol.phi_ddot)].';
+    A_sym = subs(jacobian(dX,X),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
+    B_sym = subs(jacobian(dX,U),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
+end
+
+%% 计算变长度LQR
+L_var = 0.1;   % 腿质心到机体转轴距离
+
+K=zeros(20,12);
+leglen=zeros(20,1);
+
+for i=1:20
+    L_var=L_var+0.005; % 10mm线性化一次
+    leglen(i)=L_var*2;
+    trans_A=double(subs(A_sym,[L L_M],[L_var L_var]));
+    trans_B=double(subs(B_sym,[L L_M],[L_var L_var]));
+    KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001);
+    KK_t=KK.';
+    K(i,:)=KK_t(:);
+end
+
+%% 用二项式拟合K,一共12个参数
+K_cons=zeros(12,3);
+
+for i=1:12
+    res=fit(leglen,K(:,i),"poly2");
+    K_cons(i,:)=[res.p1,res.p2,res.p3];
+end
+
+for j=1:12
+    for i=1:3
+        fprintf("%f,",K_cons(j,i));
+    end
+    fprintf("\n");
+end
+
+
diff --git a/utils/matlab/vmc.m b/utils/matlab/vmc.m
new file mode 100644
index 0000000..b3f73da
--- /dev/null
+++ b/utils/matlab/vmc.m
@@ -0,0 +1,39 @@
+clear;
+clc;
+
+syms phi1(t) phi2(t) phi3(t) phi4(t) l5 phi0 L0 T_Leg F_Leg
+syms phi_dot_1 phi_dot_4
+syms l1 l2 l3 l4
+
+%% 腿长解算
+x_B = l1*cos(phi1);
+y_B = l1*sin(phi1);
+x_C = x_B+l2*cos(phi2);
+y_C = y_B+l2*sin(phi2);
+x_D = l5+l4*cos(phi4);
+y_D = l4*sin(phi4);
+x_dot_B = diff(x_B,t);
+y_dot_B = diff(y_B,t);
+x_dot_C = diff(x_C,t);
+y_dot_C = diff(y_C,t);
+x_dot_D = diff(x_D,t);
+y_dot_D = diff(y_D,t);
+
+%% 速度导数求解
+phi_dot_2 = ((x_dot_D-x_dot_B)*cos(phi3)+(y_dot_D-y_dot_B)*sin(phi3))/l2/sin(phi3-phi2);
+
+x_dot_C = subs(x_dot_C,diff(phi2,t),phi_dot_2);
+x_dot_C = subs(x_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
+y_dot_C = subs(y_dot_C,diff(phi2,t),phi_dot_2);
+y_dot_C = subs(y_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
+
+%% 运动映射(虚功原理)+极坐标转换
+x_dot = [x_dot_C; y_dot_C];
+q_dot = [phi_dot_1; phi_dot_4];
+x_dot = collect(simplify(collect(x_dot,q_dot)),q_dot);
+J = simplify(jacobian(x_dot,q_dot));
+rotate = [cos(phi0-pi/2) -sin(phi0-pi/2);
+          sin(phi0-pi/2)  cos(phi0-pi/2)];
+map = [0 -1/L0;
+       1 0];
+Trans_Jacobian = simplify(J.'*rotate*map);
diff --git a/utils/matlab/wheel_leg.slx b/utils/matlab/wheel_leg.slx
new file mode 100644
index 0000000..981433e
Binary files /dev/null and b/utils/matlab/wheel_leg.slx differ
diff --git a/utils/matlab/wheel_leg.slxc b/utils/matlab/wheel_leg.slxc
new file mode 100644
index 0000000..854b3f3
Binary files /dev/null and b/utils/matlab/wheel_leg.slxc differ
diff --git a/utils/vmc.m b/utils/vmc.m
new file mode 100644
index 0000000..e116f34
--- /dev/null
+++ b/utils/vmc.m
@@ -0,0 +1,219 @@
+function [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual)
+% VMC (Virtual Model Control) for serial leg mechanism
+% 基于虚拟模型控制的串联腿机构力矩计算
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m) 
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+%   F_virtual: 虚拟力大小 (N)
+%   theta_virtual: 虚拟力方向角度 (rad)
+%
+% 输出:
+%   tau_hip: 髋关节所需力矩 (N*m)
+%   tau_knee: 膝关节所需力矩 (N*m)
+
+    % 计算雅可比矩阵
+    J = compute_jacobian(L1, L2, q1, q2);
+    
+    % 虚拟力向量 (笛卡尔空间)
+    F_cartesian = [F_virtual * cos(theta_virtual);
+                   F_virtual * sin(theta_virtual)];
+    
+    % 通过雅可比转置将笛卡尔力转换为关节力矩
+    tau = J' * F_cartesian;
+    
+    tau_hip = tau(1);
+    tau_knee = tau(2);
+end
+
+function J = compute_jacobian(L1, L2, q1, q2)
+% 计算串联腿机构的雅可比矩阵
+% 从关节空间到足端笛卡尔空间的映射
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+%
+% 输出:
+%   J: 2x2雅可比矩阵
+
+    % 足端位置 (相对于髋关节)
+    % x = L1*cos(q1) + L2*cos(q1+q2)
+    % y = L1*sin(q1) + L2*sin(q1+q2)
+    
+    % 雅可比矩阵元素
+    J11 = -L1*sin(q1) - L2*sin(q1+q2);  % dx/dq1
+    J12 = -L2*sin(q1+q2);               % dx/dq2
+    J21 = L1*cos(q1) + L2*cos(q1+q2);   % dy/dq1
+    J22 = L2*cos(q1+q2);                % dy/dq2
+    
+    J = [J11, J12;
+         J21, J22];
+end
+
+function [x, y] = forward_kinematics(L1, L2, q1, q2)
+% 串联腿正运动学 - 足端位置
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+%
+% 输出:
+%   x: 足端x坐标 (m)
+%   y: 足端y坐标 (m)
+
+    x = L1*cos(q1) + L2*cos(q1+q2);
+    y = L1*sin(q1) + L2*sin(q1+q2);
+end
+
+function [q1, q2] = inverse_kinematics(L1, L2, x, y)
+% 串联腿逆运动学 - 关节角度
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   x: 足端目标x坐标 (m)
+%   y: 足端目标y坐标 (m)
+%
+% 输出:
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+
+    % 计算到足端的距离
+    r = sqrt(x^2 + y^2);
+    
+    % 检查工作空间
+    if r > (L1 + L2) || r < abs(L1 - L2)
+        error('目标位置超出工作空间范围');
+    end
+    
+    % 使用余弦定理计算膝关节角度
+    cos_q2 = (r^2 - L1^2 - L2^2) / (2*L1*L2);
+    q2 = acos(cos_q2);  % 肘部向上的解
+    
+    % 计算髋关节角度
+    alpha = atan2(y, x);
+    beta = atan2(L2*sin(q2), L1 + L2*cos(q2));
+    q1 = alpha - beta;
+end
+
+function [angle_equiv, length_equiv] = serial_to_virtual_leg(L1, L2, q1, q2)
+% 将串联腿转换为等效摆动杆表示 (对应C代码中的正运动学)
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+%
+% 输出:
+%   angle_equiv: 等效摆动杆角度 (rad)
+%   length_equiv: 等效摆动杆长度 (m)
+
+    % 对应C代码中的计算
+    q4 = (pi - q1 - q2) / 2;
+    
+    % 使用余弦定理求解等效摆动杆长度
+    a = 1.0;
+    b = -2.0 * L1 * cos(q4);
+    c = L1^2 - L2^2;
+    
+    discriminant = b^2 - 4*a*c;
+    
+    if discriminant < 0
+        error('无实数解，配置不可达');
+    end
+    
+    sqrt_discriminant = sqrt(discriminant);
+    L3_1 = (-b + sqrt_discriminant) / (2*a);
+    L3_2 = (-b - sqrt_discriminant) / (2*a);
+    
+    % 选择正的解
+    if L3_1 > 0 && L3_2 > 0
+        L3 = min(L3_1, L3_2);  % 选择较小的解
+    elseif L3_1 > 0
+        L3 = L3_1;
+    elseif L3_2 > 0
+        L3 = L3_2;
+    else
+        error('无正解');
+    end
+    
+    angle_equiv = q1 + q4;
+    length_equiv = L3;
+end
+
+function [q1, q2] = virtual_to_serial_leg(L1, L2, angle_equiv, length_equiv)
+% 将等效摆动杆转换为串联腿表示 (对应C代码中的逆运动学)
+%
+% 输入参数:
+%   L1: 大腿长度 (m)
+%   L2: 小腿长度 (m)
+%   angle_equiv: 等效摆动杆角度 (rad)
+%   length_equiv: 等效摆动杆长度 (m)
+%
+% 输出:
+%   q1: 髋关节角度 (rad)
+%   q2: 膝关节角度 (rad)
+
+    h = length_equiv;
+    q = angle_equiv;
+    
+    % 计算cos(q4)
+    cos_q4 = (L1^2 + h^2 - L2^2) / (2.0 * L1 * h);
+    
+    % 检查范围
+    if cos_q4 < -1.0 || cos_q4 > 1.0
+        error('不可达配置');
+    end
+    
+    q4 = acos(cos_q4);
+    
+    % 计算关节角度
+    q1 = q - q4;
+    q2 = pi - 2.0 * q4 - q1;
+end
+
+% 示例使用函数
+function demo_vmc_control()
+% VMC控制示例
+    
+    % 腿部参数
+    L1 = 0.15;  % 大腿长度 15cm
+    L2 = 0.15;  % 小腿长度 15cm
+    
+    % 当前关节角度
+    q1 = pi/6;   % 30度
+    q2 = pi/3;   % 60度
+    
+    % 期望的虚拟力
+    F_virtual = 50;  % 50N
+    theta_virtual = -pi/2;  % 向下的力
+    
+    % 计算所需关节力矩
+    [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual);
+    
+    fprintf('髋关节所需力矩: %.3f N*m\n', tau_hip);
+    fprintf('膝关节所需力矩: %.3f N*m\n', tau_knee);
+    
+    % 显示雅可比矩阵
+    J = compute_jacobian(L1, L2, q1, q2);
+    fprintf('雅可比矩阵:\n');
+    disp(J);
+    
+    % 测试等效摆动杆转换
+    [angle_eq, length_eq] = serial_to_virtual_leg(L1, L2, q1, q2);
+    fprintf('等效摆动杆角度: %.3f rad (%.1f deg)\n', angle_eq, rad2deg(angle_eq));
+    fprintf('等效摆动杆长度: %.3f m\n', length_eq);
+    
+    % 验证逆变换
+    [q1_back, q2_back] = virtual_to_serial_leg(L1, L2, angle_eq, length_eq);
+    fprintf('验证 - 原始角度: q1=%.3f, q2=%.3f\n', q1, q2);
+    fprintf('验证 - 恢复角度: q1=%.3f, q2=%.3f\n', q1_back, q2_back);
+end
\ No newline at end of file