refactor(main): 统一外设初始化流程

集中所有外设初始化于main函数起始处，通过返回值确认各模块是否成功启动；
refactor(adc): 修改采样器初始化返回值
将sampler构造函数替换为带返回值的init方法，在main函数中加入初始化结果判断，
确保系统启动时硬件配置正确加载。

feat(motor): 引入motor模块并重构电机控制逻辑
将GPIO操作封装进motor_sleep和motor_resume函数，提升代码可读性和维护性。
引入motor.h头文件，并在main.cpp中进行电机初始化统一管理。
修改FOC控制器中的shutdown与resume方法调用新接口。

refactor(filter): 优化卡尔曼滤波器实现
重命名状态变量xp为x，简化访问逻辑；移除冗余注释及未使用的getlastAngle方法；
调整getVelocity计算系数，提高速度估算精度；增加init方法支持初始状态设置。

refactor(angle_sensor): 移除冗余速度计算功能
删除传感器内部的速度估算相关数组和方法，交由外部滤波器处理以降低耦合度。
更新SPI片选宏定义以区分不同设备，并增强错误处理机制。

refactor(can): 调整CAN通信ID及初始化流程
更改状态与配置帧的标识符ID，添加对特定命令ID的响应处理如电机复位指令。
将CAN_transmiter构造函数改为init方法，配合主程序完成模块化初始化检查。

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;
 }