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

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

Robot_V2/device/Can.cpp

@@ -1,5 +1,6 @@
 #include "Can.h"
 #include "DengFOC.h"
+#include "Filter.h"
 #include "adc.h"
 #include "ang_sensor.h"
 #include "gd32c10x.h"
@@ -23,6 +24,7 @@ extern CAN_transmiter CAN0_transmiter;
 extern FOC_Controlroller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
+extern EncoderVelocityKalman Vel_kalman;
 
 // 添加DMA相关变量
 dma_parameter_struct dma_rx_parameter;
@@ -431,7 +433,8 @@ 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 = KTH7823.getVelocity();
+	// rt_velocity_union.f = KTH7823.getVelocity();
+	rt_velocity_union.f = FOC.rt_velocity;
 	Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
 
 	Target_Control_Mode = FOC.control_mode;
@@ -440,6 +443,7 @@ void CAN_transmiter::update(void) {
 	u_current_union.f = sampler.get_current('U') * 1000;
 	w_current_union.f = sampler.get_current('W') * 1000;
 	v_current_union.f = sampler.get_current('V') * 1000;
+	// testfloat_union.f = KTH7823.getVelocity();
 }
 
 /*!
@@ -621,7 +625,8 @@ 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, 24, transmit_data);
+	pack_float_to_bytes(transmit_data, 21, testfloat_union);
+	success_flag += CANFD_Send(STATUS_ID, 32, transmit_data);
 
 	// 打包config数据
 	transmit_data[0] = controller_Mode;
@@ -677,7 +682,7 @@ 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 * ANGLE_FACTOR;
+			FOC.target_velocity = target_velocity_union.f;
 
 			can0_receive_flag = RESET;
 		}

Robot_V2/device/Can.h

@@ -106,6 +106,7 @@ class CAN_transmiter {
 	can_receive_message_struct receive_message; // 接收消息结构体
 	FlagStatus can0_receive_flag;				// CAN0接收标志
 	FlagStatus can0_send_flag;					// CAN0发送标志
+	float_union_t testfloat_union = {0};			// 测试用
 
 	void communication_check();					// 通信检查
 };

Robot_V2/device/Timer.cpp

@@ -1,6 +1,7 @@
 #include "Timer.h"
 #include "Can.h"
 #include "DengFOC.h"
+#include "Filter.h"
 #include "Timer.h"
 #include "adc.h"
 #include "ang_sensor.h"
@@ -9,6 +10,7 @@
 #include "gd32c10x_misc.h"
 #include "gd32c10x_rcu.h"
 #include "gd32c10x_timer.h"
+#include <cstdio>
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -35,6 +37,7 @@ extern FOC_Controlroller FOC;
 // 声明外部采样器对象
 extern sampler sampler;
 extern angle_sensor KTH7823;
+extern EncoderVelocityKalman Vel_kalman;
 
 /**
  * @brief 设置引脚中断优先级
@@ -156,6 +159,7 @@ uint64_t get_system_runtime_us(void) { return system_runtime_us; }
  * @return void
  */
 void TIMER2_IRQHandler(void) {
+	static u8 mode = 0;
 	// 检查更新中断标志并处理
 	if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
 		// 清除更新中断标志
@@ -168,9 +172,15 @@ void TIMER2_IRQHandler(void) {
 		// 每100us执行一次的操作
 		sampler.update_current(); // 更新电流采样值
 		KTH7823.update();		  // 更新角度传感器值
-								  // FOC.velocity_open_loop(); // 速度开环控制
-		// FOC.velocity_control(FOC.target_velocity); // 速度闭环控制
-		FOC.angle_control(FOC.target_angle);
+		FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity());
+
+		if (FOC.control_mode == 1) {
+			FOC.angle_control(FOC.target_angle); // 角度闭环控制
+		} else if (FOC.control_mode == 2) {
+			FOC.velocity_control(FOC.target_velocity);// 速度闭环控制
+		} else if (FOC.control_mode == 4) {
+			FOC.velocity_open_loop(); // 速度开环控制
+		}
 
 		if (total_us100_counter >= 10) {
 			// 每1ms执行的代码
@@ -178,7 +188,8 @@ void TIMER2_IRQHandler(void) {
 
 			// 设置CAN发送标志
 			CAN0_transmiter.can0_send_flag = SET;
-
+			mode = FOC.control_mode;
+			// 过流检测
 			if (sampler.get_current('U') > 2000 ||
 				sampler.get_current('V') > 2000 ||
 				sampler.get_current('W') > 2000) {
@@ -202,7 +213,7 @@ void TIMER2_IRQHandler(void) {
 			if (ms10_counter >= 10) {
 				ms10_counter = 0;
 				// 可以添加每10ms执行的代码
-
+				FOC.update_power();
 				static u8 ms100_counter = 0;
 				ms100_counter++;
 				if (ms100_counter >= 10) {

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];
 }
 
 /**

Robot_V2/device/ang_sensor.h

@@ -11,7 +11,7 @@ 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;
+constexpr u16 VEL_FILTER_SIZE = 3;
 
 class angle_sensor {
   public: