feat(FOC): 增加闭环速度控制算法

- 重构setPhaseVoltage函数实现，提高执行效率
- 增加update_power函数，动态更新电源电压
- 优化velocity_open_loop函数，调用、setPhaseVoltage避免重复代码
- 实现angle_control和velocity_control闭环控制函数
- 调整PID控制器参数初始化及增加参数设置接口
- 改进电流和编码器速度卡尔曼滤波器实现
- 更新CAN通信数据打包逻辑，增加测试字段
- 在定时器中断中集成多种控制模式切换逻辑
- 完善角度传感器数据处理，改进速度计算方法
- 减小速度滤波数组大小，提升响应速度

Robot_V2/device/ang_sensor.cpp

@@ -10,9 +10,14 @@
 #define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
 
 constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
+constexpr float RAD_FACTOR = 1.0f / 65536.0f * 2.0f * 3.14159265358979323846f;
 constexpr u16 DEFAULT_COMMAND = 0x0000;
 constexpr u16 FAULT_RECEIVE = 0xFFFF;
 constexpr u16 REDDUCTION_RATIO = 25;
+constexpr float VELOCITY_SCALE = 1e4f;
+constexpr float ANGLE_WRAP_THRESHOLD = 270.0f;
+constexpr float ANGLE_WRAP_VALUE = 360.0f;
+constexpr int SIZE_FLOAT = sizeof(float);
 
 // DMA传输完成标志
 volatile bool spi_dma_tx_complete = false;
@@ -203,7 +208,7 @@ float angle_sensor::read_sensorAngle(void) {
 
 	SPI_CS_ENABLE();
 	// 使用DMA发送命令并接收数据
-	if (spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data) == 0) {
+	if (!spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data)) {
 		SPI_CS_DISABLE();
 		return u16data * ANGLE_FACTOR;
 	} else {
@@ -218,6 +223,7 @@ float angle_sensor::read_sensorAngle(void) {
  * @return 累积角度值
  */
 float angle_sensor::update(void) {
+
 	float currentAngle = read_sensorAngle();
 
 	if (m_firstUpdate) {
@@ -231,17 +237,17 @@ float angle_sensor::update(void) {
 
 	// 处理角度绕回情况（0度和360度之间的跳变）
 	// 如果变化超过180度，说明发生了绕回
-	if (delta > 270.0f) {
+	if (delta > ANGLE_WRAP_THRESHOLD) {
 		// 从接近360度绕回到接近0度
-		delta -= 360.0f;
-	} else if (delta < -270.0f) {
+		delta -= ANGLE_WRAP_VALUE;
+	} else if (delta < -ANGLE_WRAP_THRESHOLD) {
 		// 从接近0度绕到接近360度
-		delta += 360.0f;
+		delta += ANGLE_WRAP_VALUE;
 	}
-	
-	std::memmove(velocity, velocity + 1, VEL_FILTER_SIZE * sizeof(float));
-	velocity[VEL_FILTER_SIZE - 1] = delta * 1e4;
-	
+
+	std::memmove(velocity, velocity + 1, SIZE_FLOAT * (VEL_FILTER_SIZE - 1));
+	velocity[VEL_FILTER_SIZE - 1] = delta * VELOCITY_SCALE;
+
 	m_cumulativeAngle += delta;
 	m_lastAngle = currentAngle;
 
@@ -279,7 +285,8 @@ float angle_sensor::getRawVelocity(void) const {
 	for (int i = 0; i < VEL_FILTER_SIZE - 1; i++) {
 		velocity_filted += velocity[i];
 	}
-	return velocity_filted / VEL_FILTER_SIZE;
+	return velocity_filted / (VEL_FILTER_SIZE - 1);
+	// return velocity[VEL_FILTER_SIZE - 1];
 }
 
 /**