feat(can): 增强CAN通信缓冲区管理机制

- 实现循环缓冲区队列管理CAN消息接收 - 添加缓冲区满/空状态检查函数 - 实现消息出队列dequeue_rx_message功能 - 在中断处理程序中添加消息入队列机制 - 优化CAN消息处理流程，支持批量处理 refactor(motor): 优化电机控制类结构和安全机制 - 重命名PID控制器参数变量名以提高可读性 - 添加过热关机标志位thermal_shutdown和相关处理函数 - 实现过热关机shut_down_thermal和过热恢复resume_from_thermal功能 - 调整关机逻辑，添加drv_stop调用确保电机完全停止 - 修正电流环控制中的占空比设置，改为单相保持 feat(motor): 实现FOC控制算法改进和零点校准功能 - 实现电机零点校准函数calibrate_zero_position用于位置补偿 - 修正FOC控制器中的电气角度计算，集成KTH7823校准角度补偿 - 新增d轴电流计算功能 - 重构PID参数初始化方式，使用统一的电压限制配置 - 更新SVPWM相电压设置算法，改进角度归一化处理 - 修改缺省扇区输出占空比从0.0f改为0.5f以提高稳定性 fix(control): 修复控制回路方向和频率计算问题 - 修正角度控制回路中的误差计算方向 - 修正开环速度控制中的频率参数使用 - 修正速度环控制中的误差计算方向 - 修正MIT模式下多级PID控制器的参数设置和误差计算 refactor(filter): 修正编码器速度计算算法 - 移除速度计算中的冗余乘法因子2 feat(pid): 扩展PID控制器功能 - 添加reset方法用于重置PID内部状态 chore(build): 更新开发工具版本和ADC配置 - 更新csolution版本至2.13.0 - 调整ADC采样时间配置以优化性能
2026-03-09 01:35:26 +08:00
parent 9689841148
commit a26dd1cc8e
14 changed files with 514 additions and 313 deletions
@@ -7,6 +7,7 @@
 #include "config.h"
 #include "motor.h"
 #include "pid.h"
+#include "systick.h"
 #include <arm_math.h>
 #include <cmath>
 #include <gd32c10x.h>
@@ -24,11 +25,35 @@ extern CurrentKalman Current_kalman;
 #define constrain(value, min, max, target_min, target_max)                     \
 	((value - min) / (max - min) * (target_max - target_min))

+// 校准步骤（代码模板）
+void calibrate_zero_position(void) {
+	// Set_3Pwm(.25, 0, 0); // 起劲
+	// delay_1ms(1000);
+	// KTH7823.update();
+
+	// // 改进校准算法：获取当前传感器角度并计算补偿值
+	// float current_sensor_angle = KTH7823.getcurrent_sensorangle();
+	// float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f); //
+	// 计算当前电角度
+
+	// // 计算需要的补偿值，使得当前位置对应0°电角度
+	// KTH7823.calibration_angle = -electrical_angle;
+
+	// // 对补偿角进行归一化处理，确保在[-180, 180]范围内
+	// KTH7823.calibration_angle = fmodf(KTH7823.calibration_angle + 180.0f,
+	// 360.0f); if (KTH7823.calibration_angle < 0) { KTH7823.calibration_angle
+	// += 360.0f;
+	// }
+	// KTH7823.calibration_angle -= 180.0f;
+
+	// Set_3Pwm(0, 0, 0); // 停止
+}
+
 // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
 inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 									   float current_c) {
 	// 计算电气角度
-	float angle_el = fmod(shaft_angle * PP, 360.0f);
+	float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);

 	// Clarke变换 (使用三相电流)
 	float I_alpha, I_beta;
@@ -41,9 +66,11 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
 	float I_q, I_d;
 	I_q = I_beta * cos_el - I_alpha * sin_el;
+	I_d = I_beta * sin_el + I_alpha * cos_el;

 	// 返回Iq值，乘以1000以转换为mA
 	return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
+	// return I_q * 1000 - KE_FACTOR * rt_velocity;
 }

 /**
@@ -53,25 +80,16 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 */
 u8 FOC_Controller::init() {
 	motor_init(); // 电机驱动初始化
-	vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply,
-					 vel_loop_Ki,
-					 vel_loop_Kd); // 设置电机速度控制参数
-	angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply,
-					   100.0f,
-					   0.2f); // 设置电机角度控制参数
-	current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply,
-						 current_loop_Ki,
-						 current_loop_Kd); // 设置电机电流控制参数

