refactor(foc): 重构FOC控制算法与角度传感器接口

- 移除冗余的电机方向和极对数参数传递逻辑
- 简化电气角度计算函数，直接读取角度传感器数据
- 调整电压设置函数，优化PWM输出逻辑
- 注释掉未使用的速度开环控制调用，启用角度闭环控制
- 增加角度和速度获取接口，支持原始值和输出级值区分

feat(sensor): 完善角度传感器数据处理与速度估算
- 增加速度滤波缓冲区，提高速度估算精度
- 改进角度绕回检测阈值，增强鲁棒性
- 添加获取原始角度和速度的接口
- 增加SPI DMA传输超时打印信息

fix(can): 修正CAN通信中的数据打包与解析问题
- 正确发送电机实时速度数据
- 修正CAN FD数据包长度以匹配实际内容
- 角度目标值增加缩放因子适配外部控制指令

Robot_V2/device/Can.cpp

@@ -431,7 +431,7 @@ void CAN_transmiter::update(void) {
 	motor_temp = sampler.get_drv_temp() * 100;
 	Power_Voltage = sampler.get_vbus_voltage() * 1000;
 	rt_angle_union.f = KTH7823.getAngle();
-	rt_velocity_union.f = 0.0f;
+	rt_velocity_union.f = KTH7823.getVelocity();
 	Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
 
 	Target_Control_Mode = FOC.control_mode;
@@ -621,7 +621,7 @@ u8 CAN_transmiter::CANFD_all_Send() {
 	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);
-	success_flag += CANFD_Send(STATUS_ID, 20, transmit_data);
+ 	success_flag += CANFD_Send(STATUS_ID, 24, transmit_data);
 
 	// 打包config数据
 	transmit_data[0] = controller_Mode;
@@ -671,8 +671,10 @@ void CAN_transmiter::communication_check() {
 					unpack_bytes_to_float(receive_message.rx_data, 3).f;
 				FOC.is_shut_down = 100; // 1s滤除启动电流
 			}
-			FOC.target_angle =
+			target_angle_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 7).f;
+			FOC.target_angle = target_angle_union.f * ANGLE_FACTOR;
+
 			target_velocity_union.f =
 				unpack_bytes_to_float(receive_message.rx_data, 11).f;
 			FOC.target_velocity = target_velocity_union.f * ANGLE_FACTOR;

Robot_V2/device/Timer.cpp

@@ -166,9 +166,11 @@ void TIMER2_IRQHandler(void) {
 		system_runtime_us += 100;
 
 		// 每100us执行一次的操作
-		sampler.update_current();					 // 更新电流采样值
-		KTH7823.update();							 // 更新角度传感器值
-		FOC.velocity_open_loop(FOC.target_velocity); // 速度开环控制
+		sampler.update_current(); // 更新电流采样值
+		KTH7823.update();		  // 更新角度传感器值
+								  // FOC.velocity_open_loop(); // 速度开环控制
+		// FOC.velocity_control(FOC.target_velocity); // 速度闭环控制
+		FOC.angle_control(FOC.target_angle);
 
 		if (total_us100_counter >= 10) {
 			// 每1ms执行的代码

Robot_V2/device/ang_sensor.cpp

@@ -2,6 +2,8 @@
 #include "gd32c10x_dma.h"
 #include "gd32c10x_gpio.h"
 #include "gd32c10x_spi.h"
+#include <cstdio>
+#include <cstring>
 #include <stdint.h>
 
 #define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
@@ -103,6 +105,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 		// 禁用DMA通道
 		dma_channel_disable(DMA0, DMA_CH2);
 		dma_channel_disable(DMA0, DMA_CH1);
+		printf("SPI DMA transfer timeout\r\n");
 		return -1;
 	}
 
@@ -206,11 +209,14 @@ float angle_sensor::read_sensorAngle(void) {
 	} else {
 		SPI_CS_DISABLE();
 		// 处理读取错误
-		// printf("Error reading angle from sensor.\n");
+		printf("Error reading angle from sensor.\n");
 		return FAULT_RECEIVE; // 错误返回
 	}
 }
-
+/**
+ * @brief 更新传感器角度（输出级）
+ * @return 累积角度值
+ */
 float angle_sensor::update(void) {
 	float currentAngle = read_sensorAngle();
 
@@ -225,22 +231,60 @@ float angle_sensor::update(void) {
 
 	// 处理角度绕回情况（0度和360度之间的跳变）
 	// 如果变化超过180度，说明发生了绕回
-	if (delta > 180.0f) {
+	if (delta > 270.0f) {
 		// 从接近360度绕回到接近0度
 		delta -= 360.0f;
-	} else if (delta < -180.0f) {
+	} else if (delta < -270.0f) {
 		// 从接近0度绕到接近360度
 		delta += 360.0f;
 	}
-
-	m_cumulativeAngle += delta / REDDUCTION_RATIO;
+	
+	std::memmove(velocity, velocity + 1, VEL_FILTER_SIZE * sizeof(float));
+	velocity[VEL_FILTER_SIZE - 1] = delta * 1e4;
+	
+	m_cumulativeAngle += delta;
 	m_lastAngle = currentAngle;
 
 	return m_cumulativeAngle;
 }
 
-float angle_sensor::getAngle(void) const { return m_cumulativeAngle; }
+/**
+ * @brief 获取累积角度值(输出级)
+ * @return 累积角度值
+ */
+float angle_sensor::getAngle(void) const {
+	return m_cumulativeAngle / REDDUCTION_RATIO;
+}
 
+/**
+ * @brief 获取累积角度值(原始级)
+ * @return 累积角度值
+ */
+float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
+
+/**
+ * @brief 获取速度值(输出级)
+ * @return 速度值(输出级)
+ */
+float angle_sensor::getVelocity(void) const {
+	return getRawVelocity() / REDDUCTION_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;
+}
+
+/**
+ * @brief 重置累积角度值
+ */
 void angle_sensor::reset(void) {
 	m_cumulativeAngle = 0.0f;
 	m_firstUpdate = true;

Robot_V2/device/ang_sensor.h

@@ -11,6 +11,8 @@ u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord);
 int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 						 uint16_t *rx_data);
 
+constexpr u16 VEL_FILTER_SIZE = 24;
+
 class angle_sensor {
   public:
 	angle_sensor();
@@ -18,13 +20,17 @@ class angle_sensor {
 	float update(void);
 	void reset(void);
 	float getAngle(void) const;
-
+	float getRawAngle(void) const;
+	float getVelocity(void) const;
+	float getRawVelocity(void) const;
 
   protected:
 	void KTH7823_init(void);
 	bool m_firstUpdate;
 	float m_lastAngle;
 	float m_cumulativeAngle;
+	float m_lastcumulativeAngle;
+	float velocity[VEL_FILTER_SIZE] = {0.0f};
 };
 
 #ifdef __cplusplus