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

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

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

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

Robot_V2/Algorithm/DengFOC.cpp

@@ -2,6 +2,8 @@
 #include "Filter.h"
 #include "Timer.h"
 #include "adc.h"
+#include "ang_sensor.h"
+#include "gd32c10x_dbg.h"
 #include "pid.h"
 #include "systick.h"
 #include <cmath>
@@ -10,21 +12,22 @@
 #include <stdint.h>
 
 constexpr double PI = 3.1415926535897932384626433832795f;
+constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
+constexpr float RAD2DEG = 360.0f / 6.28318530718f;
 constexpr float _1_SQRT3 = 0.57735026919f;
 constexpr float _2_SQRT3 = 1.15470053838f;
 constexpr float _3PI_2 = 4.71238898038f;
-constexpr u8 PP = 11, DIR = 1;
+constexpr u8 PP = 11;
+constexpr float DIR = -1.0;
 constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
 constexpr float zero_electric_angle = 0.0f;
 
 extern sampler sampler;
+extern angle_sensor KTH7823;
 
 CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
 CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
 CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
-PIDController vel_loop_M0 = PIDController{2, 0, 0, 100000, 24.0 / 2.0};
-PIDController angle_loop_M0 = PIDController{2, 0, 0, 100000, 100};
-PIDController current_loop_M0 = PIDController{2, 0, 0, 100000, 800};
 
 // 初始变量及函数定义
 #define _constrain(amt, low, high)                                             \
@@ -32,11 +35,6 @@ PIDController current_loop_M0 = PIDController{2, 0, 0, 100000, 800};
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))
 
-inline float _electricalAngle(float shaft_angle) {
-	return _normalizeAngle((float)(DIR * PP) * shaft_angle -
-						   zero_electric_angle);
-}
-
 /**
  * @brief 归一化角度
  *
@@ -48,18 +46,34 @@ inline float _normalizeAngle(float angle) {
 	// 取余运算可以用于归一化，列出特殊值例子算便知
 	return a >= 0 ? a : (a + 2 * PI);
 }
+inline float _electricalAngle() {
+	return _normalizeAngle((float)(DIR * PP) * KTH7823.read_sensorAngle() *
+						   -zero_electric_angle);
+}
 /**
  * @brief 电机控制类构造函数
  *
  */
 FOC_Controlroller::FOC_Controlroller() { ; }
-/**
- * @brief 获取电机 shaft_angle
- *
- * @return void
- */
-void FOC_Controlroller::driver_output(float Ua, float Ub, float Uc) {
 
+/**
+ * @brief 设置电机输出
+ *
+ * @param Uq: q轴电压
+ * @param Ud: d轴电压
+ * @param angle_el: 电机轴角度
+ */
+inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
+											   float angle_el) {
+	angle_el = _normalizeAngle(angle_el);
+	// 帕克逆变换
+	Ualpha = -Uq * sin(angle_el);
+	Ubeta = Uq * cos(angle_el);
+
+	// 克拉克逆变换
+	Ua = Ualpha + voltage_power_supply / 2;
+	Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
+	Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
 	// 计算占空比
 	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
 	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
@@ -71,34 +85,13 @@ void FOC_Controlroller::driver_output(float Ua, float Ub, float Uc) {
 	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
 }
 
-/**
- * @brief 设置电机输出
- *
- * @param Uq: q轴电压
- * @param Ud: d轴电压
- * @param angle_el: 电机轴角度
- */
-void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) {
-	Uq = _constrain(Uq, -voltage_power_supply / 2, voltage_power_supply / 2);
-	angle_el = _normalizeAngle(angle_el + zero_electric_angle);
-	// 帕克逆变换
-	Ualpha = -Uq * sin(angle_el);
-	Ubeta = Uq * cos(angle_el);
-
-	// 克拉克逆变换
-	Ua = Ualpha + voltage_power_supply / 2;
-	Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
-	Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
-	driver_output(Ua, Ub, Uc);
-}
-
 /**
  * @brief 速度开环控制
  *
  * @param target_velocity: 目标速度
  * @return float: q轴电压
  */