-	vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
-							 vel_angloop_Kd); // 设置电机速度控制参数
-	ang_inVelAngloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
-							 ang_angloop_Ki,
-							 ang_angloop_Kd); // 设置电机角度控制参数
+	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
+	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
+	FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
+	FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
+	FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
+	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
+	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
+	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;

-	vel_inMITloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
-						  vel_MIT_Ki,
-						  vel_MIT_Kd); // 设置电机角度控制参数
 	return 0;
 }

@@ -126,14 +144,17 @@ inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
 }

 /**
- * @brief 使用SVPWM算法设置电机输出（优化版）
+ * @brief 使用SVPWM算法设置电机输出
 *
 * @param Uq: q轴电压
 * @param Ud: d轴电压
 */
 inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
-	// 计算电气角度（保持原有角度单位和范围）
-	float angle_el = fmod(shaft_angle * PP, 360.0f);
+	// 计算电气角度
+	float angle_el = shaft_angle * PP + KTH7823.calibration_angle;
+
+	// 对电角度进行归一化，确保在[0, 360)范围内
+	angle_el = fmodf(angle_el, 360.0f);

 	// 帕克逆变换
 	float sin_el, cos_el;
@@ -143,7 +164,7 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {

 	// 计算角度并归一化到 [0, 2π)
 	float angle = atan2f(Ubeta, Ualpha);
-	if (angle < 0)
+	if (angle < 0.0f)
 		angle += 2.0f * PI;

 	// 扇区与局部角度
@@ -213,9 +234,9 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
 		duty_c = 0.5f * T0 + T1;
 		break;
 	default:
-		duty_a = 0.0f;
-		duty_b = 0.0f;
-		duty_c = 0.0f;
+		duty_a = 0.5f;
+		duty_b = 0.5f;
+		duty_c = 0.5f;
 		break;
 	}

