diff --git a/.gitignore b/.gitignore
index 880ac59..2269f9f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,3 @@
 build/
+.firecrawl/
 .DS_Store
diff --git a/BALANCE/balance.c b/BALANCE/balance.c
index ce1f598..7985c45 100644
--- a/BALANCE/balance.c
+++ b/BALANCE/balance.c
@@ -1,52 +1,108 @@
 #include "balance.h"
 
-int Time_count=0; //Time variable //计时变量
+int Time_count=0; //Time variable //锟斤拷时锟斤拷锟斤拷
 
 // Robot mode is wrong to detect flag bits
-//机器人模式是否出错检测标志位
+//锟斤拷锟斤拷锟斤拷模式锟角凤拷锟斤拷锟斤拷锟斤拷锟街疚�
 int robot_mode_check_flag=0; 
 
 short test_num;
 
-Encoder OriginalEncoder; //Encoder raw data //编码器原始数据
+Encoder OriginalEncoder; //Encoder raw data //锟斤拷锟斤拷锟斤拷原始锟斤拷锟斤拷
 
-u8 command_lost_count=0; //串口、CAN控制命令丢失时间计数，丢失1秒后停止控制
+u8 command_lost_count=0; //锟斤拷锟节★拷CAN锟斤拷锟斤拷锟斤拷锟筋丢失时锟斤拷锟斤拷锟斤拷锟斤拷锟绞�1锟斤拷锟酵Ｖ癸拷锟斤拷锟�
+
+/* Calibrated Ackermann steering model (manual push-test, motors disabled).
+   Maps signed path curvature kappa = 1/R [1/m, R at the rear-axle center] to
+   servo PWM. Sign convention: kappa > 0 -> right turn, kappa < 0 -> left turn.
+   Quadratic fit of measured (servo, 1/R) points, max residual ~12 PWM:
+       servo = AKM_C0 + AKM_C1*kappa + AKM_C2*kappa*kappa
+   NOTE: kappa is in 1/m. The raw calibration table listed 1/R in 1/cm; these
+   coefficients were fit after converting R from cm to m, so they must be fed
+   SI curvature (kappa = wz/Vx, both SI). Straight-ahead lands near 1656 PWM. */
+#define AKM_SERVO_C0        1656.373f
+#define AKM_SERVO_C1         140.548f
+#define AKM_SERVO_C2          (-7.654f)
+/* Steering range from the calibration table: PWM 1100 (left) .. 2000 (right). */
+#define AKM_SERVO_MIN       1100
+#define AKM_SERVO_MAX       2000
+/* Largest curvature the car can actually track: R_min ~= 0.30 m -> 3.33 /m. */
+#define AKM_KAPPA_MAX       3.331f
+
+/* Remote CH1 neutral pulse width [us]. The servo straight-ahead neutral is
+   SERVO_INIT (motor.h). The two differ, so the CH1 passthrough is shifted by
+   (SERVO_INIT - AKM_REMOTER_CH1_MID) to keep stick-center = wheels-straight. */
+#define AKM_REMOTER_CH1_MID 1500
+
+/* Linear steering map that honors the servo's real straight-ahead neutral
+   (SERVO_INIT) instead of the arithmetic midpoint of [MIN, MAX]. norm > 0 =
+   left -> toward AKM_SERVO_MIN; norm < 0 = right -> toward AKM_SERVO_MAX;
+   norm == 0 -> SERVO_INIT. Each side is scaled to its own end stop so the full
+   mechanical travel is used even though the neutral is off-center. */
+#if AKM_DIRECT_MAP || AKM_YAW_ASSIST
+static int Akm_Norm_To_Servo(float norm)
+{
+	float span, pwm;
+
+	norm = target_limit_float(norm, -1.0f, 1.0f);
+	span = (norm >= 0.0f) ? (float)(SERVO_INIT - AKM_SERVO_MIN)
+	                      : (float)(AKM_SERVO_MAX - SERVO_INIT);
+	pwm = (float)SERVO_INIT - norm * span;
+
+	return (int)(pwm + (pwm >= 0.0f ? 0.5f : -0.5f));
+}
+#endif
+
+/* Needed by Mode 0 (calibrated Ackermann) and Mode 2 (yaw-rate assist), i.e.
+   whenever direct-map is off and CH1 is not overriding the servo. Direct-map
+   (Mode 1) and the CH1 debug override never call it. */
+#if !AKM_DIRECT_MAP && !AKM_SERVO_DEBUG_REMOTE_CH1
+static int Akm_Curvature_To_Servo(float kappa)
+{
+	float pwm;
+
+	kappa = target_limit_float(kappa, -AKM_KAPPA_MAX, AKM_KAPPA_MAX);
+	pwm = AKM_SERVO_C0 + AKM_SERVO_C1 * kappa + AKM_SERVO_C2 * kappa * kappa;
+
+	return (int)(pwm + (pwm >= 0.0f ? 0.5f : -0.5f));
+}
+#endif
 /**************************************************************************
 Function: The inverse kinematics solution is used to calculate the target speed of each wheel according to the target speed of three axes
 Input   : X and Y, Z axis direction of the target movement speed
 Output  : none
-函数功能：运动学逆解，根据三轴目标速度计算各车轮目标转速
-入口参数：X和Y、Z轴方向的目标运动速度
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟剿讹拷学锟斤拷猓拷锟斤拷锟斤拷锟斤拷锟侥匡拷锟斤拷俣燃锟斤拷锟斤拷锟斤拷锟斤拷目锟斤拷转锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟絏锟斤拷Y锟斤拷Z锟结方锟斤拷锟侥匡拷锟斤拷硕锟斤拷俣锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Drive_Motor(float Vx,float Vy,float Vz)
 {
-		float amplitude=3.5; //Wheel target speed limit //车轮目标速度限幅
+		float amplitude=3.5; //Wheel target speed limit //锟斤拷锟斤拷目锟斤拷锟劫讹拷锟睫凤拷
 	
 	  //Speed smoothing is enabled when moving the omnidirectional trolley
-	  //全向移动小车才开启速度平滑处理
+	  //全锟斤拷锟狡讹拷小锟斤拷锟脚匡拷锟斤拷锟劫讹拷平锟斤拷锟斤拷锟斤拷
 	  if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
 		{
-			Smooth_control(Vx,Vy,Vz); //Smoothing the input speed //对输入速度进行平滑处理
+			Smooth_control(Vx,Vy,Vz); //Smoothing the input speed //锟斤拷锟斤拷锟斤拷锟劫度斤拷锟斤拷平锟斤拷锟斤拷锟斤拷
   
       //Get the smoothed data 
-			//获取平滑处理后的数据			
+			//锟斤拷取平锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�			
 			Vx=smooth_control.VX;     
 			Vy=smooth_control.VY;
 			Vz=smooth_control.VZ;
 		}
 		
 		//Mecanum wheel car
-	  //麦克纳姆轮小车
+	  //锟斤拷锟斤拷锟侥凤拷锟叫★拷锟�
 	  if (Car_Mode==Mec_Car) 
     {
-			//Inverse kinematics //运动学逆解
+			//Inverse kinematics //锟剿讹拷学锟斤拷锟�
 			MOTOR_A.Target   = +Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
 			MOTOR_B.Target   = -Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
 			MOTOR_C.Target   = +Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
 			MOTOR_D.Target   = -Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
 		
-			//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
+			//Wheel (motor) target speed limit //锟斤拷锟斤拷(锟斤拷锟�)目锟斤拷锟劫讹拷锟睫凤拷
 			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
 			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
 			MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude); 
@@ -54,89 +110,204 @@ void Drive_Motor(float Vx,float Vy,float Vz)
 		} 
 		
 		//Omni car
-		//全向轮小车
+		//全锟斤拷锟斤拷小锟斤拷
 		else if (Car_Mode==Omni_Car) 
 		{
-			//Inverse kinematics //运动学逆解
+			//Inverse kinematics //锟剿讹拷学锟斤拷锟�
 			MOTOR_A.Target   =   Vy + Omni_turn_radiaus*Vz;
 			MOTOR_B.Target   =  -X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz;
 			MOTOR_C.Target   =  +X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz;
 		
-			//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
+			//Wheel (motor) target speed limit //锟斤拷锟斤拷(锟斤拷锟�)目锟斤拷锟劫讹拷锟睫凤拷
 			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
 			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
 			MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude); 
-			MOTOR_D.Target=0;	//Out of use //没有使用到
+			MOTOR_D.Target=0;	//Out of use //没锟斤拷使锟矫碉拷
 		}
 		
 		//Ackermann structure car
-		//阿克曼小车
-		else if (Car_Mode==Akm_Car) 
+		//锟斤拷锟斤拷锟斤拷小锟斤拷
+		else if (Car_Mode==Akm_Car)
 		{
-			//Ackerman car specific related variables //阿克曼小车专用相关变量
-			float R, Ratio=636.56, AngleR, Angle_Servo;
-			
-			// For Ackerman small car, Vz represents the front wheel steering Angle
-			//对于阿克曼小车Vz代表右前轮转向角度
-			AngleR=Vz;
-			R=Axle_spacing/tan(AngleR)-0.5f*Wheel_spacing;
-			//R=Axle_spacing/tan(AngleR);
-			
-			// Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad
-			//前轮转向角度限幅(舵机控制前轮转向角度)，单位：rad
-			AngleR=target_limit_float(AngleR,-0.6f,0.6f);
-			
-			//Inverse kinematics //运动学逆解
-			if(AngleR!=0)
-			{
-				MOTOR_A.Target = Vx*(R-0.5f*Wheel_spacing)/R;
-				MOTOR_B.Target = Vx*(R+0.5f*Wheel_spacing)/R;			
-			}
-			else 
-			{
-				MOTOR_A.Target = Vx;
-				MOTOR_B.Target = Vx;
-			}
-			// The PWM value of the servo controls the steering Angle of the front wheel
-			//舵机PWM值，舵机控制前轮转向角度
+		#if AKM_DIRECT_MAP
+			// Direct passthrough mode (tuning/debug, not physically Ackermann):
+			//   - Vz is linearly mapped across the full servo travel, independent
+			//     of speed. Vz > 0 = left (ROS) -> AKM_SERVO_MIN (left end).
+			//   - Vx is sent to both drive wheels unchanged (no differential).
+			// The AKM_SERVO_DEBUG_REMOTE_CH1 override below still takes priority
+			// over this servo value.
+			// vz_norm > 0 (left) -> AKM_SERVO_MIN; 0 -> SERVO_INIT (straight).
+			float vz_norm = target_limit_float(Vz / AKM_DIRECT_VZ_FULL, -1.0f, 1.0f);
+			Servo = Akm_Norm_To_Servo(vz_norm);
 
-			Angle_Servo    =  -0.628f*pow(AngleR, 3) + 1.269f*pow(AngleR, 2) - 1.772f*AngleR + 1.573f;
-			Servo=SERVO_INIT + (Angle_Servo - 1.572f)*Ratio;
-			//	Servo=SERVO_INIT + (Angle_Servo)*Ratio;
-			//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
-			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
-			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); 
-			MOTOR_C.Target=0; //Out of use //没有使用到
-			MOTOR_D.Target=0; //Out of use //没有使用到
-			Servo=target_limit_int(Servo,800,2200);	//Servo PWM value limit //舵机PWM值限幅
+			MOTOR_A.Target = Vx;
+			MOTOR_B.Target = Vx;
+			MOTOR_C.Target = 0;
+			MOTOR_D.Target = 0;
+		#elif AKM_YAW_ASSIST
+			// Mode 2: DECOUPLED steering + IMU yaw-rate differential assist
+			// (simplified torque vectoring). This deliberately breaks the Ackermann
+			// w-v coupling that shrinks the steering angle at speed:
+			//
+			//   * Steering (w): the servo is a DIRECT map of Vz across the full
+			//     curvature range, INDEPENDENT of Vx (like Mode 1). So a large Vz
+			//     always yields a large front-wheel angle, even at high Vx.
+			//   * Drive (v): Vx sets the base wheel speed independently.
+			//   * Ackermann is used only as the FEEDFORWARD reference for the rear
+			//     differential; an IMU yaw-rate PI loop trims on top.
+			//
+			// Sign bookkeeping (fit domain, kappa>0 = right; ROS Vz>0 = left = CCW):
+			//   vz_norm  = clamp(Vz/AKM_DIRECT_VZ_FULL, +-1)
+			//   Servo    = linear FULL-TRAVEL map of vz_norm (SAME as Mode 1 /
+			//              direct map), NOT the calibration fit -- this is what
+			//              gives full steering authority at any speed.
+			//   kappa_cmd = -vz_norm * AKM_KAPPA_MAX is used only to build the
+			//              Ackermann feedforward reference for the rear diff.
+			// The yaw loop works in the ROS/geometric frame (r>0 = left).
+			static float yaw_integral = 0.0f;   // PI integrator state [m/s]
+			static float r_filt = 0.0f;         // low-pass filtered yaw rate [rad/s]
+			float vz_norm, kappa_cmd;
+			float r_ref, r_meas, e_r, dv_ff, dv_fb, dv, dv_max;
+
+			// Decoupled steering command: Vz -> normalized steering, NOT via Vx.
+			vz_norm   = target_limit_float(Vz / AKM_DIRECT_VZ_FULL, -1.0f, 1.0f);
+
+			// Servo = linear full-travel map, identical to Mode 1 (direct map),
+			// centered on SERVO_INIT. vz_norm > 0 (left) -> AKM_SERVO_MIN.
+			Servo = Akm_Norm_To_Servo(vz_norm);
+
+			// Ackermann feedforward reference yaw rate for the commanded steering.
+			// kappa_cmd maps full stick to the car's max trackable curvature.
+			// Back in the ROS/geometric frame: r_ref > 0 = left turn.
+			kappa_cmd = -vz_norm * AKM_KAPPA_MAX;
+			r_ref = -kappa_cmd * Vx;
+
+			// Measured yaw rate from the gyro, de-biased LSB -> rad/s, with an
+			// optional sign flip and a light first-order low-pass.
+			r_meas = AKM_GYRO_Z_SIGN * (float)gyro[2] / AKM_GYRO_Z_TO_RADPS;
+			r_filt += AKM_YAW_IMU_LPF * (r_meas - r_filt);
+
+			if(float_abs(Vx) < AKM_YAW_MIN_SPEED)
+			{
+				// Too slow for a meaningful yaw rate: freeze the loop, no assist.
+				yaw_integral = 0.0f;
+				dv = 0.0f;
+			}
+			else
+			{
+				e_r = r_ref - r_filt;
+
+				// Feedforward: alpha=1 reproduces the Mode-0 geometric differential
+				// exactly (dv = 0.5*track*|kappa|*Vx expressed via r_ref).
+				dv_ff = AKM_YAW_FF_ALPHA * 0.5f * Wheel_spacing * r_ref;
+
+				// PI feedback with rectangular integration at the fixed control
+				// period (Drive_Motor runs at CONTROL_FREQUENCY Hz).
+				yaw_integral += e_r * (1.0f / (float)CONTROL_FREQUENCY);
+				dv_fb = AKM_YAW_KP * e_r + AKM_YAW_KI * yaw_integral;
+
+				dv = dv_ff + dv_fb;
+
+				// Clamp the differential to a fraction of Vx and anti-windup: if the
+				// PI part alone saturates, roll the integrator back.
+				dv_max = AKM_YAW_MAX_DIFF_RATIO * float_abs(Vx);
+				if(dv > dv_max)
+				{
+					if(AKM_YAW_KI > 0.0f)
+						yaw_integral -= (dv - dv_max) / AKM_YAW_KI;
+					dv = dv_max;
+				}
+				else if(dv < -dv_max)
+				{
+					if(AKM_YAW_KI > 0.0f)
+						yaw_integral -= (dv + dv_max) / AKM_YAW_KI;
+					dv = -dv_max;
+				}
+			}
+
+			// r_ref > 0 (left) means the left wheel is inner (slower). This matches
+			// Mode 0's MOTOR_A = Vx*(1 + 0.5*track*kappa_fit) once dv_ff is expanded,
+			// because kappa_fit = -r_ref/Vx.
+			MOTOR_A.Target = Vx - dv; // left
+			MOTOR_B.Target = Vx + dv; // right
+			MOTOR_C.Target = 0;
+			MOTOR_D.Target = 0;
+		#else
+			// Inputs: Vx = rear-axle-center linear speed [m/s],
+			//         Vz = rotation speed wz about the turn center [rad/s].
+			// Both refer to the rear-axle center -- exactly the point the servo
+			// calibration measured R against -- so the geometric curvature is
+			//   kappa_geom = wz / v = Vz / Vx = 1/R_rear   [1/m]
+			// with the ROS sign convention: Vz > 0 = CCW = left turn.
+			// The calibration table / servo fit use the opposite sign (kappa > 0
+			// = right turn), so we negate to get the fit-domain curvature:
+			//   kappa_fit = -kappa_geom  -> Vz > 0 gives kappa_fit < 0 = left.
+			float kappa_geom, kappa_fit;
+
+			if(float_abs(Vx) > 0.001f)
+				/* Vx sign is drive direction, not steering direction. */
+				kappa_geom = Vz / float_abs(Vx);
+			else
+				kappa_geom = 0.0f; // Ackermann geometry cannot steer without forward motion.
+
+			kappa_fit = target_limit_float(-kappa_geom, -AKM_KAPPA_MAX, AKM_KAPPA_MAX);
+
+			// Rear-wheel differential about the rear-axle center, expressed in the
+			// fit-domain curvature. On a right turn (kappa_fit > 0) the turn center
+			// is to the right, so the right wheel is inner (slower) and the left
+			// wheel is outer (faster):
+			//   MOTOR_A (left)  = Vx*(1 + 0.5*track*kappa_fit)
+			//   MOTOR_B (right) = Vx*(1 - 0.5*track*kappa_fit)
+			MOTOR_A.Target = Vx * (1.0f + 0.5f * Wheel_spacing * kappa_fit);
+			MOTOR_B.Target = Vx * (1.0f - 0.5f * Wheel_spacing * kappa_fit);
+
+			// The PWM value of the servo controls the steering Angle of the front wheel
+			//锟斤拷锟絇WM值锟斤拷锟斤拷锟斤拷锟斤拷锟角帮拷锟阶拷锟角讹拷
+			Servo=Akm_Curvature_To_Servo(kappa_fit);
+		#endif /* AKM_DIRECT_MAP */
+
+			// Servo source override: when enabled, the remote CH1 drives the servo
+			// directly, taking priority over BOTH control laws above. The remote
+			// neutral (AKM_REMOTER_CH1_MID) is shifted onto the servo straight-ahead
+			// neutral (SERVO_INIT) so stick-center = wheels-straight.
+			#if AKM_SERVO_DEBUG_REMOTE_CH1
+			Servo = Remoter_Ch1 + (SERVO_INIT - AKM_REMOTER_CH1_MID);
+			#endif
+
+			//Wheel (motor) target speed limit //锟斤拷锟斤拷(锟斤拷锟�)目锟斤拷锟劫讹拷锟睫凤拷
+			MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
+			MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
+			MOTOR_C.Target=0; //Out of use //没锟斤拷使锟矫碉拷
+			MOTOR_D.Target=0; //Out of use //没锟斤拷使锟矫碉拷
+			Servo=target_limit_int(Servo,AKM_SERVO_MIN,AKM_SERVO_MAX);	//Servo PWM value limit //PWM值薹
 		}
-		
+
 		//Differential car
-		//差速小车
+		//锟斤拷锟斤拷小锟斤拷
 		else if (Car_Mode==Diff_Car) 
 		{
-			//Inverse kinematics //运动学逆解
-			MOTOR_A.Target  = Vx - Vz * Wheel_spacing / 2.0f; //计算出左轮的目标速度
-		  MOTOR_B.Target =  Vx + Vz * Wheel_spacing / 2.0f; //计算出右轮的目标速度
+			//Inverse kinematics //锟剿讹拷学锟斤拷锟�
+			MOTOR_A.Target  = Vx - Vz * Wheel_spacing / 2.0f; //锟斤拷锟斤拷锟斤拷锟斤拷值锟侥匡拷锟斤拷俣锟�
+		  MOTOR_B.Target =  Vx + Vz * Wheel_spacing / 2.0f; //锟斤拷锟斤拷锟斤拷锟斤拷值锟侥匡拷锟斤拷俣锟�
 			
-			//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
+			//Wheel (motor) target speed limit //锟斤拷锟斤拷(锟斤拷锟�)目锟斤拷锟劫讹拷锟睫凤拷
 		  MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); 
 	    MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); 
-			MOTOR_C.Target=0; //Out of use //没有使用到
-			MOTOR_D.Target=0; //Out of use //没有使用到
+			MOTOR_C.Target=0; //Out of use //没锟斤拷使锟矫碉拷
+			MOTOR_D.Target=0; //Out of use //没锟斤拷使锟矫碉拷
 		}
 		
 		//FourWheel car
-		//四驱车
+		//锟斤拷锟斤拷锟斤拷
 		else if(Car_Mode==FourWheel_Car) 
 		{	
-			//Inverse kinematics //运动学逆解
-			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //计算出左轮的目标速度
-			MOTOR_B.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //计算出左轮的目标速度
-			MOTOR_C.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //计算出右轮的目标速度
-			MOTOR_D.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //计算出右轮的目标速度
+			//Inverse kinematics //锟剿讹拷学锟斤拷锟�
+			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //锟斤拷锟斤拷锟斤拷锟斤拷值锟侥匡拷锟斤拷俣锟�
+			MOTOR_B.Target  = Vx - Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //锟斤拷锟斤拷锟斤拷锟斤拷值锟侥匡拷锟斤拷俣锟�
+			MOTOR_C.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //锟斤拷锟斤拷锟斤拷锟斤拷值锟侥匡拷锟斤拷俣锟�
+			MOTOR_D.Target  = Vx + Vz * (Wheel_spacing +  Axle_spacing) / 2.0f; //锟斤拷锟斤拷锟斤拷锟斤拷值锟侥匡拷锟斤拷俣锟�
 					
-			//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
+			//Wheel (motor) target speed limit //锟斤拷锟斤拷(锟斤拷锟�)目锟斤拷锟劫讹拷锟睫凤拷
 			MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); 
 			MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); 
 			MOTOR_C.Target=target_limit_float( MOTOR_C.Target,-amplitude,amplitude); 
@@ -144,95 +315,104 @@ void Drive_Motor(float Vx,float Vy,float Vz)
 		}
 		
 		//Tank Car
-		//履带车
+		//锟侥达拷锟斤拷
 		else if (Car_Mode==Tank_Car) 
 		{
-			//Inverse kinematics //运动学逆解
-			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing) / 2.0f;    //计算出左轮的目标速度
-		  MOTOR_B.Target =  Vx + Vz * (Wheel_spacing) / 2.0f;    //计算出右轮的目标速度
+			//Inverse kinematics //锟剿讹拷学锟斤拷锟�
+			MOTOR_A.Target  = Vx - Vz * (Wheel_spacing) / 2.0f;    //锟斤拷锟斤拷锟斤拷锟斤拷值锟侥匡拷锟斤拷俣锟�
+		  MOTOR_B.Target =  Vx + Vz * (Wheel_spacing) / 2.0f;    //锟斤拷锟斤拷锟斤拷锟斤拷值锟侥匡拷锟斤拷俣锟�
 			
-			//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
+			//Wheel (motor) target speed limit //锟斤拷锟斤拷(锟斤拷锟�)目锟斤拷锟劫讹拷锟睫凤拷
 		  MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude); 
 	    MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude); 
