diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp
index 9fa6d01..00d95e5 100644
--- a/Robot_V2/Algorithm/DengFOC.cpp
+++ b/Robot_V2/Algorithm/DengFOC.cpp
@@ -8,6 +8,7 @@
 #include <cmath>
 #include <gd32c10x.h>
 #include <stdint.h>
+#include "motor.h"
 
 constexpr double PI = 3.1415926535897932384626433832795f;
 constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
@@ -112,7 +113,7 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) {
  */
 void FOC_Controlroller::shut_down() {
 	is_shut_down = 1;
-	gpio_bit_reset(GPIOB, GPIO_PIN_12);
+	motor_sleep();
 }
 /**
  * @brief 占空比置0
@@ -138,7 +139,7 @@ void FOC_Controlroller::resume() {
 	over_current = false;
 	voltage_error = false;
 	is_shut_down = false;
-	gpio_bit_set(GPIOB, GPIO_PIN_12);
+	motor_resume();
 }
 /**
  * @brief 更新电源电压
@@ -235,13 +236,13 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) {
  */
 float FOC_Controlroller::velocity_control(float target_velocity) {
 	// PID 控制计算
-	float Uq = vel_loop(target_velocity - rt_velocity);
+	float Uq = vel_loop((target_velocity - rt_velocity)*25);
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
 	// 设置相电压
 	setPhaseVoltage(Uq, 0.0f);
-	CAN0_transmiter.testfloat_union.f =
+	// CAN0_transmiter.testfloat_union.f =
 		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
 	return rt_velocity;
 }
diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h
index b5dbde9..50e5c12 100644
--- a/Robot_V2/Algorithm/DengFOC.h
+++ b/Robot_V2/Algorithm/DengFOC.h
@@ -43,7 +43,7 @@ class FOC_Controlroller {
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
 
-	PIDController vel_loop = PIDController{0.7f, 0.45f, 0.001f, 1000000,
+	PIDController vel_loop = PIDController{0.13f, 0.4f, 2.8e-4, 1000000,
 										   voltage_power_supply}; // 12V
 	PIDController angle_loop = PIDController{25.0f, 30.0f, 0.15f, 10000000,
 											 voltage_power_supply}; // 12V
diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp
index ca40507..f1a658b 100644
--- a/Robot_V2/Algorithm/Filter.cpp
+++ b/Robot_V2/Algorithm/Filter.cpp
@@ -105,18 +105,16 @@ EncoderAngleKalman::EncoderAngleKalman(float initial_angle,
 	\brief      更新卡尔曼滤波器状态
 	\param[in]  measured_angle 测量的角度值 (degree)
 	\retval     估计的角度值 (degree)
-	\note       实现卡尔曼滤波的预测和更新步骤，处理角度环绕问题
+	\note       实现卡尔曼滤波的预测和更新步骤
 */
 float EncoderAngleKalman::update(float measured_angle) {
-	// 更新last_angle以保持同步
+	// 更新last_angle以支持速度计算
 	last_angle = kalman.getState();
 	// 1. 预测步骤 - 速度估计
 	kalman.predict(last_angle);
 
 	// 2. 更新步骤 - 使用处理后的角度值更新
 	float filtered_angle = kalman.update(measured_angle);
-
-
 	return filtered_angle;
 }
 
@@ -129,12 +127,7 @@ float EncoderAngleKalman::getAngle() const { return kalman.getState(); }
 	\brief      获取当前角度估计值
 	\retval     当前角度估计值 (degree)
 */
-float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*1e4; }
-/*!
-	\brief      获取前一个角度估计值
-	\retval     前一个角度估计值 (degree)
-*/
-float EncoderAngleKalman::getlastAngle() const { return last_angle; }
+float EncoderAngleKalman::getVelocity() const { return (kalman.getState()-last_angle)*3.75e3; }
 
 /*!
 	\brief      设置过程噪声协方差
diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h
index d34cc47..1cd1122 100644
--- a/Robot_V2/Algorithm/Filter.h
+++ b/Robot_V2/Algorithm/Filter.h
@@ -24,12 +24,12 @@ float medium_filter(float values[], u8 size);
 */
 template <typename T> class KalmanFilter {
   private:
-	T xp; // 状态估计
 	T P; // 估计误差协方差
 	T Q; // 过程噪声协方差
 	T R; // 测量噪声协方差
 
   public:
+	T x; // 状态估计
 	/*!
 		\brief 构造函数
 		\param[in] initial_state 初始状态值
@@ -39,7 +39,7 @@ template <typename T> class KalmanFilter {
 	*/
 	KalmanFilter(T initial_state, T initial_error_covariance, T process_noise,
 				 T measurement_noise)
-		: xp(initial_state), P(initial_error_covariance), Q(process_noise),
+		: x(initial_state), P(initial_error_covariance), Q(process_noise),
 		  R(measurement_noise) {}
 
 	/*!
@@ -48,7 +48,7 @@ template <typename T> class KalmanFilter {
 		\retval none
 	*/
 	void predict(T state_prediction) {
-		xp = state_prediction;
+		x = state_prediction;
 		P += Q;
 	}
 
@@ -63,10 +63,10 @@ template <typename T> class KalmanFilter {
 		T S = P + R;								// 残差协方差
 		T K = P / S;								// 卡尔曼增益
 
-		xp += K * y;		 // 更新状态估计
+		x += K * y;		 // 更新状态估计
 		P = (1 - K) * P; // 更新误差协方差
 
-		return xp;
+		return x;
 	}
 
 	/*!
@@ -77,16 +77,16 @@ template <typename T> class KalmanFilter {
 	T update(T measurement) {
 		T K = P / (P + R); // 卡尔曼增益
 
-		xp += K * (measurement - xp); // 更新状态估计
+		x += K * (measurement - x); // 更新状态估计
 		P = (1 - K) * P;			// 更新误差协方差
-		return xp;
+		return x;
 	}
 
 	/*!
 		\brief 获取当前状态估计值
 		\retval 当前状态估计值
 	*/
-	T getState() const { return xp; }
+	T getState() const { return x; }
 
 	/*!
 		\brief 设置过程噪声协方差
@@ -103,7 +103,6 @@ template <typename T> class KalmanFilter {
 	void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
 };
 
-
 /*!
 	\brief 编码器速度卡尔曼滤波器类
 	\note  用于滤除编码器速度测量噪声的卡尔曼滤波器实现
@@ -124,6 +123,11 @@ class EncoderVelocityKalman {
 	void setProcessNoise(float q);
 
 	void setMeasurementNoise(float r);
+
+	u8 init(float init_velocity) {
+		kalman.x = init_velocity;
+		return 0;
+	}
 };
 
 /*!
@@ -131,28 +135,30 @@ class EncoderVelocityKalman {
 	\note  用于滤除编码器角度测量噪声的卡尔曼滤波器实现
 */
 class EncoderAngleKalman {
-private:
+  private:
 	KalmanFilter<float> kalman;
-	float last_angle;
 	bool initialized;
 
-public:
+  public:
+	float last_angle;
 	EncoderAngleKalman(float initial_angle, float initial_error_covariance,
 					   float process_noise, float measurement_noise);
 
 	float update(float measured_angle);
 
 	float getAngle() const;
-	float getlastAngle() const;
 	float getVelocity() const;
 
-
 	void setProcessNoise(float q);
+	float getstate() const { return kalman.getState(); };
 
 	void setMeasurementNoise(float r);
+	u8 init(float init_angle) {
+		kalman.x = init_angle;
+		return 0;
+	}
 };
 
-
 #endif // __cplusplus
 
 #endif // _FILTER_H
\ No newline at end of file
diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml
index ce727a6..9c5425b 100644
--- a/Robot_V2/Robot_V2.cproject.yml
+++ b/Robot_V2/Robot_V2.cproject.yml
@@ -23,6 +23,8 @@ project:
         - file: device/Timer.cpp
         - file: device/Timer.h
         - file: device/test.cpp
+        - file: device/motor.cpp
+        - file: device/motor.h
     - group: Algorithm
       files:
         - file: Algorithm/Filter.cpp
diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp
index eb11ec8..afa3e72 100644
--- a/Robot_V2/device/ADC.cpp
+++ b/Robot_V2/device/ADC.cpp
@@ -6,6 +6,7 @@
 #include <cstring>
 #include <gd32c10x_dma.h>
 #include <stdint.h>
+#include "systick.h"
 
 constexpr float ERROR_ADC_VAL = 255.0f;
 constexpr float ADC_SCALE = 3.3f / 4096.0f;
@@ -110,9 +111,10 @@ void adc_config(void) {
 /**
  * @brief 采样类初始化
  */
-sampler::sampler() {
+u8 sampler::init() {
 	dma_config(); // DMA采样ADC初始化
 	adc_config(); // ADC初始化
+	delay_1ms(1);
 	// 初始采样
 	for (int i = 0; i < FILTER_SIZE + 1; i++) {
 		mcu_temp_samples[i] =
@@ -130,7 +132,9 @@ sampler::sampler() {
 		float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
 		drv_temp_samples[i] =
 			1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
+		delay_1ms(1);
 	}
+	return 0;
 }
 /**
  * @brief 获取MCU温度
diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp
index cc4a8a0..ea72bb2 100644
--- a/Robot_V2/device/Can.cpp
+++ b/Robot_V2/device/Can.cpp
@@ -6,6 +6,7 @@
 #include "gd32c10x.h"
 #include "gd32c10x_can.h"
 #include "gd32c10x_dma.h"
+#include "motor.h"
 #include "systick.h"
 #include <cstdio>
 #include <cstdlib>
@@ -21,7 +22,7 @@ constexpr u16 DEV_CAN0_MASK = 0x0000;
 constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
 constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
 
-CAN_transmiter CAN0_transmiter;		 // CAN初始化
+CAN_transmiter CAN0_transmiter; // CAN初始化
 extern FOC_Controlroller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
@@ -260,7 +261,7 @@ void can_loopback_init(void) {
 	can_parameter.prescaler = 6;
 	if (can_init(CANX, &can_parameter) != SUCCESS) {
 		u16 i = 200;
-		while (--i>0){
+		while (--i > 0) {
 			delay_1ms(10);
 			printf("can init error \r\n");
 		}
@@ -413,7 +414,7 @@ void can_loopback_test(void) {
 	\brief CAN_transmiter构造函数
 	\note  初始化CAN通信相关参数和硬件配置
 */
-CAN_transmiter::CAN_transmiter() {
+u8 CAN_transmiter::init() {
 	// 初始化CAN硬件
 	// can_config();
 	canfd_config();
@@ -426,6 +427,7 @@ CAN_transmiter::CAN_transmiter() {
 	// 初始化DMA缓冲区
 	memset(can_rx_buffer, 0, sizeof(can_rx_buffer));
 	memset(can_tx_buffer, 0, sizeof(can_tx_buffer));
+	return 0;
 }
 
 /*!
@@ -440,7 +442,7 @@ void CAN_transmiter::update(void) {
 	rt_angle_union.f = FOC.rt_angle;
 	// rt_velocity_union.f = KTH7823.getVelocity();
 	rt_velocity_union.f = FOC.rt_velocity;
-	MotorStatus = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
+	MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET;
 
 	Target_Control_Mode = FOC.control_mode;
 	controller_Mode = FOC.control_mode;
@@ -618,19 +620,18 @@ u8 CAN_transmiter::CAN_all_Send() {
 u8 CAN_transmiter::CANFD_all_Send() {
 	u32 success_flag = 0;
 
-	constexpr u32 STATUS_ID = 0x18FF0100;
-	constexpr u32 CONFIG_ID = 0x18FF0101;
+	constexpr u32 STATUS_ID = 0x00010100;
+	constexpr u32 CONFIG_ID = 0x00010101;
 	// 打包status数据
 	pack_u16_to_bytes(transmit_data, 0, mcu_temp);
 	pack_u16_to_bytes(transmit_data, 2, motor_temp);
 	pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f));
 	pack_u16_to_bytes(transmit_data, 6, u16(v_current_union.f));
 	pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f));
-	transmit_data[10] = (transmit_data[10] & ~0x07) | 
-	                    ((MotorStatus & 0x01) |
-	                    ((MagStatus & 0x01) << 1) |
-	                    ((ThermalStatus & 0x01) << 2));
-	
+	transmit_data[10] = (transmit_data[10] & ~0x07) |
+						((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) |
+						 ((ThermalStatus & 0x01) << 2));
+
 	pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
 	pack_float_to_bytes(transmit_data, 13, rt_angle_union);
 	pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
@@ -669,7 +670,7 @@ void CAN_transmiter::communication_check() {
 		// for (i = 0; i < receive_message.rx_dlen; i++) {
 		// 	printf(" %02x", receive_message.rx_data[i]);
 		// }
-		if (receive_message.rx_efid == 0x18ff0200) {
+		if (receive_message.rx_efid == 0x00010200) {
 			FOC.control_mode = receive_message.rx_data[0];
 			if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 !=
 				FOC.limit_voltage) {
@@ -689,9 +690,13 @@ void CAN_transmiter::communication_check() {
 			target_velocity_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 11).f;
 			FOC.target_velocity = target_velocity_union.f;
-
-			can0_receive_flag = RESET;
+		} else if (receive_message.rx_efid == 0x00010301) {
+			motor_reset();
 		}
+		else {
+			printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
+		}
+		can0_receive_flag = RESET;
 	}
 }
 
diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h
index 56dc00a..5bc1e74 100644
--- a/Robot_V2/device/Can.h
+++ b/Robot_V2/device/Can.h
@@ -101,7 +101,7 @@ class CAN_transmiter {
 
   public:
 	// 添加构造函数声明
-	CAN_transmiter();
+	u8 init(void);
 
 	can_receive_message_struct receive_message; 	// 接收消息结构体
 	FlagStatus can0_receive_flag;					// CAN0接收标志
diff --git a/Robot_V2/device/adc.h b/Robot_V2/device/adc.h
index 9c126e2..3a65951 100644
--- a/Robot_V2/device/adc.h
+++ b/Robot_V2/device/adc.h
@@ -21,7 +21,7 @@ class sampler {
 	float tempora_sample;
 
   public:
-	sampler();
+	u8 init(void);
 	void update_mcu_temp(void);
 	void update_drv_temp(void);
 	void update_vbus_voltage(void);
diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp
index 4f39520..19af117 100644
--- a/Robot_V2/device/ang_sensor.cpp
+++ b/Robot_V2/device/ang_sensor.cpp
@@ -9,9 +9,10 @@
 #include <cstring>
 #include <stdint.h>
 
-
-#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
-#define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
+#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
+#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
+#define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
+#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
 
 constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
 constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
@@ -204,37 +205,39 @@ void angle_sensor::KTH7823_init(void) {
 /**
  * @brief 角度传感器构造函数
  */
-angle_sensor::angle_sensor(void) { KTH7823_init(); }
+u8 angle_sensor::init(void) {
+	KTH7823_init();
+	return 0;
+}
 
 /**
  * @brief 读取传感器数据
  * @return 传感器数据
  */
 inline float angle_sensor::read_sensorAngle(void) {
-    uint16_t raw_data;
-    
+	uint16_t raw_data;
 
-    SPI_CS_ENABLE();	
-    u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
-    SPI_CS_DISABLE();
-    
-    if (result == 0) {
-        // 单次浮点乘法优化：只计算一次度数，然后转换为弧度
-        float angle_deg = (float)raw_data * ANGLE_FACTOR;
-        current_angle_deg = angle_deg;
-        current_angle_rad = angle_deg * DEG2RAD;  // M_PI/180.0f
-        return angle_deg;
-    }
-    
-    #ifdef DEBUG
-    // 仅在调试模式下输出错误，避免发布版本中的I/O开销
-    if (result != 0) {
-        // 使用更高效的串口输出替代printf（如果可用）
+	ANG_SPI_CS_ENABLE();
+	u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
+	ANG_SPI_CS_DISABLE();
+
+	if (result == 0) {
+		// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
+		float angle_deg = (float)raw_data * ANGLE_FACTOR;
+		current_angle_deg = angle_deg;
+		current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f
+		return angle_deg;
+	}
+
+#ifdef DEBUG
+	// 仅在调试模式下输出错误，避免发布版本中的I/O开销
+	if (result != 0) {
+		// 使用更高效的串口输出替代printf（如果可用）
 		printf("Error reading angle from sensor.\n");
-    }
-    #endif
-    
-    return (float)FAULT_RECEIVE;
+	}
+#endif
+
+	return (float)FAULT_RECEIVE;
 }
 /**
  * @brief 更新传感器角度（输出级）
@@ -262,9 +265,6 @@ float angle_sensor::update(void) {
 		delta += ANGLE_WRAP_VALUE;
 	}
 
-	std::memmove(velocity, velocity + 1, SIZE_FLOAT * (VEL_FILTER_SIZE - 1));
-	velocity[VEL_FILTER_SIZE - 1] = delta * VELOCITY_SCALE;
-
 	m_cumulativeAngle += delta;
 	m_lastAngle = current_angle_deg;
 
@@ -285,26 +285,6 @@ float angle_sensor::getAngle(void) const {
  */
 float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
 
-/**
- * @brief 获取速度值(输出级)
- * @return 速度值(输出级)
- */
-float angle_sensor::getVelocity(void) const {
-	return getRawVelocity() / REDUCTION_RATIO;
-}
-
-/**
- * @brief 获取速度值(原始级)
- * @return 速度值(原始级)
- */
-float angle_sensor::getRawVelocity(void) const {
-	float velocity_filted = 0;
-	for (int i = 0; i < VEL_FILTER_SIZE - 1; i++) {
-		velocity_filted += velocity[i];
-	}
-	return velocity_filted / (VEL_FILTER_SIZE - 1);
-	// return velocity[VEL_FILTER_SIZE - 1];
-}
 /**
  * @brief 获取角度值(原始级)
  * @return 角度值(原始级)
diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h
index c29a23c..2f86def 100644
--- a/Robot_V2/device/ang_sensor.h
+++ b/Robot_V2/device/ang_sensor.h
@@ -14,14 +14,12 @@ constexpr u16 VEL_FILTER_SIZE = 4;
 
 class angle_sensor {
   public:
-	angle_sensor();
+	u8 init(void);
 	float read_sensorAngle(void);
 	float update(void);
 	void reset(void);
 	float getAngle(void) const;
 	float getRawAngle(void) const;
-	float getVelocity(void) const;
-	float getRawVelocity(void) const;
 	float getcurrent_sensorangle(void) const;
 
   protected:
diff --git a/Robot_V2/device/motor.cpp b/Robot_V2/device/motor.cpp
new file mode 100644
index 0000000..083740e
--- /dev/null
+++ b/Robot_V2/device/motor.cpp
@@ -0,0 +1,38 @@
+#include "motor.h"
+#include "Timer.h"
+#include "gd32c10x_eval.h"
+#include <cstdio>
+
+/**
+ * @brief  driver输出初始化
+ */
+void motor_init(void) {
+	timer1_gpio_config();
+	timer1_pwm_config();
+	// 使能GPIOB时钟
+	rcu_periph_clock_enable(RCU_GPIOB);
+	// 配置PB2为输入引脚，用于读取电机状态
+	gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
+	/* 配置GPIOB的PB14为推挽输出 SPICS */
+	gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,
+			  GPIO_PIN_14 | GPIO_PIN_12);
+}
+
+/**
+ * @brief  电机复位
+ */
+void motor_reset(void) {
+	motor_sleep();
+	printf("motor reseting...\r\n");
+	motor_resume();
+}
+
+/**
+ * @brief  电机休眠
+ */
+void motor_sleep(void) { gpio_bit_reset(GPIOB, GPIO_PIN_12); }
+
+/**
+ * @brief  电机启动
+ */
+void motor_resume(void) { gpio_bit_set(GPIOB, GPIO_PIN_12); }
\ No newline at end of file
diff --git a/Robot_V2/device/motor.h b/Robot_V2/device/motor.h
new file mode 100644
index 0000000..dc198a3
--- /dev/null
+++ b/Robot_V2/device/motor.h
@@ -0,0 +1,11 @@
+#ifndef _MOTOR_H
+#define _MOTOR_H
+
+
+void motor_init(void);
+void motor_reset(void);
+void motor_resume(void);
+void motor_sleep(void);
+
+
+#endif
\ No newline at end of file
diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp
index 72bb07e..5d3cfe9 100644
--- a/Robot_V2/main.cpp
+++ b/Robot_V2/main.cpp
@@ -7,39 +7,49 @@
 #include "ang_sensor.h"
 #include "core_cm4.h"
 #include "core_cmFunc.h"
+#include "motor.h"
 #include "health.h"
 #include "initial.h"
-#include "sys/health.h"
-#include "sys/systick.h"
 #include "systick.h"
+#include <cstdio>
 #include <stdint.h>
 
 TaskScheduler scheduler = {0};
 int main(void) {
-	systick_config();							// 系统时钟初始化
-	USART0_init();								// 调试串口初始化
-	User_timer_init();							// Timer初始化
-	driver_init();								// 电机驱动初始化
-	extern FOC_Controlroller FOC;				// FOC控制
+	systick_config();  // 系统时钟初始化
+	USART0_init();	   // 调试串口初始化
+	User_timer_init(); // 任务Timer初始化
+	motor_init();	   // 电机驱动初始化
+
+	extern CAN_transmiter CAN0_transmiter;		// CAN初始化
 	extern sampler sampler;						// 采样器
+	extern angle_sensor KTH7823;				// 角度传感器
 	extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
 	extern EncoderAngleKalman Angle_kalman;		// 角度滤波
-	extern angle_sensor KTH7823;				// 角度传感器
-	extern CAN_transmiter CAN0_transmiter;		// CAN
 	extern ThermalController thermalController; // 热量控制器
+	extern FOC_Controlroller FOC;				// FOC控制
+
+	u8 init_status = 0;
+	init_status += KTH7823.init();						   // 角度传感器初始化
+	init_status += sampler.init();						   // 采样器初始化
+	init_status += CAN0_transmiter.init();				   // CAN初始化
+	init_status += Angle_kalman.init(KTH7823.getAngle());  // 角度滤波初始化
+	init_status += Vel_kalman.init(0.0f); // 速度滤波初始化
+	while (init_status) {								   // 初始化检测
+		std::printf("初始化失败\r\n");
+	}
 
 	u32 cnt_test = 0;
-	delay_1ms(5);
+	watchdog_init(500); // 看门狗初始化
 	angsens_MG_check();	 // 角度传感器磁感应强度测试
-	watchdog_init(5000); // 看门狗初始化
 	while (1) {
 		__disable_irq();
-		// 保存当前任务标志快照以提高效率并确保一致性
-		u32 currentTasks = scheduler.taskFlags;
-		// 一次性清除所有已处理的任务标志
-		scheduler.taskFlags &= ~currentTasks;
+		u32 currentTasks =
+			scheduler.taskFlags; // 保存当前任务标志快照以提高效率并确保一致性
+		scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志
 		__enable_irq();
 		cnt_test++;
+
 		// 按照优先级和重要性处理任务
 		if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
 			sampler.update_current();
@@ -49,6 +59,7 @@ int main(void) {
 			FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
 		}
 		if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
+			CAN0_transmiter.testfloat_union.f =  (Angle_kalman.getstate()-Angle_kalman.last_angle)*10000.0f; // 速度滤波前的速度值，单位: deg/s
 			FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
 		}
 		if (currentTasks &
@@ -102,4 +113,5 @@ int main(void) {
 			fwdgt_counter_reload(); // 看门狗复位
 		}
 	}
+	return 0;
 }
\ No newline at end of file
diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp
index 0b47d45..e574ab2 100644
--- a/Robot_V2/sys/initial.cpp
+++ b/Robot_V2/sys/initial.cpp
@@ -1,15 +1,7 @@
 #include "initial.h"
-#include "Timer.h"
-#include "ang_sensor.h"
 #include <stdint.h>
 
 
-void GPIO_config(void) {
-	// 使能GPIOB时钟
-	rcu_periph_clock_enable(RCU_GPIOB);
-	// 配置PB2为输入引脚，用于读取电机状态
-	gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
-}
 
 /* retarget the C library printf function to the USART */
 u8 USART0_init() {
@@ -33,14 +25,6 @@ u8 watchdog_init(u32 time) {
 }
 
 
-/**
- * @brief  driver输出初始化
- */
-void driver_init(void) {
-	timer1_gpio_config();
-	timer1_pwm_config();
-}
-
 int fputc(int ch, FILE *f) throw() {
 	usart_data_transmit(EVAL_COM0, (uint8_t)ch);
 	while (RESET == usart_flag_get(EVAL_COM0, USART_FLAG_TBE))
diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h
index 38ea28c..f2c6e31 100644
--- a/Robot_V2/sys/initial.h
+++ b/Robot_V2/sys/initial.h
@@ -17,10 +17,8 @@ void rcu_config(void);
 static volatile uint32_t timer_delay_count = 0; // volatile 很重要！
 int fputc(int ch, FILE *f);						// 声明fputc函数以重载标准库实现
 
-u8 tle5210b_init(void);
 u8 USART0_init(void);
 u8 watchdog_init(u32 time);
-void driver_init(void);
 // 任务标志定义
 typedef enum {
 	TASK_MOTOR_CONTROL = 0,