@@ -224,12 +245,36 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
 }

 /**
- * @brief 电机关机
+ * @brief 电机关机（通用关机，不带原因标记）
 *
 */
 void FOC_Controller::shut_down() {
-	is_shut_down = 1;
+	drv_stop();
 	motor_sleep();
+	is_shut_down = true;
+}
+
+/**
+ * @brief 过热关机
+ *
+ */
+void FOC_Controller::shut_down_thermal() {
+	drv_stop();
+	motor_sleep();
+	is_shut_down = true;
+	thermal_shutdown = true;
+}
+
+/**
+ * @brief 过热恢复
+ *
+ */
+void FOC_Controller::resume_from_thermal() {
+	thermal_shutdown = false;
+	if (!voltage_error && !over_current) {
+		is_shut_down = false;
+		motor_resume();
+	}
 }
 /**
 * @brief 占空比置0
@@ -246,7 +291,7 @@ float FOC_Controller::drv_hold() {
 	} else if (HOLD_PWM_DUTY < 0.0f) {
 		hold_pwm_duty = 0.0f;
 	}
-	Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
+	Set_3Pwm(hold_pwm_duty, 0, 0);
 	return hold_pwm_duty;
 }
 /**
@@ -283,17 +328,17 @@ void FOC_Controller::update_power(float voltage) {
 float FOC_Controller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
 	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
-										 TIMER2_FREQUENCY,
+										 MOTOR_FREQUENCY / 2,
 					   360.0f);
+	// 获取电机轴角度
+	float shaft_angle = KTH7823.getcurrent_sensorangle();
+	CAN0_transmiter.testfloat_union.f = shaft_angle;
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));

 	// 因为在开环控制中，Ud始终为0，只控制Uq
 	const float Uq = voltage_power_supply / 2.0f;
-
 	setSVPWM_PhaseVoltage(Uq, 0.0f);
-	// CAN0_transmiter.testfloat_union.f = Uq;
-	// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-	// 		  sampler.get_current('W'));
-
 	return limit_voltage;
 }

@@ -305,7 +350,7 @@ float FOC_Controller::velocity_open_loop() {
 */
 float FOC_Controller::angle_control() {
 	// PID 控制计算
-	float Uq = angle_loop(target_angle - rt_angle);
+	float Uq = angle_loop(rt_angle - target_angle);

 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -316,6 +361,46 @@ float FOC_Controller::angle_control() {
 	return shaft_angle;
 }

+/**
+ * @brief 速度闭环控制
+ *
+ * @param target_velocity: 目标速度
+ * @return float: 当前速度
+ */
+float FOC_Controller::velocity_control() {
+	// PID 控制计算
+	float Uq = vel_loop((target_velocity - rt_velocity) * REDUCTION_RATIO);
+	CAN0_transmiter.testfloat_union.f = Uq;
+
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
+
+	return rt_velocity;
+}
+
+float FOC_Controller::current_control() {
+	// PID 控制计算
+	float Iq = rt_current =
+		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+				  sampler.get_current('W'));
+	float Uq = current_loop(target_current - Iq);
+	if (Uq > limit_voltage) {
+		Uq = limit_voltage;
+	} else if (Uq < -limit_voltage) {
+		Uq = -limit_voltage;
+	}
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle();
+	CAN0_transmiter.testfloat_union.f = shaft_angle;
+	// 设置相电压
+	setSVPWM_PhaseVoltage(Uq, 0.0f);
+	return 1;
+}
+
 /**
 * @brief 带速度限制的角度闭环控制
 *
@@ -327,6 +412,7 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 	// 计算角度误差
 	float angle_error = target_angle - rt_angle;

+	ang_inVelAngloop.set_P(ang_angloop_p);
 	// 使用角度PID控制器计算目标速度
 	float target_velocity = ang_inVelAngloop(angle_error);

@@ -339,13 +425,24 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 	} else {
 		vel_inVelAngloop.set_P(vel_angloop_p);
 	}
-	float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
+	float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
+	CAN0_transmiter.testfloat_union.f = Uq;
+
+	// // 实现力度控制：如果电流超过目标电流，则限制输出
+	// if (abs(rt_current) > abs(target_current)) {
+	// 	// 当实际电流超过目标电流时，减小输出以限制电流
+	// 	// 可以采用比例限制或直接限制输出电压的方式
+	// 	float current_ratio = abs(target_current) / abs(rt_current);
+	// 	if (current_ratio < 1.0f) {
+	// 		Uq *= current_ratio;  // 按比例缩小输出电压
+	// 	}
+	// }

-	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'));
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
-
+	// 获取当前实际电流值
+	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
+						   sampler.get_current('W'));
 	// 设置相电压
 	setSVPWM_PhaseVoltage(Uq, 0.0f);

@@ -363,13 +460,13 @@ float FOC_Controller::MIT_Mode() {
 	// 第一层：角度环（外环）
 	// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
 	float angle_error = target_angle - rt_angle;
+	ang_inMITloop.set_P(ang_MIT_p);
 	float target_velocity =
-		ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
-	CAN0_transmiter.testfloat_union.f = target_velocity;
+		ang_inMITloop(angle_error); // 使用angle_loop作为角度到速度的映射

 	// 第二层：速度环（中环）
 	// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
-	float velocity_error = rt_velocity - target_velocity;
+	float velocity_error = (rt_velocity - target_velocity);
 	vel_inMITloop.set_P(vel_MIT_p);
 	float target_current = vel_inMITloop(velocity_error);

@@ -380,7 +477,8 @@ float FOC_Controller::MIT_Mode() {

 	// 通过电流环PID控制器计算输出电压
 	float current_error = target_current - rt_current;
-	float Uq = current_loop(current_error);
+	current_inMITloop.set_P(cur_MIT_p);
+	float Uq = current_inMITloop(current_error);

 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -390,37 +488,3 @@ float FOC_Controller::MIT_Mode() {

 	return shaft_angle;
 }
-
-/**
- * @brief 速度闭环控制
- *
- * @param target_velocity: 目标速度
- * @return float: 当前速度
- */
-float FOC_Controller::velocity_control() {
-	// PID 控制计算
-	float Uq = vel_loop((rt_velocity - target_velocity) * 25);
-
-	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle();
-	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq, 0.0f);
-	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'));
-
-	return rt_velocity;
-}
-
-float FOC_Controller::current_control() {
-	// PID 控制计算
-	float Iq = rt_current =
-		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-				  sampler.get_current('W'));
-	current_loop.set_P(current_loop_p);
-	float Uq = current_loop(target_current - Iq);
-	// 获取电机轴角度
-	shaft_angle = KTH7823.getcurrent_sensorangle();
-	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq, 0.0f);
-	return 1;
-}