-			MOTOR_C.Target=0; //Out of use //没有使用到
-			MOTOR_D.Target=0; //Out of use //没有使用到
+			MOTOR_C.Target=0; //Out of use //没锟斤拷使锟矫碉拷
+			MOTOR_D.Target=0; //Out of use //没锟斤拷使锟矫碉拷
 		}
 }
 /**************************************************************************
 Function: FreerTOS task, core motion control task
 Input   : none
 Output  : none
-函数功能：FreeRTOS任务，核心运动控制任务
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷FreeRTOS锟斤拷锟今，猴拷锟斤拷锟剿讹拷锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Balance_task(void *pvParameters)
 { 
 	  u32 lastWakeTime = getSysTickCnt();
+	  u32 lastControlTime = lastWakeTime;
 
     while(1)
-    {	
-			// This task runs at a frequency of 100Hz (10ms control once)
-			//此任务以100Hz的频率运行（10ms控制一次）
-			vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ)); 
+    {
+			u32 currentControlTime;
+			float controlDt;
+
+			// Run the wheel-speed control loop at 200 Hz.
+			vTaskDelayUntil(&lastWakeTime, F2T(RATE_200_HZ));
+			currentControlTime = getSysTickCnt();
+			controlDt = (float)(currentControlTime - lastControlTime) /
+				(float)configTICK_RATE_HZ;
+			lastControlTime = currentControlTime;
+			// Avoid a large integral jump if the task is stalled while debugging.
+			controlDt = target_limit_float(controlDt, 0.001f, 0.050f);
 			
 			//Time count is no longer needed after 30 seconds
-			//时间计数，30秒后不再需要
-			if(Time_count<3000)Time_count++;
+			//时锟斤拷锟斤拷锟斤拷锟�30锟斤拷锟斤拷锟斤拷锟揭�
+			if(Time_count<(30*CONTROL_FREQUENCY))Time_count++;
 			
 			//Get the encoder data, that is, the real time wheel speed, 
 			//and convert to transposition international units
-			//获取编码器数据，即车轮实时速度，并转换位国际单位
+			//锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷锟捷ｏ拷锟斤拷锟斤拷锟斤拷实时锟劫度ｏ拷锟斤拷转锟斤拷位锟斤拷锟绞碉拷位
 			Get_Velocity_Form_Encoder();   
 			
-			if(Check==0) //If self-check mode is not enabled //如果没有启动自检模式
+			if(Check==0) //If self-check mode is not enabled //锟斤拷锟矫伙拷锟斤拷锟斤拷锟斤拷约锟侥Ｊ�
 			{
-//				command_lost_count++; //串口、CAN控制命令丢失时间计数，丢失1秒后停止控制
-//				if(command_lost_count>RATE_100_HZ && APP_ON_Flag==0 && Remote_ON_Flag==0 && PS2_ON_Flag==0) //不是APP、PS2、航模遥控模式，就是CAN、串口1、串口3控制模式
+//				command_lost_count++; //锟斤拷锟节★拷CAN锟斤拷锟斤拷锟斤拷锟筋丢失时锟斤拷锟斤拷锟斤拷锟斤拷锟绞�1锟斤拷锟酵Ｖ癸拷锟斤拷锟�
+//				if(command_lost_count>RATE_100_HZ && APP_ON_Flag==0 && Remote_ON_Flag==0 && PS2_ON_Flag==0) //锟斤拷锟斤拷APP锟斤拷PS2锟斤拷锟斤拷模遥锟斤拷模式锟斤拷锟斤拷锟斤拷CAN锟斤拷锟斤拷锟斤拷1锟斤拷锟斤拷锟斤拷3锟斤拷锟斤拷模式
 //					Move_X=0, Move_Y=0, Move_Z=0;
 
-				if      (APP_ON_Flag)      Get_RC();         //Handle the APP remote commands //处理APP遥控命令
-				else if (Remote_ON_Flag)   Remote_Control(); //Handle model aircraft remote commands //处理航模遥控命令
-				else if (PS2_ON_Flag)      PS2_control();    //Handle PS2 controller commands //处理PS2手柄控制命令
+				if      (APP_ON_Flag)      Get_RC();         //Handle the APP remote commands //锟斤拷锟斤拷APP遥锟斤拷锟斤拷锟斤拷
+				else if (Remote_ON_Flag)   Remote_Control(); //Handle model aircraft remote commands //锟斤拷锟斤拷锟斤拷模遥锟斤拷锟斤拷锟斤拷
+				else if (PS2_ON_Flag)      PS2_control();    //Handle PS2 controller commands //锟斤拷锟斤拷PS2锟街憋拷锟斤拷锟斤拷锟斤拷锟斤拷
 				
 				//CAN, Usart 1, Usart 3, Uart5 control can directly get the three axis target speed, 
 				//without additional processing
-				//CAN、串口1、串口3(ROS)、串口5控制直接得到三轴目标速度，无须额外处理
+				//CAN锟斤拷锟斤拷锟斤拷1锟斤拷锟斤拷锟斤拷3(ROS)锟斤拷锟斤拷锟斤拷5锟斤拷锟斤拷直锟接得碉拷锟斤拷锟斤拷目锟斤拷锟劫度ｏ拷锟斤拷锟斤拷锟斤拷獯︼拷锟�
 				else                      Drive_Motor(Move_X, Move_Y, Move_Z);
 				
 				//Click the user button to update the gyroscope zero
-				//单击用户按键更新陀螺仪零点
+				//锟斤拷锟斤拷锟矫伙拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
 				Key(); 
 				
 				//If there is no abnormity in the battery voltage, and the enable switch is in the ON position,
         //and the software failure flag is 0
-				//如果电池电压不存在异常，而且使能开关在ON档位，而且软件失能标志位为0
+				//锟斤拷锟斤拷锟截碉拷压锟斤拷锟斤拷锟斤拷锟届常锟斤拷锟斤拷锟斤拷使锟杰匡拷锟斤拷锟斤拷ON锟斤拷位锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷失锟杰憋拷志位为0
 				if(Turn_Off(Voltage)==0) 
 				 { 			
            //Speed closed-loop control to calculate the PWM value of each motor, 
 					 //PWM represents the actual wheel speed					 
-					 //速度闭环控制计算各电机PWM值，PWM代表车轮实际转速
-					 MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
-					 MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
-					 MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
-					 MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);
+					 //锟劫度闭伙拷锟斤拷锟狡硷拷锟斤拷锟斤拷锟斤拷PWM值锟斤拷PWM锟斤拷锟斤拷锟斤拷锟斤拷实锟斤拷转锟斤拷
+					 MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target, controlDt);
+					 MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target, controlDt);
+					 MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target, controlDt);
+					 MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target, controlDt);
 						 
 					 Limit_Pwm(16700);
 					 
 					 //Set different PWM control polarity according to different car models
-					 //根据不同小车型号设置不同的PWM控制极性
+					 //锟斤拷锟捷诧拷同小锟斤拷锟酵猴拷锟斤拷锟矫诧拷同锟斤拷PWM锟斤拷锟狡硷拷锟斤拷
 					 switch(Car_Mode)
 					 {
-							case Mec_Car:       Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Mecanum wheel car       //麦克纳姆轮小车
-							case Omni_Car:      Set_Pwm(-MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Omni car                //全向轮小车
-							case Akm_Car:       Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  16799,-16799 ,                   Servo); break; //Ackermann structure car //阿克曼小车
-							case Diff_Car:      Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Differential car        //两轮差速小车
-							case FourWheel_Car: Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //FourWheel car           //四驱车 
-							case Tank_Car:      Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Tank Car                //履带车
+							case Mec_Car:       Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Mecanum wheel car       //锟斤拷锟斤拷锟侥凤拷锟叫★拷锟�
+							case Omni_Car:      Set_Pwm(-MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Omni car                //全锟斤拷锟斤拷小锟斤拷
+							case Akm_Car:       Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  16799,-16799 ,                   Servo); break; //Ackermann structure car //锟斤拷锟斤拷锟斤拷小锟斤拷
+							case Diff_Car:      Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Differential car        //锟斤拷锟街诧拷锟斤拷小锟斤拷
+							case FourWheel_Car: Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //FourWheel car           //锟斤拷锟斤拷锟斤拷 
+							case Tank_Car:      Set_Pwm( MOTOR_A.Motor_Pwm,  MOTOR_B.Motor_Pwm,  MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0    ); break; //Tank Car                //锟侥达拷锟斤拷
 					 }
 				 }
 				 //If Turn_Off(Voltage) returns to 1, the car is not allowed to move, and the PWM value is set to 0
-				 //如果Turn_Off(Voltage)返回值为1，不允许控制小车进行运动，PWM值设置为0
+				 //锟斤拷锟絋urn_Off(Voltage)锟斤拷锟斤拷值为1锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷小锟斤拷锟斤拷锟斤拷锟剿讹拷锟斤拷PWM值锟斤拷锟斤拷为0
 				 else	Set_Pwm(0,0,0,0,(Car_Mode == Akm_Car) ? SERVO_INIT : 0);
 			 }	
 		 }  
@@ -241,35 +421,35 @@ void Balance_task(void *pvParameters)
 Function: Assign a value to the PWM register to control wheel speed and direction
 Input   : PWM
 Output  : none
-函数功能：赋值给PWM寄存器，控制车轮转速与方向
-入口参数：PWM
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷值锟斤拷PWM锟侥达拷锟斤拷锟斤拷锟斤拷锟狡筹拷锟斤拷转锟斤拷锟诫方锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟絇WM
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo)
 {
 	//Forward and reverse control of motor
-	//电机正反转控制
+	//锟斤拷锟斤拷锟斤拷锟阶拷锟斤拷锟�
 	if(motor_a<0)			PWMA1=16799,PWMA2=16799+motor_a;
 	else 	            PWMA2=16799,PWMA1=16799-motor_a;
 	
 	//Forward and reverse control of motor
-	//电机正反转控制	
+	//锟斤拷锟斤拷锟斤拷锟阶拷锟斤拷锟�	
 	if(motor_b<0)			PWMB1=16799,PWMB2=16799+motor_b;
 	else 	            PWMB2=16799,PWMB1=16799-motor_b;
 //  PWMB1=10000,PWMB2=5000;
 
 	//Forward and reverse control of motor
-	//电机正反转控制	
+	//锟斤拷锟斤拷锟斤拷锟阶拷锟斤拷锟�	
 	if(motor_c<0)			PWMC1=16799,PWMC2=16799+motor_c;
 	else 	            PWMC2=16799,PWMC1=16799-motor_c;
 	
 	//Forward and reverse control of motor
-	//电机正反转控制
+	//锟斤拷锟斤拷锟斤拷锟阶拷锟斤拷锟�
 	if(motor_d<0)			PWMD1=16799,PWMD2=16799+motor_d;
 	else 	            PWMD2=16799,PWMD1=16799-motor_d;
 	
 	//Servo control
-	//舵机控制
+	//锟斤拷锟斤拷锟斤拷锟�
 	Servo_PWM =servo;
 }
 
@@ -277,9 +457,9 @@ void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo)
 Function: Limit PWM value
 Input   : Value
 Output  : none
-函数功能：限制PWM值 
-入口参数：幅值
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷PWM值 
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟街�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Limit_Pwm(int amplitude)
 {	
@@ -292,9 +472,9 @@ void Limit_Pwm(int amplitude)
 Function: Limiting function
 Input   : Value
 Output  : none
-函数功能：限幅函数
-入口参数：幅值
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟睫凤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟街�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 float target_limit_float(float insert,float low,float high)
 {
@@ -318,9 +498,9 @@ int target_limit_int(int insert,int low,int high)
 Function: Check the battery voltage, enable switch status, software failure flag status
 Input   : Voltage
 Output  : Whether control is allowed, 1: not allowed, 0 allowed
-函数功能：检查电池电压、使能开关状态、软件失能标志位状态
-入口参数：电压
-返回  值：是否允许控制，1：不允许，0允许
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷氐锟窖癸拷锟绞癸拷芸锟斤拷锟阶刺拷锟斤拷锟斤拷锟绞э拷鼙锟街疚蛔刺�
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟窖�
+锟斤拷锟斤拷  值锟斤拷锟角凤拷锟斤拷锟斤拷锟斤拷锟狡ｏ拷1锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷0锟斤拷锟斤拷
 **************************************************************************/
 u8 Turn_Off( int voltage)
 {
@@ -341,9 +521,9 @@ u8 Turn_Off( int voltage)
 Function: Calculate absolute value
 Input   : long int
 Output  : unsigned int
-函数功能：求绝对值
-入口参数：long int
-返回  值：unsigned int
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷锟街�
+锟斤拷诓锟斤拷锟斤拷锟絣ong int
+锟斤拷锟斤拷  值锟斤拷unsigned int
 **************************************************************************/
 u32 myabs(long int a)
 { 		   
@@ -357,78 +537,78 @@ Function: Incremental PI controller
 Input   : Encoder measured value (actual speed), target speed
 Output  : Motor PWM
 According to the incremental discrete PID formula
-pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
+pwm+=Kp[e锟斤拷k锟斤拷-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
 e(k) represents the current deviation
 e(k-1) is the last deviation and so on
 PWM stands for incremental output
 In our speed control closed loop system, only PI control is used
-pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
+pwm+=Kp[e锟斤拷k锟斤拷-e(k-1)]+Ki*e(k)*dt
 
-函数功能：增量式PI控制器
-入口参数：编码器测量值(实际速度)，目标速度
-返回  值：电机PWM
-根据增量式离散PID公式 
-pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
-e(k)代表本次偏差 
-e(k-1)代表上一次的偏差  以此类推 
-pwm代表增量输出
-在我们的速度控制闭环系统里面，只使用PI控制
-pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷式PI锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟街�(实锟斤拷锟劫讹拷)锟斤拷目锟斤拷锟劫讹拷
+锟斤拷锟斤拷  值锟斤拷锟斤拷锟絇WM
+锟斤拷锟斤拷锟斤拷锟斤拷式锟斤拷散PID锟斤拷式 
+pwm+=Kp[e锟斤拷k锟斤拷-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
+e(k)锟斤拷锟斤拷锟斤拷锟斤拷偏锟斤拷 
+e(k-1)锟斤拷锟斤拷锟斤拷一锟轿碉拷偏锟斤拷  锟皆达拷锟斤拷锟斤拷 
+pwm锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷锟角碉拷锟劫度匡拷锟狡闭伙拷系统锟斤拷锟芥，只使锟斤拷PI锟斤拷锟斤拷
+pwm+=Kp[e锟斤拷k锟斤拷-e(k-1)]+Ki*e(k)*dt
 **************************************************************************/
-int Incremental_PI_A (float Encoder,float Target)
+int Incremental_PI_A (float Encoder,float Target,float dt)
 { 	
 	 static float Bias,Pwm,Last_bias;
-	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
-	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; 
+	 Bias=Target-Encoder; //Calculate the deviation //锟斤拷锟斤拷偏锟斤拷
+	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
 	 if(Pwm>16700)Pwm=16700;
 	 if(Pwm<-16700)Pwm=-16700;
-	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
+	 Last_bias=Bias; //Save the last deviation //锟斤拷锟斤拷锟斤拷一锟斤拷偏锟斤拷 
 	 return Pwm;    
 }
-int Incremental_PI_B (float Encoder,float Target)
+int Incremental_PI_B (float Encoder,float Target,float dt)
 {  
 	 static float Bias,Pwm,Last_bias;
-	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
-	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;  
+	 Bias=Target-Encoder; //Calculate the deviation //锟斤拷锟斤拷偏锟斤拷
+	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
 	 if(Pwm>16700)Pwm=16700;
 	 if(Pwm<-16700)Pwm=-16700;
-	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
+	 Last_bias=Bias; //Save the last deviation //锟斤拷锟斤拷锟斤拷一锟斤拷偏锟斤拷 
 	 return Pwm;
 }
-int Incremental_PI_C (float Encoder,float Target)
+int Incremental_PI_C (float Encoder,float Target,float dt)
 {  
 	 static float Bias,Pwm,Last_bias;
-	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
-	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; 
+	 Bias=Target-Encoder; //Calculate the deviation //锟斤拷锟斤拷偏锟斤拷
+	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
 	 if(Pwm>16700)Pwm=16700;
 	 if(Pwm<-16700)Pwm=-16700;
-	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
+	 Last_bias=Bias; //Save the last deviation //锟斤拷锟斤拷锟斤拷一锟斤拷偏锟斤拷 
 	 return Pwm; 
 }
-int Incremental_PI_D (float Encoder,float Target)
+int Incremental_PI_D (float Encoder,float Target,float dt)
 {  
 	 static float Bias,Pwm,Last_bias;
-	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
-	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;  
+	 Bias=Target-Encoder; //Calculate the deviation //锟斤拷锟斤拷偏锟斤拷
+	 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias*dt;
 	 if(Pwm>16700)Pwm=16700;
 	 if(Pwm<-16700)Pwm=-16700;
-	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
+	 Last_bias=Bias; //Save the last deviation //锟斤拷锟斤拷锟斤拷一锟斤拷偏锟斤拷 
 	 return Pwm; 
 }
 /**************************************************************************
 Function: Processes the command sent by APP through usart 2
 Input   : none
 Output  : none
-函数功能：对APP通过串口2发送过来的命令进行处理
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷APP通锟斤拷锟斤拷锟斤拷2锟斤拷锟酵癸拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷写锟斤拷锟�
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Get_RC(void)
 {
 	u8 Flag_Move=1;
-	if(Car_Mode==Mec_Car||Car_Mode==Omni_Car) //The omnidirectional wheel moving trolley can move laterally //全向轮运动小车可以进行横向移动
+	if(Car_Mode==Mec_Car||Car_Mode==Omni_Car) //The omnidirectional wheel moving trolley can move laterally //全锟斤拷锟斤拷锟剿讹拷小锟斤拷锟斤拷锟皆斤拷锟叫猴拷锟斤拷锟狡讹拷
 	{
-	 switch(Flag_Direction)  //Handle direction control commands //处理方向控制命令
+	 switch(Flag_Direction)  //Handle direction control commands //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
 	 { 
 			case 1:      Move_X=RC_Velocity;  	 Move_Y=0;             Flag_Move=1;    break;
 			case 2:      Move_X=RC_Velocity;  	 Move_Y=-RC_Velocity;  Flag_Move=1; 	 break;
@@ -443,15 +623,15 @@ void Get_RC(void)
 	 if(Flag_Move==0)		
 	 {	
 		 //If no direction control instruction is available, check the steering control status
-		 //如果无方向控制指令，检查转向控制状态
-		 if     (Flag_Left ==1)  Move_Z= PI/2*(RC_Velocity/500); //left rotation  //左自转  
-		 else if(Flag_Right==1)  Move_Z=-PI/2*(RC_Velocity/500); //right rotation //右自转
+		 //锟斤拷锟斤拷薹锟斤拷锟斤拷锟斤拷指锟筋，锟斤拷锟阶拷锟斤拷锟斤拷状态
+		 if     (Flag_Left ==1)  Move_Z= PI/2*(RC_Velocity/500); //left rotation  //锟斤拷锟斤拷转  
+		 else if(Flag_Right==1)  Move_Z=-PI/2*(RC_Velocity/500); //right rotation //锟斤拷锟斤拷转
 		 else 		               Move_Z=0;                       //stop           //停止
 	 }
 	}	
-	else //Non-omnidirectional moving trolley //非全向移动小车
+	else //Non-omnidirectional moving trolley //锟斤拷全锟斤拷锟狡讹拷小锟斤拷
 	{
-	 switch(Flag_Direction) //Handle direction control commands //处理方向控制命令
+	 switch(Flag_Direction) //Handle direction control commands //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
 	 { 
 			case 1:      Move_X=+RC_Velocity;  	 Move_Z=0;         break;
 			case 2:      Move_X=+RC_Velocity;  	 Move_Z=-PI/2;   	 break;
@@ -463,29 +643,29 @@ void Get_RC(void)
 			case 8:      Move_X=+RC_Velocity; 	 Move_Z=+PI/2;     break; 
 			default:     Move_X=0;               Move_Z=0;         break;
 	 }
-	 if     (Flag_Left ==1)  Move_Z= PI/2; //left rotation  //左自转 
-	 else if(Flag_Right==1)  Move_Z=-PI/2; //right rotation //右自转	
+	 if     (Flag_Left ==1)  Move_Z= PI/2; //left rotation  //锟斤拷锟斤拷转 
+	 else if(Flag_Right==1)  Move_Z=-PI/2; //right rotation //锟斤拷锟斤拷转	
 	}
 	
-	//Z-axis data conversion //Z轴数据转化
+	//Z-axis data conversion //Z锟斤拷锟斤拷锟斤拷转锟斤拷
 	if(Car_Mode==Akm_Car)
 	{
 		//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
-		//阿克曼结构小车转换为前轮转向角度
+		//锟斤拷锟斤拷锟斤拷锟结构小锟斤拷转锟斤拷为前锟斤拷转锟斤拷嵌锟�
 		Move_Z=Move_Z*2/9; 
 	}
 	else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
 	{
-	  if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //差速控制原理系列需要此处理
+	  if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //锟斤拷锟劫匡拷锟斤拷原锟斤拷系锟斤拷锟斤拷要锟剿达拷锟斤拷
 		Move_Z=Move_Z*RC_Velocity/500;
 	}		
 	
 	//Unit conversion, mm/s -> m/s
-  //单位转换，mm/s -> m/s	
+  //锟斤拷位转锟斤拷锟斤拷mm/s -> m/s	
 	Move_X=Move_X/1000;       Move_Y=Move_Y/1000;         Move_Z=Move_Z;
 	
 	//Control target value is obtained and kinematics analysis is performed
-	//得到控制目标值，进行运动学分析
+	//锟矫碉拷锟斤拷锟斤拷目锟斤拷值锟斤拷锟斤拷锟斤拷锟剿讹拷学锟斤拷锟斤拷
 	Drive_Motor(Move_X,Move_Y,Move_Z);
 }
 
@@ -493,38 +673,38 @@ void Get_RC(void)
 Function: Handle PS2 controller control commands
 Input   : none
 Output  : none
-函数功能：对PS2手柄控制命令进行处理
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷PS2锟街憋拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷写锟斤拷锟�
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void PS2_control(void)
 {
    	int LX,LY,RY;
-		int Threshold=20; //Threshold to ignore small movements of the joystick //阈值，忽略摇杆小幅度动作
+		int Threshold=20; //Threshold to ignore small movements of the joystick //锟斤拷值锟斤拷锟斤拷锟斤拷摇锟斤拷小锟斤拷锟饺讹拷锟斤拷
 			
 	  //128 is the median.The definition of X and Y in the PS2 coordinate system is different from that in the ROS coordinate system
-	  //128为中值。PS2坐标系与ROS坐标系对X、Y的定义不一样
+	  //128为锟斤拷值锟斤拷PS2锟斤拷锟斤拷系锟斤拷ROS锟斤拷锟斤拷系锟斤拷X锟斤拷Y锟侥讹拷锟藉不一锟斤拷
 		LY=-(PS2_LX-128);  
 		LX=-(PS2_LY-128); 
 		RY=-(PS2_RX-128); 
 	
-	  //Ignore small movements of the joystick //忽略摇杆小幅度动作
+	  //Ignore small movements of the joystick //锟斤拷锟斤拷摇锟斤拷小锟斤拷锟饺讹拷锟斤拷
 		if(LX>-Threshold&&LX<Threshold)LX=0; 
 		if(LY>-Threshold&&LY<Threshold)LY=0; 
 		if(RY>-Threshold&&RY<Threshold)RY=0; 
 	
-	  if (PS2_KEY==11)		RC_Velocity+=5;  //To accelerate//加速
-	  else if(PS2_KEY==9)	RC_Velocity-=5;  //To slow down //减速	
+	  if (PS2_KEY==11)		RC_Velocity+=5;  //To accelerate//锟斤拷锟斤拷
+	  else if(PS2_KEY==9)	RC_Velocity-=5;  //To slow down //锟斤拷锟斤拷	
 	
 		if(RC_Velocity<0)   RC_Velocity=0;
 	
 	  //Handle PS2 controller control commands
-	  //对PS2手柄控制命令进行处理
+	  //锟斤拷PS2锟街憋拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷写锟斤拷锟�
 		Move_X=LX*RC_Velocity/128; 
 		Move_Y=LY*RC_Velocity/128; 
 	  Move_Z=RY*(PI/2)/128;      
 	
-	  //Z-axis data conversion //Z轴数据转化
+	  //Z-axis data conversion //Z锟斤拷锟斤拷锟斤拷转锟斤拷
 	  if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
 		{
 			Move_Z=Move_Z*RC_Velocity/500;
@@ -532,23 +712,23 @@ void PS2_control(void)
 		else if(Car_Mode==Akm_Car)
 		{
 			//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
-		  //阿克曼结构小车转换为前轮转向角度
+		  //锟斤拷锟斤拷锟斤拷锟结构小锟斤拷转锟斤拷为前锟斤拷转锟斤拷嵌锟�
 			Move_Z=Move_Z*2/9;
 		}
 		else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
 		{
-			if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //差速控制原理系列需要此处理
+			if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //锟斤拷锟劫匡拷锟斤拷原锟斤拷系锟斤拷锟斤拷要锟剿达拷锟斤拷
 			Move_Z=Move_Z*RC_Velocity/500;
 		}	
 		 
 	  //Unit conversion, mm/s -> m/s
-    //单位转换，mm/s -> m/s	
+    //锟斤拷位转锟斤拷锟斤拷mm/s -> m/s	
 		Move_X=Move_X/1000;        
 		Move_Y=Move_Y/1000;    
 		Move_Z=Move_Z;
 		
 		//Control target value is obtained and kinematics analysis is performed
-	  //得到控制目标值，进行运动学分析
+	  //锟矫碉拷锟斤拷锟斤拷目锟斤拷值锟斤拷锟斤拷锟斤拷锟剿讹拷学锟斤拷锟斤拷
 		Drive_Motor(Move_X,Move_Y,Move_Z);		 			
 } 
 
@@ -556,18 +736,18 @@ void PS2_control(void)
 Function: The remote control command of model aircraft is processed
 Input   : none
 Output  : none
-函数功能：对航模遥控控制命令进行处理
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟皆猴拷模遥锟截匡拷锟斤拷锟斤拷锟斤拷锟斤拷写锟斤拷锟�
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Remote_Control(void)
 {
 	  //Data within 1 second after entering the model control mode will not be processed
-	  //对进入航模控制模式后1秒内的数据不处理
-    static u8 thrice=100; 
-    int Threshold=100; //Threshold to ignore small movements of the joystick //阈值，忽略摇杆小幅度动作
+	  //锟皆斤拷锟诫航模锟斤拷锟斤拷模式锟斤拷1锟斤拷锟节碉拷锟斤拷锟捷诧拷锟斤拷锟斤拷
+    static u8 thrice=CONTROL_FREQUENCY;
+    int Threshold=100; //Threshold to ignore small movements of the joystick //锟斤拷值锟斤拷锟斤拷锟斤拷摇锟斤拷小锟斤拷锟饺讹拷锟斤拷
 
-	  //limiter //限幅
+	  //limiter //锟睫凤拷
     int LX,LY,RY,RX,Remote_RCvelocity; 
 		Remoter_Ch1=target_limit_int(Remoter_Ch1,1000,2000);
 		Remoter_Ch2=target_limit_int(Remoter_Ch2,1000,2000);
@@ -575,20 +755,20 @@ void Remote_Control(void)
 		Remoter_Ch4=target_limit_int(Remoter_Ch4,1000,2000);
 
 	  // Front and back direction of left rocker. Control forward and backward.
-	  //左摇杆前后方向。控制前进后退。
+	  //锟斤拷摇锟斤拷前锟斤拷锟津。匡拷锟斤拷前锟斤拷锟斤拷锟剿★拷
     LX=Remoter_Ch2-1500; 
 	
 	  //Left joystick left and right.Control left and right movement. Only the wheelie omnidirectional wheelie will use the channel.
 	  //Ackerman trolleys use this channel as a PWM output to control the steering gear
-	  //左摇杆左右方向。控制左右移动。麦轮全向轮才会使用到改通道。阿克曼小车使用该通道作为PWM输出控制舵机
+	  //锟斤拷摇锟斤拷锟斤拷锟揭凤拷锟津。匡拷锟斤拷锟斤拷锟斤拷锟狡讹拷锟斤拷锟斤拷锟斤拷全锟斤拷锟街才伙拷使锟矫碉拷锟斤拷通锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷小锟斤拷使锟矫革拷通锟斤拷锟斤拷为PWM锟斤拷锟斤拷锟斤拷贫锟斤拷
     LY=Remoter_Ch4-1500;
 
     //Front and back direction of right rocker. Throttle/acceleration/deceleration.
-		//右摇杆前后方向。油门/加减速。
+		//锟斤拷摇锟斤拷前锟斤拷锟斤拷锟斤拷锟斤拷/锟接硷拷锟劫★拷
 	  RX=Remoter_Ch3-1500;
 
     //Right stick left and right. To control the rotation. 
-		//右摇杆左右方向。控制自转。
+		//锟斤拷摇锟斤拷锟斤拷锟揭凤拷锟津。匡拷锟斤拷锟斤拷转锟斤拷
     RY=Remoter_Ch1-1500; 
 
     if(LX>-Threshold&&LX<Threshold)LX=0;
@@ -596,17 +776,17 @@ void Remote_Control(void)
     if(RX>-Threshold&&RX<Threshold)RX=0;
 	  if(RY>-Threshold&&RY<Threshold)RY=0;
 		
-		//Throttle related //油门相关
+		//Throttle related //锟斤拷锟斤拷锟斤拷锟�
 		Remote_RCvelocity=RC_Velocity+RX;
 	  if(Remote_RCvelocity<0)Remote_RCvelocity=0;
 		
 		//The remote control command of model aircraft is processed
-		//对航模遥控控制命令进行处理
+		//锟皆猴拷模遥锟截匡拷锟斤拷锟斤拷锟斤拷锟斤拷写锟斤拷锟�
     Move_X= LX*Remote_RCvelocity/500; 
 		Move_Y=-LY*Remote_RCvelocity/500;
 		Move_Z=-RY*(PI/2)/500;      
 			 
-		//Z轴数据转化
+		//Z锟斤拷锟斤拷锟斤拷转锟斤拷
 	  if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
 		{
 			Move_Z=Move_Z*Remote_RCvelocity/500;
@@ -614,36 +794,36 @@ void Remote_Control(void)
 		else if(Car_Mode==Akm_Car)
 		{
 			//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
-		  //阿克曼结构小车转换为前轮转向角度
+		  //锟斤拷锟斤拷锟斤拷锟结构小锟斤拷转锟斤拷为前锟斤拷转锟斤拷嵌锟�
 			Move_Z=Move_Z*2/9;
 		}
 		else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
 		{
-			if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //差速控制原理系列需要此处理
+			if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //锟斤拷锟劫匡拷锟斤拷原锟斤拷系锟斤拷锟斤拷要锟剿达拷锟斤拷
 			Move_Z=Move_Z*Remote_RCvelocity/500;
 		}
 		
 	  //Unit conversion, mm/s -> m/s
-    //单位转换，mm/s -> m/s	
+    //锟斤拷位转锟斤拷锟斤拷mm/s -> m/s	
 		Move_X=Move_X/1000;       
     Move_Y=Move_Y/1000;      
 		Move_Z=Move_Z;
 		
 	  //Data within 1 second after entering the model control mode will not be processed
-	  //对进入航模控制模式后1秒内的数据不处理
+	  //锟皆斤拷锟诫航模锟斤拷锟斤拷模式锟斤拷1锟斤拷锟节碉拷锟斤拷锟捷诧拷锟斤拷锟斤拷
     if(thrice>0) Move_X=0,Move_Z=0,thrice--;
 				
 		//Control target value is obtained and kinematics analysis is performed
-	  //得到控制目标值，进行运动学分析
+	  //锟矫碉拷锟斤拷锟斤拷目锟斤拷值锟斤拷锟斤拷锟斤拷锟剿讹拷学锟斤拷锟斤拷
 		Drive_Motor(Move_X,Move_Y,Move_Z);
 }
 /**************************************************************************
 Function: Click the user button to update gyroscope zero
 Input   : none
 Output  : none
-函数功能：单击用户按键更新陀螺仪零点
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷锟矫伙拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Key(void)
 {	
@@ -655,24 +835,28 @@ void Key(void)
 Function: Read the encoder value and calculate the wheel speed, unit m/s
 Input   : none
 Output  : none
-函数功能：读取编码器数值并计算车轮速度，单位m/s
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷值锟斤拷锟斤拷锟姐车锟斤拷锟劫度ｏ拷锟斤拷位m/s
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Get_Velocity_Form_Encoder(void)
 {
 	  //Retrieves the original data of the encoder
-	  //获取编码器的原始数据
-		float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr; 
-		OriginalEncoder.A=Read_Encoder(2);	
-		OriginalEncoder.B=Read_Encoder(3);	
-		OriginalEncoder.C=Read_Encoder(4);	
-		OriginalEncoder.D=Read_Encoder(5);	
+	  //锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷原始锟斤拷锟斤拷
+		float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr;
+		//Stamp the moment the encoder counters are latched, as close to the
+		//real sample instant as possible.
+		//锟节讹拷取锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷瞬锟斤拷锟绞憋拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟绞碉拷杉锟绞憋拷獭锟�
+		g_speed_sample_time_us = mcu_time_us();
+		OriginalEncoder.A=Read_Encoder(2);
+		OriginalEncoder.B=Read_Encoder(3);
+		OriginalEncoder.C=Read_Encoder(4);
+		OriginalEncoder.D=Read_Encoder(5);
 
 	//test_num=OriginalEncoder.B;
 	
 	  //Decide the encoder numerical polarity according to different car models
-		//根据不同小车型号决定编码器数值极性
+		//锟斤拷锟捷诧拷同小锟斤拷锟酵号撅拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷值锟斤拷锟斤拷
 		switch(Car_Mode)
 		{
 			case Mec_Car:       Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C;  Encoder_D_pr=-OriginalEncoder.D; break; 
@@ -684,7 +868,7 @@ void Get_Velocity_Form_Encoder(void)
 		}
 		
 		//The encoder converts the raw data to wheel speed in m/s
-		//编码器原始数据转换为车轮速度，单位m/s
+		//锟斤拷锟斤拷锟斤拷原始锟斤拷锟斤拷转锟斤拷为锟斤拷锟斤拷锟劫度ｏ拷锟斤拷位m/s
 		MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
 		MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
 		MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; 
@@ -694,9 +878,9 @@ void Get_Velocity_Form_Encoder(void)
 Function: Smoothing the three axis target velocity
 Input   : Three-axis target velocity
 Output  : none
-函数功能：对三轴目标速度做平滑处理
-入口参数：三轴目标速度
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷锟斤拷目锟斤拷锟劫讹拷锟斤拷平锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟斤拷锟侥匡拷锟斤拷俣锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void Smooth_control(float vx,float vy,float vz)
 {
@@ -722,9 +906,9 @@ void Smooth_control(float vx,float vy,float vz)
 Function: Floating-point data calculates the absolute value
 Input   : float
 Output  : The absolute value of the input number
-函数功能：浮点型数据计算绝对值
-入口参数：浮点数
-返回  值：输入数的绝对值
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷锟斤拷锟斤拷锟捷硷拷锟斤拷锟斤拷锟街�
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷锟斤拷锟斤拷锟侥撅拷锟斤拷值
 **************************************************************************/
 float float_abs(float insert)
 {
@@ -735,9 +919,9 @@ float float_abs(float insert)
 Function: Prevent the potentiometer to choose the wrong mode, resulting in initialization error caused by the motor spinning.Out of service
 Input   : none
 Output  : none
-函数功能：防止电位器选错模式，导致初始化出错引发电机乱转。已停止使用
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷止锟斤拷位锟斤拷选锟斤拷模式锟斤拷锟斤拷锟铰筹拷始锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟阶拷锟斤拷锟酵Ｖ故癸拷锟�
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void robot_mode_check(void)
 {
@@ -745,6 +929,6 @@ void robot_mode_check(void)
 
 	if(abs(MOTOR_A.Motor_Pwm)>2500||abs(MOTOR_B.Motor_Pwm)>2500||abs(MOTOR_C.Motor_Pwm)>2500||abs(MOTOR_D.Motor_Pwm)>2500)   error++;
 	//If the output is close to full amplitude for 6 times in a row, it is judged that the motor rotates wildly and makes the motor incapacitated
-	//如果连续6次接近满幅输出，判断为电机乱转，让电机失能	
+	//锟斤拷锟斤拷锟斤拷锟�6锟轿接斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷卸锟轿拷锟斤拷锟斤拷转锟斤拷锟矫碉拷锟绞э拷锟�	
 	if(error>6) EN=0,Flag_Stop=1,robot_mode_check_flag=1;  
 }
diff --git a/BALANCE/balance.h b/BALANCE/balance.h
index eb7c51b..fa9b60b 100644
--- a/BALANCE/balance.h
+++ b/BALANCE/balance.h
@@ -22,10 +22,10 @@ float target_limit_float(float insert,float low,float high);
 int target_limit_int(int insert,int low,int high);
 u8 Turn_Off( int voltage);
 u32 myabs(long int a);
-int Incremental_PI_A (float Encoder,float Target);
-int Incremental_PI_B (float Encoder,float Target);
-int Incremental_PI_C (float Encoder,float Target);
-int Incremental_PI_D (float Encoder,float Target);
+int Incremental_PI_A (float Encoder,float Target,float dt);
+int Incremental_PI_B (float Encoder,float Target,float dt);
+int Incremental_PI_C (float Encoder,float Target,float dt);
+int Incremental_PI_D (float Encoder,float Target,float dt);
 void Get_RC(void);
 void Remote_Control(void);
 void Drive_Motor(float Vx,float Vy,float Vz);
@@ -36,4 +36,3 @@ void PS2_control(void);
 float float_abs(float insert);
 void robot_mode_check(void);
 #endif  
-
diff --git a/BALANCE/robot_select_init.h b/BALANCE/robot_select_init.h
index 4f66e6b..c551eed 100644
--- a/BALANCE/robot_select_init.h
+++ b/BALANCE/robot_select_init.h
@@ -4,19 +4,19 @@
 #include "system.h"
 
 //Parameter structure of robot
-//机器人参数结构体
+//锟斤拷锟斤拷锟剿诧拷锟斤拷锟结构锟斤拷
 typedef struct  
 {
-  float WheelSpacing;      //Wheelspacing, Mec_Car is half wheelspacing //轮距 麦轮车为半轮距
-  float AxleSpacing;       //Axlespacing, Mec_Car is half axlespacing //轴距 麦轮车为半轴距	
-  int GearRatio;           //Motor_gear_ratio //电机减速比
-  int EncoderAccuracy;     //Number_of_encoder_lines //编码器精度(编码器线数)
-  float WheelDiameter;     //Diameter of driving wheel //主动轮直径	
-  float OmniTurnRadiaus;   //Rotation radius of omnidirectional trolley //全向轮小车旋转半径	
+  float WheelSpacing;      //Wheelspacing, Mec_Car is half wheelspacing //锟街撅拷 锟斤拷锟街筹拷为锟斤拷锟街撅拷
+  float AxleSpacing;       //Axlespacing, Mec_Car is half axlespacing //锟斤拷锟� 锟斤拷锟街筹拷为锟斤拷锟斤拷锟�	
+  int GearRatio;           //Motor_gear_ratio //锟斤拷锟斤拷锟斤拷俦锟�
+  int EncoderAccuracy;     //Number_of_encoder_lines //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷(锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷)
+  float WheelDiameter;     //Diameter of driving wheel //锟斤拷锟斤拷锟斤拷直锟斤拷	
+  float OmniTurnRadiaus;   //Rotation radius of omnidirectional trolley //全锟斤拷锟斤拷小锟斤拷锟斤拷转锟诫径	
 }Robot_Parament_InitTypeDef;
 
 // Encoder structure
-//编码器结构体
+//锟斤拷锟斤拷锟斤拷锟结构锟斤拷
 typedef struct  
 {
   int A;      
@@ -27,28 +27,74 @@ typedef struct
 
 //The minimum turning radius of Ackermann models is determined by the mechanical structure: 
 //the maximum Angle of the wheelbase, wheelbase and front wheels
-//阿克曼车型的最小转弯半径，由机械结构决定：轮距、轴距、前轮最大转角
+//锟斤拷锟斤拷锟斤拷锟斤拷锟酵碉拷锟斤拷小转锟斤拷刖讹拷锟斤拷苫锟叫碉拷峁癸拷锟斤拷锟斤拷锟斤拷志唷拷锟洁、前锟斤拷锟斤拷锟阶拷锟�
 #define MINI_AKM_MIN_TURN_RADIUS 0.350f 
 
 //Wheelspacing, Mec_Car is half wheelspacing
-//轮距 麦轮是一半
+//锟街撅拷 锟斤拷锟斤拷锟斤拷一锟斤拷
 //#define MEC_wheelspacing         0.109
-#define MEC_wheelspacing         0.0930 //修正2021.03.30
-#define Akm_wheelspacing         0.162f
+#define MEC_wheelspacing         0.0930 //锟斤拷锟斤拷2021.03.30
+#define Akm_wheelspacing         0.160f
 #define Diff_wheelSpacing        0.177f
 #define Four_Mortor_wheelSpacing 0.26f
 #define Tank_wheelSpacing        0.235f
 
 //Axlespacing, Mec_Car is half axlespacing
-//轴距 麦轮是一半
+//锟斤拷锟� 锟斤拷锟斤拷锟斤拷一锟斤拷
 #define MEC_axlespacing           0.085
-#define Akm_axlespacing           0.158f
+#define Akm_axlespacing           0.160f
+// Set to 1 to drive the Ackermann servo directly from TIM8 channel 1 (bench debug).
+// Set to 0 to use the calibrated curvature->servo model in balance.c.
+#define AKM_SERVO_DEBUG_REMOTE_CH1 0
+
+// Ackermann control law selector (mutually exclusive with the debug switch above):
+//   0 = calibrated kinematic model: kappa = wz/Vx -> quadratic servo fit,
+//       rear wheels get Ackermann differential. Physically correct.
+//   1 = direct passthrough (tuning/debug): Vz is linearly mapped across the
+//       full servo travel, Vx is sent to both drive wheels unchanged (no
+//       differential, no curvature math). Handy for isolating servo/motor.
+#define AKM_DIRECT_MAP 0
+// Direct-map input span: |Vz| >= AKM_DIRECT_VZ_FULL maps to the servo end stop.
+// Vz > 0 = left (ROS), which maps toward AKM_SERVO_MIN (left end).
+// TUNING KNOB (Mode 1 & Mode 2 share it): set this to the MAX angular.z [rad/s]
+// your commander actually sends, so a full stick/command uses the full servo
+// travel. Too high -> steering stays small; too low -> servo saturates (always
+// full lock) and loses proportional control. Vz arrives in rad/s (usartx.c
+// XYZ_Target_Speed_transition: raw/1000).
+#define AKM_DIRECT_VZ_FULL 1.0f
+
+// Ackermann yaw-rate closed-loop assist (Mode 2), mutually exclusive with
+// AKM_DIRECT_MAP (direct-map wins if both are 1). "Front wheel does the main
+// steering, rear wheels add a yaw-rate differential" -- a simplified torque-
+// vectoring / yaw-rate closed loop:
+//   path -> (Vx, kappa_cmd) -> servo main steering (calibrated fit)
+//                            + IMU yaw-rate PI differential on the rear wheels.
+// Degenerates EXACTLY to the Mode-0 Ackermann differential when
+// AKM_YAW_FF_ALPHA = 1 and AKM_YAW_KP = AKM_YAW_KI = 0.
+#define AKM_YAW_ASSIST 1
+// PI gains on the yaw-rate error e_r = r_ref - r_imu [rad/s], output in m/s.
+#define AKM_YAW_KP             0.10f
+#define AKM_YAW_KI             0.00f
+// Feedforward blend: 0 = pure IMU feedback, 1 = full geometric differential.
+#define AKM_YAW_FF_ALPHA       0.00f
+// Below this |Vx| the yaw loop is frozen (integrator reset, no differential).
+#define AKM_YAW_MIN_SPEED      0.10f
+// |dv| clamp as a fraction of |Vx|, so the differential cannot stall a wheel.
+#define AKM_YAW_MAX_DIFF_RATIO 0.35f
+// gyro[2] LSB -> rad/s at FS +-500 dps (see MPU6050.c: FS_500 -> /3754.9).
+#define AKM_GYRO_Z_TO_RADPS    3754.9f
+// Light first-order low-pass on the measured yaw rate (0 = none, 1 = no filter
+// lag). r_f += beta*(r_meas - r_f). ~0.3 gives gentle smoothing at 200 Hz.
+#define AKM_YAW_IMU_LPF        0.30f
+// Flip to -1.0f if the IMU +z spins opposite to the ROS convention (+ = left).
+// MUST be verified on hardware: command a left turn and confirm gyro[2] > 0.
+#define AKM_GYRO_Z_SIGN        (+1.0f)
 #define Diff_axlespacing          0.155f
 #define Four_Mortor__axlespacing  0.28f
 #define Tank_axlespacing          0.222f
 
 //Motor_gear_ratio
-//电机减速比
+//锟斤拷锟斤拷锟斤拷俦锟�
 #define   HALL_30F    30
 #define   HALL_60F    60
 #define   MD36N_5_18  5.18
@@ -59,12 +105,12 @@ typedef struct
 #define   MD60N_47    47
 
 //Number_of_encoder_lines
-//编码器精度
+//锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
 #define		Photoelectric_500 500
 #define	  Hall_13           13
 
 //Mecanum wheel tire diameter series
-//麦轮轮胎直径
+//锟斤拷锟斤拷锟斤拷胎直锟斤拷
 #define		Mecanum_60  0.060f
 #define		Mecanum_75  0.075f
 #define		Mecanum_100 0.100f
@@ -72,7 +118,7 @@ typedef struct
 #define		Mecanum_152 0.152f
  
 //Omni wheel tire diameter series
-//轮径全向轮直径系列
+//锟街撅拷全锟斤拷锟斤拷直锟斤拷系锟斤拷
 #define	  FullDirecion_60  0.060
 #define	  FullDirecion_75  0.075
 #define	  FullDirecion_127 0.127
@@ -81,26 +127,26 @@ typedef struct
 #define	  FullDirecion_217 0.217
 
 //Black tire, tank_car wheel diameter
-//黑色轮胎、履带车轮直径
+//锟斤拷色锟斤拷胎锟斤拷锟侥达拷锟斤拷锟斤拷直锟斤拷
 #define	  Black_WheelDiameter   0.065
 //#define	  Tank_WheelDiameter 0.047
 #define	  Tank_WheelDiameter 0.043
 
 //Rotation radius of omnidirectional trolley
-//全向轮小车旋转半径
+//全锟斤拷锟斤拷小锟斤拷锟斤拷转锟诫径
 #define   Omni_Turn_Radiaus_109 0.109
 #define   Omni_Turn_Radiaus_164 0.164
 #define   Omni_Turn_Radiaus_180 0.180
 #define   Omni_Turn_Radiaus_290 0.290
 
 //The encoder octave depends on the encoder initialization Settings
-//编码器倍频数，取决于编码器初始化设置
+//锟斤拷锟斤拷锟斤拷锟斤拷频锟斤拷锟斤拷取锟斤拷锟节憋拷锟斤拷锟斤拷锟斤拷始锟斤拷锟斤拷锟斤拷
 #define   EncoderMultiples  4
-//Encoder data reading frequency
-//编码器数据读取频率
-#define   CONTROL_FREQUENCY 100
+//Wheel-speed control and encoder reading frequency
+//锟斤拷锟劫匡拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟饺∑碉拷锟�
+#define   CONTROL_FREQUENCY 200
 
-//#define PI 3.1415f  //PI //圆周率
+//#define PI 3.1415f  //PI //圆锟斤拷锟斤拷
 
 void Robot_Select(void);
 void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter);
diff --git a/BALANCE/show.c b/BALANCE/show.c
index 4f130ac..3f41d4c 100644
--- a/BALANCE/show.c
+++ b/BALANCE/show.c
@@ -26,8 +26,8 @@ void show_task(void *pvParameters)
 		
 		//开机时蜂鸣器短暂蜂鸣，开机提醒
 		//The buzzer will beep briefly when the machine is switched on
-		if(Time_count<50)Buzzer=1; 
-		else if(Time_count>=51 && Time_count<100)Buzzer=0;
+		if(Time_count<(CONTROL_FREQUENCY/2))Buzzer=1;
+		else if(Time_count>=(CONTROL_FREQUENCY/2) && Time_count<CONTROL_FREQUENCY)Buzzer=0;
 		 
 		if(LowVoltage_1==1 || LowVoltage_2==1)Buzzer_count=0;
 		if(Buzzer_count<5)Buzzer_count++;
@@ -340,5 +340,3 @@ void APP_Show(void)
 		 printf("{B%d:%d:%d}$",(int)gyro[0],(int)gyro[1],(int)gyro[2]);
 	 }
 }
-
-
diff --git a/BALANCE/system.c b/BALANCE/system.c
index 743385e..0ac6039 100644
--- a/BALANCE/system.c
+++ b/BALANCE/system.c
@@ -1,197 +1,203 @@
 #include "system.h"
 
 //Robot software fails to flag bits
-//机器人软件失能标志位
+//锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷失锟杰憋拷志位
 u8 Flag_Stop=1;   
 
 //The ADC value is variable in segments, depending on the number of car models. Currently there are 6 car models
-//ADC值分段变量，取决于小车型号数量，目前有6种小车型号
+//ADC值锟街段憋拷锟斤拷锟斤拷取锟斤拷锟斤拷小锟斤拷锟酵猴拷锟斤拷锟斤拷锟斤拷目前锟斤拷6锟斤拷小锟斤拷锟酵猴拷
 int Divisor_Mode;
 
 // Robot type variable
-//机器人型号变量
-//0=Mec_Car，1=Omni_Car，2=Akm_Car，3=Diff_Car，4=FourWheel_Car，5=Tank_Car
+//锟斤拷锟斤拷锟斤拷锟酵号憋拷锟斤拷
+//0=Mec_Car锟斤拷1=Omni_Car锟斤拷2=Akm_Car锟斤拷3=Diff_Car锟斤拷4=FourWheel_Car锟斤拷5=Tank_Car
 u8 Car_Mode=4; 
 
 //Servo control PWM value, Ackerman car special
-//舵机控制PWM值，阿克曼小车专用
+//锟斤拷锟斤拷锟斤拷锟絇WM值锟斤拷锟斤拷锟斤拷锟斤拷小锟斤拷专锟斤拷
 int Servo = SERVO_INIT;
 
 //Default speed of remote control car, unit: mm/s
-//遥控小车的默认速度，单位：mm/s
+//遥锟斤拷小锟斤拷锟斤拷默锟斤拷锟劫度ｏ拷锟斤拷位锟斤拷mm/s
 float RC_Velocity=500; 
 
 //Vehicle three-axis target moving speed, unit: m/s
-//小车三轴目标运动速度，单位：m/s
+//小锟斤拷锟斤拷锟斤拷目锟斤拷锟剿讹拷锟劫度ｏ拷锟斤拷位锟斤拷m/s
 float Move_X, Move_Y, Move_Z;   
 
-//PID parameters of Speed control
-//速度控制PID参数
-float Velocity_KP=300,Velocity_KI=800; 
+//Speed PI parameters: Ki is in PWM/(m/s*s) and multiplies measured dt.
+//锟劫讹拷PI锟斤拷锟斤拷锟斤拷Ki锟斤拷位为PWM/(m/s*s)锟斤拷锟斤拷实锟斤拷dt锟斤拷锟�
+float Velocity_KP=300,Velocity_KI=80000;
 
 //Smooth control of intermediate variables, dedicated to omni-directional moving cars
-//平滑控制中间变量，全向移动小车专用
+//平锟斤拷锟斤拷锟斤拷锟叫硷拷锟斤拷锟斤拷锟饺拷锟斤拷贫锟叫★拷锟阶拷锟�
 Smooth_Control smooth_control;  
 
 //The parameter structure of the motor
-//电机的参数结构体
+//锟斤拷锟斤拷牟锟斤拷锟斤拷峁癸拷锟�
 Motor_parameter MOTOR_A,MOTOR_B,MOTOR_C,MOTOR_D;  
 
-/************ 小车型号相关变量 **************************/
+/************ 小锟斤拷锟酵猴拷锟斤拷乇锟斤拷锟� **************************/
 /************ Variables related to car model ************/
 //Encoder accuracy
-//编码器精度
+//锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
 float Encoder_precision; 
 //Wheel circumference, unit: m
-//轮子周长，单位：m
+//锟斤拷锟斤拷锟杰筹拷锟斤拷锟斤拷位锟斤拷m
 float Wheel_perimeter; 
 //Drive wheel base, unit: m
-//主动轮轮距，单位：m
+//锟斤拷锟斤拷锟斤拷锟街距，锟斤拷位锟斤拷m
 float Wheel_spacing; 
 //The wheelbase of the front and rear axles of the trolley, unit: m
-//小车前后轴的轴距，单位：m
+//小锟斤拷前锟斤拷锟斤拷锟斤拷锟洁，锟斤拷位锟斤拷m
 float Axle_spacing; 
 //All-directional wheel turning radius, unit: m
-//全向轮转弯半径，单位：m
+//全锟斤拷锟斤拷转锟斤拷刖讹拷锟斤拷锟轿伙拷锟絤
 float Omni_turn_radiaus; 
-/************ 小车型号相关变量 **************************/
+/************ 小锟斤拷锟酵猴拷锟斤拷乇锟斤拷锟� **************************/
 /************ Variables related to car model ************/
 
 //PS2 controller, Bluetooth APP, aircraft model controller, CAN communication, serial port 1, serial port 5 communication control flag bit.
 //These 6 flag bits are all 0 by default, representing the serial port 3 control mode
-//PS2手柄、蓝牙APP、航模手柄、CAN通信、串口1、串口5通信控制标志位。这6个标志位默认都为0，代表串口3控制模式
+//PS2锟街憋拷锟斤拷锟斤拷锟斤拷APP锟斤拷锟斤拷模锟街憋拷锟斤拷CAN通锟脚★拷锟斤拷锟斤拷1锟斤拷锟斤拷锟斤拷5通锟脚匡拷锟狡憋拷志位锟斤拷锟斤拷6锟斤拷锟斤拷志位默锟较讹拷为0锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷3锟斤拷锟斤拷模式
 u8 PS2_ON_Flag=0, APP_ON_Flag=0, Remote_ON_Flag=0, CAN_ON_Flag=0, Usart1_ON_Flag, Usart5_ON_Flag; 
 
 //Bluetooth remote control associated flag bits
-//蓝牙遥控相关的标志位
+//锟斤拷锟斤拷遥锟斤拷锟斤拷氐谋锟街疚�
 u8 Flag_Left, Flag_Right, Flag_Direction=0, Turn_Flag; 
 
 //Sends the parameter's flag bit to the Bluetooth APP
-//向蓝牙APP发送参数的标志位
+//锟斤拷锟斤拷锟斤拷APP锟斤拷锟酵诧拷锟斤拷锟侥憋拷志位
 u8 PID_Send; 
 
 //The PS2 gamepad controls related variables
-//PS2手柄控制相关变量
+//PS2锟街憋拷锟斤拷锟斤拷锟斤拷乇锟斤拷锟�
 float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY; 
 
 //Self-check the relevant flag variables
-//自检相关标志变量
+//锟皆硷拷锟斤拷乇锟街撅拷锟斤拷锟�
 int Check=0, Checking=0, Checked=0, CheckCount=0, CheckPhrase1=0, CheckPhrase2=0; 
 
 //Check the result code
-//自检结果代码
+//锟皆硷拷锟斤拷锟斤拷锟斤拷
 long int ErrorCode=0; 
 
 void systemInit(void)
 {       
 	
 //	//Interrupt priority group setti  ng
-//	//中断优先级分组设置
+//	//锟叫讹拷锟斤拷锟饺硷拷锟斤拷锟斤拷锟斤拷锟斤拷
 	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
 //	
 //	//Delay function initialization
-//	//延时函数初始化
+//	//锟斤拷时锟斤拷锟斤拷锟斤拷始锟斤拷
 	delay_init(168);	
 	
 	//Initialize the hardware interface connected to the LED lamp
-	//初始化与LED灯连接的硬件接口
+	//锟斤拷始锟斤拷锟斤拷LED锟斤拷锟斤拷锟接碉拷硬锟斤拷锟接匡拷
 	LED_Init();                     
 	    
   //Initialize the hardware interface connected to the buzzer	
-  //初始化与蜂鸣器连接的硬件接口
+  //锟斤拷始锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷拥锟接诧拷锟斤拷涌锟�
 	Buzzer_Init();  
 	
 	//Initialize the hardware interface connected to the enable switch
-	//初始化与使能开关连接的硬件接口
+	//锟斤拷始锟斤拷锟斤拷使锟杰匡拷锟斤拷锟斤拷锟接碉拷硬锟斤拷锟接匡拷
 	Enable_Pin();
 
   //Initialize the hardware interface connected to the OLED display
-  //初始化与OLED显示屏连接的硬件接口	
+  //锟斤拷始锟斤拷锟斤拷OLED锟斤拷示锟斤拷锟斤拷锟接碉拷硬锟斤拷锟接匡拷	
 	OLED_Init();     
 	
 	//Initialize the hardware interface connected to the user's key
-	//初始化与用户按键连接的硬件接口
+	//锟斤拷始锟斤拷锟斤拷锟矫伙拷锟斤拷锟斤拷锟斤拷锟接碉拷硬锟斤拷锟接匡拷
 	KEY_Init();	
 	
 	//Serial port 1 initialization, communication baud rate 115200, 
 	//can be used to communicate with ROS terminal
-	//串口1初始化，通信波特率115200，可用于与ROS端通信
+	//锟斤拷锟斤拷1锟斤拷始锟斤拷锟斤拷通锟脚诧拷锟斤拷锟斤拷115200锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷ROS锟斤拷通锟斤拷
 	uart1_init(115200);	  
 	
 	//Serial port 2 initialization, communication baud rate 9600, 
 	//used to communicate with Bluetooth APP terminal
-	//串口2初始化，通信波特率9600，用于与蓝牙APP端通信
+	//锟斤拷锟斤拷2锟斤拷始锟斤拷锟斤拷通锟脚诧拷锟斤拷锟斤拷9600锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷APP锟斤拷通锟斤拷
 	uart2_init(9600);  
 	
-	//Serial port 3 is initialized and the baud rate is 115200. 
-	//Serial port 3 is the default port used to communicate with ROS terminal
-	//串口3初始化，通信波特率115200，串口3为默认用于与ROS端通信的串口
-	uart3_init(115200);
+	//Serial port 3 is initialized and the baud rate is 921600.
+	//Serial port 3 is the default port used to communicate with ROS terminal.
+	//NOTE: the ROS side must open this port at 921600 to match.
+	//锟斤拷锟斤拷3锟斤拷始锟斤拷锟斤拷通锟脚诧拷锟斤拷锟斤拷921600锟斤拷锟斤拷锟斤拷3为默锟斤拷锟斤拷锟斤拷锟斤拷ROS锟斤拷通锟脚的达拷锟节★拷
+	//注锟解：ROS 锟剿打开达拷锟斤拷时锟斤拷锟斤拷锟斤拷同锟斤拷锟斤拷 921600锟斤拷锟斤拷锟斤拷锟斤拷前锟斤拷氩黄ワ拷洹�
+	uart3_init(921600);
 	
 	//Serial port 5 initialization, communication baud rate 115200, 
 	//can be used to communicate with ROS terminal
-	//串口5初始化，通信波特率115200，可用于与ROS端通信
+	//锟斤拷锟斤拷5锟斤拷始锟斤拷锟斤拷通锟脚诧拷锟斤拷锟斤拷115200锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷ROS锟斤拷通锟斤拷
 	uart5_init(115200);
 
 	//ADC pin initialization, used to read the battery voltage and potentiometer gear, 
 	//potentiometer gear determines the car after the boot of the car model
-	//ADC引脚初始化，用于读取电池电压与电位器档位，电位器档位决定小车开机后的小车适配型号
+	//ADC锟斤拷锟脚筹拷始锟斤拷锟斤拷锟斤拷锟节讹拷取锟斤拷氐锟窖癸拷锟斤拷位锟斤拷锟斤拷位锟斤拷锟斤拷位锟斤拷锟斤拷位锟斤拷锟斤拷小锟斤拷锟斤拷锟斤拷锟斤拷锟叫★拷锟斤拷锟斤拷锟斤拷秃锟�
  	Adc_Init();  
 	Adc_POWER_Init();
 	
 	//Initialize the CAN communication interface
-  //CAN通信接口初始化
+  //CAN通锟脚接口筹拷始锟斤拷
 	CAN1_Mode_Init(1,7,6,3,0); 
 	
   //According to the tap position of the potentiometer, determine which type of car needs to be matched, 
   //and then initialize the corresponding parameters	
-  //根据电位器的档位判断需要适配的是哪一种型号的小车，然后进行对应的参数初始化	
+  //锟斤拷锟捷碉拷位锟斤拷锟侥碉拷位锟叫讹拷锟斤拷要锟斤拷锟斤拷锟斤拷锟斤拷锟揭伙拷锟斤拷秃诺锟叫★拷锟斤拷锟饺伙拷锟斤拷锟叫讹拷应锟侥诧拷锟斤拷锟斤拷始锟斤拷	
 	Robot_Select();                 
 	
 	 //Encoder A is initialized to read the real time speed of motor C  
-  //编码器A初始化，用于读取电机C的实时速度	
+  //锟斤拷锟斤拷锟斤拷A锟斤拷始锟斤拷锟斤拷锟斤拷锟节讹拷取锟斤拷锟紺锟斤拷实时锟劫讹拷	
 	 Encoder_Init_TIM2();
 	//Encoder B is initialized to read the real time speed of motor D
-  //编码器B初始化，用于读取电机D的实时速度	
+  //锟斤拷锟斤拷锟斤拷B锟斤拷始锟斤拷锟斤拷锟斤拷锟节讹拷取锟斤拷锟紻锟斤拷实时锟劫讹拷	
 	  Encoder_Init_TIM3();   
 	//Encoder C is initialized to read the real time speed of motor B
-  //编码器C初始化，用于读取电机B的实时速度	
+  //锟斤拷锟斤拷锟斤拷C锟斤拷始锟斤拷锟斤拷锟斤拷锟节讹拷取锟斤拷锟紹锟斤拷实时锟劫讹拷	
 	  Encoder_Init_TIM4(); 
 	//Encoder D is initialized to read the real time speed of motor A
-	//编码器D初始化，用于读取电机A的实时速度
+	//锟斤拷锟斤拷锟斤拷D锟斤拷始锟斤拷锟斤拷锟斤拷锟节讹拷取锟斤拷锟紸锟斤拷实时锟劫讹拷
 		Encoder_Init_TIM5(); 
 	
-	//定时器12用作舵机的PWM接口
-		TIM12_SERVO_Init(9999,84-1);  //APB1的时钟频率为84M , 频率=84M/((9999+1)*(83+1))=100Hz
+	//锟斤拷时锟斤拷12锟斤拷锟斤拷锟斤拷锟斤拷锟絇WM锟接匡拷
+		TIM12_SERVO_Init(9999,84-1);  //APB1锟斤拷时锟斤拷频锟斤拷为84M , 频锟斤拷=84M/((9999+1)*(83+1))=100Hz
 		
-		//普通小车默认定时器8用作航模接口
-   // TIM8_SERVO_Init(9999,168-1);//APB2的时钟频率为168M , 频率=168M/((9999+1)*(167+1))=100Hz
-	 //Initialize the model remote control interface		
-	 //初始化航模遥控接口
-	 TIM8_Cap_Init(9999,168-1);  //高级定时器TIM8的时钟频率为168M    
+		//锟斤拷通小锟斤拷默锟较讹拷时锟斤拷8锟斤拷锟斤拷锟斤拷模锟接匡拷
+   // TIM8_SERVO_Init(9999,168-1);//APB2锟斤拷时锟斤拷频锟斤拷为168M , 频锟斤拷=168M/((9999+1)*(167+1))=100Hz
+	 //Initialize the model remote control interface
+	 //锟斤拷始锟斤拷锟斤拷模遥锟截接匡拷
+	 TIM8_Cap_Init(9999,168-1);  //锟竭硷拷锟斤拷时锟斤拷TIM8锟斤拷时锟斤拷频锟斤拷为168M
+
+	 //Free-running 1 MHz microsecond time base for sensor sample timestamps.
+	 //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷时锟斤拷锟斤拷 1MHz 微锟诫级锟斤拷锟斤拷时锟斤拷准锟斤拷
+	 TIM7_Init();
 
   //Initialize motor speed control and, for controlling motor speed, PWM frequency 10kHz
-  //初始化电机速度控制以及，用于控制电机速度，PWM频率10KHZ
-  //APB2时钟频率为168M，满PWM为16799，频率=168M/((16799+1)*(0+1))=10k
+  //锟斤拷始锟斤拷锟斤拷锟斤拷俣瓤锟斤拷锟斤拷约锟斤拷锟斤拷锟斤拷诳锟斤拷频锟斤拷锟劫度ｏ拷PWM频锟斤拷10KHZ
+  //APB2时锟斤拷频锟斤拷为168M锟斤拷锟斤拷PWM为16799锟斤拷频锟斤拷=168M/((16799+1)*(0+1))=10k
 		TIM1_PWM_Init(16799,0);
 		TIM9_PWM_Init(16799,0);
 		TIM10_PWM_Init(16799,0);
 		TIM11_PWM_Init(16799,0);
 		
   //IIC initialization for MPU6050 
-  //IIC初始化，用于MPU6050  
+  //IIC锟斤拷始锟斤拷锟斤拷锟斤拷锟斤拷MPU6050  
 	I2C_GPIOInit();
 
   //MPU6050  is initialized to read the vehicle's three-axis attitude, 
 	//three-axis angular velocity and three-axis acceleration information
-  //MPU6050 初始化，用于读取小车三轴姿态、三轴角速度、三轴加速度信息
+  //MPU6050 锟斤拷始锟斤拷锟斤拷锟斤拷锟节讹拷取小锟斤拷锟斤拷锟斤拷锟斤拷态锟斤拷锟斤拷锟斤拷锟斤拷俣取锟斤拷锟斤拷锟斤拷锟劫讹拷锟斤拷息
    MPU6050_initialize();        		
 	
 	//Initialize the hardware interface to the PS2 controller
-	//初始化与PS2手柄连接的硬件接口
+	//锟斤拷始锟斤拷锟斤拷PS2锟街憋拷锟斤拷锟接碉拷硬锟斤拷锟接匡拷
 	PS2_Init();
 	
 	//PS2 gamepad configuration is initialized and configured in analog mode
-  //PS2手柄配置初始化,配置为模拟量模式	
+  //PS2锟街憋拷锟斤拷锟矫筹拷始锟斤拷,锟斤拷锟斤拷为模锟斤拷锟斤拷模式	
 	PS2_SetInit();		 							
 }
diff --git a/BALANCE/system.h b/BALANCE/system.h
index eda2a55..9da1a9a 100644
--- a/BALANCE/system.h
+++ b/BALANCE/system.h
@@ -94,9 +94,11 @@ extern long int ErrorCode;
 void systemInit(void);
 
 /***Macros define***/ /***宏定义***/
-//After starting the car (1000/100Hz =10) for seconds, it is allowed to control the car to move
-//开机(1000/100hz=10)秒后才允许控制小车进行运动
-#define CONTROL_DELAY		1000
+//Collect IMU zero-bias samples for 2 seconds at 100 Hz before enabling control.
+//开机后以100Hz采集2秒IMU零偏，再允许控制小车运动
+#define CONTROL_DELAY		200
+//Balance_task counter needed to represent the same 2 second startup delay.
+#define CONTROL_STARTUP_DELAY_COUNT (2 * CONTROL_FREQUENCY)
 //The number of robot types to determine the value of Divisor_Mode. There are currently 6 car types
 //机器人型号数量，决定Divisor_Mode的值，目前有6种小车类型
 #define CAR_NUMBER    6      
diff --git a/HARDWARE/MPU6050/MPU6050.c b/HARDWARE/MPU6050/MPU6050.c
index 9bb2332..2701e57 100644
--- a/HARDWARE/MPU6050/MPU6050.c
+++ b/HARDWARE/MPU6050/MPU6050.c
@@ -13,14 +13,14 @@
 #define FLASH_MEM_START ((void*)0x1800)
 #define q30  1073741824.0f
 short gyro[3], accel[3], sensors;
-//零点漂移计数
+//锟斤拷锟狡拷萍锟斤拷锟�
 int Deviation_Count;
 
 short sum_gyro[3];
 short sum_accel[3];
 
 // Gyro static error, raw data
-//陀螺仪静差，原始数据
+//锟斤拷锟斤拷锟角撅拷锟筋，原始锟斤拷锟斤拷
 short Deviation_gyro[3],Original_gyro[3];  
 short Deviation_accel[3],Original_accel[3]; 
 float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
@@ -55,11 +55,11 @@ void MPU6050_task(void *pvParameters)
     while(1)
     {	
 			//This task runs at 100Hz
-			//此任务以100Hz的频率运行
+			//锟斤拷锟斤拷锟斤拷锟斤拷100Hz锟斤拷频锟斤拷锟斤拷锟斤拷
 			vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));	
 		
 			//Read the gyroscope zero before starting
-      //开机前，读取陀螺仪零点			
+      //锟斤拷锟斤拷前锟斤拷锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷锟�			
 		  if(Deviation_Count<CONTROL_DELAY)
 		  {	 
 		  	Deviation_Count++;
@@ -78,10 +78,14 @@ void MPU6050_task(void *pvParameters)
 				memcpy(Deviation_accel,accel,sizeof(accel));	
 		  }		
 
-     MPU_Get_Gyroscope(); //得到陀螺仪数据
-     MPU_Get_Accelscope(); //获得加速度计值(原始值)
+     //Stamp just before the I2C burst read, as close to the physical IMU
+     //sample as we can get without a data-ready interrupt.
+     //锟斤拷 I2C 锟斤拷取前锟斤拷悖拷诰锟斤拷锟� DRDY 锟叫断碉拷锟斤拷锟斤拷戮锟斤拷锟斤拷咏锟斤拷锟斤拷锟斤拷锟斤拷锟绞憋拷獭锟�
+     g_imu_sample_time_us = mcu_time_us();
+     MPU_Get_Gyroscope(); //锟矫碉拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+     MPU_Get_Accelscope(); //锟斤拷眉锟斤拷俣燃锟街�(原始值)
     }
-}  
+}
 
 //static  unsigned short inv_orientation_matrix_to_scalar(
 //    const signed char *mtx)