-float FOC_Controlroller::velocity_open_loop(float target_velocity) {
+float FOC_Controlroller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
 	// 预计算常量 1e-4 = 100微秒控制周期
 	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
@@ -139,17 +132,15 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
  * @brief 电机初始化
  *
  * @param _PP: 电机极队数
- * @param _DIR: 电机极性
  */
-void FOC_Controlroller::DFOC_alignSensor(int _PP, int _DIR) {
+// void FOC_Controlroller::DFOC_alignSensor(int _PP) {
-	u8 PP = _PP;
+// 	u8 PP = _PP;
-	u8 DIR = _DIR;
+// 	setPhaseVoltage(3, 0, _3PI_2);
-	setPhaseVoltage(3, 0, _3PI_2);
+// 	delay_1ms(3000);
-	delay_1ms(3000);
+// 	zero_electric_angle = _electricalAngle();
-	zero_electric_angle = _electricalAngle(0);
+// 	setPhaseVoltage(0, 0, _3PI_2);
-	setPhaseVoltage(0, 0, _3PI_2);
+// 	std::printf("0电角度:%f", zero_electric_angle);
-	std::printf("0电角度:%f", zero_electric_angle);
+// }
-}
 
 void FOC_Controlroller::shut_down() {
 	is_shut_down = 100;
@@ -164,62 +155,42 @@ void FOC_Controlroller::resume() {
 	gpio_bit_set(GPIOB, GPIO_PIN_12);
 }
 
-// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
+void FOC_Controlroller::update_power(void) {
-float cal_Iq_Id(float current_a, float current_b, float angle_el) {
+	voltage_power_supply = sampler.get_vbus_voltage();
-	float I_alpha = current_a;
-	float I_beta = _1_SQRT3 * current_a + _2_SQRT3 * current_b;
-
-	float ct = cos(angle_el);
-	float st = sin(angle_el);
-	// float I_d = I_alpha * ct + I_beta * st;
-	float I_q = I_beta * ct - I_alpha * st;
-	return I_q;
 }
 
-void FOC_Controlroller::setTorque(float Uq, float angle_el) {
+/**
-	Uq =
+ * @brief 角度闭环控制
-		_constrain(Uq, -(voltage_power_supply) / 2, (voltage_power_supply) / 2);
+ *
-	float Ud = 0;
+ * @param target_velocity: 目标速度
-	angle_el = _normalizeAngle(angle_el);
+ * @return float: q轴电压
+ */
+float FOC_Controlroller::angle_control(float target_angle) {
+	float Sensor_angel = KTH7823.getRawAngle() * DEG2RAD;
+	float Kp = 2.0f;
+
+	float Uq = _constrain(Kp * (target_angle - DIR * Sensor_angel) * RAD2DEG,
+						  -voltage_power_supply, voltage_power_supply);
+	shaft_angle = _normalizeAngle(KTH7823.read_sensorAngle() * DEG2RAD);
+	float Ud = 0.0f;
+	// 直接计算电气角度，避免函数调用开销
+	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
+
 	// 帕克逆变换
 	Ualpha = -Uq * sin(angle_el);
 	Ubeta = Uq * cos(angle_el);
 
 	// 克拉克逆变换
 	Ua = Ualpha + voltage_power_supply / 2;
-	Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
+	Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
-	Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
+	Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
-	driver_output(Ua, Ub, Uc);
+	// 计算占空比
-}
+	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
-
+	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
-float FOC_Controlroller::DFOC_M0_Current() {
+	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
-	// 使用卡尔曼滤波后的三相电流计算Iq
+	float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
-	float Iu_filtered = curU_kalman.update(sampler.get_current('U'), Ua);
+	float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
-	float Iv_filtered = curV_kalman.update(sampler.get_current('V'), Ub);
+	float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
-	float Iw_filtered = curW_kalman.update(sampler.get_current('W'), Uc);
+	Set_3Pwm(dc_a, dc_b, dc_c);
-
+	return Sensor_angel;
-	// 根据基尔霍夫电流定律验证三相电流总和是否接近0
-	// float current_sum = Iu_filtered + Iv_filtered + Iw_filtered;
-
-	// 用三相滤波后的电流计算Iq（通过Clarke和Park变换）
-	// 方法1: 使用U和V相计算
-	float I_q_M0_ori =
-		cal_Iq_Id(Iu_filtered, Iv_filtered, _electricalAngle(shaft_angle));
-
-	// 方法2: 也可以使用其他两相的组合计算，然后取平均值以提高精度
-	// float I_q_UW = cal_Iq_Id(Iu_filtered, Iw_filtered,
-	// _electricalAngle(shaft_angle)); float I_q_VW = cal_Iq_Id(Iv_filtered,
-	// Iw_filtered, _electricalAngle(shaft_angle)); float I_q_M0_ori = (I_q_UV +
-	// I_q_UW + I_q_VW) / 3.0f;
-
-	return I_q_M0_ori;
-	// return 0;
-}
-void FOC_Controlroller::DFOC_M0_setTorque(float Target) // 电流力矩环
-{
-	//   setTorque(current_loop_M0(Target-DFOC_M0_Current()),_electricalAngle());
-}
-
-void FOC_Controlroller::update_power(void) {
-	voltage_power_supply = sampler.get_vbus_voltage();
 }

Robot_V2/Algorithm/DengFOC.h

@@ -1,6 +1,7 @@
 // 函数声明
 #ifndef _DENGFOC_H
 #define _DENGFOC_H
+#include "pid.h"
 #include <stdint.h>
 class FOC_Controlroller {
   public:
@@ -18,13 +19,16 @@ class FOC_Controlroller {
 	u8 voltage_error = 0;
 	u8 is_shut_down = 0;
 
-	float velocity_open_loop(float target_velocity);
+	float velocity_open_loop();
+	float angle_control(float target_angle);
+	// void SET_VEL_PID(float P, float I, float D, float ramp);   // 速度环PID设置
+	// void SET_ANGLE_PID(float P, float I, float D, float ramp); // 角度环PID设置
+
 	void update_power(void);
-	void DFOC_alignSensor(int _PP, int _DIR);
+	void DFOC_alignSensor(int _PP);
 	void shut_down(void);
 	void resume(void);
 
-
   protected:
 	std::u64 last_time_us = 0;
 	std::u64 now_us = 0;
@@ -33,17 +37,16 @@ class FOC_Controlroller {
 	float zero_electric_angle = 0;
 	float voltage_power_supply = 24.0f; // 电源电压
 
-	void driver_output(float Ua, float Ub, float Uc);
+	PIDController vel_loop =
+		PIDController{2, 0, 0, 100000, voltage_power_supply / 2};
+	PIDController angle_loop = PIDController{2, 0, 0, 100000, 100};
+
 	void setPhaseVoltage(float Uq, float Ud, float angle_el);
-	void setTorque(float Uq,float angle_el);
-	float DFOC_M0_Current();
-	void DFOC_M0_setTorque(float Target);
 };
 
 float _normalizeAngle(float angle);
 void DFOC_Vbus(float power_supply);
-float _electricalAngle(float shaft_angle);
+inline float _electricalAngle();
-float DFOC_M0_Angle();
+float DFOC_M0_Angle(void);
-float serial_motor_target();
 
 #endif

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();					 // 更新电流采样值
+		sampler.update_current(); // 更新电流采样值
-		KTH7823.update();							 // 更新角度传感器值
+		KTH7823.update();		  // 更新角度传感器值
-		FOC.velocity_open_loop(FOC.target_velocity); // 速度开环控制
+								  // 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

Robot_V2/main.cpp

@@ -7,6 +7,7 @@
 #include "core_cm4.h"
 #include "initial.h"
 #include "systick.h"
+#include <cstdio>
 
 // 全局变量定义
 CAN_transmiter CAN0_transmiter; // CAN初始化
@@ -22,9 +23,10 @@ int main(void) {
 	watchdog_init(5000); // 看门狗初始化
 
 	angsens_MG_test(); // 角度传感器磁感应强度测试
-	
+
 	while (1) {
 		// delay_1ms(100);
+		// std::printf("1\n");
 		fwdgt_counter_reload();				   // 看门狗复位
 		CAN0_transmiter.communication_check(); // CAN通信
 	}