@@ -132,17 +136,17 @@ int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;
 
 /**************************************************************************
 Function: The new ADC data is updated to FIFO array for filtering
-Input   : ax，ay，az：x，y, z-axis acceleration data；gx，gy，gz：x. Y, z-axis angular acceleration data
+Input   : ax锟斤拷ay锟斤拷az锟斤拷x锟斤拷y, z-axis acceleration data锟斤拷gx锟斤拷gy锟斤拷gz锟斤拷x. Y, z-axis angular acceleration data
 Output  : none
-函数功能：将新的ADC数据更新到 FIFO数组，进行滤波处理
-入口参数：ax，ay，az：x，y，z轴加速度数据；gx，gy，gz：x，y，z轴角加速度数据
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟铰碉拷ADC锟斤拷锟捷革拷锟铰碉拷 FIFO锟斤拷锟介，锟斤拷锟斤拷锟剿诧拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟絘x锟斤拷ay锟斤拷az锟斤拷x锟斤拷y锟斤拷z锟斤拷锟斤拷俣锟斤拷锟斤拷荩锟絞x锟斤拷gy锟斤拷gz锟斤拷x锟斤拷y锟斤拷z锟斤拷羌锟斤拷俣锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void  MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
 {
 unsigned char i ;
 int32_t sum=0;
-for(i=1;i<10;i++){	//FIFO 操作
+for(i=1;i<10;i++){	//FIFO 锟斤拷锟斤拷
 MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
 MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
 MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
@@ -150,7 +154,7 @@ MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
 MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
 MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
 }
-MPU6050_FIFO[0][9]=ax;//将新的数据放置到 数据的最后面
+MPU6050_FIFO[0][9]=ax;//锟斤拷锟铰碉拷锟斤拷锟捷凤拷锟矫碉拷 锟斤拷锟捷碉拷锟斤拷锟斤拷锟�
 MPU6050_FIFO[1][9]=ay;
 MPU6050_FIFO[2][9]=az;
 MPU6050_FIFO[3][9]=gx;
@@ -158,7 +162,7 @@ MPU6050_FIFO[4][9]=gy;
 MPU6050_FIFO[5][9]=gz;
 
 sum=0;
-for(i=0;i<10;i++){	//求当前数组的合，再取平均值
+for(i=0;i<10;i++){	//锟斤拷前锟斤拷锟斤拷暮希锟斤拷锟饺∑斤拷锟街�
    sum+=MPU6050_FIFO[0][i];
 }
 MPU6050_FIFO[0][10]=sum/10;
@@ -196,11 +200,11 @@ MPU6050_FIFO[5][10]=sum/10;
 
 /**************************************************************************
 Function: Setting the clock source of mpu6050
-Input   : source：Clock source number
+Input   : source锟斤拷Clock source number
 Output  : none
-函数功能：设置  MPU6050 的时钟源
-入口参数：source：时钟源编号
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷  MPU6050 锟斤拷时锟斤拷源
+锟斤拷诓锟斤拷锟斤拷锟絪ource锟斤拷时锟斤拷源锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
  * CLK_SEL | Clock Source
  * --------+--------------------------------------
  * 0       | Internal oscillator
@@ -231,27 +235,27 @@ void MPU6050_setFullScaleGyroRange(uint8_t range) {
 
 /**************************************************************************
 Function: Setting the maximum range of mpu6050 accelerometer
-Input   : range：Acceleration maximum range number
+Input   : range锟斤拷Acceleration maximum range number
 Output  : none
-函数功能：设置 MPU6050 加速度计的最大量程
-入口参数：range：加速度最大量程编号
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷 MPU6050 锟斤拷锟劫度计碉拷锟斤拷锟斤拷锟斤拷锟�
+锟斤拷诓锟斤拷锟斤拷锟絩ange锟斤拷锟斤拷锟劫讹拷锟斤拷锟斤拷锟斤拷瘫锟斤拷
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
-//#define MPU6050_ACCEL_FS_2          0x00  		//===最大量程+-2G
-//#define MPU6050_ACCEL_FS_4          0x01			//===最大量程+-4G
-//#define MPU6050_ACCEL_FS_8          0x02			//===最大量程+-8G
-//#define MPU6050_ACCEL_FS_16         0x03			//===最大量程+-16G
+//#define MPU6050_ACCEL_FS_2          0x00  		//===锟斤拷锟斤拷锟斤拷锟�+-2G
+//#define MPU6050_ACCEL_FS_4          0x01			//===锟斤拷锟斤拷锟斤拷锟�+-4G
+//#define MPU6050_ACCEL_FS_8          0x02			//===锟斤拷锟斤拷锟斤拷锟�+-8G
+//#define MPU6050_ACCEL_FS_16         0x03			//===锟斤拷锟斤拷锟斤拷锟�+-16G
 void MPU6050_setFullScaleAccelRange(uint8_t range) {
     I2C_WriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
 }
 
 /**************************************************************************
 Function: Set mpu6050 to sleep mode or not
-Input   : enable：1，sleep；0，work；
+Input   : enable锟斤拷1锟斤拷sleep锟斤拷0锟斤拷work锟斤拷
 Output  : none
-函数功能：设置 MPU6050 是否进入睡眠模式
-入口参数：enable：1，睡觉；0，工作；
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷 MPU6050 锟角凤拷锟斤拷锟剿拷锟侥Ｊ�
+锟斤拷诓锟斤拷锟斤拷锟絜nable锟斤拷1锟斤拷睡锟斤拷锟斤拷0锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void MPU6050_setSleepEnabled(uint8_t enabled) {
     I2C_WriteOneBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
@@ -261,9 +265,9 @@ void MPU6050_setSleepEnabled(uint8_t enabled) {
 Function: Read identity
 Input   : none
 Output  : 0x68
-函数功能：读取  MPU6050 WHO_AM_I 标识
-入口参数：无
-返回  值：0x68
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷取  MPU6050 WHO_AM_I 锟斤拷识
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷0x68
 **************************************************************************/
 uint8_t MPU6050_getDeviceID(void) {
 
@@ -277,10 +281,10 @@ uint8_t MPU6050_getDeviceID(void) {
 /**************************************************************************
 Function: Check whether mpu6050 is connected
 Input   : none
-Output  : 1：Connected；0：Not connected
-函数功能：检测MPU6050 是否已经连接
-入口参数：无
-返回  值：1：已连接；0：未连接
+Output  : 1锟斤拷Connected锟斤拷0锟斤拷Not connected
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟組PU6050 锟角凤拷锟窖撅拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷1锟斤拷锟斤拷锟斤拷锟接ｏ拷0锟斤拷未锟斤拷锟斤拷
 **************************************************************************/
 uint8_t MPU6050_testConnection(void) {
    if(MPU6050_getDeviceID() == 0x68)  //0b01101000;
@@ -290,11 +294,11 @@ uint8_t MPU6050_testConnection(void) {
 
 /**************************************************************************
 Function: Setting whether mpu6050 is the host of aux I2C cable
-Input   : enable：1，yes；0;not
+Input   : enable锟斤拷1锟斤拷yes锟斤拷0;not
 Output  : none
-函数功能：设置 MPU6050 是否为AUX I2C线的主机
-入口参数：enable：1，是；0：否
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷 MPU6050 锟角凤拷为AUX I2C锟竭碉拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟絜nable锟斤拷1锟斤拷锟角ｏ拷0锟斤拷锟斤拷
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
     I2C_WriteOneBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
@@ -302,11 +306,11 @@ void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
 
 /**************************************************************************
 Function: Setting whether mpu6050 is the host of aux I2C cable
-Input   : enable：1，yes；0;not
+Input   : enable锟斤拷1锟斤拷yes锟斤拷0;not
 Output  : none
-函数功能：设置 MPU6050 是否为AUX I2C线的主机
-入口参数：enable：1，是；0：否
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷 MPU6050 锟角凤拷为AUX I2C锟竭碉拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟絜nable锟斤拷1锟斤拷锟角ｏ拷0锟斤拷锟斤拷
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
     I2C_WriteOneBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
@@ -316,38 +320,38 @@ void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
 Function: initialization Mpu6050 to enter the available state
 Input   : none
 Output  : none
-函数功能：初始化	MPU6050 以进入可用状态
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷始锟斤拷	MPU6050 锟皆斤拷锟斤拷锟斤拷锟阶刺�
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 u8 MPU6050_initialize(void) 
 	{
 		u8 res;
-	//IIC_Init();  //Initialize the IIC bus //初始化IIC总线
-	I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X80);	//Reset MPUrobot_select_init.h //复位MPUrobot_select_init.h
-  delay_ms(200); //Delay 200 ms //延时200ms
-	I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X00);	//Wake mpurobot_select_init.h //唤醒MPUrobot_select_init.h
+	//IIC_Init();  //Initialize the IIC bus //锟斤拷始锟斤拷IIC锟斤拷锟斤拷
+	I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X80);	//Reset MPUrobot_select_init.h //锟斤拷位MPUrobot_select_init.h
+  delay_ms(200); //Delay 200 ms //锟斤拷时200ms
+	I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X00);	//Wake mpurobot_select_init.h //锟斤拷锟斤拷MPUrobot_select_init.h
 	
-  //MPU6050_Set_Gyro_Fsr(1);  //Gyroscope sensor              //陀螺仪传感器,±500dps=±500°/s ±32768 (gyro/32768*500)*PI/180(rad/s)=gyro/3754.9(rad/s)
+  //MPU6050_Set_Gyro_Fsr(1);  //Gyroscope sensor              //锟斤拷锟斤拷锟角达拷锟斤拷锟斤拷,锟斤拷500dps=锟斤拷500锟斤拷/s 锟斤拷32768 (gyro/32768*500)*PI/180(rad/s)=gyro/3754.9(rad/s)
 	MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_500);
-	//MPU6050_Set_Accel_Fsr(0);	//Acceleration sensor           //加速度传感器,±2g=±2*9.8m/s^2 ±32768 accel/32768*19.6=accel/1671.84
+	//MPU6050_Set_Accel_Fsr(0);	//Acceleration sensor           //锟斤拷锟劫度达拷锟斤拷锟斤拷,锟斤拷2g=锟斤拷2*9.8m/s^2 锟斤拷32768 accel/32768*19.6=accel/1671.84
   MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
-	MPU6050_Set_Rate(50);			//Set the sampling rate to 50Hz //设置采样率50Hz
+	MPU6050_Set_Rate(50);			//Set the sampling rate to 50Hz //锟斤拷锟矫诧拷锟斤拷锟斤拷50Hz
 	
-	I2C_WriteOneByte(devAddr,MPU6050_RA_INT_ENABLE,0X00);	  //Turn off all interrupts //关闭所有中断
-	I2C_WriteOneByte(devAddr,MPU6050_RA_USER_CTRL,0X00);	//The I2C main mode is off //I2C主模式关闭
-	I2C_WriteOneByte(devAddr,MPU6050_RA_FIFO_EN,0X00);	  //Close the FIFO //关闭FIFO
+	I2C_WriteOneByte(devAddr,MPU6050_RA_INT_ENABLE,0X00);	  //Turn off all interrupts //锟截憋拷锟斤拷锟斤拷锟叫讹拷
+	I2C_WriteOneByte(devAddr,MPU6050_RA_USER_CTRL,0X00);	//The I2C main mode is off //I2C锟斤拷模式锟截憋拷
+	I2C_WriteOneByte(devAddr,MPU6050_RA_FIFO_EN,0X00);	  //Close the FIFO //锟截憋拷FIFO
 	//The INT pin is low, enabling bypass mode to read the magnetometer directly
-	//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
+	//INT锟斤拷锟脚低碉拷平锟斤拷效锟斤拷锟斤拷锟斤拷bypass模式锟斤拷锟斤拷锟斤拷直锟接讹拷取锟斤拷锟斤拷锟斤拷
 	I2C_WriteOneByte(devAddr,MPU6050_RA_INT_PIN_CFG,0X80);
 	//Read the ID of MPU6050 
-	//读取MPU6050的ID	
+	//锟斤拷取MPU6050锟斤拷ID	
 	res=I2C_ReadOneByte(devAddr,MPU6050_RA_WHO_AM_I);
-	if(res==MPU6050_DEFAULT_ADDRESS) //The device ID is correct, The correct device ID depends on the AD pin //器件ID正确, 器件ID的正确取决于AD引脚
+	if(res==MPU6050_DEFAULT_ADDRESS) //The device ID is correct, The correct device ID depends on the AD pin //锟斤拷锟斤拷ID锟斤拷确, 锟斤拷锟斤拷ID锟斤拷锟斤拷确取锟斤拷锟斤拷AD锟斤拷锟斤拷
 	{
-		I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X01);	//Set CLKSEL,PLL X axis as reference //设置CLKSEL,PLL X轴为参考
-		I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_2,0X00);	//Acceleration and gyroscope both work //加速度与陀螺仪都工作
-		MPU6050_Set_Rate(50);	                      //Set the sampling rate to 50Hz //设置采样率为50Hz   
+		I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_1,0X01);	//Set CLKSEL,PLL X axis as reference //锟斤拷锟斤拷CLKSEL,PLL X锟斤拷为锟轿匡拷
+		I2C_WriteOneByte(devAddr,MPU6050_RA_PWR_MGMT_2,0X00);	//Acceleration and gyroscope both work //锟斤拷锟劫讹拷锟斤拷锟斤拷锟斤拷锟角讹拷锟斤拷锟斤拷
+		MPU6050_Set_Rate(50);	                      //Set the sampling rate to 50Hz //锟斤拷锟矫诧拷锟斤拷锟斤拷为50Hz   
  	}else return 1;
 	return 0;
 
@@ -357,9 +361,9 @@ u8 MPU6050_initialize(void)
 Function: Initialization of DMP in mpu6050
 Input   : none
 Output  : none
-函数功能：MPU6050内置DMP的初始化
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷MPU6050锟斤拷锟斤拷DMP锟侥筹拷始锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 //void DMP_Init(void)
 //{ 
@@ -395,9 +399,9 @@ Output  : none
 Function: Read the attitude information of DMP in mpu6050
 Input   : none
 Output  : none
-函数功能：读取MPU6050内置DMP的姿态信息
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷取MPU6050锟斤拷锟斤拷DMP锟斤拷锟斤拷态锟斤拷息
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 //void Read_DMP(void)
 //{	
@@ -405,16 +409,16 @@ Output  : none
 //		unsigned char more;
 //		long quat[4];
 
-//				dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);		//读取DMP数据
+//				dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);		//锟斤拷取DMP锟斤拷锟斤拷
 //				if (sensors & INV_WXYZ_QUAT )
 //				{    
 //					 q0=quat[0] / q30;
 //					 q1=quat[1] / q30;
 //					 q2=quat[2] / q30;
-//					 q3=quat[3] / q30; 		//四元数
-//					 Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 	//计算出横滚角
-//					 Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // 计算出俯仰角
-//					 Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	 //计算出偏航角
+//					 q3=quat[3] / q30; 		//锟斤拷元锟斤拷
+//					 Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 	//锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+//					 Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
+//					 Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	 //锟斤拷锟斤拷锟狡拷锟斤拷锟�
 //				}
 
 //}
@@ -422,16 +426,16 @@ Output  : none
 Function: Read mpu6050 built-in temperature sensor data
 Input   : none
 Output  : Centigrade temperature
-函数功能：读取MPU6050内置温度传感器数据
-入口参数：无
-返回  值：摄氏温度
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷取MPU6050锟斤拷锟斤拷锟铰度达拷锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷锟斤拷锟铰讹拷
 **************************************************************************/
 int Read_Temperature(void)
 {	   
 	  float Temp;
 	  Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L);
-		if(Temp>32768) Temp-=65536;	//数据类型转换
-		Temp=(36.53f+Temp/340)*10;	  //温度放大十倍存放
+		if(Temp>32768) Temp-=65536;	//锟斤拷锟斤拷锟斤拷锟斤拷转锟斤拷
+		Temp=(36.53f+Temp/340)*10;	  //锟铰度放达拷十锟斤拷锟斤拷锟�
 	  return (int)Temp;
 }
 
@@ -439,9 +443,9 @@ int Read_Temperature(void)
 Function: Initialize TIM2 as the encoder interface mode
 Input   : LPF: Digital low-pass filtering frequency (Hz)
 Output  : 0: Settings successful, others: Settings failed
-函数功能：设置MPUrobot_select_init.h的数字低通滤波器
-入口参数：lpf:数字低通滤波频率(Hz)
-返回  值：0:设置成功, 其他:设置失败
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷MPUrobot_select_init.h锟斤拷锟斤拷锟街碉拷通锟剿诧拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟絣pf:锟斤拷锟街碉拷通锟剿诧拷频锟斤拷(Hz)
+锟斤拷锟斤拷  值锟斤拷0:锟斤拷锟矫成癸拷, 锟斤拷锟斤拷:锟斤拷锟斤拷失锟斤拷
 **************************************************************************/
 unsigned char MPU6050_Set_LPF(u16 lpf)
 {
@@ -452,15 +456,15 @@ unsigned char MPU6050_Set_LPF(u16 lpf)
 	else if(lpf>=20)data=4;
 	else if(lpf>=10)data=5;
 	else data=6; 
-	return I2C_WriteOneByte(devAddr,MPU6050_RA_CONFIG,data); //Set the digital lowpass filter//设置数字低通滤波器  
+	return I2C_WriteOneByte(devAddr,MPU6050_RA_CONFIG,data); //Set the digital lowpass filter//锟斤拷锟斤拷锟斤拷锟街碉拷通锟剿诧拷锟斤拷  
 }
 /**************************************************************************
 Function: Initialize TIM2 as the encoder interface mode
 Input   : rate:4~1000(Hz)
 Output  : 0: Settings successful, others: Settings failed
-函数功能：设置MPUrobot_select_init.h的采样率(假定Fs=1KHz)
-入口参数：rate:4~1000(Hz)
-返回  值：0:设置成功, 其他:设置失败
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷MPUrobot_select_init.h锟侥诧拷锟斤拷锟斤拷(锟劫讹拷Fs=1KHz)
+锟斤拷诓锟斤拷锟斤拷锟絩ate:4~1000(Hz)
+锟斤拷锟斤拷  值锟斤拷0:锟斤拷锟矫成癸拷, 锟斤拷锟斤拷:锟斤拷锟斤拷失锟斤拷
 **************************************************************************/
 unsigned char MPU6050_Set_Rate(u16 rate)
 {
@@ -468,42 +472,42 @@ unsigned char MPU6050_Set_Rate(u16 rate)
 	if(rate>1000)rate=1000;
 	if(rate<4)rate=4;
 	data=1000/rate-1;
-	data=I2C_WriteOneByte(devAddr,MPU6050_RA_SMPLRT_DIV,data);	//Set the digital lowpass filter//设置数字低通滤波器  
- 	return MPU6050_Set_LPF(rate/2);	//Automatically sets LPF to half of the sampling rate //自动设置LPF为采样率的一半
+	data=I2C_WriteOneByte(devAddr,MPU6050_RA_SMPLRT_DIV,data);	//Set the digital lowpass filter//锟斤拷锟斤拷锟斤拷锟街碉拷通锟剿诧拷锟斤拷  
+ 	return MPU6050_Set_LPF(rate/2);	//Automatically sets LPF to half of the sampling rate //锟皆讹拷锟斤拷锟斤拷LPF为锟斤拷锟斤拷锟绞碉拷一锟斤拷
 }
 
 /**************************************************************************
 Function: Initialize TIM2 as the encoder interface mode
 Input   : Gx, Gy, Gz: raw readings (plus or minus) of the x,y, and z axes of the gyroscope
 Output  : 0: success, others: error code
-函数功能：获得陀螺仪值(原始值)
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷锟斤拷锟斤拷锟街�(原始值)
 **************************************************************************/
 void MPU_Get_Gyroscope(void)
 {
-		gyro[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L);    //读取X轴陀螺仪
-		gyro[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);    //读取Y轴陀螺仪
-		gyro[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);    //读取Z轴陀螺仪
+		gyro[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L);    //锟斤拷取X锟斤拷锟斤拷锟斤拷锟斤拷
+		gyro[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);    //锟斤拷取Y锟斤拷锟斤拷锟斤拷锟斤拷
+		gyro[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);    //锟斤拷取Z锟斤拷锟斤拷锟斤拷锟斤拷
 	
-	if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //开机前10秒
+	if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //锟斤拷锟斤拷前10锟斤拷
 		{
 
-			Led_Count=1; //LED high frequency flashing //LED高频闪烁
-			Flag_Stop=1; //The software fails to flag location 1 //软件失能标志位置1		
+			Led_Count=1; //LED high frequency flashing //LED锟斤拷频锟斤拷烁
+			Flag_Stop=1; //The software fails to flag location 1 //锟斤拷锟斤拷失锟杰憋拷志位锟斤拷1		
 		}
-	else //10 seconds after starting //开机10秒后
+	else //10 seconds after starting //锟斤拷锟斤拷10锟斤拷锟�
 		{  
 			if(Deviation_Count==CONTROL_DELAY)
-				Flag_Stop=0; //The software fails to flag location 0 //软件失能标志位置0
-			Led_Count=300; //The LED returns to normal flicker frequency //LED恢复正常闪烁频率	
+				Flag_Stop=0; //The software fails to flag location 0 //锟斤拷锟斤拷失锟杰憋拷志位锟斤拷0
+			Led_Count=300; //The LED returns to normal flicker frequency //LED锟街革拷锟斤拷锟斤拷锟斤拷烁频锟斤拷	
 			
 			//Save the raw data to update zero by clicking the user button
-			//保存原始数据用于单击用户按键更新零点
+			//锟斤拷锟斤拷原始锟斤拷锟斤拷锟斤拷锟节碉拷锟斤拷锟矫伙拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
 			Original_gyro[0] =gyro[0];  
 			Original_gyro[1] =gyro[1];  
 			Original_gyro[2]= gyro[2];			
 			
 			//Removes zero drift data
-			//去除零点漂移的数据
+			//去锟斤拷锟斤拷锟狡拷频锟斤拷锟斤拷锟�
 			gyro[0] =Original_gyro[0]-Deviation_gyro[0];  
 			gyro[1] =Original_gyro[1]-Deviation_gyro[1];  
 			gyro[2]= Original_gyro[2]-Deviation_gyro[2];
@@ -514,28 +518,28 @@ void MPU_Get_Gyroscope(void)
 Function: Initialize TIM2 as the encoder interface mode
 Input   : Gx, Gy, Gz: raw readings (plus or minus) of the x,y, and z axes of the gyroscope
 Output  : 0: success, others: error code
-函数功能：获得加速度计值(原始值)
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷眉锟斤拷俣燃锟街�(原始值)
 **************************************************************************/
 void MPU_Get_Accelscope(void)
 {
-		accel[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度计
-		accel[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //读取X轴加速度计
-		accel[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度计
+		accel[0]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //锟斤拷取X锟斤拷锟斤拷俣燃锟�
+		accel[1]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); //锟斤拷取X锟斤拷锟斤拷俣燃锟�
+		accel[2]=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //锟斤拷取Z锟斤拷锟斤拷俣燃锟�
 	
-		if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //开机前10秒
+		if(Deviation_Count<CONTROL_DELAY) // 10 seconds before starting //锟斤拷锟斤拷前10锟斤拷
 		{
 	
 		}
-		else //10 seconds after starting //开机10秒后
+		else //10 seconds after starting //锟斤拷锟斤拷10锟斤拷锟�
 		{  		
 			//Save the raw data to update zero by clicking the user button
-			//保存原始数据用于单击用户按键更新零点
+			//锟斤拷锟斤拷原始锟斤拷锟斤拷锟斤拷锟节碉拷锟斤拷锟矫伙拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟�
 			Original_accel[0] =accel[0];  
 			Original_accel[1] =accel[1];  
 			Original_accel[2]= accel[2];			
 			
 			//Removes zero drift data
-			//去除零点漂移的数据
+			//去锟斤拷锟斤拷锟狡拷频锟斤拷锟斤拷锟�
 			accel[0] =Original_accel[0]-Deviation_accel[0];  
 			accel[1] =Original_accel[1]-Deviation_accel[1];  
 			accel[2]= Original_accel[2]-Deviation_accel[2]+16384;
diff --git a/HARDWARE/motor.h b/HARDWARE/motor.h
index c8e4f32..c1eeaea 100644
--- a/HARDWARE/motor.h
+++ b/HARDWARE/motor.h
@@ -49,7 +49,7 @@
 #define EN     PDin(3)  
 
 #define Servo_PWM  TIM12->CCR2
-#define SERVO_INIT 1590  //Servo zero point //锟斤拷锟斤拷锟斤拷
+#define SERVO_INIT 1670  //Servo straight-ahead point (calibrated 1/R~=0) //锟斤拷直锟斤拷前锟斤拷
 
 void Enable_Pin(void);
 void Servo_PWM_Init(u16 arr,u16 psc);
diff --git a/HARDWARE/timer.c b/HARDWARE/timer.c
index 846e553..42dd2b2 100644
--- a/HARDWARE/timer.c
+++ b/HARDWARE/timer.c
@@ -2,28 +2,28 @@
 
 //Input the capture flag for channel 1, 
 //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
-//通道1输入捕获标志，高两位做捕获标志，低6位做溢出标志		
+//通锟斤拷1锟斤拷锟诫捕锟斤拷锟街撅拷锟斤拷锟斤拷锟轿伙拷锟斤拷锟斤拷锟斤拷志锟斤拷锟斤拷6位锟斤拷锟斤拷锟斤拷锟街�		
 u8 TIM8CH1_CAPTURE_STA = 0;	
 u16 TIM8CH1_CAPTURE_UPVAL;
 u16 TIM8CH1_CAPTURE_DOWNVAL;
 
 //Input the capture flag for channel 2, 
 //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
-//通道2输入捕获标志，高两位做捕获标志，低6位做溢出标志	
+//通锟斤拷2锟斤拷锟诫捕锟斤拷锟街撅拷锟斤拷锟斤拷锟轿伙拷锟斤拷锟斤拷锟斤拷志锟斤拷锟斤拷6位锟斤拷锟斤拷锟斤拷锟街�	
 u8 TIM8CH2_CAPTURE_STA = 0;		
 u16 TIM8CH2_CAPTURE_UPVAL;
 u16 TIM8CH2_CAPTURE_DOWNVAL;
 
 //Input the capture flag for channel 3, 
 //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
-//通道3输入捕获标志，高两位做捕获标志，低6位做溢出标志	
+//通锟斤拷3锟斤拷锟诫捕锟斤拷锟街撅拷锟斤拷锟斤拷锟轿伙拷锟斤拷锟斤拷锟斤拷志锟斤拷锟斤拷6位锟斤拷锟斤拷锟斤拷锟街�	
 u8 TIM8CH3_CAPTURE_STA = 0;		
 u16 TIM8CH3_CAPTURE_UPVAL;
 u16 TIM8CH3_CAPTURE_DOWNVAL;
 
 //Input the capture flag for channel 4, 
 //the capture flag for the higher bits, and the overflow flag for the lower 6 bits
-//通道4输入捕获标志，高两位做捕获标志，低6位做溢出标志
+//通锟斤拷4锟斤拷锟诫捕锟斤拷锟街撅拷锟斤拷锟斤拷锟轿伙拷锟斤拷锟斤拷锟斤拷志锟斤拷锟斤拷6位锟斤拷锟斤拷锟斤拷锟街�
 u8 TIM8CH4_CAPTURE_STA = 0;			
 u16 TIM8CH4_CAPTURE_UPVAL;
 u16 TIM8CH4_CAPTURE_DOWNVAL;
@@ -34,19 +34,19 @@ u32 TIM8_T3;
 u32 TIM8_T4;
 
 //Variables related to remote control acquisition of model aircraft
-//航模遥控采集相关变量
+//锟斤拷模遥锟截采硷拷锟斤拷乇锟斤拷锟�
 int Remoter_Ch1=1500,Remoter_Ch2=1500,Remoter_Ch3=1500,Remoter_Ch4=1500;
 //Model aircraft remote control receiver variable
-//航模遥控接收变量
+//锟斤拷模遥锟截斤拷锟秸憋拷锟斤拷
 int L_Remoter_Ch1=1500,L_Remoter_Ch2=1500,L_Remoter_Ch3=1500,L_Remoter_Ch4=1500;  
 
 /**************************************************************************
 Function: Model aircraft remote control initialization function, timer 1 input capture initialization
 Input   : arr: Automatic reload value, psc: clock preset frequency
 Output  : none
-函数功能：航模遥控初始化函数，定时器1输入捕获初始化
-入口参数：arr：自动重装值，psc：时钟预分频数 
-返 回 值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷模遥锟截筹拷始锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷时锟斤拷1锟斤拷锟诫捕锟斤拷锟绞硷拷锟�
+锟斤拷诓锟斤拷锟斤拷锟絘rr锟斤拷锟皆讹拷锟斤拷装值锟斤拷psc锟斤拷时锟斤拷预锟斤拷频锟斤拷 
+锟斤拷 锟斤拷 值锟斤拷锟斤拷
 **************************************************************************/ 
 void TIM8_Cap_Init(u16 arr, u16 psc)
 {
@@ -55,14 +55,14 @@ void TIM8_Cap_Init(u16 arr, u16 psc)
 	NVIC_InitTypeDef NVIC_InitStructure;
 	TIM_ICInitTypeDef TIM_ICInitStructure;
 	
-	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);  	//TIM1时钟使能    
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); 	//使能PORTE时钟	
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);  	//TIM1时锟斤拷使锟斤拷    
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); 	//使锟斤拷PORTE时锟斤拷	
 		
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; //GPIOC
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
-	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;	//速度100MHz
-	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
-	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; //下拉
+	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//锟斤拷锟矫癸拷锟斤拷
+	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;	//锟劫讹拷100MHz
+	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //锟斤拷锟届复锟斤拷锟斤拷锟�
+	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; //锟斤拷锟斤拷
 	GPIO_Init(GPIOC,&GPIO_InitStructure); 
 
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); 
@@ -70,104 +70,104 @@ void TIM8_Cap_Init(u16 arr, u16 psc)
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8);
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_TIM8);
 
-	/*** Initialize timer 1 || 初始化定时器1 ***/
-	//Set the counter to automatically reload //设定计数器自动重装值 
+	/*** Initialize timer 1 || 锟斤拷始锟斤拷锟斤拷时锟斤拷1 ***/
+	//Set the counter to automatically reload //锟借定锟斤拷锟斤拷锟斤拷锟皆讹拷锟斤拷装值 
 	TIM_TimeBaseStructure.TIM_Period = arr; 
-	//Pre-divider //预分频器 
+	//Pre-divider //预锟斤拷频锟斤拷 
 	TIM_TimeBaseStructure.TIM_Prescaler = psc; 	
-	//Set the clock split: TDTS = Tck_tim //设置时钟分割:TDTS = Tck_tim
+	//Set the clock split: TDTS = Tck_tim //锟斤拷锟斤拷时锟接分革拷:TDTS = Tck_tim
 	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
-	//TIM up count mode //TIM向上计数模式	
+	//TIM up count mode //TIM锟斤拷锟较硷拷锟斤拷模式	
 	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 
 	//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
-	//根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
+	//锟斤拷锟斤拷TIM_TimeBaseInitStruct锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷TIMx锟斤拷时锟斤拷锟斤拷锟斤拷锟轿�
 	TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); 
 
-	/*** 初始化TIM1输入捕获参数，通道1 || Initialize TIM1 for the capture parameter, channel 1 ***/
-	//Select input //选择输入端 
+	/*** 锟斤拷始锟斤拷TIM1锟斤拷锟诫捕锟斤拷锟斤拷锟斤拷锟酵拷锟�1 || Initialize TIM1 for the capture parameter, channel 1 ***/
+	//Select input //选锟斤拷锟斤拷锟斤拷锟� 
 	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1; 
-  //Rising edge capture //上升沿捕获
+  //Rising edge capture //锟斤拷锟斤拷锟截诧拷锟斤拷
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;	
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 
-	//Configure input frequency division, regardless of frequency //配置输入分频,不分频 
+	//Configure input frequency division, regardless of frequency //锟斤拷锟斤拷锟斤拷锟斤拷锟狡�,锟斤拷锟斤拷频 
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	
-  //IC1F=0000 Configure input filter //配置输入滤波器
+  //IC1F=0000 Configure input filter //锟斤拷锟斤拷锟斤拷锟斤拷锟剿诧拷锟斤拷
 	TIM_ICInitStructure.TIM_ICFilter = 0x0F;	  
 	TIM_ICInit(TIM8, &TIM_ICInitStructure);
 
-	/*** 初始化TIM1输入捕获参数，通道2 || Initialize TIM1 for the capture parameter, channel 2 ***/
-	//CC1S=01 Select input //选择输入端 
+	/*** 锟斤拷始锟斤拷TIM1锟斤拷锟诫捕锟斤拷锟斤拷锟斤拷锟酵拷锟�2 || Initialize TIM1 for the capture parameter, channel 2 ***/
+	//CC1S=01 Select input //选锟斤拷锟斤拷锟斤拷锟� 
 	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
-	//Rising edge capture //上升沿捕获
+	//Rising edge capture //锟斤拷锟斤拷锟截诧拷锟斤拷
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;	
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 
-	//Configure input frequency division, regardless of frequency //配置输入分频,不分频 
+	//Configure input frequency division, regardless of frequency //锟斤拷锟斤拷锟斤拷锟斤拷锟狡�,锟斤拷锟斤拷频 
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	  
-	TIM_ICInitStructure.TIM_ICFilter = 0x00;	  //IC1F=0000 配置输入滤波器
+	TIM_ICInitStructure.TIM_ICFilter = 0x00;	  //IC1F=0000 锟斤拷锟斤拷锟斤拷锟斤拷锟剿诧拷锟斤拷
 	TIM_ICInit(TIM8, &TIM_ICInitStructure);
 
-	/*** 初始化TIM1输入捕获参数，通道3 || Initialize TIM1 for the capture parameter, channel 3 ***/
-	//Select input //选择输入端 
+	/*** 锟斤拷始锟斤拷TIM1锟斤拷锟诫捕锟斤拷锟斤拷锟斤拷锟酵拷锟�3 || Initialize TIM1 for the capture parameter, channel 3 ***/
+	//Select input //选锟斤拷锟斤拷锟斤拷锟� 
 	TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;   
-	//Rising edge capture //上升沿捕获
+	//Rising edge capture //锟斤拷锟斤拷锟截诧拷锟斤拷
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 
-	//Configure input frequency division, regardless of frequency //配置输入分频,不分频 
+	//Configure input frequency division, regardless of frequency //锟斤拷锟斤拷锟斤拷锟斤拷锟狡�,锟斤拷锟斤拷频 
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	  
-	//IC1F=0000 Configure input filter //配置输入滤波器，不滤波  
+	//IC1F=0000 Configure input filter //锟斤拷锟斤拷锟斤拷锟斤拷锟剿诧拷锟斤拷锟斤拷锟斤拷锟剿诧拷  
 	TIM_ICInitStructure.TIM_ICFilter = 0x00;	  
 	TIM_ICInit(TIM8, &TIM_ICInitStructure);
 
-	/*** 初始化TIM1输入捕获参数，通道4 || Initialize TIM1 for the capture parameter, channel 4 ***/
-	//Select input //选择输入端 
+	/*** 锟斤拷始锟斤拷TIM1锟斤拷锟诫捕锟斤拷锟斤拷锟斤拷锟酵拷锟�4 || Initialize TIM1 for the capture parameter, channel 4 ***/
+	//Select input //选锟斤拷锟斤拷锟斤拷锟� 
 	TIM_ICInitStructure.TIM_Channel = TIM_Channel_4; 
-	//Rising edge capture //上升沿捕获
+	//Rising edge capture //锟斤拷锟斤拷锟截诧拷锟斤拷
 	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;	
 	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; 
-	//Configure input frequency division, regardless of frequency //配置输入分频,不分频 
+	//Configure input frequency division, regardless of frequency //锟斤拷锟斤拷锟斤拷锟斤拷锟狡�,锟斤拷锟斤拷频 
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;	   
-	//IC1F=0000 Configure input filter //配置输入滤波器，不滤波  
+	//IC1F=0000 Configure input filter //锟斤拷锟斤拷锟斤拷锟斤拷锟剿诧拷锟斤拷锟斤拷锟斤拷锟剿诧拷  
 	TIM_ICInitStructure.TIM_ICFilter = 0x00;	  
 	TIM_ICInit(TIM8, &TIM_ICInitStructure);
 
-  /*** interrupt packet initialization || 中断分组初始化 ***/
-  //TIM1 interrupts //TIM1中断
+  /*** interrupt packet initialization || 锟叫断凤拷锟斤拷锟绞硷拷锟� ***/
+  //TIM1 interrupts //TIM1锟叫讹拷
 	NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn; 
-  //Preempt priority 0 //先占优先级0级	
+  //Preempt priority 0 //锟斤拷占锟斤拷锟饺硷拷0锟斤拷	
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;  
-	//Level 0 from priority //从优先级0级
+	//Level 0 from priority //锟斤拷锟斤拷锟饺硷拷0锟斤拷
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; 
-	//IRQ channels are enabled //IRQ通道被使能
+	//IRQ channels are enabled //IRQ通锟斤拷锟斤拷使锟斤拷
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 
 	//Initializes the peripheral NVIC register according to the parameters specified in NVIC_InitStruct
-	//根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器 
+	//锟斤拷锟斤拷NVIC_InitStruct锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷锟斤拷锟斤拷NVIC锟侥达拷锟斤拷 
 	NVIC_Init(&NVIC_InitStructure);   
 	
   //Allow CC1IE,CC2IE,CC3IE,CC4IE to catch interrupts, not allowed update_interrupts
-  //不允许更新中断，允许CC1IE,CC2IE,CC3IE,CC4IE捕获中断	
+  //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟叫断ｏ拷锟斤拷锟斤拷CC1IE,CC2IE,CC3IE,CC4IE锟斤拷锟斤拷锟叫讹拷	
 	TIM_ITConfig(TIM8, TIM_IT_CC1|TIM_IT_CC2|TIM_IT_CC3|TIM_IT_CC4,	ENABLE);   
-	//Advanced timer output must be enabled //高级定时器输出必须使能这句	
+	//Advanced timer output must be enabled //锟竭硷拷锟斤拷时锟斤拷锟斤拷锟斤拷锟斤拷锟绞癸拷锟斤拷锟斤拷	
 	TIM_CtrlPWMOutputs(TIM8,ENABLE); 	
-	//Enable timer //使能定时器
+	//Enable timer //使锟杰讹拷时锟斤拷
 	TIM_Cmd(TIM8, ENABLE); 		
 }
 /**************************************************************************
 Function: Model aircraft remote control receiving interrupt, namely timer 8 input capture interrupt
 Input   : none
 Output  : none
-函数功能：航模遥控接收中断，即定时器8输入捕获中断
-入口参数：无
-返 回 值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷模遥锟截斤拷锟斤拷锟叫断ｏ拷锟斤拷锟斤拷时锟斤拷8锟斤拷锟诫捕锟斤拷锟叫讹拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷 锟斤拷 值锟斤拷锟斤拷
 **************************************************************************/ 
 void TIM8_CC_IRQHandler(void)
 {
-	//连接航模遥遥控器后，需要推下前进杆，才可以正式航模控制小车
+	//锟斤拷锟接猴拷模遥遥锟斤拷锟斤拷锟斤拷锟斤拷要锟斤拷锟斤拷前锟斤拷锟剿ｏ拷锟脚匡拷锟斤拷锟斤拷式锟斤拷模锟斤拷锟斤拷小锟斤拷
 	//After connecting the remote controller of the model aircraft, 
 	//you need to push down the forward lever to officially control the car of the model aircraft
   if(Remoter_Ch2>1600&&Remote_ON_Flag==0&&Deviation_Count>=CONTROL_DELAY)
   {
 		//Model aircraft remote control mark position 1, other marks position 0
-		//航模遥控标志位置1，其它标志位置0
+		//锟斤拷模遥锟截憋拷志位锟斤拷1锟斤拷锟斤拷锟斤拷锟斤拷志位锟斤拷0
 		Remote_ON_Flag=1;
 	  APP_ON_Flag=0;
 		PS2_ON_Flag=0;
@@ -179,131 +179,131 @@ void TIM8_CC_IRQHandler(void)
 	
 
 
-//	//Channel 1 //通道一
+//	//Channel 1 //通锟斤拷一
 	if ((TIM8CH1_CAPTURE_STA & 0X80) == 0) 			
 	{
-		if (TIM_GetITStatus(TIM8, TIM_IT_CC1) != RESET) //A capture event occurred on channel 1 //通道1发生捕获事件
+		if (TIM_GetITStatus(TIM8, TIM_IT_CC1) != RESET) //A capture event occurred on channel 1 //通锟斤拷1锟斤拷锟斤拷锟斤拷锟斤拷锟铰硷拷
 		{
-			TIM_ClearITPendingBit(TIM8, TIM_IT_CC1); //Clear the interrupt flag bit //清除中断标志位
-			if (TIM8CH1_CAPTURE_STA & 0X40)	//A falling edge is caught //捕获到一个下降沿
+			TIM_ClearITPendingBit(TIM8, TIM_IT_CC1); //Clear the interrupt flag bit //锟斤拷锟斤拷卸媳锟街疚�
+			if (TIM8CH1_CAPTURE_STA & 0X40)	//A falling edge is caught //锟斤拷锟斤拷一锟斤拷锟铰斤拷锟斤拷
 			{
-				TIM8CH1_CAPTURE_DOWNVAL = TIM_GetCapture1(TIM8); //Record the timer value at this point //记录下此时的定时器计数值
+				TIM8CH1_CAPTURE_DOWNVAL = TIM_GetCapture1(TIM8); //Record the timer value at this point //锟斤拷录锟铰达拷时锟侥讹拷时锟斤拷锟斤拷锟斤拷值
 				if (TIM8CH1_CAPTURE_DOWNVAL < TIM8CH1_CAPTURE_UPVAL)
 				{
 					TIM8_T1 = 9999;
 				}
 				else
 					TIM8_T1 = 0;
-				Remoter_Ch1 = TIM8CH1_CAPTURE_DOWNVAL - TIM8CH1_CAPTURE_UPVAL + TIM8_T1;	//Time to get the total high level //得到总的高电平的时间
-				if(abs(Remoter_Ch1-L_Remoter_Ch1)>500) Remoter_Ch1=L_Remoter_Ch1; //Filter //滤波
+				Remoter_Ch1 = TIM8CH1_CAPTURE_DOWNVAL - TIM8CH1_CAPTURE_UPVAL + TIM8_T1;	//Time to get the total high level //锟矫碉拷锟杰的高碉拷平锟斤拷时锟斤拷
+				if(abs(Remoter_Ch1-L_Remoter_Ch1)>500) Remoter_Ch1=L_Remoter_Ch1; //Filter //锟剿诧拷
 					 L_Remoter_Ch1=Remoter_Ch1;
 				
-				TIM8CH1_CAPTURE_STA = 0; //Capture flag bit to zero	//捕获标志位清零
-				TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //设置为上升沿捕获		  
+				TIM8CH1_CAPTURE_STA = 0; //Capture flag bit to zero	//锟斤拷锟斤拷锟街疚伙拷锟斤拷锟�
+				TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //锟斤拷锟斤拷为锟斤拷锟斤拷锟截诧拷锟斤拷		  
 			}
 			else 
 			{
 				//When the capture time occurs but not the falling edge, the first time the rising edge is captured, record the timer value at this time
-				//发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值
-				TIM8CH1_CAPTURE_UPVAL = TIM_GetCapture1(TIM8); //Obtain rising edge data //获取上升沿数据
-				TIM8CH1_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //标记已捕获到上升沿
-				TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //设置为下降沿捕获
+				//锟斤拷锟斤拷锟斤拷锟斤拷时锟戒但锟斤拷锟斤拷锟铰斤拷锟截ｏ拷锟斤拷一锟轿诧拷锟斤拷锟斤拷锟斤拷锟截ｏ拷锟斤拷录锟斤拷时锟侥讹拷时锟斤拷锟斤拷锟斤拷值
+				TIM8CH1_CAPTURE_UPVAL = TIM_GetCapture1(TIM8); //Obtain rising edge data //锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+				TIM8CH1_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //锟斤拷锟斤拷巡锟斤拷锟斤拷锟斤拷锟斤拷锟�
+				TIM_OC1PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //锟斤拷锟斤拷为锟铰斤拷锟截诧拷锟斤拷
 			}
 		}
 	}
-  //Channel 2 //通道二
+  //Channel 2 //通锟斤拷锟斤拷
 	if ((TIM8CH2_CAPTURE_STA & 0X80) == 0)		
 	{
-		if (TIM_GetITStatus(TIM8, TIM_IT_CC2) != RESET)	//A capture event occurred on channel 2 //通道2发生捕获事件
+		if (TIM_GetITStatus(TIM8, TIM_IT_CC2) != RESET)	//A capture event occurred on channel 2 //通锟斤拷2锟斤拷锟斤拷锟斤拷锟斤拷锟铰硷拷
 		{
-			TIM_ClearITPendingBit(TIM8, TIM_IT_CC2); //Clear the interrupt flag bit //清除中断标志位
-			if (TIM8CH2_CAPTURE_STA & 0X40)	//A falling edge is caught //捕获到一个下降沿
+			TIM_ClearITPendingBit(TIM8, TIM_IT_CC2); //Clear the interrupt flag bit //锟斤拷锟斤拷卸媳锟街疚�
+			if (TIM8CH2_CAPTURE_STA & 0X40)	//A falling edge is caught //锟斤拷锟斤拷一锟斤拷锟铰斤拷锟斤拷
 			{
-				TIM8CH2_CAPTURE_DOWNVAL = TIM_GetCapture2(TIM8); //Record the timer value at this point //记录下此时的定时器计数值
+				TIM8CH2_CAPTURE_DOWNVAL = TIM_GetCapture2(TIM8); //Record the timer value at this point //锟斤拷录锟铰达拷时锟侥讹拷时锟斤拷锟斤拷锟斤拷值
 				if (TIM8CH2_CAPTURE_DOWNVAL < TIM8CH2_CAPTURE_UPVAL)
 				{
 					TIM8_T2 = 9999;
 				}
 				else
 					TIM8_T2 = 0;
-				Remoter_Ch2 = TIM8CH2_CAPTURE_DOWNVAL - TIM8CH2_CAPTURE_UPVAL + TIM8_T2; //Time to get the total high level //得到总的高电平的时间
-				if(abs(Remoter_Ch2-L_Remoter_Ch2)>500)Remoter_Ch2=L_Remoter_Ch2; //Filter //滤波
+				Remoter_Ch2 = TIM8CH2_CAPTURE_DOWNVAL - TIM8CH2_CAPTURE_UPVAL + TIM8_T2; //Time to get the total high level //锟矫碉拷锟杰的高碉拷平锟斤拷时锟斤拷
+				if(abs(Remoter_Ch2-L_Remoter_Ch2)>500)Remoter_Ch2=L_Remoter_Ch2; //Filter //锟剿诧拷
 				L_Remoter_Ch2=Remoter_Ch2;
 				
-				TIM8CH2_CAPTURE_STA = 0; //Capture flag bit to zero	//捕获标志位清零
-				TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //设置为上升沿捕获		  
+				TIM8CH2_CAPTURE_STA = 0; //Capture flag bit to zero	//锟斤拷锟斤拷锟街疚伙拷锟斤拷锟�
+				TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //锟斤拷锟斤拷为锟斤拷锟斤拷锟截诧拷锟斤拷		  
 			}
 			else 
 			{
 				//When the capture time occurs but not the falling edge, the first time the rising edge is captured, record the timer value at this time
-				//发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值
-				TIM8CH2_CAPTURE_UPVAL = TIM_GetCapture2(TIM8); //Obtain rising edge data //获取上升沿数据
-				TIM8CH2_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //标记已捕获到上升沿
-				TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //设置为下降沿捕获
+				//锟斤拷锟斤拷锟斤拷锟斤拷时锟戒但锟斤拷锟斤拷锟铰斤拷锟截ｏ拷锟斤拷一锟轿诧拷锟斤拷锟斤拷锟斤拷锟截ｏ拷锟斤拷录锟斤拷时锟侥讹拷时锟斤拷锟斤拷锟斤拷值
+				TIM8CH2_CAPTURE_UPVAL = TIM_GetCapture2(TIM8); //Obtain rising edge data //锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+				TIM8CH2_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //锟斤拷锟斤拷巡锟斤拷锟斤拷锟斤拷锟斤拷锟�
+				TIM_OC2PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //锟斤拷锟斤拷为锟铰斤拷锟截诧拷锟斤拷
 			}
 		}
 	}
-  //Channel 3 //通道三
+  //Channel 3 //通锟斤拷锟斤拷
 	if ((TIM8CH3_CAPTURE_STA & 0X80) == 0)			
 	{
-		if (TIM_GetITStatus(TIM8, TIM_IT_CC3) != RESET)	//A capture event occurred on channel 3 //通道3发生捕获事件
+		if (TIM_GetITStatus(TIM8, TIM_IT_CC3) != RESET)	//A capture event occurred on channel 3 //通锟斤拷3锟斤拷锟斤拷锟斤拷锟斤拷锟铰硷拷
 		{
-			TIM_ClearITPendingBit(TIM8, TIM_IT_CC3); //Clear the interrupt flag bit //清除中断标志位
-			if (TIM8CH3_CAPTURE_STA & 0X40)	//A falling edge is caught //捕获到一个下降沿
+			TIM_ClearITPendingBit(TIM8, TIM_IT_CC3); //Clear the interrupt flag bit //锟斤拷锟斤拷卸媳锟街疚�
+			if (TIM8CH3_CAPTURE_STA & 0X40)	//A falling edge is caught //锟斤拷锟斤拷一锟斤拷锟铰斤拷锟斤拷
 			{
-				TIM8CH3_CAPTURE_DOWNVAL = TIM_GetCapture3(TIM8); //Record the timer value at this point //记录下此时的定时器计数值
+				TIM8CH3_CAPTURE_DOWNVAL = TIM_GetCapture3(TIM8); //Record the timer value at this point //锟斤拷录锟铰达拷时锟侥讹拷时锟斤拷锟斤拷锟斤拷值
 				if (TIM8CH3_CAPTURE_DOWNVAL < TIM8CH3_CAPTURE_UPVAL)
 				{
 					TIM8_T3 = 9999;
 				}
 				else
 					TIM8_T3 = 0;
-				Remoter_Ch3 = TIM8CH3_CAPTURE_DOWNVAL - TIM8CH3_CAPTURE_UPVAL + TIM8_T3; //Time to get the total high level //得到总的高电平的时间
-				if(abs(Remoter_Ch3-L_Remoter_Ch3)>500)Remoter_Ch3=L_Remoter_Ch3; //Filter //滤波
+				Remoter_Ch3 = TIM8CH3_CAPTURE_DOWNVAL - TIM8CH3_CAPTURE_UPVAL + TIM8_T3; //Time to get the total high level //锟矫碉拷锟杰的高碉拷平锟斤拷时锟斤拷
+				if(abs(Remoter_Ch3-L_Remoter_Ch3)>500)Remoter_Ch3=L_Remoter_Ch3; //Filter //锟剿诧拷
 									L_Remoter_Ch3=Remoter_Ch3;
-				TIM8CH3_CAPTURE_STA = 0; //Capture flag bit to zero	//捕获标志位清零
-				TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //设置为上升沿捕获		  
+				TIM8CH3_CAPTURE_STA = 0; //Capture flag bit to zero	//锟斤拷锟斤拷锟街疚伙拷锟斤拷锟�
+				TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //锟斤拷锟斤拷为锟斤拷锟斤拷锟截诧拷锟斤拷		  
 			}
 			else 
 			{
 				//When the capture time occurs but not the falling edge, the first time the rising edge is captured, record the timer value at this time
-				//发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值
-				TIM8CH3_CAPTURE_UPVAL = TIM_GetCapture3(TIM8); //Obtain rising edge data //获取上升沿数据
-				TIM8CH3_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //标记已捕获到上升沿
-				TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //设置为下降沿捕获
+				//锟斤拷锟斤拷锟斤拷锟斤拷时锟戒但锟斤拷锟斤拷锟铰斤拷锟截ｏ拷锟斤拷一锟轿诧拷锟斤拷锟斤拷锟斤拷锟截ｏ拷锟斤拷录锟斤拷时锟侥讹拷时锟斤拷锟斤拷锟斤拷值
+				TIM8CH3_CAPTURE_UPVAL = TIM_GetCapture3(TIM8); //Obtain rising edge data //锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+				TIM8CH3_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //锟斤拷锟斤拷巡锟斤拷锟斤拷锟斤拷锟斤拷锟�
+				TIM_OC3PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //锟斤拷锟斤拷为锟铰斤拷锟截诧拷锟斤拷
 			}
 		}
 	}
 //	
 
-		//Channel 4 //通道四
+		//Channel 4 //通锟斤拷锟斤拷
 		if ((TIM8CH4_CAPTURE_STA & 0X80) == 0)		
 		{
-			if (TIM_GetITStatus(TIM8, TIM_IT_CC4) != RESET)	//A capture event occurred on channel 4 //通道4发生捕获事件
+			if (TIM_GetITStatus(TIM8, TIM_IT_CC4) != RESET)	//A capture event occurred on channel 4 //通锟斤拷4锟斤拷锟斤拷锟斤拷锟斤拷锟铰硷拷
 			{
-				TIM_ClearITPendingBit(TIM8, TIM_IT_CC4); //Clear the interrupt flag bit //清除中断标志位
-				if (TIM8CH4_CAPTURE_STA & 0X40)	//A falling edge is caught //捕获到一个下降沿
+				TIM_ClearITPendingBit(TIM8, TIM_IT_CC4); //Clear the interrupt flag bit //锟斤拷锟斤拷卸媳锟街疚�
+				if (TIM8CH4_CAPTURE_STA & 0X40)	//A falling edge is caught //锟斤拷锟斤拷一锟斤拷锟铰斤拷锟斤拷
 				{
-					TIM8CH4_CAPTURE_DOWNVAL = TIM_GetCapture4(TIM8); //Record the timer value at this point //记录下此时的定时器计数值
+					TIM8CH4_CAPTURE_DOWNVAL = TIM_GetCapture4(TIM8); //Record the timer value at this point //锟斤拷录锟铰达拷时锟侥讹拷时锟斤拷锟斤拷锟斤拷值
 					if (TIM8CH4_CAPTURE_DOWNVAL < TIM8CH4_CAPTURE_UPVAL)
 					{
 						TIM8_T4 = 9999;
 					}
 					else
 						TIM8_T4 = 0;
-					Remoter_Ch4 = TIM8CH4_CAPTURE_DOWNVAL - TIM8CH4_CAPTURE_UPVAL + TIM8_T4; //Time to get the total high level //得到总的高电平的时间
-					if(abs(Remoter_Ch4-L_Remoter_Ch4)>500)Remoter_Ch4=L_Remoter_Ch4; //Filter //滤波
+					Remoter_Ch4 = TIM8CH4_CAPTURE_DOWNVAL - TIM8CH4_CAPTURE_UPVAL + TIM8_T4; //Time to get the total high level //锟矫碉拷锟杰的高碉拷平锟斤拷时锟斤拷
+					if(abs(Remoter_Ch4-L_Remoter_Ch4)>500)Remoter_Ch4=L_Remoter_Ch4; //Filter //锟剿诧拷
 					L_Remoter_Ch4=Remoter_Ch4;				
-					TIM8CH4_CAPTURE_STA = 0; //Capture flag bit to zero	//捕获标志位清零
-					TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //设置为上升沿捕获		  
+					TIM8CH4_CAPTURE_STA = 0; //Capture flag bit to zero	//锟斤拷锟斤拷锟街疚伙拷锟斤拷锟�
+					TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Rising); //Set to rising edge capture //锟斤拷锟斤拷为锟斤拷锟斤拷锟截诧拷锟斤拷		  
 				}
 				else 
 				{
 					//When the capture time occurs but not the falling edge, the first time the rising edge is captured, record the timer value at this time
-				  //发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值
-					TIM8CH4_CAPTURE_UPVAL = TIM_GetCapture4(TIM8); //Obtain rising edge data //获取上升沿数据
-					TIM8CH4_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //标记已捕获到上升沿
-					TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //设置为下降沿捕获
+				  //锟斤拷锟斤拷锟斤拷锟斤拷时锟戒但锟斤拷锟斤拷锟铰斤拷锟截ｏ拷锟斤拷一锟轿诧拷锟斤拷锟斤拷锟斤拷锟截ｏ拷锟斤拷录锟斤拷时锟侥讹拷时锟斤拷锟斤拷锟斤拷值
+					TIM8CH4_CAPTURE_UPVAL = TIM_GetCapture4(TIM8); //Obtain rising edge data //锟斤拷取锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+					TIM8CH4_CAPTURE_STA |= 0X40; //The flag has been caught on the rising edge //锟斤拷锟斤拷巡锟斤拷锟斤拷锟斤拷锟斤拷锟�
+					TIM_OC4PolarityConfig(TIM8, TIM_ICPolarity_Falling); //Set to Falling Edge Capture //锟斤拷锟斤拷为锟铰斤拷锟截诧拷锟斤拷
 				}
 			}
 		}
@@ -312,25 +312,25 @@ void TIM8_CC_IRQHandler(void)
 Function: TIM1 Update Interrupt
 Input   : none
 Output  : none
-函数功能：定时器8更新中断
-入口参数：无
-返回  值：无 
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷时锟斤拷8锟斤拷锟斤拷锟叫讹拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷 
 **************************************************************************/
 void TIM8_UP_TIM13_IRQHandler(void) 
 { 
 	//Clear the interrupt flag bit
-	//清除中断标志位 
+	//锟斤拷锟斤拷卸媳锟街疚� 
   TIM8->SR&=~(1<<0);	    
 }
 
 void TIM8_SERVO_Init(u16 arr,u16 psc)
 {
 	GPIO_InitTypeDef GPIO_InitStructure;           //IO
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //定时器
-	TIM_OCInitTypeDef  TIM_OCInitStructure;        //PWM输出
+	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //锟斤拷时锟斤拷
+	TIM_OCInitTypeDef  TIM_OCInitStructure;        //PWM锟斤拷锟�
 	
-	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);  	//TIM1时钟使能    
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); 	//使能PORTE时钟	
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE);  	//TIM1时锟斤拷使锟斤拷    
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); 	//使锟斤拷PORTE时锟斤拷	
 		
 	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF; 
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;
@@ -345,55 +345,55 @@ void TIM8_SERVO_Init(u16 arr,u16 psc)
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource9,GPIO_AF_TIM8); 
 
 
-	/*** Initialize timer 1 || 初始化定时器1 ***/
-	//Set the counter to automatically reload //设定计数器自动重装值 
+	/*** Initialize timer 1 || 锟斤拷始锟斤拷锟斤拷时锟斤拷1 ***/
+	//Set the counter to automatically reload //锟借定锟斤拷锟斤拷锟斤拷锟皆讹拷锟斤拷装值 
 	TIM_TimeBaseStructure.TIM_Period = arr; 
-	//Pre-divider //预分频器 
+	//Pre-divider //预锟斤拷频锟斤拷 
 	TIM_TimeBaseStructure.TIM_Prescaler = psc; 	
-	//Set the clock split: TDTS = Tck_tim //设置时钟分割:TDTS = Tck_tim
+	//Set the clock split: TDTS = Tck_tim //锟斤拷锟斤拷时锟接分革拷:TDTS = Tck_tim
 	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
-	//TIM up count mode //TIM向上计数模式	
+	//TIM up count mode //TIM锟斤拷锟较硷拷锟斤拷模式	
 	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 
 	//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
-	//根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
+	//锟斤拷锟斤拷TIM_TimeBaseInitStruct锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷TIMx锟斤拷时锟斤拷锟斤拷锟斤拷锟轿�
 	TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); 
 
 
-	//-----------舵机初始化-----------//
+	//-----------锟斤拷锟斤拷锟绞硷拷锟�-----------//
 	//Select Timer mode :TIM Pulse Width Modulation mode 1
-  //选择定时器模式:TIM脉冲宽度调制模式1
+  //选锟斤拷时锟斤拷模式:TIM锟斤拷锟斤拷锟斤拷鹊锟斤拷锟侥Ｊ�1
  	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; 
 	//Compare output enablement
-	//比较输出使能
+	//锟饺斤拷锟斤拷锟绞癸拷锟�
 	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; 
 	//Set the pulse value of the capture comparison register to be loaded
-	//设置待装入捕获比较寄存器的脉冲值
+	//锟斤拷锟矫达拷装锟诫捕锟斤拷冉霞拇锟斤拷锟斤拷锟斤拷锟斤拷锟街�
 	TIM_OCInitStructure.TIM_Pulse = 0; 
   //Output polarity :TIM output polarity is higher	
-  //输出极性:TIM输出比较极性高	
+  //锟斤拷锟斤拷锟斤拷锟�:TIM锟斤拷锟斤拷冉霞锟斤拷愿锟�	
 	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;   
 	TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;   
 	
 	//Initialize the peripheral TIMX based on the parameter specified in TIM_OCINITSTRUCT
-  //根据TIM_OCInitStruct中指定的参数初始化外设TIMx	
+  //锟斤拷锟斤拷TIM_OCInitStruct锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷锟斤拷锟斤拷TIMx	
 	TIM_OC1Init(TIM8, &TIM_OCInitStructure); 
 	TIM_OC2Init(TIM8, &TIM_OCInitStructure); 
 	TIM_OC3Init(TIM8, &TIM_OCInitStructure); 
 	TIM_OC4Init(TIM8, &TIM_OCInitStructure); 
 	//Channel preload enable
-	//通道预装载使能	 
+	//通锟斤拷预装锟斤拷使锟斤拷	 
 	TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
 	TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
 	TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);
 	TIM_OC4PreloadConfig(TIM8, TIM_OCPreload_Enable);
-	//-----------舵机初始化-----------//
+	//-----------锟斤拷锟斤拷锟绞硷拷锟�-----------//
 
 	TIM_CtrlPWMOutputs(TIM8,ENABLE); 	
-	//Enable timer //使能定时器
+	//Enable timer //使锟杰讹拷时锟斤拷
 	TIM_Cmd(TIM8, ENABLE); 		 
 
   //The channel value is initialized to 1500, corresponding to the steering gear zero
-	//通道值初始化为1500，舵机零点对应值
+	//通锟斤拷值锟斤拷始锟斤拷为1500锟斤拷锟斤拷锟斤拷锟斤拷锟接χ�
 //	TIM8->CCR1=1500;
 //	TIM8->CCR2=1500;
 //	TIM8->CCR3=1500;
@@ -404,11 +404,11 @@ void TIM8_SERVO_Init(u16 arr,u16 psc)
 void TIM12_SERVO_Init(u16 arr,u16 psc)
 {
 	GPIO_InitTypeDef GPIO_InitStructure;           //IO
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //定时器
-	TIM_OCInitTypeDef  TIM_OCInitStructure;        //PWM输出
+	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; //锟斤拷时锟斤拷
+	TIM_OCInitTypeDef  TIM_OCInitStructure;        //PWM锟斤拷锟�
 	
-	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE);  	//TIM1时钟使能    
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); 	//使能PORTE时钟	
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12,ENABLE);  	//TIM1时锟斤拷使锟斤拷    
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); 	//使锟斤拷PORTE时锟斤拷	
 		
 	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF; 
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14|GPIO_Pin_15;
@@ -421,48 +421,48 @@ void TIM12_SERVO_Init(u16 arr,u16 psc)
 	GPIO_PinAFConfig(GPIOB,GPIO_PinSource15,GPIO_AF_TIM12); 
 
 
-	/*** Initialize timer 1 || 初始化定时器1 ***/
-	//Set the counter to automatically reload //设定计数器自动重装值 
+	/*** Initialize timer 1 || 锟斤拷始锟斤拷锟斤拷时锟斤拷1 ***/
+	//Set the counter to automatically reload //锟借定锟斤拷锟斤拷锟斤拷锟皆讹拷锟斤拷装值 
 	TIM_TimeBaseStructure.TIM_Period = arr; 
-	//Pre-divider //预分频器 
+	//Pre-divider //预锟斤拷频锟斤拷 
 	TIM_TimeBaseStructure.TIM_Prescaler = psc; 	
-	//Set the clock split: TDTS = Tck_tim //设置时钟分割:TDTS = Tck_tim
+	//Set the clock split: TDTS = Tck_tim //锟斤拷锟斤拷时锟接分革拷:TDTS = Tck_tim
 	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
-	//TIM up count mode //TIM向上计数模式	
+	//TIM up count mode //TIM锟斤拷锟较硷拷锟斤拷模式	
 	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 
 	//Initializes the timebase unit for TIMX based on the parameter specified in TIM_TimeBaseInitStruct
-	//根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
+	//锟斤拷锟斤拷TIM_TimeBaseInitStruct锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷TIMx锟斤拷时锟斤拷锟斤拷锟斤拷锟轿�
 	TIM_TimeBaseInit(TIM12, &TIM_TimeBaseStructure); 
 
 
-	//-----------舵机初始化-----------//
+	//-----------锟斤拷锟斤拷锟绞硷拷锟�-----------//
 	//Select Timer mode :TIM Pulse Width Modulation mode 1
-  //选择定时器模式:TIM脉冲宽度调制模式1
+  //选锟斤拷时锟斤拷模式:TIM锟斤拷锟斤拷锟斤拷鹊锟斤拷锟侥Ｊ�1
  	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; 
 	//Compare output enablement
-	//比较输出使能
+	//锟饺斤拷锟斤拷锟绞癸拷锟�
 	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; 
 	//Set the pulse value of the capture comparison register to be loaded
-	//设置待装入捕获比较寄存器的脉冲值
+	//锟斤拷锟矫达拷装锟诫捕锟斤拷冉霞拇锟斤拷锟斤拷锟斤拷锟斤拷锟街�
 	TIM_OCInitStructure.TIM_Pulse = 0; 
   //Output polarity :TIM output polarity is higher	
-  //输出极性:TIM输出比较极性高	
+  //锟斤拷锟斤拷锟斤拷锟�:TIM锟斤拷锟斤拷冉霞锟斤拷愿锟�	
 	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;   
 	TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;   
 	
 	//Initialize the peripheral TIMX based on the parameter specified in TIM_OCINITSTRUCT
-  //根据TIM_OCInitStruct中指定的参数初始化外设TIMx	
+  //锟斤拷锟斤拷TIM_OCInitStruct锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷锟斤拷锟斤拷TIMx	
 	TIM_OC1Init(TIM12, &TIM_OCInitStructure); 
 		TIM_OC2Init(TIM12, &TIM_OCInitStructure); 
 	//Channel preload enable
-	//通道预装载使能	 
+	//通锟斤拷预装锟斤拷使锟斤拷	 
 	TIM_OC1PreloadConfig(TIM12, TIM_OCPreload_Enable);
 		TIM_OC2PreloadConfig(TIM12, TIM_OCPreload_Enable);
-	//-----------舵机初始化-----------//
+	//-----------锟斤拷锟斤拷锟绞硷拷锟�-----------//
 
 
 	TIM_CtrlPWMOutputs(TIM12,ENABLE); 	
-	//Enable timer //使能定时器
+	//Enable timer //使锟杰讹拷时锟斤拷
 	TIM_Cmd(TIM12, ENABLE); 		 
 
   //Initialize both servo channels to the configured steering center.
@@ -471,3 +471,83 @@ void TIM12_SERVO_Init(u16 arr,u16 psc)
 
 }
 
+/**************************************************************************
+Function: Free-running microsecond time base for sensor timestamps
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷锟斤拷锟节达拷锟斤拷锟斤拷锟斤拷锟斤拷时锟斤拷锟轿拷爰讹拷锟斤拷锟绞憋拷锟阶�
+TIM7 counts at 1 MHz (1 tick = 1 us) and wraps every 65.536 ms. An update
+interrupt increments a 32-bit high word, extending the clock to 48 usable
+bits (~8.9 years) that only ever counts up. Timers TIM2-TIM5 are taken by
+the encoders and TIM8 by input capture, so TIM7 is the free APB1 timer.
+TIM7 锟斤拷 1MHz 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷 65.536ms 锟斤拷锟狡ｏ拷锟斤拷锟铰革拷锟叫讹拷锟斤拷位锟斤拷 32位锟斤拷锟街高ｏ拷
+锟斤拷展锟斤拷只锟斤拷锟斤拷锟斤拷锟斤拷牡锟斤拷锟绞憋拷洹IM2~TIM5 锟斤拷锟斤拷锟斤拷锟斤拷锟矫ｏ拷TIM8 锟斤拷锟诫捕锟斤拷锟斤拷锟矫ｏ拷
+锟斤拷 TIM7 锟斤拷锟叫★拷
+**************************************************************************/
+static volatile uint32_t g_time_high = 0;
+
+void TIM7_Init(void)
+{
+	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+	NVIC_InitTypeDef NVIC_InitStructure;
+
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
+
+	//APB1 timer clock = 84 MHz, prescaler 84 -> 1 MHz (1 us per tick).
+	//APB1 锟斤拷时锟斤拷时锟斤拷 84MHz锟斤拷预锟斤拷频 84 -> 1MHz锟斤拷每锟斤拷锟斤拷 1us锟斤拷
+	TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+	TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1;
+	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
+	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+	TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStructure);
+
+	TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
+	TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE);
+
+	//Keep the overflow ISR below configMAX_SYSCALL_INTERRUPT_PRIORITY so it
+	//never calls FreeRTOS APIs; it only bumps a counter.
+	//锟叫断诧拷锟斤拷锟斤拷 FreeRTOS API锟斤拷锟斤拷锟斤拷锟斤拷一锟斤拷锟斤拷锟斤拷锟斤拷锟饺硷拷锟较低ｏ拷锟斤拷锟斤拷锟斤拷锟饺匡拷锟狡★拷
+	NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+
+	TIM_Cmd(TIM7, ENABLE);
+}
+
+void TIM7_IRQHandler(void)
+{
+	if(TIM_GetITStatus(TIM7, TIM_IT_Update) != RESET)
+	{
+		TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
+		g_time_high++;
+	}
+}
+
+/**************************************************************************
+Function: Read the 64-bit monotonic microsecond timestamp
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷取 64位锟斤拷锟斤拷锟斤拷锟斤拷微锟斤拷时锟斤拷锟�
+Glitch-free composition: re-read the high word around CNT and, if the
+update flag is pending while CNT is still low, account for the overflow
+that the ISR has not serviced yet.
+锟斤拷止锟斤拷隙锟斤拷锟窖ｏ拷锟斤拷 CNT 锟斤拷围锟斤拷锟轿讹拷锟竭帮拷锟街ｏ拷锟斤拷锟斤拷锟斤拷锟斤拷锟截凤拷锟斤拷锟斤拷 CNT 锟斤拷系锟酵ｏ拷
+说锟斤拷 ISR 锟斤拷未锟斤拷锟斤拷锟矫回ｏ拷锟街讹拷锟斤拷位锟斤拷
+**************************************************************************/
+uint64_t mcu_time_us(void)
+{
+	uint32_t high1, high2, cnt, sr;
+
+	do {
+		high1 = g_time_high;
+		cnt = TIM7->CNT;
+		sr = TIM7->SR;
+		high2 = g_time_high;
+	} while(high1 != high2);
+
+	//Overflow happened but the update ISR has not run yet.
+	//锟窖伙拷锟狡碉拷锟斤拷锟铰革拷锟叫讹拷锟斤拷未执锟叫★拷
+	if((sr & TIM_SR_UIF) && cnt < 0x8000U)
+		high1++;
+
+	return ((uint64_t)high1 << 16) | cnt;
+}
+
diff --git a/HARDWARE/timer.h b/HARDWARE/timer.h
index 598f2ee..203a56f 100644
--- a/HARDWARE/timer.h
+++ b/HARDWARE/timer.h
@@ -4,6 +4,8 @@
 void TIM8_Cap_Init(u16 arr, u16 psc);
 void TIM12_SERVO_Init(u16 arr,u16 psc);
 void TIM8_SERVO_Init(u16 arr,u16 psc);
+void TIM7_Init(void);
+uint64_t mcu_time_us(void);
 
 extern int L_Remoter_Ch1,L_Remoter_Ch2,L_Remoter_Ch3,L_Remoter_Ch4;
 extern int Remoter_Ch1,Remoter_Ch2,Remoter_Ch3,Remoter_Ch4;
diff --git a/HARDWARE/usartx.c b/HARDWARE/usartx.c
index 9fa47ef..59e960f 100644
--- a/HARDWARE/usartx.c
+++ b/HARDWARE/usartx.c
@@ -3,13 +3,22 @@ SEND_DATA Send_Data;
 RECEIVE_DATA Receive_Data;
 extern int Time_count;
 
+//Sample timestamps written by the acquisition tasks (see usartx.h).
+//Session ID stays constant for one MCU boot; a fixed nonzero marker is
+//enough for the host to detect a restart (counter cleared to 0).
+//锟缴采硷拷锟斤拷锟斤拷写锟斤拷牟锟斤拷锟绞憋拷锟斤拷(锟斤拷 usartx.h)锟斤拷锟结话ID锟节憋拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷洌�
+//锟教讹拷锟斤拷0值锟斤拷锟较达拷锟斤拷锟斤拷锟斤拷锟绞讹拷锟斤拷鼗锟斤拷锟�
+volatile unsigned long long g_speed_sample_time_us = 0;
+volatile unsigned long long g_imu_sample_time_us = 0;
+volatile unsigned int g_session_id = 0xA5A5A5A5U;
+
 /**************************************************************************
 Function: Usartx3, Usartx1,Usartx5 and CAN send data task 
 Input   : none
 Output  : none
-函数功能：串口3、串口1、串口5、CAN发送数据任务
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷3锟斤拷锟斤拷锟斤拷1锟斤拷锟斤拷锟斤拷5锟斤拷CAN锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void data_task(void *pvParameters)
 {
@@ -17,25 +26,22 @@ void data_task(void *pvParameters)
 	
    while(1)
     {	
-			//The task is run at 20hz
-			//此任务以20Hz的频率运行
-			vTaskDelayUntil(&lastWakeTime, F2T(RATE_20_HZ));
+			//The task is run at 200hz. Only ROS (USART3) is sent, via non-blocking DMA.
+			//锟斤拷锟斤拷锟斤拷锟斤拷200Hz锟斤拷频锟斤拷锟斤拷锟斤拷锟斤拷只锟斤拷ROS(锟斤拷锟斤拷3)锟斤拷锟斤拷锟斤拷锟斤拷DMA锟斤拷锟藉发锟酵★拷
+			vTaskDelayUntil(&lastWakeTime, F2T(RATE_200_HZ));
 			//Assign the data to be sent
-			//对要进行发送的数据进行赋值
-			data_transition(); 
-			USART1_SEND();     //Serial port 1 sends data //串口1发送数据
-			USART3_SEND();     //Serial port 3 (ROS) sends data  //串口3(ROS)发送数据
-			USART5_SEND();		 //Serial port 5 sends data //串口5发送数据
-			CAN_SEND();        //CAN send data //CAN发送数据	
+			//锟斤拷要锟斤拷锟叫凤拷锟酵碉拷锟斤拷锟捷斤拷锟叫革拷值
+			data_transition();
+			USART3_SEND();     //Serial port 3 (ROS) sends data  //锟斤拷锟斤拷3(ROS)锟斤拷锟斤拷锟斤拷锟斤拷
 		}
 }
 /**************************************************************************
 Function: The data sent by the serial port is assigned
 Input   : none
 Output  : none
-函数功能：串口发送的数据进行赋值
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟节凤拷锟酵碉拷锟斤拷锟捷斤拷锟叫革拷值
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void data_transition(void)
 {
@@ -44,7 +50,7 @@ void data_transition(void)
 	
 	//According to different vehicle types, different kinematics algorithms were selected to carry out the forward kinematics solution, 
 	//and the three-axis velocity was obtained from each wheel velocity
-	//根据不同车型选择不同运动学算法进行运动学正解，从各车轮速度求出三轴速度
+	//锟斤拷锟捷诧拷同锟斤拷锟斤拷选锟斤拷同锟剿讹拷学锟姐法锟斤拷锟斤拷锟剿讹拷学锟斤拷锟解，锟接革拷锟斤拷锟斤拷锟劫讹拷锟斤拷锟斤拷锟斤拷锟斤拷俣锟�
 	switch(Car_Mode)
 	{	
 		case Mec_Car:      
@@ -84,32 +90,32 @@ void data_transition(void)
 			break; 
 	}
 	
-	//The acceleration of the triaxial acceleration //加速度计三轴加速度
-	Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; //The accelerometer Y-axis is converted to the ros coordinate X axis //加速度计Y轴转换到ROS坐标X轴
-	Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; //The accelerometer X-axis is converted to the ros coordinate y axis //加速度计X轴转换到ROS坐标Y轴
-	Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; //The accelerometer Z-axis is converted to the ros coordinate Z axis //加速度计Z轴转换到ROS坐标Z轴
+	//The acceleration of the triaxial acceleration //锟斤拷锟劫度硷拷锟斤拷锟斤拷锟斤拷俣锟�
+	Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; //The accelerometer Y-axis is converted to the ros coordinate X axis //锟斤拷锟劫度硷拷Y锟斤拷转锟斤拷锟斤拷ROS锟斤拷锟斤拷X锟斤拷
+	Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; //The accelerometer X-axis is converted to the ros coordinate y axis //锟斤拷锟劫度硷拷X锟斤拷转锟斤拷锟斤拷ROS锟斤拷锟斤拷Y锟斤拷
+	Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; //The accelerometer Z-axis is converted to the ros coordinate Z axis //锟斤拷锟劫度硷拷Z锟斤拷转锟斤拷锟斤拷ROS锟斤拷锟斤拷Z锟斤拷
 	
-	//The Angle velocity of the triaxial velocity //角速度计三轴角速度
-	Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; //The Y-axis is converted to the ros coordinate X axis //角速度计Y轴转换到ROS坐标X轴
-	Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; //The X-axis is converted to the ros coordinate y axis //角速度计X轴转换到ROS坐标Y轴
+	//The Angle velocity of the triaxial velocity //锟斤拷锟劫度硷拷锟斤拷锟斤拷锟斤拷俣锟�
+	Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; //The Y-axis is converted to the ros coordinate X axis //锟斤拷锟劫度硷拷Y锟斤拷转锟斤拷锟斤拷ROS锟斤拷锟斤拷X锟斤拷
+	Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; //The X-axis is converted to the ros coordinate y axis //锟斤拷锟劫度硷拷X锟斤拷转锟斤拷锟斤拷ROS锟斤拷锟斤拷Y锟斤拷
 	if(Flag_Stop==0) 
 		//If the motor control bit makes energy state, the z-axis velocity is sent normall
-	  //如果电机控制位使能状态，那么正常发送Z轴角速度
+	  //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷位使锟斤拷状态锟斤拷锟斤拷么锟斤拷锟斤拷锟斤拷锟斤拷Z锟斤拷锟斤拷俣锟�
 		Send_Data.Sensor_Str.Gyroscope.Z_data=gyro[2];  
 	else  
 		//If the robot is static (motor control dislocation), the z-axis is 0
-    //如果机器人是静止的（电机控制位失能），那么发送的Z轴角速度为0		
+    //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷蔷锟街癸拷模锟斤拷锟斤拷锟斤拷锟斤拷位失锟杰ｏ拷锟斤拷锟斤拷么锟斤拷锟酵碉拷Z锟斤拷锟斤拷俣锟轿�0		
 		Send_Data.Sensor_Str.Gyroscope.Z_data=0;        
 	
 	//Battery voltage (this is a thousand times larger floating point number, which will be reduced by a thousand times as well as receiving the data).
-	//电池电压(这里将浮点数放大一千倍传输，相应的在接收端在接收到数据后也会缩小一千倍)
+	//锟斤拷氐锟窖�(锟斤拷锟斤将锟斤拷锟斤拷锟斤拷锟脚达拷一千锟斤拷锟斤拷锟戒，锟斤拷应锟斤拷锟节斤拷锟秸讹拷锟节斤拷锟秸碉拷锟斤拷锟捷猴拷也锟斤拷锟斤拷小一千锟斤拷)
 	Send_Data.Sensor_Str.Power_Voltage = Voltage*1000; 
 	
 	Send_Data.buffer[0]=Send_Data.Sensor_Str.Frame_Header; //Frame_heade //帧头
-  Send_Data.buffer[1]=Flag_Stop; //Car software loss marker //小车软件失能标志位
+  Send_Data.buffer[1]=Flag_Stop; //Car software loss marker //小锟斤拷锟斤拷锟斤拷失锟杰憋拷志位
 	
 	//The three-axis speed of / / car is split into two eight digit Numbers
-	//小车三轴速度,各轴都拆分为两个8位数据再发送
+	//小锟斤拷锟斤拷锟斤拷锟劫讹拷,锟斤拷锟结都锟斤拷锟轿拷锟斤拷锟�8位锟斤拷锟斤拷锟劫凤拷锟斤拷
 	Send_Data.buffer[2]=Send_Data.Sensor_Str.X_speed >>8; 
 	Send_Data.buffer[3]=Send_Data.Sensor_Str.X_speed ;    
 	Send_Data.buffer[4]=Send_Data.Sensor_Str.Y_speed>>8;  
@@ -118,7 +124,7 @@ void data_transition(void)
 	Send_Data.buffer[7]=Send_Data.Sensor_Str.Z_speed ;    
 	
 	//The acceleration of the triaxial axis of / / imu accelerometer is divided into two eight digit reams
-	//IMU加速度计三轴加速度,各轴都拆分为两个8位数据再发送
+	//IMU锟斤拷锟劫度硷拷锟斤拷锟斤拷锟斤拷俣锟�,锟斤拷锟结都锟斤拷锟轿拷锟斤拷锟�8位锟斤拷锟斤拷锟劫凤拷锟斤拷
 	Send_Data.buffer[8]=Send_Data.Sensor_Str.Accelerometer.X_data>>8; 
 	Send_Data.buffer[9]=Send_Data.Sensor_Str.Accelerometer.X_data;   
 	Send_Data.buffer[10]=Send_Data.Sensor_Str.Accelerometer.Y_data>>8;
@@ -127,7 +133,7 @@ void data_transition(void)
 	Send_Data.buffer[13]=Send_Data.Sensor_Str.Accelerometer.Z_data;
 	
 	//The axis of the triaxial velocity of the / /imu is divided into two eight digits
-	//IMU角速度计三轴角速度,各轴都拆分为两个8位数据再发送
+	//IMU锟斤拷锟劫度硷拷锟斤拷锟斤拷锟斤拷俣锟�,锟斤拷锟结都锟斤拷锟轿拷锟斤拷锟�8位锟斤拷锟斤拷锟劫凤拷锟斤拷
 	Send_Data.buffer[14]=Send_Data.Sensor_Str.Gyroscope.X_data>>8;
 	Send_Data.buffer[15]=Send_Data.Sensor_Str.Gyroscope.X_data;
 	Send_Data.buffer[16]=Send_Data.Sensor_Str.Gyroscope.Y_data>>8;
@@ -136,72 +142,112 @@ void data_transition(void)
 	Send_Data.buffer[19]=Send_Data.Sensor_Str.Gyroscope.Z_data;
 	
 	//Battery voltage, split into two 8 digit Numbers
-	//电池电压,拆分为两个8位数据发送
-	Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8; 
-	Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage; 
+	//锟斤拷氐锟窖�,锟斤拷锟轿拷锟斤拷锟�8位锟斤拷锟捷凤拷锟斤拷
+	Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8;
+	Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage;
 
-  //Data check digit calculation, Pattern 1 is a data check
-  //数据校验位计算，模式1是发送数据校验
-	Send_Data.buffer[22]=Check_Sum(22,1); 
-	
-	Send_Data.buffer[23]=Send_Data.Sensor_Str.Frame_Tail; //Frame_tail //帧尾
+	//Snapshot the session ID and the two sample timestamps captured by the
+	//acquisition tasks. All appended fields are big-endian (high byte first)
+	//to match the existing >>8 speed/IMU packing.
+	//锟斤拷锟斤拷锟斤拷锟皆采硷拷锟斤拷锟斤拷锟斤拷录锟侥会话ID锟斤拷锟斤拷锟斤拷锟斤拷时锟斤拷锟斤拷锟阶凤拷锟斤拷侄锟斤拷镁锟斤拷锟�(锟斤拷位锟斤拷前)锟斤拷
+	//锟斤拷锟斤拷锟叫碉拷 >>8 锟斤拷锟�/IMU锟斤拷锟斤拷锟绞揭伙拷隆锟�
+	Send_Data.Sensor_Str.Session_Id     = g_session_id;
+	Send_Data.Sensor_Str.Speed_Time_us  = g_speed_sample_time_us;
+	Send_Data.Sensor_Str.Imu_Time_us    = g_imu_sample_time_us;
+
+	//session_id (uint32, big-endian) //锟结话ID
+	Send_Data.buffer[22]=(unsigned char)(Send_Data.Sensor_Str.Session_Id>>24);
+	Send_Data.buffer[23]=(unsigned char)(Send_Data.Sensor_Str.Session_Id>>16);
+	Send_Data.buffer[24]=(unsigned char)(Send_Data.Sensor_Str.Session_Id>>8);
+	Send_Data.buffer[25]=(unsigned char)(Send_Data.Sensor_Str.Session_Id);
+
+	//speed_sample_time_us (uint64, big-endian) //锟劫度诧拷锟斤拷时锟斤拷锟�
+	Send_Data.buffer[26]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>56);
+	Send_Data.buffer[27]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>48);
+	Send_Data.buffer[28]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>40);
+	Send_Data.buffer[29]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>32);
+	Send_Data.buffer[30]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>24);
+	Send_Data.buffer[31]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>16);
+	Send_Data.buffer[32]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us>>8);
+	Send_Data.buffer[33]=(unsigned char)(Send_Data.Sensor_Str.Speed_Time_us);
+
+	//imu_sample_time_us (uint64, big-endian) //IMU锟斤拷锟斤拷时锟斤拷锟�
+	Send_Data.buffer[34]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>56);
+	Send_Data.buffer[35]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>48);
+	Send_Data.buffer[36]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>40);
+	Send_Data.buffer[37]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>32);
+	Send_Data.buffer[38]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>24);
+	Send_Data.buffer[39]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>16);
+	Send_Data.buffer[40]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us>>8);
+	Send_Data.buffer[41]=(unsigned char)(Send_Data.Sensor_Str.Imu_Time_us);
+
+  //Data check digit calculation, Pattern 1 is a data check. Now covers the
+  //42 payload bytes (0..41) preceding the checksum.
+  //锟斤拷锟斤拷校锟斤拷位锟斤拷锟姐，模式1锟角凤拷锟斤拷锟斤拷锟斤拷校锟介。锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷校锟斤拷前 42 锟街斤拷(0..41)锟斤拷
+	Send_Data.buffer[42]=Check_Sum(42,1);
+
+	Send_Data.buffer[43]=Send_Data.Sensor_Str.Frame_Tail; //Frame_tail //帧尾
 }
 /**************************************************************************
 Function: Serial port 1 sends data
 Input   : none
 Output  : none
-函数功能：串口1发送数据
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷1锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void USART1_SEND(void)
 {
-  unsigned char i = 0;	
-	
-	for(i=0; i<24; i++)
+  unsigned char i = 0;
+
+	for(i=0; i<SEND_DATA_SIZE; i++)
 	{
 		usart1_send(Send_Data.buffer[i]);
-	}	 
+	}
 }
 /**************************************************************************
 Function: Serial port 3 sends data
 Input   : none
 Output  : none
-函数功能：串口3发送数据
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷3锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void USART3_SEND(void)
 {
-  unsigned char i = 0;	
-	for(i=0; i<24; i++)
-	{
-		usart3_send(Send_Data.buffer[i]);
-	}	 
+	//Non-blocking DMA transmit of the 24-byte frame. If the previous transfer
+	//is still running, skip this cycle rather than busy-wait.
+	//DMA锟斤拷锟斤拷锟斤拷锟斤拷锟�24锟街斤拷帧锟斤拷锟斤拷一帧未锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷冢锟斤拷锟矫︼拷却锟斤拷锟�
+	if(DMA_GetCmdStatus(DMA1_Stream3) != DISABLE) return;
+
+	DMA_ClearFlag(DMA1_Stream3, DMA_FLAG_TCIF3 | DMA_FLAG_HTIF3 |
+	              DMA_FLAG_TEIF3 | DMA_FLAG_DMEIF3 | DMA_FLAG_FEIF3);
+	DMA_SetCurrDataCounter(DMA1_Stream3, SEND_DATA_SIZE);
+	DMA_Cmd(DMA1_Stream3, ENABLE);
 }
 /**************************************************************************
 Function: Serial port 5 sends data
 Input   : none
 Output  : none
-函数功能：串口5发送数据
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷5锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void USART5_SEND(void)
 {
-  unsigned char i = 0;	
-	for(i=0; i<24; i++)
+  unsigned char i = 0;
+	for(i=0; i<SEND_DATA_SIZE; i++)
 	{
 		usart5_send(Send_Data.buffer[i]);
-	}	 
+	}
 }
 /**************************************************************************
 Function: CAN sends data
 Input   : none
 Output  : none
-函数功能：CAN发送数据
-入口参数：无
-返 回 值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷CAN锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷 锟斤拷 值锟斤拷锟斤拷
 **************************************************************************/
 void CAN_SEND(void) 
 {
@@ -229,9 +275,9 @@ void CAN_SEND(void)
 Function: Serial port 1 initialization
 Input   : none
 Output  : none
-函数功能：串口1初始化
-入口参数：无
-返 回 值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷1锟斤拷始锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷 锟斤拷 值锟斤拷锟斤拷
 **************************************************************************/
 void uart1_init(u32 bound)
 {  	 
@@ -239,50 +285,50 @@ void uart1_init(u32 bound)
 	USART_InitTypeDef USART_InitStructure;
 	NVIC_InitTypeDef NVIC_InitStructure;
 	
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);	 //Enable the gpio clock //使能GPIO时钟
-	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); //Enable the Usart clock //使能USART时钟
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);	 //Enable the gpio clock //使锟斤拷GPIO时锟斤拷
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); //Enable the Usart clock //使锟斤拷USART时锟斤拷
 
 	GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);	
 	GPIO_PinAFConfig(GPIOA,GPIO_PinSource10 ,GPIO_AF_USART1);	 
 	
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9|GPIO_Pin_10;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //输出模式
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //推挽输出
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //高速50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //上拉
-	GPIO_Init(GPIOA, &GPIO_InitStructure);  		          //初始化
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //锟斤拷锟侥Ｊ�
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //锟斤拷锟斤拷锟斤拷锟�
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //锟斤拷锟斤拷50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //锟斤拷锟斤拷
+	GPIO_Init(GPIOA, &GPIO_InitStructure);  		          //锟斤拷始锟斤拷
 	
-  //UsartNVIC configuration //UsartNVIC配置
+  //UsartNVIC configuration //UsartNVIC锟斤拷锟斤拷
 	NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
-	//Preempt priority //抢占优先级
+	//Preempt priority //锟斤拷占锟斤拷锟饺硷拷
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1 ;
-	//Subpriority //子优先级
+	//Subpriority //锟斤拷锟斤拷锟饺硷拷
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
-	//Enable the IRQ channel //IRQ通道使能
+	//Enable the IRQ channel //IRQ通锟斤拷使锟斤拷
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
   //Initialize the VIC register with the specified parameters 
-	//根据指定的参数初始化VIC寄存器	
+	//锟斤拷锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷VIC锟侥达拷锟斤拷	
 	NVIC_Init(&NVIC_InitStructure);	
 	
-  //USART Initialization Settings 初始化设置
-	USART_InitStructure.USART_BaudRate = bound; //Port rate //串口波特率
-	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //字长为8位数据格式
-	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一个停止位
-	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //无奇偶校验位
-	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //无硬件数据流控制
-	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //收发模式
-	USART_Init(USART1, &USART_InitStructure); //Initialize serial port 1 //初始化串口1
+  //USART Initialization Settings 锟斤拷始锟斤拷锟斤拷锟斤拷
+	USART_InitStructure.USART_BaudRate = bound; //Port rate //锟斤拷锟节诧拷锟斤拷锟斤拷
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //锟街筹拷为8位锟斤拷锟捷革拷式
+	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一锟斤拷停止位
+	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //锟斤拷锟斤拷偶校锟斤拷位
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //锟斤拷硬锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //锟秸凤拷模式
+	USART_Init(USART1, &USART_InitStructure); //Initialize serial port 1 //锟斤拷始锟斤拷锟斤拷锟斤拷1
 	
-	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //开启串口接受中断
-	USART_Cmd(USART1, ENABLE);                     //Enable serial port 1 //使能串口1
+	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //锟斤拷锟斤拷锟斤拷锟节斤拷锟斤拷锟叫讹拷
+	USART_Cmd(USART1, ENABLE);                     //Enable serial port 1 //使锟杰达拷锟斤拷1
 }
 /**************************************************************************
 Function: Serial port 2 initialization
 Input   : none
 Output  : none
-函数功能：串口2初始化
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷2锟斤拷始锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void uart2_init(u32 bound)
 {  	 
@@ -290,50 +336,50 @@ void uart2_init(u32 bound)
 	USART_InitTypeDef USART_InitStructure;
 	NVIC_InitTypeDef NVIC_InitStructure;
 
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //使能GPIO时钟
-	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); //Enable the Usart clock //使能USART时钟
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //使锟斤拷GPIO时锟斤拷
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); //Enable the Usart clock //使锟斤拷USART时锟斤拷
 	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource5,GPIO_AF_USART2);	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource6 ,GPIO_AF_USART2);	 
 	
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5|GPIO_Pin_6;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //输出模式
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //推挽输出
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //高速50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //上拉
-	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //初始化
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //锟斤拷锟侥Ｊ�
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //锟斤拷锟斤拷锟斤拷锟�
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //锟斤拷锟斤拷50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //锟斤拷锟斤拷
+	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //锟斤拷始锟斤拷
 	
-	//UsartNVIC configuration //UsartNVIC配置
+	//UsartNVIC configuration //UsartNVIC锟斤拷锟斤拷
 	NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
-	//Preempt priority //抢占优先级
+	//Preempt priority //锟斤拷占锟斤拷锟饺硷拷
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1 ;
-	//Subpriority //子优先级
+	//Subpriority //锟斤拷锟斤拷锟饺硷拷
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;	
-  //Enable the IRQ channel //IRQ通道使能	
+  //Enable the IRQ channel //IRQ通锟斤拷使锟斤拷	
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   //Initialize the VIC register with the specified parameters 
-	//根据指定的参数初始化VIC寄存器		
+	//锟斤拷锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷VIC锟侥达拷锟斤拷		
 	NVIC_Init(&NVIC_InitStructure);	
 	
-	//USART Initialization Settings 初始化设置
-	USART_InitStructure.USART_BaudRate = bound; //Port rate //串口波特率
-	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //字长为8位数据格式
-	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一个停止
-	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //无奇偶校验位
-	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //无硬件数据流控制
-	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //收发模式
-	USART_Init(USART2, &USART_InitStructure);      //Initialize serial port 2 //初始化串口2
+	//USART Initialization Settings 锟斤拷始锟斤拷锟斤拷锟斤拷
+	USART_InitStructure.USART_BaudRate = bound; //Port rate //锟斤拷锟节诧拷锟斤拷锟斤拷
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //锟街筹拷为8位锟斤拷锟捷革拷式
+	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一锟斤拷停止
+	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //锟斤拷锟斤拷偶校锟斤拷位
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //锟斤拷硬锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //锟秸凤拷模式
+	USART_Init(USART2, &USART_InitStructure);      //Initialize serial port 2 //锟斤拷始锟斤拷锟斤拷锟斤拷2
 	
-	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //开启串口接受中断
-	USART_Cmd(USART2, ENABLE);                     //Enable serial port 2 //使能串口2 
+	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //锟斤拷锟斤拷锟斤拷锟节斤拷锟斤拷锟叫讹拷
+	USART_Cmd(USART2, ENABLE);                     //Enable serial port 2 //使锟杰达拷锟斤拷2 
 }
 /**************************************************************************
 Function: Serial port 3 initialization
 Input   : none
 Output  : none
-函数功能：串口3初始化
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷3锟斤拷始锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void uart3_init(u32 bound)
 {  	 
@@ -341,50 +387,86 @@ void uart3_init(u32 bound)
 	USART_InitTypeDef USART_InitStructure;
 	NVIC_InitTypeDef NVIC_InitStructure;
 	
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //使能GPIO时钟
-  RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); //Enable the Usart clock //使能USART时钟
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //使锟斤拷GPIO时锟斤拷
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); //Enable the Usart clock //使锟斤拷USART时锟斤拷
 	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource8,GPIO_AF_USART3);	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource9,GPIO_AF_USART3);	 
 	
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //输出模式
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //推挽输出
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //高速50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //上拉
-	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //初始化
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //锟斤拷锟侥Ｊ�
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //锟斤拷锟斤拷锟斤拷锟�
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //锟斤拷锟斤拷50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //锟斤拷锟斤拷
+	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //锟斤拷始锟斤拷
 	
-  //UsartNVIC configuration //UsartNVIC配置
+  //UsartNVIC configuration //UsartNVIC锟斤拷锟斤拷
   NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
-	//Preempt priority //抢占优先级
+	//Preempt priority //锟斤拷占锟斤拷锟饺硷拷
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;
-	//Preempt priority //抢占优先级
+	//Preempt priority //锟斤拷占锟斤拷锟饺硷拷
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
-	//Enable the IRQ channel //IRQ通道使能	
+	//Enable the IRQ channel //IRQ通锟斤拷使锟斤拷	
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
   //Initialize the VIC register with the specified parameters 
-	//根据指定的参数初始化VIC寄存器		
+	//锟斤拷锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷VIC锟侥达拷锟斤拷		
 	NVIC_Init(&NVIC_InitStructure);
 	
-  //USART Initialization Settings 初始化设置
-	USART_InitStructure.USART_BaudRate = bound; //Port rate //串口波特率
-	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //字长为8位数据格式
-	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一个停止
-	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //无奇偶校验位
-	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //无硬件数据流控制
-	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //收发模式
-  USART_Init(USART3, &USART_InitStructure);      //Initialize serial port 3 //初始化串口3
+  //USART Initialization Settings 锟斤拷始锟斤拷锟斤拷锟斤拷
+	USART_InitStructure.USART_BaudRate = bound; //Port rate //锟斤拷锟节诧拷锟斤拷锟斤拷
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //锟街筹拷为8位锟斤拷锟捷革拷式
+	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一锟斤拷停止
+	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //锟斤拷锟斤拷偶校锟斤拷位
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //锟斤拷硬锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //锟秸凤拷模式
+  USART_Init(USART3, &USART_InitStructure);      //Initialize serial port 3 //锟斤拷始锟斤拷锟斤拷锟斤拷3
 	
-  USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //开启串口接受中断
-  USART_Cmd(USART3, ENABLE);                     //Enable serial port 3 //使能串口3 
+  USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //锟斤拷锟斤拷锟斤拷锟节斤拷锟斤拷锟叫讹拷
+  USART_Cmd(USART3, ENABLE);                     //Enable serial port 3 //使锟杰达拷锟斤拷3
+
+	uart3_dma_tx_init();                            //Init DMA for USART3 TX //锟斤拷始锟斤拷锟斤拷锟斤拷3锟斤拷锟斤拷DMA
+}
+/**************************************************************************
+Function: USART3 TX DMA initialization (memory->peripheral, one-shot per frame)
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷3锟斤拷锟斤拷DMA锟斤拷始锟斤拷锟斤拷锟节达拷->锟斤拷锟借，每帧锟斤拷锟斤拷一锟轿ｏ拷
+DMA1 Stream3 Channel4 is the TX request line for USART3 on STM32F407.
+STM32F407 锟较达拷锟斤拷3锟侥凤拷锟斤拷DMA锟斤拷锟斤拷为DMA1_Stream3_Channel4锟斤拷
+**************************************************************************/
+void uart3_dma_tx_init(void)
+{
+	DMA_InitTypeDef DMA_InitStructure;
+
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
+
+	DMA_DeInit(DMA1_Stream3);
+	while(DMA_GetCmdStatus(DMA1_Stream3) != DISABLE);
+
+	DMA_InitStructure.DMA_Channel = DMA_Channel_4;
+	DMA_InitStructure.DMA_PeripheralBaseAddr = (u32)&USART3->DR;
+	DMA_InitStructure.DMA_Memory0BaseAddr = (u32)Send_Data.buffer;
+	DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+	DMA_InitStructure.DMA_BufferSize = SEND_DATA_SIZE;
+	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+	DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+	DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
+	DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+	DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
+	DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+	DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+	DMA_Init(DMA1_Stream3, &DMA_InitStructure);
+
+	USART_DMACmd(USART3, USART_DMAReq_Tx, ENABLE);
 }
 /**************************************************************************
 Function: Serial port 5 initialization
 Input   : none
 Output  : none
-函数功能：串口5初始化
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷5锟斤拷始锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void uart5_init(u32 bound)
 {  	 
@@ -393,99 +475,99 @@ void uart5_init(u32 bound)
 	NVIC_InitTypeDef NVIC_InitStructure;
 	
 	//PC12 TX
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);	 //Enable the gpio clock  //使能GPIO时钟
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);	 //Enable the gpio clock  //使锟斤拷GPIO时锟斤拷
 		//PD2 RX
-	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //使能GPIO时钟
-  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); //Enable the Usart clock //使能USART时钟
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 //Enable the gpio clock  //使锟斤拷GPIO时锟斤拷
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); //Enable the Usart clock //使锟斤拷USART时锟斤拷
 
 	GPIO_PinAFConfig(GPIOC,GPIO_PinSource12,GPIO_AF_UART5);	
 	GPIO_PinAFConfig(GPIOD,GPIO_PinSource2 ,GPIO_AF_UART5);	 
 	
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //输出模式
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //推挽输出
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //高速50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //上拉
-	GPIO_Init(GPIOC, &GPIO_InitStructure);  		          //初始化
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //锟斤拷锟侥Ｊ�
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //锟斤拷锟斤拷锟斤拷锟�
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //锟斤拷锟斤拷50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //锟斤拷锟斤拷
+	GPIO_Init(GPIOC, &GPIO_InitStructure);  		          //锟斤拷始锟斤拷
 
 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
-	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //输出模式
-	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //推挽输出
-	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //高速50MHZ
-	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //上拉
-	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //初始化
+	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            //锟斤拷锟侥Ｊ�
+	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          //锟斤拷锟斤拷锟斤拷锟�
+	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       //锟斤拷锟斤拷50MHZ
+	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            //锟斤拷锟斤拷
+	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          //锟斤拷始锟斤拷
 	
-  //UsartNVIC configuration //UsartNVIC配置
+  //UsartNVIC configuration //UsartNVIC锟斤拷锟斤拷
   NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
-	//Preempt priority //抢占优先级
+	//Preempt priority //锟斤拷占锟斤拷锟饺硷拷
 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;
-	//Preempt priority //抢占优先级
+	//Preempt priority //锟斤拷占锟斤拷锟饺硷拷
 	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
-	//Enable the IRQ channel //IRQ通道使能	
+	//Enable the IRQ channel //IRQ通锟斤拷使锟斤拷	
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
   //Initialize the VIC register with the specified parameters 
-	//根据指定的参数初始化VIC寄存器		
+	//锟斤拷锟斤拷指锟斤拷锟侥诧拷锟斤拷锟斤拷始锟斤拷VIC锟侥达拷锟斤拷		
 	NVIC_Init(&NVIC_InitStructure);
 	
-  //USART Initialization Settings 初始化设置
-	USART_InitStructure.USART_BaudRate = bound; //Port rate //串口波特率
-	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //字长为8位数据格式
-	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一个停止
-	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //无奇偶校验位
-	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //无硬件数据流控制
-	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //收发模式
-  USART_Init(UART5, &USART_InitStructure);      //Initialize serial port 5 //初始化串口5
-  USART_ITConfig(UART5, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //开启串口接受中断
-  USART_Cmd(UART5, ENABLE);                     //Enable serial port 5 //使能串口5
+  //USART Initialization Settings 锟斤拷始锟斤拷锟斤拷锟斤拷
+	USART_InitStructure.USART_BaudRate = bound; //Port rate //锟斤拷锟节诧拷锟斤拷锟斤拷
+	USART_InitStructure.USART_WordLength = USART_WordLength_8b; //The word length is 8 bit data format //锟街筹拷为8位锟斤拷锟捷革拷式
+	USART_InitStructure.USART_StopBits = USART_StopBits_1; //A stop bit //一锟斤拷停止
+	USART_InitStructure.USART_Parity = USART_Parity_No; //Prosaic parity bits //锟斤拷锟斤拷偶校锟斤拷位
+	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //No hardware data flow control //锟斤拷硬锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
+	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//Sending and receiving mode //锟秸凤拷模式
+  USART_Init(UART5, &USART_InitStructure);      //Initialize serial port 5 //锟斤拷始锟斤拷锟斤拷锟斤拷5
+  USART_ITConfig(UART5, USART_IT_RXNE, ENABLE); //Open the serial port to accept interrupts //锟斤拷锟斤拷锟斤拷锟节斤拷锟斤拷锟叫讹拷
+  USART_Cmd(UART5, ENABLE);                     //Enable serial port 5 //使锟杰达拷锟斤拷5
 }
 /**************************************************************************
 Function: Serial port 1 receives interrupted
 Input   : none
 Output  : none
-函数功能：串口1接收中断
-入口参数：无
-返 回 值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷1锟斤拷锟斤拷锟叫讹拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷 锟斤拷 值锟斤拷锟斤拷
 **************************************************************************/
 int USART1_IRQHandler(void)
 {	
-	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //Check if data is received //判断是否接收到数据
+	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //Check if data is received //锟叫讹拷锟角凤拷锟斤拷盏锟斤拷锟斤拷锟�
 	{
 		u8 Usart_Receive;
 		static u8 Count;
 		static u8 rxbuf[11];
 		int check=0,error=1,i;
 		
-		Usart_Receive = USART_ReceiveData(USART1); //Read the data //读取数据
-		if(Time_count<CONTROL_DELAY)
-			// Data is not processed until 10 seconds after startup
-		  //开机10秒前不处理数据
+		Usart_Receive = USART_ReceiveData(USART1); //Read the data //锟斤拷取锟斤拷锟斤拷
+		if(Time_count<CONTROL_STARTUP_DELAY_COUNT)
+			// Data is not processed until the 2 second startup delay expires.
+		  //锟斤拷锟斤拷2锟斤拷前锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
 			return 0;	
 			
 		//Fill the array with serial data
-		//串口数据填入数组
+		//锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
     rxbuf[Count]=Usart_Receive;  
 		
 		//Ensure that the first data in the array is FRAME_HEADER
-		//确保数组第一个数据为FRAME_HEADER
+		//确锟斤拷锟斤拷锟斤拷锟揭伙拷锟斤拷锟斤拷锟轿狥RAME_HEADER
     if(Usart_Receive == FRAME_HEADER||Count>0) 
 			Count++; 
 		else 
 			Count=0;
 		
-		if (Count == 11) //Verify the length of the packet //验证数据包的长度
+		if (Count == 11) //Verify the length of the packet //锟斤拷证锟斤拷锟捷帮拷锟侥筹拷锟斤拷
 		{   
-				Count=0; //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备
-				if(rxbuf[10] == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾
+				Count=0; //Prepare for the serial port data to be refill into the array //为锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷准锟斤拷
+				if(rxbuf[10] == FRAME_TAIL) //Verify the frame tail of the packet //锟斤拷证锟斤拷锟捷帮拷锟斤拷帧尾
 				{			
 					for(i=0; i<9; i++)
 					{
 						//XOR bit check, used to detect data error
-						//异或位校验，用于检测数据是否出错
+						//锟斤拷锟轿恍ｏ拷椋拷锟斤拷诩锟斤拷锟斤拷锟斤拷锟角凤拷锟斤拷锟�
 						check=rxbuf[i]^check; 
 					}
 					if(check==rxbuf[9]) 
 						//XOR bit check successful
-					  //异或位校验成功
+					  //锟斤拷锟轿恍ｏ拷锟缴癸拷
 					  error=0; 
 					
 					if(error==0)	 
@@ -494,7 +576,7 @@ int USART1_IRQHandler(void)
             if(Usart1_ON_Flag==0)
 						{	
 							//Serial port 1 controls flag position 1, other flag position 0
-							//串口1控制标志位置1，其它标志位置0
+							//锟斤拷锟斤拷1锟斤拷锟狡憋拷志位锟斤拷1锟斤拷锟斤拷锟斤拷锟斤拷志位锟斤拷0
 							//Usart_ON_Flag=1;
 							Usart1_ON_Flag=1;
 							APP_ON_Flag=0;
@@ -502,21 +584,17 @@ int USART1_IRQHandler(void)
 							Remote_ON_Flag=0;
 							CAN_ON_Flag=0;
 						}		
-							command_lost_count=0; //CAN/串口控制命令丢失计数清零
+							command_lost_count=0; //CAN/锟斤拷锟节匡拷锟斤拷锟斤拷锟筋丢失锟斤拷锟斤拷锟斤拷锟斤拷
 		
 						//Calculate the 3-axis target velocity from the serial data, which is divided into 8-bit high and 8-bit low units mm/s
-						//从串口数据求三轴目标速度，分高8位和低8位 单位mm/s
+						//锟接达拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷目锟斤拷锟劫度ｏ拷锟街革拷8位锟酵碉拷8位 锟斤拷位mm/s
 						Move_X=XYZ_Target_Speed_transition(rxbuf[3],rxbuf[4]);
 						Move_Y=XYZ_Target_Speed_transition(rxbuf[5],rxbuf[6]);
 						Vz    =XYZ_Target_Speed_transition(rxbuf[7],rxbuf[8]);
-						if(Car_Mode==Akm_Car)
-						{
-							Move_Z=Vz_to_Akm_Angle(Move_X, Vz);
-						}
-						else
-						{
-							Move_Z=XYZ_Target_Speed_transition(rxbuf[7],rxbuf[8]);
-						}		
+						// Ackermann: pass the raw rotation speed wz [rad/s] straight
+						// through. Drive_Motor derives curvature kappa = wz/v itself,
+						// so no front-wheel-angle pre-conversion here.
+						Move_Z=Vz;
 					}
 			  }
 		 }
@@ -527,46 +605,46 @@ int USART1_IRQHandler(void)
 Function: Refresh the OLED screen
 Input   : none
 Output  : none
-函数功能：串口2接收中断
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷2锟斤拷锟斤拷锟叫讹拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 int USART2_IRQHandler(void)
 {	
 	int Usart_Receive;
-	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) //Check if data is received //判断是否接收到数据
+	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) //Check if data is received //锟叫讹拷锟角凤拷锟斤拷盏锟斤拷锟斤拷锟�
 	{	      
 		static u8 Flag_PID,i,j,Receive[50],Last_Usart_Receive;
 		static float Data;
 				
-		Usart_Receive=USART2->DR; //Read the data //读取数据
+		Usart_Receive=USART2->DR; //Read the data //锟斤拷取锟斤拷锟斤拷
 		
 		if(Deviation_Count<CONTROL_DELAY)
-			// Data is not processed until 10 seconds after startup
-		  //开机10秒前不处理数据
+			// Data is not processed until the 2 second startup delay expires.
+		  //锟斤拷锟斤拷2锟斤拷前锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
 		  return 0;	
 
 		if(Usart_Receive==0x41&&Last_Usart_Receive==0x41&&APP_ON_Flag==0)
 			//10 seconds after startup, press the forward button of APP to enter APP control mode
 		  //The APP controls the flag position 1 and the other flag position 0
-			//开机10秒之后，按下APP的前进键进入APP控制模式
-		  //APP控制标志位置1，其它标志位置0
+			//锟斤拷锟斤拷10锟斤拷之锟襟，帮拷锟斤拷APP锟斤拷前锟斤拷锟斤拷锟斤拷锟斤拷APP锟斤拷锟斤拷模式
+		  //APP锟斤拷锟狡憋拷志位锟斤拷1锟斤拷锟斤拷锟斤拷锟斤拷志位锟斤拷0
 			PS2_ON_Flag=0,Remote_ON_Flag=0,APP_ON_Flag=1,CAN_ON_Flag=0,Usart1_ON_Flag=0,Usart5_ON_Flag=0;
     Last_Usart_Receive=Usart_Receive;			
 	  
 		if(Usart_Receive==0x4B) 
 			//Enter the APP steering control interface
-		  //进入APP转向控制界面
+		  //锟斤拷锟斤拷APP转锟斤拷锟斤拷平锟斤拷锟�
 			Turn_Flag=1;  
 	  else	if(Usart_Receive==0x49||Usart_Receive==0x4A) 
       // Enter the APP direction control interface		
-			//进入APP方向控制界面	
+			//锟斤拷锟斤拷APP锟斤拷锟斤拷锟斤拷平锟斤拷锟�	
 			Turn_Flag=0;	
 		
 		if(Turn_Flag==0) 
 		{
 			//App rocker control interface command
-			//APP摇杆控制界面命令
+			//APP摇锟剿匡拷锟狡斤拷锟斤拷锟斤拷锟斤拷
 			if(Usart_Receive>=0x41&&Usart_Receive<=0x48)  
 			{	
 				Flag_Direction=Usart_Receive-0x40;
@@ -580,27 +658,27 @@ int USART2_IRQHandler(void)
 		else if(Turn_Flag==1)
 		{
 			//APP steering control interface command
-			//APP转向控制界面命令
-			if     (Usart_Receive==0x43) Flag_Left=0,Flag_Right=1; //Right rotation //右自转
-			else if(Usart_Receive==0x47) Flag_Left=1,Flag_Right=0; //Left rotation  //左自转
+			//APP转锟斤拷锟斤拷平锟斤拷锟斤拷锟斤拷锟�
+			if     (Usart_Receive==0x43) Flag_Left=0,Flag_Right=1; //Right rotation //锟斤拷锟斤拷转
+			else if(Usart_Receive==0x47) Flag_Left=1,Flag_Right=0; //Left rotation  //锟斤拷锟斤拷转
 			else                         Flag_Left=0,Flag_Right=0;
 			if     (Usart_Receive==0x41||Usart_Receive==0x45) Flag_Direction=Usart_Receive-0x40;
 			else  Flag_Direction=0;
 		}
-		if(Usart_Receive==0x58)  RC_Velocity=RC_Velocity+100; //Accelerate the keys, +100mm/s //加速按键，+100mm/s
-		if(Usart_Receive==0x59)  RC_Velocity=RC_Velocity-100; //Slow down buttons,   -100mm/s //减速按键，-100mm/s
+		if(Usart_Receive==0x58)  RC_Velocity=RC_Velocity+100; //Accelerate the keys, +100mm/s //锟斤拷锟劫帮拷锟斤拷锟斤拷+100mm/s
+		if(Usart_Receive==0x59)  RC_Velocity=RC_Velocity-100; //Slow down buttons,   -100mm/s //锟斤拷锟劫帮拷锟斤拷锟斤拷-100mm/s
 	  
 	 // The following is the communication with the APP debugging interface
-	 //以下是与APP调试界面通讯
-	 if(Usart_Receive==0x7B) Flag_PID=1;   //The start bit of the APP parameter instruction //APP参数指令起始位
-	 if(Usart_Receive==0x7D) Flag_PID=2;   //The APP parameter instruction stops the bit    //APP参数指令停止位
+	 //锟斤拷锟斤拷锟斤拷锟斤拷APP锟斤拷锟皆斤拷锟斤拷通讯
+	 if(Usart_Receive==0x7B) Flag_PID=1;   //The start bit of the APP parameter instruction //APP锟斤拷锟斤拷指锟斤拷锟斤拷始位
+	 if(Usart_Receive==0x7D) Flag_PID=2;   //The APP parameter instruction stops the bit    //APP锟斤拷锟斤拷指锟斤拷停止位
 
-	 if(Flag_PID==1) //Collect data //采集数据
+	 if(Flag_PID==1) //Collect data //锟缴硷拷锟斤拷锟斤拷
 	 {
 		Receive[i]=Usart_Receive;
 		i++;
 	 }
-	 if(Flag_PID==2) //Analyze the data //分析数据
+	 if(Flag_PID==2) //Analyze the data //锟斤拷锟斤拷锟斤拷锟斤拷
 	 {
 			if(Receive[3]==0x50) 	 PID_Send=1;
 			else  if(Receive[1]!=0x23) 
@@ -623,12 +701,12 @@ int USART2_IRQHandler(void)
 				 }
       }		
       //Relevant flag position is cleared			
-      //相关标志位清零			
+      //锟斤拷乇锟街疚伙拷锟斤拷锟�			
 			Flag_PID=0;
 			i=0;
 			j=0;
 			Data=0;
-			memset(Receive, 0, sizeof(u8)*50); //Clear the array to zero//数组清零
+			memset(Receive, 0, sizeof(u8)*50); //Clear the array to zero//锟斤拷锟斤拷锟斤拷锟斤拷
 	 }
    if(RC_Velocity<0)   RC_Velocity=0; 	 
   }
@@ -638,9 +716,9 @@ int USART2_IRQHandler(void)
 Function: Serial port 3 receives interrupted
 Input   : none
 Output  : none
-函数功能：串口3接收中断
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷3锟斤拷锟斤拷锟叫讹拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 float test_movz = 0;
 int USART3_IRQHandler(void)
@@ -648,59 +726,54 @@ int USART3_IRQHandler(void)
 	static u8 Count=0;
 	u8 Usart_Receive;
 
-	if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) //Check if data is received //判断是否接收到数据
+	if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) //Check if data is received //锟叫讹拷锟角凤拷锟斤拷盏锟斤拷锟斤拷锟�
 	{
-		Usart_Receive = USART_ReceiveData(USART3);//Read the data //读取数据
-		if(Time_count<CONTROL_DELAY)
-			// Data is not processed until 10 seconds after startup
-		  //开机10秒前不处理数据
+		Usart_Receive = USART_ReceiveData(USART3);//Read the data //锟斤拷取锟斤拷锟斤拷
+		if(Time_count<CONTROL_STARTUP_DELAY_COUNT)
+			// Data is not processed until the 2 second startup delay expires.
+		  //锟斤拷锟斤拷2锟斤拷前锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
 		  return 0;	
 		
 		//Fill the array with serial data
-		//串口数据填入数组
+		//锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
     Receive_Data.buffer[Count]=Usart_Receive;
 		
 		// Ensure that the first data in the array is FRAME_HEADER
-		//确保数组第一个数据为FRAME_HEADER
+		//确锟斤拷锟斤拷锟斤拷锟揭伙拷锟斤拷锟斤拷锟轿狥RAME_HEADER
 		if(Usart_Receive == FRAME_HEADER||Count>0) 
 			Count++; 
 		else 
 			Count=0;
 		
-		if (Count == 11) //Verify the length of the packet //验证数据包的长度
+		if (Count == 11) //Verify the length of the packet //锟斤拷证锟斤拷锟捷帮拷锟侥筹拷锟斤拷
 		{   
-				Count=0; //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备
-				if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾
+				Count=0; //Prepare for the serial port data to be refill into the array //为锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷准锟斤拷
+				if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //锟斤拷证锟斤拷锟捷帮拷锟斤拷帧尾
 				{
 					//Data exclusionary or bit check calculation, mode 0 is sent data check
-					//数据异或位校验计算，模式0是发送数据校验
+					//锟斤拷锟斤拷锟斤拷锟轿恍ｏ拷锟斤拷锟姐，模式0锟角凤拷锟斤拷锟斤拷锟斤拷校锟斤拷
 					if(Receive_Data.buffer[9] ==Check_Sum(9,0))	 
 				  {	
 						float Vz;						
 						//All modes flag position 0, USART3 control mode
-            //所有模式标志位置0，为Usart3控制模式						
+            //锟斤拷锟斤拷模式锟斤拷志位锟斤拷0锟斤拷为Usart3锟斤拷锟斤拷模式						
 						PS2_ON_Flag=0;
 						Remote_ON_Flag=0;
 						APP_ON_Flag=0;
 						CAN_ON_Flag=0;
 						Usart1_ON_Flag=0;
 						Usart5_ON_Flag=0;
-						command_lost_count=0; //CAN/串口控制命令丢失计数清零
+						command_lost_count=0; //CAN/锟斤拷锟节匡拷锟斤拷锟斤拷锟筋丢失锟斤拷锟斤拷锟斤拷锟斤拷
 
 						//Calculate the target speed of three axis from serial data, unit m/s
-						//从串口数据求三轴目标速度， 单位m/s
+						//锟接达拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷目锟斤拷锟劫度ｏ拷 锟斤拷位m/s
 						Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
 						Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
 						Vz    =XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
 						test_movz = Vz;
-						if(Car_Mode==Akm_Car)
-						{
-							Move_Z=Vz_to_Akm_Angle(Move_X, Vz);
-						}
-						else
-						{
-							Move_Z=XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
-						}				  }
+						// Ackermann: pass the raw rotation speed wz [rad/s] straight
+						// through; Drive_Motor derives curvature kappa = wz/v itself.
+						Move_Z=Vz;				  }
 
 			}
 		}
@@ -712,66 +785,61 @@ int USART3_IRQHandler(void)
 Function: Serial port 5 receives interrupted
 Input   : none
 Output  : none
-函数功能：串口5接收中断
-入口参数：无
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷5锟斤拷锟斤拷锟叫讹拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 int UART5_IRQHandler(void)
 {	
 	static u8 Count=0;
 	u8 Usart_Receive;
 
-	if(USART_GetITStatus(UART5, USART_IT_RXNE) != RESET) //Check if data is received //判断是否接收到数据
+	if(USART_GetITStatus(UART5, USART_IT_RXNE) != RESET) //Check if data is received //锟叫讹拷锟角凤拷锟斤拷盏锟斤拷锟斤拷锟�
 	{
-		Usart_Receive = USART_ReceiveData(UART5);//Read the data //读取数据
-		if(Time_count<CONTROL_DELAY)
+		Usart_Receive = USART_ReceiveData(UART5);//Read the data //锟斤拷取锟斤拷锟斤拷
+		if(Time_count<CONTROL_STARTUP_DELAY_COUNT)
 			// Data is not processed until 10 seconds after startup
-		  //开机10秒前不处理数据
+		  //锟斤拷锟斤拷10锟斤拷前锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
 		  return 0;	
 		
 		//Fill the array with serial data
-		//串口数据填入数组
+		//锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
     Receive_Data.buffer[Count]=Usart_Receive;
 		
 		// Ensure that the first data in the array is FRAME_HEADER
-		//确保数组第一个数据为FRAME_HEADER
+		//确锟斤拷锟斤拷锟斤拷锟揭伙拷锟斤拷锟斤拷锟轿狥RAME_HEADER
 		if(Usart_Receive == FRAME_HEADER||Count>0) 
 			Count++; 
 		else 
 			Count=0;
 		
-		if (Count == 11) //Verify the length of the packet //验证数据包的长度
+		if (Count == 11) //Verify the length of the packet //锟斤拷证锟斤拷锟捷帮拷锟侥筹拷锟斤拷
 		{   
-				Count=0; //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备
-				if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾
+				Count=0; //Prepare for the serial port data to be refill into the array //为锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷准锟斤拷
+				if(Receive_Data.buffer[10] == FRAME_TAIL) //Verify the frame tail of the packet //锟斤拷证锟斤拷锟捷帮拷锟斤拷帧尾
 				{
 					//Data exclusionary or bit check calculation, mode 0 is sent data check
-					//数据异或位校验计算，模式0是发送数据校验
+					//锟斤拷锟斤拷锟斤拷锟轿恍ｏ拷锟斤拷锟姐，模式0锟角凤拷锟斤拷锟斤拷锟斤拷校锟斤拷
 					if(Receive_Data.buffer[9] ==Check_Sum(9,0))	 
 				  {	
 						float Vz;						
 						//All modes flag position 0, USART3 control mode
-            //所有模式标志位置0，为Usart5控制模式						
+            //锟斤拷锟斤拷模式锟斤拷志位锟斤拷0锟斤拷为Usart5锟斤拷锟斤拷模式						
 						PS2_ON_Flag=0;
 						Remote_ON_Flag=0;
 						APP_ON_Flag=0;
 						CAN_ON_Flag=0;
 						Usart5_ON_Flag=0;
-						command_lost_count=0; //CAN/串口控制命令丢失计数清零
+						command_lost_count=0; //CAN/锟斤拷锟节匡拷锟斤拷锟斤拷锟筋丢失锟斤拷锟斤拷锟斤拷锟斤拷
 
 						//Calculate the target speed of three axis from serial data, unit m/s
-						//从串口数据求三轴目标速度， 单位m/s
+						//锟接达拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷目锟斤拷锟劫度ｏ拷 锟斤拷位m/s
 						Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
 						Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
 						Vz    =XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
-						if(Car_Mode==Akm_Car)
-						{
-							Move_Z=Vz_to_Akm_Angle(Move_X, Vz);
-						}
-						else
-						{
-							Move_Z=XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
-						}				  }
+						// Ackermann: pass the raw rotation speed wz [rad/s] straight
+						// through; Drive_Motor derives curvature kappa = wz/v itself.
+						Move_Z=Vz;				  }
 					
 			}
 		}
@@ -782,77 +850,58 @@ int UART5_IRQHandler(void)
 Function: After the top 8 and low 8 figures are integrated into a short type data, the unit reduction is converted
 Input   : 8 bits high, 8 bits low
 Output  : The target velocity of the robot on the X/Y/Z axis
-函数功能：将上位机发过来目标前进速度Vx、目标角速度Vz，转换为阿克曼小车的右前轮转角
-入口参数：目标前进速度Vx、目标角速度Vz，单位：m/s，rad/s
-返回  值：阿克曼小车的右前轮转角，单位：rad
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷位锟斤拷锟斤拷锟斤拷锟斤拷目锟斤拷前锟斤拷锟劫讹拷Vx锟斤拷目锟斤拷锟斤拷俣锟絍z锟斤拷转锟斤拷为锟斤拷锟斤拷锟斤拷小锟斤拷锟斤拷锟斤拷前锟斤拷转锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟侥匡拷锟角帮拷锟斤拷俣锟絍x锟斤拷目锟斤拷锟斤拷俣锟絍z锟斤拷锟斤拷位锟斤拷m/s锟斤拷rad/s
+锟斤拷锟斤拷  值锟斤拷锟斤拷锟斤拷锟斤拷小锟斤拷锟斤拷锟斤拷前锟斤拷转锟角ｏ拷锟斤拷位锟斤拷rad
 **************************************************************************/
 
 float test_z=0;
 float Vz_to_Akm_Angle(float Vx, float Vz)
 {
-	float R, AngleR, Min_Turn_Radius;
-	//float AngleL;
-	
-	//Ackermann car needs to set minimum turning radius
-	//If the target speed requires a turn radius less than the minimum turn radius,
-	//This will greatly improve the friction force of the car, which will seriously affect the control effect
-	//阿克曼小车需要设置最小转弯半径
-	//如果目标速度要求的转弯半径小于最小转弯半径，
-	//会导致小车运动摩擦力大大提高，严重影响控制效果
-	Min_Turn_Radius=MINI_AKM_MIN_TURN_RADIUS;
-	
+	float R, CenterAngle;
+
+	/* Move_Z is the vehicle center steering angle in radians. */
 	if(Vz!=0 && Vx!=0)
 	{
-		//If the target speed requires a turn radius less than the minimum turn radius
-		//如果目标速度要求的转弯半径小于最小转弯半径
-//		if(float_abs(Vx/Vz)<=Min_Turn_Radius)
-//		{
-//			//Reduce the target angular velocity and increase the turning radius to the minimum turning radius in conjunction with the forward speed
-//			//降低目标角速度，配合前进速度，提高转弯半径到最小转弯半径
-//			if(Vz>0)
-//				Vz= float_abs(Vx)/(Min_Turn_Radius);
-//			else	
-//				Vz=-float_abs(Vx)/(Min_Turn_Radius);	
-//		}		
-		//R=Vx/Vz;
-		R=Vx/Vz;
-		//AngleL=atan(Axle_spacing/(R+0.5*Wheel_spacing));
-		AngleR=atan(Axle_spacing/(R+0.05*Wheel_spacing));//2025.7.20edit
+		/* R is the turning radius of the vehicle centerline. */
+		/* Vx sets radius magnitude only; Vz owns the steering direction. */
+		R=float_abs(Vx)/float_abs(Vz);
+		CenterAngle=(Vz >= 0.0f ? 1.0f : -1.0f) * atan(Axle_spacing/R);
 	}
 	else
 	{
-		AngleR=0;
+		CenterAngle=0;
 	}
-	test_z = AngleR;//test
-	return AngleR;
+	test_z = CenterAngle;
+	return CenterAngle;
 }
 /**************************************************************************
 Function: After the top 8 and low 8 figures are integrated into a short type data, the unit reduction is converted
 Input   : 8 bits high, 8 bits low
 Output  : The target velocity of the robot on the X/Y/Z axis
-函数功能：将上位机发过来的高8位和低8位数据整合成一个short型数据后，再做单位还原换算
-入口参数：高8位，低8位
-返回  值：机器人X/Y/Z轴的目标速度
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷位锟斤拷锟斤拷锟斤拷锟斤拷锟侥革拷8位锟酵碉拷8位锟斤拷锟斤拷锟斤拷锟较筹拷一锟斤拷short锟斤拷锟斤拷锟捷猴拷锟斤拷锟斤拷锟斤拷位锟斤拷原锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟斤拷锟�8位锟斤拷锟斤拷8位
+锟斤拷锟斤拷  值锟斤拷锟斤拷锟斤拷锟斤拷X/Y/Z锟斤拷锟侥匡拷锟斤拷俣锟�
 **************************************************************************/
 float XYZ_Target_Speed_transition(u8 High,u8 Low)
 {
 	//Data conversion intermediate variable
-	//数据转换的中间变量
+	//锟斤拷锟斤拷转锟斤拷锟斤拷锟叫硷拷锟斤拷锟�
 	short transition; 
 	
-	//将高8位和低8位整合成一个16位的short型数据
+	//锟斤拷锟斤拷8位锟酵碉拷8位锟斤拷锟较筹拷一锟斤拷16位锟斤拷short锟斤拷锟斤拷锟斤拷
 	//The high 8 and low 8 bits are integrated into a 16-bit short data
 	transition=((High<<8)+Low); 
 	return 
-		transition/1000+(transition%1000)*0.001; //Unit conversion, mm/s->m/s //单位转换, mm/s->m/s						
+		transition/1000+(transition%1000)*0.001; //Unit conversion, mm/s->m/s //锟斤拷位转锟斤拷, mm/s->m/s						
 }
 /**************************************************************************
 Function: Serial port 1 sends data
 Input   : The data to send
 Output  : none
-函数功能：串口1发送数据
-入口参数：要发送的数据
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷1锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟揭拷锟斤拷偷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void usart1_send(u8 data)
 {
@@ -863,9 +912,9 @@ void usart1_send(u8 data)
 Function: Serial port 2 sends data
 Input   : The data to send
 Output  : none
-函数功能：串口2发送数据
-入口参数：要发送的数据
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷2锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟揭拷锟斤拷偷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void usart2_send(u8 data)
 {
@@ -876,9 +925,9 @@ void usart2_send(u8 data)
 Function: Serial port 3 sends data
 Input   : The data to send
 Output  : none
-函数功能：串口3发送数据
-入口参数：要发送的数据
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷3锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟揭拷锟斤拷偷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void usart3_send(u8 data)
 {
@@ -890,9 +939,9 @@ void usart3_send(u8 data)
 Function: Serial port 5 sends data
 Input   : The data to send
 Output  : none
-函数功能：串口5发送数据
-入口参数：要发送的数据
-返回  值：无
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷5锟斤拷锟斤拷锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟揭拷锟斤拷偷锟斤拷锟斤拷锟�
+锟斤拷锟斤拷  值锟斤拷锟斤拷
 **************************************************************************/
 void usart5_send(u8 data)
 {
@@ -903,16 +952,16 @@ void usart5_send(u8 data)
 Function: Calculates the check bits of data to be sent/received
 Input   : Count_Number: The first few digits of a check; Mode: 0-Verify the received data, 1-Validate the sent data
 Output  : Check result
-函数功能：计算要发送/接收的数据校验结果
-入口参数：Count_Number：校验的前几位数；Mode：0-对接收数据进行校验，1-对发送数据进行校验
-返回  值：校验结果
+锟斤拷锟斤拷锟斤拷锟杰ｏ拷锟斤拷锟斤拷要锟斤拷锟斤拷/锟斤拷锟秸碉拷锟斤拷锟斤拷校锟斤拷锟斤拷
+锟斤拷诓锟斤拷锟斤拷锟紺ount_Number锟斤拷校锟斤拷锟角帮拷锟轿伙拷锟斤拷锟組ode锟斤拷0-锟皆斤拷锟斤拷锟斤拷锟捷斤拷锟斤拷校锟介，1-锟皆凤拷锟斤拷锟斤拷锟捷斤拷锟斤拷校锟斤拷
+锟斤拷锟斤拷  值锟斤拷校锟斤拷锟斤拷
 **************************************************************************/
 u8 Check_Sum(unsigned char Count_Number,unsigned char Mode)
 {
 	unsigned char check_sum=0,k;
 	
 	//Validate the data to be sent
-	//对要发送的数据进行校验
+	//锟斤拷要锟斤拷锟酵碉拷锟斤拷锟捷斤拷锟斤拷校锟斤拷
 	if(Mode==1)
 	for(k=0;k<Count_Number;k++)
 	{
@@ -920,7 +969,7 @@ u8 Check_Sum(unsigned char Count_Number,unsigned char Mode)
 	}
 	
 	//Verify the data received
-	//对接收到的数据进行校验
+	//锟皆斤拷锟秸碉拷锟斤拷锟斤拷锟捷斤拷锟斤拷校锟斤拷
 	if(Mode==0)
 	for(k=0;k<Count_Number;k++)
 	{
@@ -931,6 +980,3 @@ u8 Check_Sum(unsigned char Count_Number,unsigned char Mode)
 
 
 
-
-
-
diff --git a/HARDWARE/usartx.h b/HARDWARE/usartx.h
index b8853d4..8e9ae8f 100644
--- a/HARDWARE/usartx.h
+++ b/HARDWARE/usartx.h
@@ -10,33 +10,41 @@
 
 #define FRAME_HEADER      0X7B //Frame_header //帧头
 #define FRAME_TAIL        0X7D //Frame_tail   //帧尾
-#define SEND_DATA_SIZE    24
+//Frame grew 24 -> 44: appended session_id(4) + two 64-bit sample
+//timestamps(8+8) before the checksum/tail. Bytes 0..21 keep the legacy
+//layout so old parsers stay compatible; new fields live in the tail.
+//帧锟斤拷 24 -> 44锟斤拷锟斤拷校锟斤拷/帧尾前追锟斤拷锟斤拷 session_id(4) 锟斤拷锟斤拷锟斤拷锟斤拷 64位锟斤拷锟斤拷时锟斤拷锟�(8+8)锟斤拷
+//锟街斤拷 0..21 锟斤拷锟街斤拷喜锟斤拷郑锟斤拷戮纸锟斤拷锟轿诧拷锟斤拷锟�
+#define SEND_DATA_SIZE    44
 #define RECEIVE_DATA_SIZE 11
 
 /*****A structure for storing triaxial data of a gyroscope accelerometer*****/
-/*****用于存放陀螺仪加速度计三轴数据的结构体*********************************/
+/*****锟斤拷锟节达拷锟斤拷锟斤拷锟斤拷羌锟斤拷俣燃锟斤拷锟斤拷锟斤拷锟斤拷莸慕峁癸拷锟�*********************************/
 typedef struct __Mpu6050_Data_ 
 {
-	short X_data; //2 bytes //2个字节
-	short Y_data; //2 bytes //2个字节
-	short Z_data; //2 bytes //2个字节
+	short X_data; //2 bytes //2锟斤拷锟街斤拷
+	short Y_data; //2 bytes //2锟斤拷锟街斤拷
+	short Z_data; //2 bytes //2锟斤拷锟街斤拷
 }Mpu6050_Data;
 
 /*******The structure of the serial port sending data************/
-/*******串口发送数据的结构体*************************************/
+/*******锟斤拷锟节凤拷锟斤拷锟斤拷锟捷的结构锟斤拷*************************************/
 typedef struct _SEND_DATA_  
 {
 	unsigned char buffer[SEND_DATA_SIZE];
 	struct _Sensor_Str_
 	{
-		unsigned char Frame_Header; //1个字节
-		short X_speed;	            //2 bytes //2个字节
-		short Y_speed;              //2 bytes //2个字节
-		short Z_speed;              //2 bytes //2个字节
-		short Power_Voltage;        //2 bytes //2个字节
-		Mpu6050_Data Accelerometer; //6 bytes //6个字节
-		Mpu6050_Data Gyroscope;     //6 bytes //6个字节	
-		unsigned char Frame_Tail;   //1 bytes //1个字节
+		unsigned char Frame_Header; //1锟斤拷锟街斤拷
+		short X_speed;	            //2 bytes //2锟斤拷锟街斤拷
+		short Y_speed;              //2 bytes //2锟斤拷锟街斤拷
+		short Z_speed;              //2 bytes //2锟斤拷锟街斤拷
+		short Power_Voltage;        //2 bytes //2锟斤拷锟街斤拷
+		Mpu6050_Data Accelerometer; //6 bytes //6锟斤拷锟街斤拷
+		Mpu6050_Data Gyroscope;     //6 bytes //6锟斤拷锟街斤拷
+		unsigned int  Session_Id;         //4 bytes: MCU boot session ID //锟斤拷MCU锟斤拷锟斤拷锟结话ID
+		unsigned long long Speed_Time_us; //8 bytes: encoder sample time //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟绞憋拷锟�
+		unsigned long long Imu_Time_us;   //8 bytes: IMU sample time //IMU锟斤拷锟斤拷时锟斤拷
+		unsigned char Frame_Tail;   //1 bytes //1锟斤拷锟街斤拷
 	}Sensor_Str;
 }SEND_DATA;
 
@@ -45,14 +53,21 @@ typedef struct _RECEIVE_DATA_
 	unsigned char buffer[RECEIVE_DATA_SIZE];
 	struct _Control_Str_
 	{
-		unsigned char Frame_Header; //1 bytes //1个字节
-		float X_speed;	            //4 bytes //4个字节
-		float Y_speed;              //4 bytes //4个字节
-		float Z_speed;              //4 bytes //4个字节
-		unsigned char Frame_Tail;   //1 bytes //1个字节
+		unsigned char Frame_Header; //1 bytes //1锟斤拷锟街斤拷
+		float X_speed;	            //4 bytes //4锟斤拷锟街斤拷
+		float Y_speed;              //4 bytes //4锟斤拷锟街斤拷
+		float Z_speed;              //4 bytes //4锟斤拷锟街斤拷
+		unsigned char Frame_Tail;   //1 bytes //1锟斤拷锟街斤拷
 	}Control_Str;
 }RECEIVE_DATA;
 
+//Sample timestamps captured at acquisition time by their owning tasks, and
+//the per-boot session ID. Read by data_transition() when packing the frame.
+//锟缴革拷锟皆采硷拷锟斤拷锟斤拷锟斤拷锟节诧拷锟斤拷瞬锟斤拷锟斤拷锟铰硷拷锟斤拷锟斤拷锟绞憋拷锟斤拷锟斤拷锟斤拷锟斤拷系统锟斤拷锟叫会话ID锟斤拷
+//锟斤拷 data_transition() 锟斤拷锟街∈憋拷锟饺★拷锟�
+extern volatile unsigned long long g_speed_sample_time_us;
+extern volatile unsigned long long g_imu_sample_time_us;
+extern volatile unsigned int g_session_id;
 
 void data_task(void *pvParameters);
 void data_transition(void);
@@ -64,6 +79,7 @@ void CAN_SEND(void);
 void uart1_init(u32 bound);
 void uart2_init(u32 bound);
 void uart3_init(u32 bound);
+void uart3_dma_tx_init(void);
 void uart5_init(u32 bound);
 
 int USART1_IRQHandler(void);
diff --git a/Makefile b/Makefile
index 86dbdaf..9b21202 100644
--- a/Makefile
+++ b/Makefile
@@ -31,7 +31,8 @@ FWLIB_SRC := \
   FWLIB/src/misc.c FWLIB/src/stm32f4xx_gpio.c FWLIB/src/stm32f4xx_rcc.c \
   FWLIB/src/stm32f4xx_syscfg.c FWLIB/src/stm32f4xx_usart.c \
   FWLIB/src/stm32f4xx_adc.c FWLIB/src/stm32f4xx_can.c \
-  FWLIB/src/stm32f4xx_tim.c FWLIB/src/stm32f4xx_pwr.c
+  FWLIB/src/stm32f4xx_tim.c FWLIB/src/stm32f4xx_pwr.c \
+  FWLIB/src/stm32f4xx_dma.c
 
 FREERTOS_SRC := \
   FreeRTOS/croutine.c FreeRTOS/event_groups.c FreeRTOS/list.c \
diff --git a/doc/ackermann_steering_calibration.md b/doc/ackermann_steering_calibration.md
new file mode 100644
index 0000000..f85f4a8
--- /dev/null
+++ b/doc/ackermann_steering_calibration.md
@@ -0,0 +1,374 @@
+# 闃垮厠鏇艰浆鍚戞爣瀹氫笌鎷熷悎璇存槑
+
+鏈枃璁板綍闃垮厠鏇煎皬杞︺��(Vx, Vz) 鈫� 鑸垫満 PWM + 宸﹀彸鍚庤疆閫熷害銆嶆帶鍒跺緥鐨勬爣瀹氭暟鎹��
+鎷熷悎鏂规硶銆佸崟浣嶇害瀹氾紝涓夌鎺у埗妯″紡锛堟爣瀹氳繍鍔ㄥ / 鐩存帴鏄犲皠 / 妯憜闂幆锛夛紝浠ュ強
+涓插彛閫氫俊锛�200 Hz DMA 鍙戦�併��921600 娉㈢壒鐜囷級銆傚搴斾唬鐮侊細
+- 鎺у埗寰嬶細`BALANCE/balance.c` 鐨� `Akm_Car` 鍒嗘敮锛�
+  `Akm_Curvature_To_Servo()`锛堟嫙鍚堬級涓� `Akm_Norm_To_Servo()`锛堢洿鎺ユ弧琛岀▼鏄犲皠锛夈��
+- 寮�鍏冲畯锛歚BALANCE/robot_select_init.h`锛涜埖鏈虹洿琛岀偣 `SERVO_INIT` 鍦� `HARDWARE/motor.h`銆�
+- 涓插彛锛歚HARDWARE/usartx.c`锛坄data_task` / `USART3_SEND` / `uart3_dma_tx_init`锛夛紝
+  娉㈢壒鐜囧湪 `BALANCE/system.c` 鐨� `systemInit`銆�
+
+## 1. 纭欢涓庡潗鏍囩害瀹�
+
+- 鍚庨┍闃垮厠鏇硷細`MOTOR_A` = 宸﹀悗杞紝`MOTOR_B` = 鍙冲悗杞紝鍓嶈疆鐢辫埖鏈猴紙TIM12 CCR2锛夎浆鍚戙��
+- 杈撳叆璇箟锛堜笂浣嶆満涓嬪彂锛夛細
+  - `Vx` = 鍚庤酱涓績绾块�熷害锛屽崟浣� m/s銆�
+  - `Vz` = 缁曡浆寮腑蹇冪殑鏃嬭浆瑙掗�熷害 wz锛屽崟浣� rad/s銆�
+- 绗﹀彿绾﹀畾锛�
+  - 涓婁綅鏈� `Vz` 閬靛惊 ROS 绾﹀畾锛�**Vz > 0 = 閫嗘椂閽� = 宸﹁浆**銆�
+  - 鏍囧畾琛� / 鑸垫満鎷熷悎浣跨敤鐩稿弽绗﹀彿锛�**鏇茬巼 魏 > 0 = 鍙宠浆**銆�
+  - 浠ｇ爜涓� `kappa_fit = -Vz/Vx`锛屼娇 `Vz > 0` 寰楀埌 `kappa_fit < 0`锛堝乏杞級锛�
+    涓庢嫙鍚堝煙涓�鑷淬��
+
+## 2. 鏍囧畾鏂规硶锛堟墜鎺ㄦ硶锛�
+
+鐢垫満澶辫兘銆佽埖鏈轰娇鑳斤紝鎵嬪姩鎺ㄥ姩灏忚溅璧板嚭绋冲畾鍦嗗姬锛屽姣忎釜鑸垫満 PWM 璁板綍锛�
+
+- `y 鍚戝崐鎴窛`锛堝姬瀵瑰簲鐨勭旱鍚戝崐鎴窛锛宑m锛�
+- `x 鍚戝崐寮撻珮`锛堝鸡鐨勭煝楂� / 鍗婂紦楂橈紝cm锛�
+- 鐢卞嚑浣曞弽鎺ㄥ悗杞翠腑蹇冭浆寮崐寰� `R`锛坈m锛変笌 `1/R`
+
+x 鍚戝崐寮撻珮鐨勭鍙风敤浜庡垽瀹氬乏鍙筹細PWM > ~1670 涓哄彸杞紙魏 鍙栨锛夛紝
+浣庝簬璇ョ偣涓哄乏杞紙魏 鍙栬礋锛夈��
+
+### 涓変釜"涓綅"鍊硷紙鏄撴贩锛屽姟蹇呭尯鍒嗭級
+
+| 鍚嶇О | 鍊� | 鍚箟 | 浠ｇ爜浣嶇疆 |
+|------|---:|------|----------|
+| 閬ユ帶 CH1 涓綅 | 1500 | 閬ユ帶鎽囨潌鐗╃悊涓偣 | `AKM_REMOTER_CH1_MID`锛坆alance.c锛� |
+| 鑸垫満鏈烘涓綅 | 1600 | 鑸垫満琛岀▼鍑犱綍涓偣 (MIN+MAX)/2 | 鈥旓紙涓嶅啀鐩存帴浣跨敤锛� |
+| **鐩磋鐐� SERVO_INIT** | **1670** | **瀹炴祴 1/R鈮�0銆佽溅鐪熸璧扮洿绾跨殑鐐�** | `SERVO_INIT`锛坢otor.h:52锛� |
+
+鍏抽敭锛�**鎵�鏈夋帶鍒跺緥鐨勮埖鏈轰腑浣嶉兘瀵归綈鍒� `SERVO_INIT = 1670`**锛堢湡姝ｇ殑鐩磋鐐癸級锛�
+鑰岄潪鏈烘涓綅 1600銆傚紑鏈轰笂鐢点�丮ode 1 / Mode 2 鐨勯浂浣嶃�丆H1 瑕嗙洊閮戒互瀹冧负鍩哄噯锛�
+- CH1 瑕嗙洊鏃惰埖鏈� = `Remoter_Ch1 + (SERVO_INIT - AKM_REMOTER_CH1_MID)`锛屽嵆鏁翠綋
+  骞崇Щ `1670 - 1500 = +170`锛屼娇鎽囨潌灞呬腑 = 杞﹁疆鍥炴銆�
+- 鏀� `SERVO_INIT` 涓�澶勶紝涓嬫父鎵�鏈夎矾寰勮嚜鍔ㄥ榻愩��
+
+### 鍘熷鏍囧畾鏁版嵁
+
+| servo(pwm) | y鍗婃埅璺�/cm | x鍗婂紦楂�/cm | R/cm | 1/R (1/cm) |
+|-----------:|----------:|----------:|-----:|-----------:|
+| 2000 | 34    | 34   | 34.00   | 0.029412 |
+| 1900 | 52.5  | 49.5 | 52.59   | 0.019015 |
+| 1800 | 90    | 60   | 97.50   | 0.010256 |
+| 1700 | 167.2 | 60   | 262.97  | 0.003803 |
+| 1670 | 120   | -1   | 7200.5  | 0.000139 |
+| 1600 | 120   | -23  | 324.54  | 0.003081 |
+| 1500 | 89    | -60  | 96.01   | 0.010416 |
+| 1400 | 61    | -60  | 61.01   | 0.016391 |
+| 1300 | 43    | -44  | 43.01   | 0.023250 |
+| 1200 | 35.5  | -34.3| 35.52   | 0.028152 |
+| 1100 | 30    | -28.9| 30.02   | 0.033310 |
+
+> 鐩磋鐐圭害鍦� PWM 1670锛�1/R 鈮� 0锛寈 鍗婂紦楂� 鈮� 0锛夛紝骞堕潪鑸垫満鏈烘涓綅 1600銆�
+
+## 3. 鍗曚綅鎹㈢畻锛堝叧閿紝鏇惧鑷� 100 鍊嶉敊璇級
+
+鏍囧畾琛� `1/R` 鍒椾互 **1/cm** 涓哄崟浣嶏紙R 鐢ㄥ帢绫筹級銆傝�屼唬鐮侀噷鐨勬洸鐜囨潵鑷�
+`kappa = wz / Vx`锛堝潎涓� SI锛夛紝鍗曚綅鏄� **1/m**銆備袱鑰呯浉宸� 100 鍊嶃��
+
+鎷熷悎鍓嶅繀椤绘妸 R 浠� cm 鎹㈢畻涓� m锛屽啀鍙� `kappa = 1/R_m`锛堝苟甯︾鍙凤級锛�
+
+| servo | kappa (1/m, +=鍙�) |
+|------:|------------------:|
+| 2000  | +2.9412 |
+| 1900  | +1.9015 |
+| 1800  | +1.0256 |
+| 1700  | +0.3803 |
+| 1670  | +0.0139 |
+| 1600  | -0.3081 |
+| 1500  | -1.0416 |
+| 1400  | -1.6391 |
+| 1300  | -2.3250 |
+| 1200  | -2.8152 |
+| 1100  | -3.3310 |
+
+- 鏇茬巼閲忕▼锛毼� 鈭� [鈭�3.331, +2.941] 1/m銆�
+- 鏈�灏忚浆寮崐寰� R_min 鈮� 0.30 m锛堝乏锛�/ 0.34 m锛堝彸锛夈��
+- 浠ｇ爜涓� `AKM_KAPPA_MAX = 3.331f`銆�
+
+## 4. 鎷熷悎鏂规硶涓庣粨鏋�
+
+浠� `servo = f(kappa)`锛坘appa 涓鸿嚜鍙橀噺锛�1/m锛夊仛澶氶」寮忔渶灏忎簩涔樸��
+姣旇緝涓�/浜�/涓夋锛屽苟鐢ㄧ暀涓�浜ゅ弶楠岃瘉锛圠OO锛夎瘎浼板鏈鐐圭殑娉涘寲锛�
+
+| 鎷熷悎 | 鍏ㄩ噺 max娈嬪樊 | 鍏ㄩ噺 rms | LOO max璇樊 | LOO rms |
+|------|-----------:|--------:|----------:|-------:|
+| 涓�娆� | 56.8 | 28.3 | 鈥� | 鈥� |
+| 浜屾 | 12.3 | 7.6 | 15.1 | 10.2 |
+| 涓夋 | 12.6 | 7.5 | 鈥� | 鈥� |
+
+缁撹锛�**浜屾鎷熷悎鏈�浼�**銆備笁娆′笉鍐嶆敼鍠勶紙杩囨嫙鍚堬級锛屼竴娆℃畫宸繃澶с��
+
+### 鏈�缁堢郴鏁帮紙1/m 鍗曚綅锛�
+
+```
+servo = 1656.373 + 140.548 * kappa - 7.654 * kappa^2
+```
+
+浠ｇ爜瀹忥紙`BALANCE/balance.c`锛夛細
+
+```c
+#define AKM_SERVO_C0   1656.373f
+#define AKM_SERVO_C1    140.548f
+#define AKM_SERVO_C2     (-7.654f)
+#define AKM_SERVO_MIN   1100
+#define AKM_SERVO_MAX   2000
+#define AKM_KAPPA_MAX   3.331f
+```
+
+### 涓轰粈涔堢敤鎷熷悎鑰岄潪鏌ヨ〃 + 绾挎�ф彃鍊�
+
+- 鏁版嵁鏄墜鎺ㄦ祴寰楋紝鍚祴閲忓櫔澹般�傛煡琛ㄨ杩┛杩囨瘡涓櫔澹扮偣锛屼袱鐐归棿鐩寸嚎娈典細
+  鏀惧ぇ鍣０锛汱OO 鏄剧ず鏌ヨ〃 max 璇樊 23 / rms 14锛屾槑鏄惧樊浜庝簩娆℃嫙鍚堬紙15 / 10锛夈��
+- 鎷熷悎鍦ㄧ鐐瑰鎺ㄦ椂褰㈢姸姝ｇ‘锛堟洸绾夸袱绔槑鏄惧集锛夛紝绾挎�ф彃鍊煎彧鑳芥寜鏈�鍚庝竴娈�
+  鏂滅巼澶栨帹锛岃秺鎺ㄨ秺鍋忋��
+- 鎷熷悎鍦ㄨ妭鐐瑰骞虫粦锛屾棤鏂滅巼绐佸彉锛涜繍琛屾垚鏈粎涓ゆ涔樺姞锛屾瘮鏌ユ壘鍖洪棿杩樼渷銆�
+
+### 杞磋窛 L 鍘诲摢浜�
+
+鏍囧畾琛ㄧ鍒扮鐩存帴娴嬩簡 `servo 鈫� R`锛屽凡鎶娿�岃埖鏈� PWM 鈫� 鍓嶈疆杞 鈫�
+`R = L/tan(未)`銆嶆暣鏉￠摼璺強浼犲姩姣斻�佽疆鑳庝晶鍋忕瓑鐪熷疄鏁堝簲鍚告敹杩涙嫙鍚堢郴鏁般��
+鍥犳鏃犻渶鍐嶆樉寮忓啓 `R = Axle_spacing/tan(未)`锛堥偅鍙嶈�屼緷璧栦笉鍑嗙殑鍋囪浼犲姩姣旓級銆�
+鍚庤疆宸�熷彧鐢ㄨ疆璺� track 涓� 魏锛屾湰灏变笉鍚� L銆傞樋鍏嬫浖鍑犱綍娌℃湁涓紝鑰屾槸浠ユ洿璐村悎
+瀹炶溅鐨勫疄娴嬪舰寮忓祵鍏ユ嫙鍚堛��
+
+## 5. 鎺у埗寰嬶紙Mode 0锛氭爣瀹氳繍鍔ㄥ锛岄粯璁わ級
+
+`AKM_DIRECT_MAP = 0` 鏃讹細
+
+```
+kappa_geom = Vz / Vx            (Vx鈮�0 鏃剁疆 0锛岄樋鍏嬫浖鏃犲墠杩涗笉鑳借浆鍚�)
+kappa_fit  = clamp(-kappa_geom, 卤AKM_KAPPA_MAX)
+Servo      = f(kappa_fit)                          # 浜屾鎷熷悎
+MOTOR_A(宸�) = Vx * (1 + 0.5*track*kappa_fit)       # 鍚庤疆宸��
+MOTOR_B(鍙�) = Vx * (1 - 0.5*track*kappa_fit)
+```
+
+鐗╃悊鐗规�э細鑸垫満瑙掑彧鍐冲畾杞集鍗婂緞 R = 1/魏銆�
+- 鍥哄畾鑸垫満瑙掋�佹敼 Vx 鈫� wz 闅忎箣鍙橈紙璧板悓涓�鍦嗭紝蹇參涓嶅悓锛夈��
+- 鍥哄畾 wz銆佹敼 Vx 鈫� 魏 = wz/Vx 鍙� 鈫� 鑸垫満瑙掑彉锛堟纭涓猴級銆�
+
+### Vx鈥搘z 鍙鍩燂紙魏_max 鈮� 3.33 /m锛�
+
+`|wz| <= Vx * 魏_max`锛�
+
+| Vx (m/s) | 鍙敤 wz 鑼冨洿 (rad/s) |
+|---------:|:--------------------|
+| 0.2 | [鈭�0.67, 0.67] |
+| 0.3 | [鈭�1.00, 1.00] |
+| 0.5 | [鈭�1.67, 1.67] |
+| 1.0 | [鈭�3.33, 3.33] |
+
+> 渚嬶細鍥哄畾 wz=0.12锛岄渶 Vx 鈮� 0.036 m/s 鎵嶄笉琚す绱с�備箣鍓� `AKM_KAPPA_MAX`
+> 璇涓� 0.0334锛堝搴� R鈮�30 m锛夊鑷翠换浣曢�熷害涓嬮兘琚す姝汇�佹敼閫熷害鑸垫満涓嶅姩锛�
+> 宸查殢鍗曚綅淇瑙ｅ喅銆�
+
+## 6. 鎺у埗寰嬶紙Mode 1锛氱洿鎺ユ槧灏勶紝璋冭瘯鐢級
+
+`AKM_DIRECT_MAP = 1` 鏃讹紙鑴辩闃垮厠鏇肩墿鐞嗭紝鐢ㄤ簬闅旂璋冭瘯鑸垫満/鐢垫満锛夛細
+
+```
+vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, 卤1)
+Servo   = Akm_Norm_To_Servo(vz_norm)     # 浠� SERVO_INIT 涓洪浂浣嶇殑鍒嗘婊¤绋嬫槧灏�
+MOTOR_A(宸�) = Vx                          # 鏃犲樊閫�
+MOTOR_B(鍙�) = Vx
+```
+
+- `Vz` 绾挎�ч摵婊℃暣涓埖鏈鸿绋� [AKM_SERVO_MIN, AKM_SERVO_MAX]锛屼笌閫熷害鏃犲叧銆�
+- **闆朵綅瀵归綈鐩磋鐐�**锛歚Akm_Norm_To_Servo` 浠� `SERVO_INIT=1670`锛堣�岄潪鏈烘涓綅
+  1600锛変负涓績锛屽乏鍙充袱渚у悇鑷缉鏀惧埌鑷繁鐨勭鐐癸紝鍗充娇涓綅鍋忕疆涔熻兘鐢ㄦ弧鍏ㄨ绋嬶細
+  ```
+  span = (norm>=0) ? (SERVO_INIT - AKM_SERVO_MIN)   # 宸︿晶琛岀▼ 1670-1100=570
+                   : (AKM_SERVO_MAX - SERVO_INIT)   # 鍙充晶琛岀▼ 2000-1670=330
+  Servo = SERVO_INIT - norm * span
+  ```
+- `Vz > 0`锛堝乏锛夆啋 `norm>0` 鈫� 闈犺繎 AKM_SERVO_MIN锛堝乏绔級锛岀鍙蜂笌 ROS 涓�鑷淬��
+- `Vx` 鍘熸牱缁欏乏鍙崇數鏈猴紝涓嶅仛鏇茬巼/宸�熻繍绠椼��
+- 婊￠噺绋嬭緭鍏ョ敱 `AKM_DIRECT_VZ_FULL`锛堥粯璁� **1.0 rad/s**锛夎瀹氥��
+
+> **`AKM_DIRECT_VZ_FULL` 鐨勭墿鐞嗗惈涔�** = 涓婁綅鏈轰細鍙戠殑鏈�澶� angular.z锛孷z 鍒拌鍊兼椂
+> 鑸垫満鎵撴弧銆傚畠鍙喅瀹�"Vz鈫掕绋�"鐨勬瘮渚嬶紝涓嶆敼鍙樺嘲鍊艰浆瑙掞紙宄板�肩敱鑸垫満绔偣 1100/2000
+> 鍐冲畾锛屾亽涓� ~28掳宸� / 25掳鍙筹級銆傚お灏忊啋灏� Vz 灏遍ケ鍜屻�佸け鍘绘瘮渚嬫帶鍒讹紱澶ぇ鈫掑父鐢ㄥ尯闂�
+> 鍙敤鍒颁竴灏忔琛岀▼銆佽浆瑙掑亸灏忋�傚綋鍓嶅畾涓� 1.0锛圴z=卤1.0 鎵撴弧锛夛紝鎶婅绋嬫憡鍒� 卤1 rad/s
+> 鍏ㄥ尯闂达紝瑙勫垝鍒嗚鲸鐜囨瘮 0.5 鏃剁炕鍊嶃�俈z 鍦� RX 涓柇閲屼互 rad/s 鍒拌揪
+> 锛坲sartx.c `XYZ_Target_Speed_transition`: raw/1000锛夈��
+
+## 7. 鎺у埗寰嬶紙Mode 2锛氭í鎽嗚閫熷害闂幆 / 绠�鍖栨壄鐭╃煝閲忥級
+
+`AKM_DIRECT_MAP = 0` 涓� `AKM_YAW_ASSIST = 1` 鏃跺惎鐢ㄣ��
+
+**鍔ㄦ満锛堜负浠�涔堣鏈� Mode 2锛�**锛歁ode 0 閲� `魏 = Vz/Vx`锛屽悓涓�涓浆鍚戞寚浠� `Vz` 鍦�
+楂橀�熸椂鏇茬巼琚� `Vx` 闄ゅ皬锛岃埖鏈鸿嚜鍔ㄥ洖姝� 鈥斺�� 杩欏氨鏄�"楂橀�熻浆寮墦涓嶅姩"鐨勬牴婧愩�侻ode 2
+**涓诲姩瑙ｈ�� w 涓� v**锛氳埖鏈虹敱 `Vz` 鐩存帴鍐冲畾锛堜笌 Mode 1 涓�鏍凤紝涓嶅啀闄や互 Vx锛夛紝
+鎵�浠ュぇ `Vz` 鍦ㄤ换浣曢�熷害閮界粰鍑哄ぇ鍓嶈疆瑙掞紱闃垮厠鏇煎彧浣滀负鍚庤疆宸�熺殑**鍓嶉鍙傝��**锛�
+鍐嶅彔鍔� IMU 妯憜瑙掗�熷害 PI 闂幆锛堢畝鍖栨壄鐭╃煝閲忥級銆傛帶鍒堕摼璺細
+
+```
+Vz 鈹�(鐩存帴婊¤绋嬫槧灏�, 涓嶸x鏃犲叧)鈫� 鑸垫満涓昏浆鍚� Akm_Norm_To_Servo   鈫� 鏂瑰悜涓绘帶锛岃В鑰�
+Vx 鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈹�鈫� 宸﹀彸杞熀閫�                      鈫� 椹卞姩涓绘帶锛岃В鑰�
+(Vx, 魏_cmd) 鈹�(闃垮厠鏇煎嚑浣�)鈫� r_ref 鈹�鈫� 鍚庤疆宸�熷墠棣� + IMU PI          鈫� akm 浠呭仛鍓嶉
+```
+
+璁＄畻锛坒it 鍩� 魏>0=鍙筹紱ROS Vz>0=宸�=CCW锛況>0=宸﹁浆锛夛細
+
+```
+vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, 卤1)   # 杞悜鎸囦护锛屼笉鍚� Vx 鈥斺�� 鍏抽敭瑙ｈ�︾偣
+Servo   = Akm_Norm_To_Servo(vz_norm)          # 涓� Mode 1 瀹屽叏鐩稿悓鐨勬弧琛岀▼鐩存帴鏄犲皠
+                                              # 浠� SERVO_INIT 涓洪浂浣嶏紝涓嶈蛋鎷熷悎鏇茬嚎
+魏_cmd   = -vz_norm * AKM_KAPPA_MAX             # 浠呯敤浜庢瀯閫犱笅鏂瑰墠棣堝弬鑰冿紝涓嶉┍鍔ㄨ埖鏈�
+
+r_ref   = -魏_cmd * Vx                         # 闃垮厠鏇煎嚑浣曞墠棣堢殑鏈熸湜妯憜瑙掗�熷害
+r_meas  = AKM_GYRO_Z_SIGN * gyro[2] / AKM_GYRO_Z_TO_RADPS
+r_filt += AKM_YAW_IMU_LPF * (r_meas - r_filt)  # 杞诲害涓�闃朵綆閫�
+
+e_r     = r_ref - r_filt
+dv_ff   = AKM_YAW_FF_ALPHA * 0.5 * track * r_ref  # 鍑犱綍鍓嶉
+dv_fb   = AKM_YAW_KP * e_r + AKM_YAW_KI * 鈭玡_r    # PI 鍙嶉锛堢煩褰㈢Н鍒�, dt=1/200s锛�
+dv      = clamp(dv_ff + dv_fb, 卤AKM_YAW_MAX_DIFF_RATIO*|Vx|)  # 甯︽姉绉垎楗卞拰
+
+MOTOR_A(宸�) = Vx - dv
+MOTOR_B(鍙�) = Vx + dv
+```
+
+瑕佺偣锛�
+- **瑙ｈ�︽槸鏍稿績**锛歚vz_norm` 鍙敱 `Vz` 鍐冲畾锛屼笉鍐� `Vz/Vx`锛屾墍浠ラ珮閫熷ぇ杞悜涓嶅啀琚�
+  鍑犱綍"绋�閲�"銆俙Vx` 鐙珛璁惧畾椹卞姩鍩洪�熴��
+- **鑸垫満璧扮洿鎺ユ弧琛岀▼鏄犲皠锛屼笉璧版嫙鍚�**锛歁ode 2 鐨勮埖鏈轰笌 Mode 1 / CH1 璋冭瘯涓�鑷达紝
+  鐢� `Akm_Norm_To_Servo(vz_norm)` 鎶婅浆鍚戞寚浠ょ嚎鎬ч摵婊¤绋嬶紙浠� `SERVO_INIT` 涓�
+  闆朵綅锛夛紝**涓嶅啀璋冪敤鏍囧畾鎷熷悎 `f(魏)`**銆傝繖鏍�"楂橀�熻浆寮墦涓嶅姩"浠庢牴涓婃秷澶扁�斺�旇埖鏈鸿
+  鍙湅鎸囦护銆佷笌閫熷害鏃犲叧銆傦紙鏃╂湡鐗堟湰杩欓噷閿欑敤浜� `f(魏_cmd)`锛屼粛闅愬惈鑰﹀悎锛屽凡鏀规銆傦級
+- **闃垮厠鏇奸檷绾т负绾墠棣�**锛歚魏_cmd = -vz_norm*AKM_KAPPA_MAX` 鍙敤鏉ユ瀯閫� `r_ref`
+  鈥斺��"鑻ヨ溅鎸夎鏇茬巼璧板嚭鐨勭悊鎯虫í鎽嗚閫熷害"锛屼粎鍠傜粰鍚庤疆宸�熷墠棣堬紱瀹為檯妯憜鐢� IMU PI
+  鏀舵暃锛屽彲鍏嬫湇鍓嶈疆鍑犱綍/鎵撴粦甯︽潵鐨勫亸宸�俙魏_cmd` 涓嶅弬涓庤埖鏈鸿绠椼��
+- **閫�鍖栧叧绯�**锛歚AKM_YAW_FF_ALPHA = 1` 涓� `AKM_YAW_KP = AKM_YAW_KI = 0` 鏃讹紝
+  `dv = 0.5*track*r_ref = -0.5*track*魏_cmd*Vx`锛屽嵆绾嚑浣曞樊閫熴�偽� 鏄钩婊戞棆閽細
+  0 = 绾� IMU 鍙嶉锛�1 = 绾嚑浣曞墠棣堛��
+- **浣庨�熷喕缁�**锛歚|Vx| < AKM_YAW_MIN_SPEED` 鏃舵竻闆剁Н鍒嗗苟鍋滅敤宸�燂紙浣庨�熸í鎽嗚
+  閫熷害淇″櫔姣斿お宸級銆�
+- 鐢ㄥ師濮嬭閫熷害锛坄gyro[2]` 鍘婚浂鍋忓悗鐨� LSB锛夛紝涓嶅仛鑸悜瑙掔Н鍒嗐��
+
+### 涓婁綅鏈鸿鍒掔敤锛氬彂甯� Vz 鈫� 澶ц嚧鍓嶈疆杞 未
+
+Mode 2 閲岃埖鏈哄彧鐢� `Vz` 鍐冲畾锛堜笌 `Vx` 鏃犲叧锛夛紝鎵�浠ヤ笂浣嶆満鍙洿鎺ユ寜涓嬭〃浼拌浆瑙掋��
+鎹㈢畻閾捐矾锛堥潪绾挎�э紝鍥犱负 PWM鈫扲 閭ｆ鏄爣瀹氭洸绾匡級锛�
+
+```
+vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, 卤1)      # 榛樿 VZ_FULL=1.0 鈫� Vz=卤1.0 鎵撴弧
+Servo   = SERVO_INIT - vz_norm * span             # 宸� span=570, 鍙� span=330
+R       = 1 / |1/R(Servo)|                         # 1/R 鐢� 搂2 鏍囧畾琛�(1/cm)鎻掑�煎啀脳100
+未       = atan(L / R)                              # L = Akm_axlespacing = 0.160 m
+```
+
+鐢ㄥ綋鍓嶅畯鍊硷紙`SERVO_INIT=1670`, `MIN=1100`, `MAX=2000`, `VZ_FULL=1.0`锛夈�佹寜 搂2 鏍囧畾琛�
+`1/R` 鍒楁彃鍊肩畻鍑虹殑瀵圭収琛紙未 涓哄悗杞寸瓑鏁堝墠杞浆瑙掞紝姝ｆ暟鍙〃澶у皬锛屾柟鍚戣鏈垪锛夛細
+
+| Vz (rad/s) | vz_norm | Servo(PWM) | R(m) | 未(掳) | 鏂瑰悜 |
+|-----------:|--------:|-----------:|-----:|-----:|:----:|
+| +1.0 | +1.0 | 1100 | 0.30 | 28.1 | 宸� |
+| +0.8 | +0.8 | 1214 | 0.36 | 23.7 | 宸� |
+| +0.6 | +0.6 | 1328 | 0.47 | 18.8 | 宸� |
+| +0.5 | +0.5 | 1385 | 0.57 | 15.6 | 宸� |
+| +0.4 | +0.4 | 1442 | 0.72 | 12.5 | 宸� |
+| +0.3 | +0.3 | 1499 | 0.95 |  9.5 | 宸� |
+| +0.2 | +0.2 | 1556 | 1.59 |  5.8 | 宸� |
+| +0.1 | +0.1 | 1613 | 3.95 |  2.3 | 宸� |
+|  0.0 |  0.0 | 1670 | 鈭�    |  0.0 | 鐩� |
+| 鈭�0.1 | 鈭�0.1 | 1703 | 2.50 |  3.7 | 鍙� |
+| 鈭�0.2 | 鈭�0.2 | 1736 | 1.63 |  5.6 | 鍙� |
+| 鈭�0.3 | 鈭�0.3 | 1769 | 1.21 |  7.5 | 鍙� |
+| 鈭�0.4 | 鈭�0.4 | 1802 | 0.96 |  9.5 | 鍙� |
+| 鈭�0.5 | 鈭�0.5 | 1835 | 0.75 | 12.0 | 鍙� |
+| 鈭�0.6 | 鈭�0.6 | 1868 | 0.62 | 14.5 | 鍙� |
+| 鈭�0.8 | 鈭�0.8 | 1934 | 0.44 | 19.8 | 鍙� |
+| 鈭�1.0 | 鈭�1.0 | 2000 | 0.34 | 25.2 | 鍙� |
+
+瑕佺偣锛堜笂浣嶆満瑙勫垝鍔″繀娉ㄦ剰锛夛細
+- **宸﹀彸涓嶅绉�**锛氱洿琛岀偣 1670 鍋忓悜鍙崇锛屽乏渚ц绋� 570銆佸彸渚т粎 330锛屾墍浠ュ悓鏍�
+  `|Vz|` 宸﹁浆瑙掓瘮鍙宠浆瑙掔暐澶э紙婊￠噺绋� 28掳 宸� vs 25掳 鍙筹級銆傝繖鏄満姊颁腑浣嶅亸缃鑷寸殑锛�
+  宸茶 `Akm_Norm_To_Servo` 鐨勫垎娈电缉鏀惧惛鏀躲��
+- **`|Vz| 鈮� 1.0` 鍏ㄩ儴楗卞拰**鍒扮鐐癸紙28掳宸� / 25掳鍙筹級锛涘啀澶х殑 Vz 涓嶄細鏈夋洿澶ц浆瑙掋��
+  `VZ_FULL` 浠� 0.5 鎻愬埌 1.0 鍚庯紝婊￠噺绋嬫寚浠� = 卤1.0 rad/s锛屽悓鏍风殑鑸垫満琛岀▼鎽婂埌鏇村鐨�
+  Vz 鍖洪棿锛岃鍒掑垎杈ㄧ巼缈诲�嶏紙宄板�艰浆瑙掍笉鍙橈級銆�
+- **闈炵嚎鎬�**锛歏z鈫捨� 涓嶆槸鐩寸嚎锛堜綆 Vz 娈垫瘡 0.1 绾� +3掳锛岄珮 Vz 娈佃秼缂擄級锛岃鍒掓椂鎸夎〃
+  鎻掑�艰�岄潪绾挎�у鎺ㄣ��
+- R 鏄�**鍚庤酱涓績杞集鍗婂緞**锛屛� = atan(L/R) 鏄瓑鏁堝墠杞浆瑙掞紱瀹炶溅鍙楄疆鑳庝晶鍋�/鎵撴粦
+  褰卞搷锛岃〃鍊间负鏍囧畾闈欐帹鐨勮繎浼硷紝鍔ㄦ�佷笅 IMU 妯憜鐜細鍐嶅仛淇銆�
+
+璋冨弬椤哄簭锛堢敤鎴峰缓璁級锛�
+1. 鍏堟爣瀹氶檧铻洪浂鍋忥紙寮�鏈洪潤姝㈤噰鏍凤紝浠ｇ爜宸插仛锛夈��
+2. **瀹炴祴纭 `gyro[2]` 绗﹀彿**锛氬懡浠ゅ乏杞紝纭 `gyro[2] > 0`锛涜嫢鐩稿弽鎶�
+   `AKM_GYRO_Z_SIGN` 鏀逛负 `-1.0f`銆�
+3. 灏� `Kp`锛�0.05~0.10锛夎捣姝ワ紝閫愭鍔犲ぇ鍒颁复鐣屽墠銆�
+4. 鏈夌ǔ鎬佽宸啀鍔犱竴鐐� `Ki`銆�
+5. 鏈�鍚庡紩鍏ュ墠棣� `伪 = 0.2~0.5` 鍑忚交 PI 璐熸媴銆�
+
+## 8. 寮�鍏充竴瑙堬紙`BALANCE/robot_select_init.h`锛�
+
+| 瀹� | 榛樿 | 鍚箟 |
+|----|-----:|------|
+| `AKM_SERVO_DEBUG_REMOTE_CH1` | 1 | 1 = 鑸垫満鐩存帴鐢遍仴鎺� CH1 椹卞姩锛岃鐩栨墍鏈夋帶鍒跺緥锛堟澘绾ц皟璇曪級 |
+| `AKM_DIRECT_MAP` | 1 | 1 = 鐩存帴鏄犲皠璋冭瘯妯″紡锛�0 = 浜ょ粰 Mode 0 / Mode 2 |
+| `AKM_DIRECT_VZ_FULL` | 1.0f | Mode 1 & Mode 2 鍏辩敤锛歏z 婊¤绋嬮噺绋嬶紙rad/s锛�= 涓婁綅鏈轰細鍙戠殑鏈�澶� angular.z |
+| `AKM_YAW_ASSIST` | 0 | 1 = 妯憜闂幆锛圡ode 2锛夛紱浠呭湪 `AKM_DIRECT_MAP=0` 鏃剁敓鏁� |
+| `AKM_YAW_KP` / `AKM_YAW_KI` | 0.10 / 0.00 | 妯憜瑙掗�熷害璇樊 PI 澧炵泭锛坢/s per rad/s锛� |
+| `AKM_YAW_FF_ALPHA` | 0.00f | 鍑犱綍鍓嶉娣峰悎绯绘暟锛�0=绾弽棣堬紝1=绾嚑浣曞樊閫�=Mode 0锛� |
+| `AKM_YAW_MIN_SPEED` | 0.10f | 浣庝簬姝ら�熷害鍐荤粨妯憜鐜紙m/s锛� |
+| `AKM_YAW_MAX_DIFF_RATIO` | 0.35f | 宸�熷箙搴︿笂闄愶紙鍗� |Vx| 鐨勬瘮渚嬶級 |
+| `AKM_GYRO_Z_TO_RADPS` | 3754.9f | gyro[2] LSB鈫抮ad/s锛團S 卤500dps锛� |
+| `AKM_GYRO_Z_SIGN` | +1.0f | IMU +z 涓� ROS锛�+=宸︼級绗﹀彿瀵归綈锛屽疄娴嬬‘璁� |
+| `AKM_YAW_IMU_LPF` | 0.30f | 妯憜瑙掗�熷害涓�闃朵綆閫氱郴鏁帮紙0=鏃犳护娉級 |
+
+妯″紡浜掓枼涓庝紭鍏堢骇锛�
+- `AKM_SERVO_DEBUG_REMOTE_CH1 = 1` 鏃讹紝**鑸垫満鎬绘槸鐢� CH1 瑕嗙洊**锛屼紭鍏堜簬浠讳綍鎺у埗
+  寰嬭绠楀嚭鐨勮埖鏈哄�硷紙鐢垫満宸�熶粛鎸夋墍閫夋ā寮忚繍琛岋級銆�
+- `AKM_DIRECT_MAP` 涓� `AKM_YAW_ASSIST` 閮戒綔鐢ㄤ簬鏁存潯鎺у埗寰嬶細`AKM_DIRECT_MAP = 1`
+  浼樺厛锛堢洿鎺ユ槧灏勶級锛沗= 0` 鏃跺啀鐪� `AKM_YAW_ASSIST`锛�1 = Mode 2 妯憜闂幆锛�
+  0 = Mode 0 鏍囧畾杩愬姩瀛︼級銆�
+
+## 9. 涓插彛閫氫俊锛圧OS 鈫� STM32锛�
+
+### 棰戠巼涓庢柟鍚�
+
+| 鏂瑰悜 | 棰戠巼 | 鏈哄埗 | 浠ｇ爜浣嶇疆 |
+|------|-----:|------|----------|
+| 鎺у埗鐜� | 200 Hz | `Balance_task`锛宍RATE_200_HZ` | balance.c:350 |
+| 鍙戦�侊紙TX锛孲TM32鈫扲OS锛� | 200 Hz | **DMA 闈為樆濉�** | usartx.c `data_task` |
+| 鎺ユ敹锛圧X锛孯OS鈫扴TM32锛� | 涓柇椹卞姩 | `USART3_IRQHandler`锛圧XNE锛� | usartx.c |
+
+- 鍙繚鐣� **USART3锛圧OS锛�**銆傚師鍏� `data_task` 20 Hz 闃诲寮忕兢鍙� USART1/3/5+CAN锛�
+  鐜板凡鍒犳帀 USART1/USART5/CAN 鍙戦�侊紝鍙暀 USART3銆�
+- 甯ч暱 24 瀛楄妭锛坄SEND_DATA_SIZE`锛夛紝`Vz` 鍦� RX 涓柇閲岀敱
+  `XYZ_Target_Speed_transition` 瑙ｆ瀽锛歚raw/1000 + (raw%1000)*0.001`锛坮ad/s锛夈��
+
+### 涓轰粈涔堟敼 DMA 闈為樆濉炲彂閫�
+
+鏃х殑 `usart3_send` 鏄繖绛夛細`USART3->DR = data; while((USART3->SR&0x40)==0);`銆�
+`data_task` 涓� `Balance_task` **鍚屼负浼樺厛绾� 4**锛團reeRTOS 鎶㈠崰 + 鏃堕棿鐗囷紝1ms tick锛夈��
+鍚屼紭鍏堢骇涓嬪繖绛夋棤娉曡鎺у埗鐜姠鍗狅紝20鈫�200 Hz 浼氭斁澶ф姈鍔ㄣ�佺櫧鑰� CPU銆傛敼鎴� DMA 鍚庯細
+
+- `USART3_SEND` 鍙Е鍙戜竴娆� DMA 浼犺緭锛圖MA1 Stream3 / Channel4 = USART3_TX锛夛紝
+  CPU 绔嬪嵆杩斿洖锛宻hift-out 鏈熼棿涓嶅崰鐢ㄦ帶鍒剁幆銆�
+- 涓婁竴甯ф湭鍙戝畬鍒欒烦杩囨湰鍛ㄦ湡锛坄DMA_GetCmdStatus != DISABLE` 鍒ゅ畾锛夛紝涓嶅繖绛夈��
+- 闇�瑕佹妸 `FWLIB/src/stm32f4xx_dma.c` 鍔犲叆 Makefile锛堝師鍏堟湭缂栬瘧璇ラ┍鍔級銆�
+
+### 娉㈢壒鐜� 921600
+
+USART3 娉㈢壒鐜� **115200 鈫� 921600**锛坰ystem.c:129锛宍uart3_init(921600)`锛夈��
+
+- 涓�甯� 24 瀛楄妭 = 240 bit锛�8N1锛�10 bit/byte锛夌殑 shift-out 鏃堕棿锛�
+  - 115200锛�240/115200 鈮� **2.08 ms**
+  - 921600锛�240/921600 鈮� **0.26 ms**锛堝揩 8 鍊嶏紝杩滀綆浜� 5 ms 鍛ㄦ湡锛�
+- 甯﹀鍗犵敤锛�200 Hz 脳 24 B = 4800 B/s锛屼袱妗ｉ兘缁扮话鏈変綑锛涙彁閫熶富瑕佹槸鐣欎綑閲忋��
+- APB1=42 MHz锛�16 鍊嶈繃閲囨牱鍒嗛鍚庡疄闄呯害 913 k锛屽亸宸� ~0.9%锛圲ART 瀹瑰繊 <2.5%锛夈��
+- **ROS 绔繀椤诲悓姝ユ敼 921600**锛屽惁鍒欎贡鐮佲�斺�旇繖鏄渶鏄撴紡鐨勪竴姝ャ��
+
+## 10. 澶嶇幇鎷熷悎
+
+鏍囧畾鍘熷鑴氭湰涓庝腑闂翠骇鐗╁湪 `calibration/` 鐩綍銆傛牳蹇冩楠わ細
+1. R(cm) 鈫� R(m)锛宍kappa = sign / R_m`锛坰ign 鐢� x 鍗婂紦楂樺畾锛�+ 涓哄彸锛夈��
+2. 瀵� `(kappa, servo)` 鍋氫簩娆℃渶灏忎簩涔橈紝寰� C0/C1/C2銆�
+3. 鐢� LOO 浜ゅ弶楠岃瘉纭浜屾浼樹簬鏌ヨ〃涓庝笁娆°��
+4. `AKM_KAPPA_MAX = max(|kappa|)`銆�
diff --git a/doc/uart_protocol_v2_timestamp.md b/doc/uart_protocol_v2_timestamp.md
new file mode 100644
index 0000000..c1197eb
--- /dev/null
+++ b/doc/uart_protocol_v2_timestamp.md
@@ -0,0 +1,111 @@
+# 涓嬩綅鏈� UART 鍙戦�佸崗璁� v2锛堝甫閲囨牱鏃堕棿鎴筹級
+
+鏈枃浠舵弿杩� STM32 涓嬩綅鏈洪�氳繃 **USART3锛圧OS 鍙ｏ紝娉㈢壒鐜� 921600锛�** 鍛ㄦ湡鍙戦�佺粰涓婁綅鏈虹殑
+浼犳劅鍣ㄦ暟鎹抚鏍煎紡銆倂2 鍦ㄥ師 24 瀛楄妭甯х殑鍩虹涓婏紝浜�**甯у熬杩藉姞**浜嗕細璇� ID 涓庝袱涓噰鏍�
+鏃堕棿鎴冲瓧娈碉紝鐢ㄤ簬涓婁綅鏈哄仛 IMU / 杞�� / 闆疯揪鐨勬椂闂村榻愩��
+
+- 瀵瑰簲婧愮爜锛歔HARDWARE/usartx.c](../HARDWARE/usartx.c)銆乕HARDWARE/usartx.h](../HARDWARE/usartx.h)
+- 鏃堕棿婧愬疄鐜帮細[HARDWARE/timer.c](../HARDWARE/timer.c) 鐨� `TIM7_Init()` / `mcu_time_us()`
+- 璁捐鑳屾櫙涓庢�讳綋鍘熷垯瑙� [鏃堕棿鎴�.md](鏃堕棿鎴�.md)
+
+## 1. 甯ф�昏
+
+| 椤圭洰 | 鍊� |
+| ---- | ---- |
+| 甯уご `FRAME_HEADER` | `0x7B` |
+| 甯у熬 `FRAME_TAIL` | `0x7D` |
+| 甯ч暱 `SEND_DATA_SIZE` | **44 瀛楄妭**锛坴1 涓� 24 瀛楄妭锛� |
+| 鍙戦�佸懆鏈� | 200 Hz锛坄data_task`锛孶SART3 闈為樆濉� DMA 鍙戦�侊級 |
+| 瀛楄妭搴� | 澶氬瓧鑺傚瓧娈典竴寰� **澶х**锛堥珮瀛楄妭鍦ㄥ墠锛� |
+| 鏍￠獙 | 鍗曞瓧鑺� XOR锛岃鐩� byte 0..41 |
+
+璁捐绾︽潫锛歜yte 0..21 涓� v1 瀹屽叏涓�鑷达紝鏂板瀛楁鍏ㄩ儴鏀惧湪鏍￠獙浣嶄箣鍓嶇殑灏鹃儴锛�
+浣垮彧璇诲彇鏃у瓧娈电殑瑙ｆ瀽鍣ㄤ粛鐒跺吋瀹广��
+
+## 2. 瀛楄妭甯冨眬
+
+| 鍋忕Щ | 闀垮害 | 瀛楁 | 绫诲瀷 | 璇存槑 |
+| ---- | ---- | ---- | ---- | ---- |
+| 0 | 1 | Frame_Header | u8 | 甯уご `0x7B` |
+| 1 | 1 | Flag_Stop | u8 | 杞欢澶辫兘鏍囧織锛�1=鍋滆溅锛� |
+| 2 | 2 | X_speed | int16 | X 杞寸嚎閫熷害锛屽崟浣� mm/s锛屽ぇ绔� |
+| 4 | 2 | Y_speed | int16 | Y 杞寸嚎閫熷害锛屽崟浣� mm/s锛屽ぇ绔� |
+| 6 | 2 | Z_speed | int16 | Z 杞磋閫熷害锛屽崟浣� mm/s锛堢瓑鏁堬級锛屽ぇ绔� |
+| 8 | 2 | Accel_X | int16 | 鍔犻�熷害璁� X锛圧OS 鍧愭爣锛夛紝澶х |
+| 10 | 2 | Accel_Y | int16 | 鍔犻�熷害璁� Y锛圧OS 鍧愭爣锛夛紝澶х |
+| 12 | 2 | Accel_Z | int16 | 鍔犻�熷害璁� Z锛圧OS 鍧愭爣锛夛紝澶х |
+| 14 | 2 | Gyro_X | int16 | 闄�铻轰华 X锛圧OS 鍧愭爣锛夛紝澶х |
+| 16 | 2 | Gyro_Y | int16 | 闄�铻轰华 Y锛圧OS 鍧愭爣锛夛紝澶х |
+| 18 | 2 | Gyro_Z | int16 | 闄�铻轰华 Z锛圧OS 鍧愭爣锛夛紝澶х |
+| 20 | 2 | Power_Voltage | int16 | 鐢垫睜鐢靛帇脳1000锛屽ぇ绔� |
+| **22** | **4** | **session_id** | **uint32** | 鏈 MCU 鍚姩浼氳瘽 ID锛屽ぇ绔� |
+| **26** | **8** | **speed_sample_time_us** | **uint64** | 缂栫爜鍣ㄩ噰鏍锋椂鍒伙紙渭s锛夛紝澶х |
+| **34** | **8** | **imu_sample_time_us** | **uint64** | IMU 閲囨牱鏃跺埢锛埼約锛夛紝澶х |
+| 42 | 1 | Checksum | u8 | byte 0..41 鐨� XOR |
+| 43 | 1 | Frame_Tail | u8 | 甯у熬 `0x7D` |
+
+鍔犵矖琛屼负 v2 鐩稿 v1 鐨勬柊澧炲瓧娈点��
+
+## 3. 鏃堕棿婧�
+
+鏃堕棿鎴崇敱鑷敱杩愯鐨� **TIM7** 鎻愪緵锛岃 [HARDWARE/timer.c](../HARDWARE/timer.c)锛�
+
+- TIM7 鎸傚湪 APB1锛屽畾鏃跺櫒鏃堕挓 84 MHz锛岄鍒嗛 84 鈫� **1 MHz 璁℃暟锛�1 tick = 1 渭s锛�**銆�
+- 16 浣嶈鏁板櫒姣� 65.536 ms 鍥炵粫涓�娆★紝鏇存柊涓柇鎶婇珮 32 浣嶈蒋浠惰鏁� `g_time_high` 鍔犱竴銆�
+- `mcu_time_us()` 杩斿洖 `(g_time_high << 16) | CNT`锛屽嵆**鑷笂鐢典互鏉ョ殑鍗曡皟閫掑寰鏁�**锛�
+  鍙敤浣嶅 48 浣嶏紙绾� 8.9 骞翠笉鍥炵粫锛夈��
+- TIM2~TIM5 琚洓璺紪鐮佸櫒鍗犵敤銆乀IM8 琚埅妯￠仴鎺ц緭鍏ユ崟鑾峰崰鐢紝鏁呴�夌┖闂茬殑 TIM7銆�
+- TIM7 鏇存柊涓柇浼樺厛绾ц涓� 5锛堟暟鍊煎ぇ浜� `configMAX_SYSCALL_INTERRUPT_PRIORITY`
+  瀵瑰簲鐨勬姠鍗犻棬闄愶級锛孖SR 鍐呭彧鍋氳嚜澧烇紝涓嶈皟鐢ㄤ换浣� FreeRTOS API銆�
+
+> 娉ㄦ剰锛歚mcu_time_us()` 鍙涓嶅噺锛岀粷涓嶅洜鏍℃椂鑰岃烦鍙樸�備笂浣嶆満璐熻矗缁存姢
+> `T_host = a路t_mcu + b` 鐨勬椂閽熸槧灏勶紙瑙� [鏃堕棿鎴�.md](鏃堕棿鎴�.md) 绗� 7銆�8 鑺傦級銆�
+
+## 4. 涓や釜鏃堕棿鎴冲垎鍒墦鍦ㄥ摢閲�
+
+v1 鍙湁"鍙戦�佹椂鍒�"闅愬惈璇箟锛屾棤娉曡〃杈� IMU锛�100 Hz锛変笌杞�燂紙200 Hz锛夋槸**寮傛閲囬泦**鐨勩��
+v2 鍦ㄥ悇鑷湡瀹為噰鏍风偣鎵撶偣锛屽啓鍏ュ叏灞�鍙橀噺锛宍data_transition()` 鎵撳寘鏃跺揩鐓э細
+
+| 鏃堕棿鎴� | 鎵撶偣浣嶇疆 | 閲囨牱鐜� |
+| ---- | ---- | ---- |
+| `speed_sample_time_us` | [balance.c](../BALANCE/balance.c) `Get_Velocity_Form_Encoder()` 璇荤紪鐮佸櫒閿佸瓨璁℃暟鍣ㄧ殑鐬棿 | 200 Hz |
+| `imu_sample_time_us` | [MPU6050.c](../HARDWARE/MPU6050/MPU6050.c) `MPU6050_task` I2C 璇诲彇鍓� | 100 Hz |
+
+鍥犳涓�甯у唴鍚屾椂甯︿袱涓椂闂存埑锛氫笂浣嶆満搴旀妸鍔犻�熷害/瑙掗�熷害瀵归綈鍒� `imu_sample_time_us`锛�
+鎶� X/Y/Z 閫熷害瀵归綈鍒� `speed_sample_time_us`锛岃�屼笉鏄叡鐢ㄤ竴涓彂閫佹椂鍒汇��
+
+> IMU 鎵撶偣鍦� I2C 绐佸彂璇诲彇涔嬪墠锛屾槸鍦ㄦ病鏈� DRDY锛堟暟鎹氨缁級涓柇鏉′欢涓嬭兘鍙栧埌鐨勩��
+> 鏈�鎺ヨ繎鐗╃悊閲囨牱鐨勬椂鍒汇�傝嫢鏃ュ悗鎺ュ叆 MPU 鐨� DRDY 寮曡剼涓柇锛屽彲鎶婃墦鐐圭Щ鍒颁腑鏂噷杩涗竴姝ュ噺灏忔姈鍔ㄣ��
+
+## 5. session_id
+
+- 瀹氫箟鍦� [usartx.c](../HARDWARE/usartx.c)锛歚g_session_id`銆�
+- 褰撳墠瀹炵幇涓�**鍥哄畾鍊� `0xA5A5A5A5`**銆�
+- 鐢ㄩ�旓細涓婁綅鏈烘嵁姝ゅ垽鏂笅浣嶆満鏄惁閲嶅惎杩囷紙閲嶅惎鍚� `mcu_time_us()` 浠� 0 閲嶆柊璁℃椂锛�
+  鏃堕挓鏄犲皠 `a, b` 蹇呴』閲嶆柊鎷熷悎锛夈��
+- 寰呭姙锛氬闇�鐪熸鍖哄垎姣忔涓婄數锛屽簲鍦ㄥ惎鍔ㄦ椂鐢ㄧ喌婧愶紙濡傛湭鍒濆鍖� RAM銆佸惎鍔� tick 绛夛級
+  鐢熸垚涓�娆℃�ч殢鏈哄�笺�傚綋鍓嶅浐瀹氬�煎彧鑳借〃鏄�"鍗忚鐗堟湰涓�鑷�"锛屼笉鑳藉尯鍒嗕笉鍚屽惎鍔ㄤ細璇濄��
+
+## 6. 鏍￠獙涓庤В鏋愬缓璁�
+
+- 鏍￠獙浣� `buffer[42] = XOR(buffer[0..41])`锛屼笌 v1 鐨勮绠楁柟寮忕浉鍚岋紝鍙槸鑼冨洿浠� 22 鎵╁埌 42銆�
+- 涓婁綅鏈鸿В鏋愭楠わ細
+  1. 鍚屾甯уご `0x7B`锛�
+  2. 璇绘弧 44 瀛楄妭锛�
+  3. 鏍￠獙 `buffer[43] == 0x7D` 涓� `XOR(buffer[0..41]) == buffer[42]`锛�
+  4. 鎸変笂琛ㄥぇ绔В鍑哄悇瀛楁銆�
+- 澶氬瓧鑺傚瓧娈靛姟蹇呮寜澶х鎵嬪姩缁勮锛屼笉瑕佺洿鎺� `memcpy` 缁撴瀯浣擄紙缂栬瘧鍣ㄥ～鍏�/瀵归綈/绔簭宸紓锛夈��
+
+## 7. 鍏朵粬鍙戦�侀�氶亾
+
+| 閫氶亾 | 鐜扮姸 |
+| ---- | ---- |
+| USART3锛圧OS 涓诲彛锛孌MA锛� | 宸茬敤 `SEND_DATA_SIZE`锛岃嚜鍔ㄥ彂閫佸畬鏁� 44 瀛楄妭 |
+| USART1 / USART5 | 宸叉敼鐢� `SEND_DATA_SIZE`锛屽彂閫佸畬鏁� 44 瀛楄妭 |
+| CAN | 褰撳墠鏈璋冪敤锛沗CAN_SEND()` 浠嶆寜 8脳3=24 瀛楄妭鍒嗗寘锛�**涓嶅惈鏃堕棿鎴冲瓧娈�**銆傚闇�缁� CAN 浼犳椂闂存埑闇�鍙﹁鎵╁睍鍒嗗寘 |
+
+## 8. 涓婁綅鏈洪�傞厤鎻愰啋
+
+USART3 鏀跺埌鐨勫抚宸蹭粠 24 瀛楄妭鍙樹负 **44 瀛楄妭**銆備笂浣嶆満锛圧OS 绔級瑙ｆ瀽浠ｇ爜蹇呴』鍚屾鏇存柊锛�
+甯ч暱銆佹牎楠岃寖鍥淬�佷互鍙婂熬閮ㄤ笁涓柊瀛楁鐨勮鍙栥�傚惁鍒欎細鍥犲抚闀夸笉绗﹀鑷磋В鏋愰敊浣嶃��
+
diff --git a/doc/鏃堕棿鎴�.md b/doc/鏃堕棿鎴�.md
new file mode 100644
index 0000000..0321bc3
--- /dev/null
+++ b/doc/鏃堕棿鎴�.md
@@ -0,0 +1,349 @@
+閽堝鐜板湪鐨勨�淔reeRTOS 涓嬩綅鏈� + Linux/ROS 2 涓婁綅鏈� + 鍚庣画杞��/IMU/2D闆疯揪铻嶅悎鈥濓紝寤鸿鐩存帴鍒跺畾涓�濂楃粺涓�鐨勬椂闂存埑涓庨�氫俊鍗忚銆�
+
+鏈�鏍稿績鐨勫師鍒欐槸锛�
+
+[
+\boxed{
+\text{MCU鍘熷鍗曡皟鏃堕棿鎴虫案涓嶄慨鏀箎
+\quad+\quad
+\text{涓婁綅鏈虹淮鎶ゆ椂閽熸槧灏剗
+}
+]
+
+鍗筹細
+
+[
+T_{\rm host}=a,t_{\rm mcu}+b
+]
+
+## 1. 涓嬩綅鏈虹粺涓�鏃堕棿鍩哄噯
+
+涓嶈鎶� `xTaskGetTickCount()` 浣滀负浼犳劅鍣ㄤ富鏃堕棿鎴炽��
+
+瀹冮�傚悎锛�
+
+* `vTaskDelayUntil()` 鍛ㄦ湡璋冨害锛�
+* 瓒呮椂鍒ゆ柇锛�
+* 浠诲姟寤舵椂锛�
+* 姣绾х姸鎬佹満銆�
+
+瀹冧笉閫傚悎锛�
+
+* IMU涓庣紪鐮佸櫒绮剧‘瀵归綈锛�
+* 200 Hz浠ヤ笂浼犳劅鍣ㄩ噰鏍凤紱
+* 鍒嗘瀽閫氫俊寤惰繜锛�
+* 鍚庣画闆疯揪銆両MU銆佽疆寮忛噷绋嬭铻嶅悎銆�
+
+FreeRTOS Tick 鐨勫垎杈ㄧ巼鐢� `configTICK_RATE_HZ` 鍐冲畾锛汭SR 涓嫢纭疄瑕佽 Tick锛屽簲浣跨敤 `xTaskGetTickCountFromISR()`銆俒FreeRTOS Task Utilities](https://www.freertos.org/Documentation/02-Kernel/04-API-references/03-Task-utilities/00-Task-utilities)
+
+### 鎺ㄨ崘鏃堕棿婧�
+
+浣跨敤涓�涓�32浣嶇‖浠跺畾鏃跺櫒锛�
+
+```text
+璁℃暟棰戠巼锛�1 MHz
+璁℃暟鍗曚綅锛�1 us
+鑷姩閲嶈鍊硷細0xFFFFFFFF
+杞欢鎵╁睍锛�64 bit
+鏃堕棿鍚箟锛歁CU鑷湰娆′笂鐢典互鏉ョ粡杩囩殑寰鏁�
+```
+
+渚嬪 STM32 鍙互浣跨敤 TIM2 鎴� TIM5锛�
+
+[
+t_{\rm mcu}\in uint64_t,\qquad 鍗曚綅=\mu s
+]
+
+32浣嶃��1 MHz璁℃暟鍣ㄧ害姣�71.58鍒嗛挓鍥炵粫涓�娆★紝鎵�浠ョ敤鏇存柊涓柇澧炲姞楂�32浣嶃��
+
+```c
+static volatile uint32_t g_time_high = 0;
+
+void TIM2_IRQHandler(void)
+{
+    if (__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_UPDATE) != RESET) {
+        __HAL_TIM_CLEAR_IT(&htim2, TIM_IT_UPDATE);
+        g_time_high++;
+    }
+}
+
+uint64_t mcu_time_us(void)
+{
+    uint32_t high1;
+    uint32_t high2;
+    uint32_t low;
+    uint32_t status;
+
+    do {
+        high1 = g_time_high;
+        __DMB();
+
+        low = TIM2->CNT;
+        status = TIM2->SR;
+
+        __DMB();
+        high2 = g_time_high;
+    } while (high1 != high2);
+
+    /*
+     * 璁℃暟鍣ㄥ凡缁忓洖缁曪紝浣嗘洿鏂颁腑鏂彲鑳借繕娌℃湁寰楀埌鎵ц銆�
+     * low 灏忎簬鍗婇噺绋嬶紝璇存槑璇诲彇鍙戠敓鍦ㄥ洖缁曚箣鍚庛��
+     */
+    if ((status & TIM_SR_UIF) && low < 0x80000000U) {
+        high1++;
+    }
+
+    return ((uint64_t)high1 << 32) | low;
+}
+```
+
+娉ㄦ剰锛�
+
+* 瀹氭椂鍣ㄦ椂閽熷繀椤荤‘璁ゆ槸鍚﹀彈 APB 鍒嗛鍚庣殑鈥滃畾鏃跺櫒鍊嶉鈥濆奖鍝嶏紱
+* 涓嶈鐩存帴璇诲彇涓�涓敱涓柇鍐欏叆鐨勬櫘閫� `uint64_t`锛�32浣峂CU涓婃湭蹇呭師瀛愶紱
+* 濡傛灉鍚敤浜� STOP 妯″紡鎴� Tickless Idle锛岃纭杩欎釜瀹氭椂鍣ㄤ紤鐪犳椂鏄惁缁х画宸ヤ綔锛�
+* 鏃堕棿鎴冲彧鑳介�掑锛屼笉鑳藉洜涓烘牎鏃惰�屽悜鍓嶆垨鍚戝悗璺冲彉銆�
+
+## 2. 鏃堕棿鎴冲簲璇ュ湪鍝噷鎵�
+
+鏃堕棿鎴冲繀椤诲敖鍙兘闈犺繎鏁版嵁鐪熸浜х敓鐨勪綅缃��
+
+| 鏁版嵁       | 鎺ㄨ崘鏃堕棿鎴充綅缃�       |
+| -------- | ------------- |
+| IMU      | DRDY涓柇鍙戠敓鏃�     |
+| SPI璇诲彇IMU | 涓嶈绛塖PI璇诲彇瀹屾垚鎵嶆墦鐐� |
+| 缂栫爜鍣�      | 鎺у埗瀹氭椂鍣ㄩ攣瀛樿鏁板櫒鏃�   |
+| 鐢垫満鎺у埗閲�    | PWM瀵勫瓨鍣ㄦ洿鏂版椂     |
+| 鎺у埗鍛ㄦ湡     | 鎺у埗鍛ㄦ湡鍏ュ彛        |
+| 涓婁綅鏈烘帴鏀�    | 瀹屾暣甯ф帴鏀跺畬鎴愭椂锛屽崟鐙褰� |
+
+渚嬪锛�
+
+```c
+typedef struct {
+    uint64_t sample_time_us;
+    int16_t gyro_raw[3];
+    int16_t accel_raw[3];
+} ImuSample;
+```
+
+IMU涓柇涓彧鍋氾細
+
+1. 璇诲彇 `mcu_time_us()`锛�
+2. 淇濆瓨鏃堕棿鎴筹紱
+3. 閫氱煡閲囬泦浠诲姟锛�
+4. 绔嬪嵆閫�鍑轰腑鏂��
+
+涓嶈鍦ㄤ腑鏂腑瀹屾垚鍗忚缂栫爜銆丆RC璁＄畻鍜屼覆鍙ｅ彂閫併��
+
+濡傛灉浼犳劅鍣ㄥ唴閮ㄥ惎鐢ㄤ簡浣庨�氭护娉紝DRDY鏃堕棿浠嶇劧鍙兘姣旂湡瀹炵墿鐞嗘祴閲忔櫄涓�涓浐瀹氱兢寤惰繜銆傚悗缁彲浠ュ鍔狅細
+
+```c
+corrected_time_us = drdy_time_us - imu_filter_delay_us;
+```
+
+浣嗗繀椤诲湪纭畾浼犳劅鍣ㄦ护娉㈠櫒寤惰繜鍚庡啀琛ュ伩銆�
+
+## 3. FreeRTOS浠诲姟缁撴瀯
+
+寤鸿浣跨敤锛�
+
+```mermaid
+flowchart TD
+    A["IMU DRDY / 鎺у埗瀹氭椂鍣� ISR"] --> B["閲囨牱鐜舰缂撳啿鍖�"]
+    B --> C["浼犳劅鍣ㄥ鐞嗕换鍔�"]
+    C --> D["鍗忚鍙戦�侀槦鍒�"]
+    D --> E["鍞竴 UART TX 浠诲姟"]
+    F["UART DMA / IDLE ISR"] --> G["鎺ユ敹鐜舰缂撳啿鍖�"]
+    G --> H["鍗忚瑙ｆ瀽浠诲姟"]
+```
+
+鍏抽敭瑙勫垯锛�
+
+* UART鍙戦�佸彧鑳芥湁涓�涓墍鏈夎�呬换鍔★紝閬垮厤澶氫釜浠诲姟鍙戦�佺殑鏁版嵁浜ゅ弶锛�
+* UART鎺ユ敹浣跨敤DMA銆両DLE涓柇鍜岀幆褰㈢紦鍐插尯锛�
+* ISR鍙礋璐ｆ惉杩愭暟鎹拰閫氱煡浠诲姟锛�
+* 浜岃繘鍒跺崗璁拰 `printf()` 璋冭瘯鏃ュ織涓嶈鍏辩敤鍚屼竴涓猆ART锛�
+* 濡傛灉ISR璋冪敤 FreeRTOS 鐨� `...FromISR()` 鎺ュ彛锛屼腑鏂紭鍏堢骇蹇呴』绗﹀悎 `configMAX_SYSCALL_INTERRUPT_PRIORITY` 绾︽潫锛�
+* 鍛ㄦ湡浠诲姟浣跨敤 `vTaskDelayUntil()`锛屽畠鑳介伩鍏嶆櫘閫氱浉瀵瑰欢鏃朵骇鐢熺殑绱婕傜Щ锛屼絾瀹為檯鎵ц鏃跺埢浠嶅簲璋冪敤纭欢寰鏃堕挓璁板綍銆俒FreeRTOS `vTaskDelayUntil()`](https://freertos.org/xtaskdelayuntiltask-control.html)
+
+渚嬪200 Hz浣庣簿搴﹀懆鏈熶换鍔★細
+
+```c
+void SensorTask(void *argument)
+{
+    TickType_t last_wake = xTaskGetTickCount();
+    const TickType_t period = pdMS_TO_TICKS(5);
+
+    for (;;) {
+        vTaskDelayUntil(&last_wake, period);
+
+        uint64_t actual_time_us = mcu_time_us();
+
+        /* 閲囬泦鍜屽鐞� */
+    }
+}
+```
+
+濡傛灉瑕佹眰鏇寸ǔ瀹氱殑200 Hz閲囨牱锛屽簲鐢辩‖浠跺畾鏃跺櫒浜х敓涓柇锛屽啀閫氱煡浠诲姟锛岃�屼笉鏄畬鍏ㄤ緷璧� FreeRTOS Tick銆�
+
+## 4. 鎺ㄨ崘浜岃繘鍒跺抚瑙勮寖 v1
+
+鍋囪浣跨敤 UART 鎴� USB CDC锛屽缓璁噰鐢細
+
+```text
+COBS缂栫爜锛堝崗璁ご + 璐熻浇 + CRC32锛� + 0x00甯у垎闅旂
+```
+
+COBS鐨勫ソ澶勬槸浠绘剰瑙ｆ瀽閿欒鍚庯紝閮藉彲浠ュ湪涓嬩竴涓� `0x00` 閲嶆柊鎵惧埌甯ц竟鐣屻��
+
+### 缁熶竴鍗忚澶达細24瀛楄妭
+
+涔嬪墠宸叉湁鐨勫熀纭�涓婇澶栧鍔�
+| session_id     | uint32 |  4 | 鏈MCU鍚姩浼氳瘽ID  |
+| sample_time_us | uint64 |  8 | MCU閲囨牱鏃堕棿      |
+
+
+浣嗕笉瑕佺洿鎺ユ墽琛岋細
+
+```c
+uart_send((uint8_t *)&header, sizeof(header)); // 涓嶆帹鑽�
+```
+
+鍥犱负涓嶅悓缂栬瘧鍣ㄥ彲鑳藉瓨鍦細
+
+* 缁撴瀯浣撳～鍏咃紱
+* 瀵归綈宸紓锛�
+* 澶у皬绔樊寮傦紱
+* 娴偣鏍煎紡宸紓銆�
+
+             |
+
+
+## 7. 鍥涙椂闂存埑鍚屾鍗忚
+
+姣忔鍚屾璁板綍锛�
+
+* (T_1)锛氫笂浣嶆満鍙戦�佽姹傦紱
+* (t_2)锛歁CU鏀跺埌骞惰В鏋愯姹傦紱
+* (t_3)锛歁CU鍑嗗鍙戦�佸搷搴旓紱
+* (T_4)锛氫笂浣嶆満鏀跺埌瀹屾暣鍝嶅簲銆�
+
+瀹氫箟MCU鏃堕棿鍑忎笂浣嶆満鏃堕棿涓猴細
+
+[
+\theta=
+\frac{(t_2-T_1)+(t_3-T_4)}{2}
+]
+
+寰�杩斿欢杩燂細
+
+[
+d=(T_4-T_1)-(t_3-t_2)
+]
+
+鍒濆杞崲锛�
+
+[
+T_{\rm host}\approx t_{\rm mcu}-\theta
+]
+
+杩愯涓敹闆嗗缁勶細
+
+[
+x_i=\frac{t_{2,i}+t_{3,i}}{2}
+]
+
+[
+y_i=\frac{T_{1,i}+T_{4,i}}{2}
+]
+
+鎷熷悎锛�
+
+[
+\boxed{T_{\rm host}=a,t_{\rm mcu}+b}
+]
+
+鎺ㄨ崘鍙傛暟锛�
+
+* 鍚姩鏃惰繛缁悓姝�20锝�50娆★紱
+* 鍒濆闃舵鍏堜护 (a=1)锛屼粠鏈�灏廟TT鏍锋湰浼拌 (b)锛�
+* 杩愯涓瘡1绉掑悓姝ヤ竴娆★紱
+* 淇濆瓨鏈�杩�60锝�120缁勶紱
+* 涓㈠純RTT鏄庢樉鍋忓ぇ鐨勬牱鏈紱
+* 浣跨敤RTT鏈�灏忕殑20%锝�30%鎷熷悎 (a,b)锛�
+* MCU鏃堕棿鎴虫湰韬案杩滀笉琚�滄牎鍑嗏�濇垨閲嶅啓銆�
+
+涓婁綅鏈轰娇鐢� `CLOCK_MONOTONIC_RAW` 鎴� `std::chrono::steady_clock`锛屼笉瑕佷娇鐢ㄤ細琚玁TP鏍℃椂鏀瑰彉鐨勫涓婃椂闂村弬涓庢帶鍒躲��
+
+## 8. ROS 2鏃堕棿鎴宠浆鎹�
+
+MCU鐨勨�滀笂鐢靛井绉掓暟鈥濅笉鑳界洿鎺ュ～杩� ROS 娑堟伅鐨� `header.stamp`锛屽惁鍒欐棤娉曞拰婵�鍏夐浄杈炬椂闂村榻愩��
+
+涓婁綅鏈哄厛寰楀埌锛�
+
+[
+T_{\rm steady}=a,t_{\rm mcu}+b
+]
+
+鍐嶅湪ROS妗ユ帴鑺傜偣涓繚瀛樹竴瀵归敋鐐癸細
+
+[
+(T_{\rm steady,0},T_{\rm ros,0})
+]
+
+杞崲涓猴細
+
+[
+T_{\rm ros,sample}
+==================
+
+T_{\rm ros,0}
++
+(T_{\rm steady,sample}-T_{\rm steady,0})
+]
+
+鐒跺悗灏嗚繖涓椂闂村～鍏ワ細
+
+* `sensor_msgs/Imu.header.stamp`锛�
+* `nav_msgs/Odometry.header.stamp`锛�
+* 缂栫爜鍣ㄦ垨杞�熻嚜瀹氫箟娑堟伅鏃堕棿鎴炽��
+
+杩欐牱杞�熴�両MU鍜屾縺鍏夐浄杈炬墠鑳藉湪鍚屼竴ROS鏃堕棿杞翠笂铻嶅悎銆�
+
+## 9. 涓婁綅鏈哄懡浠や笉瑕佷緷璧栫粷瀵规椂闂�
+
+鏅�氳繍鍔ㄥ懡浠ゅ缓璁娇鐢ㄢ�滄帴鏀跺悗绔嬪嵆鎵ц锛嬭秴鏃垛�濓細
+
+```c
+typedef struct {
+    uint32_t command_sequence;
+    uint32_t valid_for_us;
+    float velocity_ref;
+    float yaw_rate_ref;
+} MotionCommand;
+```
+
+MCU鏀跺埌鏃惰褰曪細
+
+```c
+last_command_rx_us = mcu_time_us();
+```
+
+瀹夊叏鍒ゆ柇锛�
+
+```c
+if (mcu_time_us() - last_command_rx_us > command.valid_for_us) {
+    enter_safe_deceleration();
+}
+```
+
+鍙湁鐪熸闇�瑕佹湭鏉ュ畾鏃舵墽琛屾椂锛屾墠璁╀笂浣嶆満閫氳繃閫嗘槧灏勮绠楋細
+
+[
+t_{\rm mcu,execute}
+===================
+
+\frac{T_{\rm host,execute}-b}{a}
+]
+