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采样时间配置以优化性能

Robot_V2/Algorithm/FOC.cpp

@@ -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;
-}

Robot_V2/Algorithm/FOC.h

@@ -27,20 +27,23 @@ class FOC_Controller {
 	bool over_current = false;
 	bool voltage_error = false;
 	bool is_shut_down = false;
+	bool thermal_shutdown = false; // 过热关机标志
+
 	float voltage_power_supply = 24.0f; // 电源电压
-	float v_safe = voltage_power_supply * 1.05f;
+	float v_safe = voltage_power_supply / 1.05f;
 	float duty_a = 0, duty_b = 0, duty_c = 0;
 	float Ua = 0, Ub = 0, Uc = 0;
 
-	float current_loop_p=DEFAULT_current_loop_p;
+	float cur_loop_p = DEFAULT_current_loop_p;
+	float vel_loop_p = DEFAULT_vel_loop_p;
+	float ang_loop_p = DEFAULT_angle_loop_p;
 
-	float vel_angloop_p=DEFAULT_vel_angloop_p;
-	float vel_loop_p=DEFAULT_vel_loop_p;
+	float vel_angloop_p = DEFAULT_vel_angloop_p;
+	float ang_angloop_p = DEFAULT_angle_angloop_p;
 
-	float ang_angloop_p=DEFAULT_ang_angloop_p;	
-	float angle_loop_p=DEFAULT_angle_loop_p;
-	
-	float vel_MIT_p=DEFAULT_vel_MIT_p;
+	float ang_MIT_p = DEFAULT_angle_MIT_p;
+	float vel_MIT_p = DEFAULT_vel_MIT_p;
+	float cur_MIT_p = DEFAULT_current_MIT_p;
 
 	float velocity_open_loop(void);
 	float angle_control(void);
@@ -55,24 +58,35 @@ class FOC_Controller {
 	void resume(void);
 	void drv_stop(void);
 	float drv_hold(void);
+	void shut_down_thermal(void);	// 过热关机
+	void resume_from_thermal(void); // 过热恢复
 
+	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	// 此处默认值不作数
-	PIDController vel_loop = PIDController{vel_loop_p * 100, 0.0f, 0, 10000,
-										   voltage_power_supply}; // 12V
-	PIDController angle_loop = PIDController{angle_loop_p * 100, 0, 0, 10000000,
-											 voltage_power_supply}; // 12V
-	PIDController current_loop = PIDController{current_loop_p * 100, 0, 0, 1500,
-											   voltage_power_supply}; // 12V
-	PIDController vel_inVelAngloop =	
-		PIDController{vel_angloop_p * 100, 0, 0, 1e6, v_safe};
+	PIDController vel_loop =
+		PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
+					  vel_loop_Kd, 10000, voltage_power_supply}; // 12V
+	PIDController angle_loop =
+		PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
+					  10000000, voltage_power_supply}; // 12V
+	PIDController current_loop = PIDController{
+		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply}; // 12V
+	PIDController vel_inVelAngloop =
+		PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
+					  vel_angloop_Kd, 1e6, v_safe};
 	PIDController ang_inVelAngloop =
-		PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
+		PIDController{DEFAULT_angle_angloop_p * 100 / voltage_power_supply,
+					  ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
 
-		
 	PIDController ang_inMITloop =
-		PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
-	PIDController vel_inMITloop =
-		PIDController{vel_MIT_p * 100, 0, 0, 1e9, 6000};
+		PIDController{DEFAULT_angle_MIT_p * 100 / limit_voltage, angle_MIT_Ki,
+					  angle_MIT_Kd, 1e5, 100};
+	PIDController vel_inMITloop = PIDController{
+		DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
+	PIDController current_inMITloop = PIDController{
+		DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
+		current_loop_Kd, 1500, voltage_power_supply}; // 12V
 
 	void update_pid_limits() {
 		vel_loop.limit = voltage_power_supply;
@@ -86,16 +100,14 @@ class FOC_Controller {
 	float Ualpha, Ubeta = 0;
 	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
-	float startup_ramp_val = 0.0f;      // 软启动系数
-	float startup_ramp_step = 0.01f;    // 启动斜坡步长，可调节以控制启动速度
 	void setPhaseVoltage(float Uq, float Ud);
-	void setSVPWM_PhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);
 };
 
 // 定义函数指针类型
 typedef float (FOC_Controller::*FOCFunctionPtr)(void);
 
+void calibrate_zero_position(void);
 inline float _normalizeAngle(float angle);
 inline float _electricalAngle();
 

Robot_V2/Algorithm/Filter.cpp

@@ -145,7 +145,7 @@ float EncoderAngleKalman::getVelocity() const {
 	float vel = 0.0f;
 	if (time_counts > 0) {
 		vel = (angle_now - velocity_last_angle) * TIMER2_FREQUENCY /
-			  time_counts * 2;
+			  time_counts ;
 	}
 
 	velocity_last_angle = angle_now;

Robot_V2/Algorithm/pid.cpp

@@ -76,4 +76,15 @@ void PIDController::set_P(float pp) { this->P = pp; }
  * @param limit: 输出限幅
  * @return void
  */
-void PIDController::set_limit(float limit) { this->limit = limit; }
+void PIDController::set_limit(float limit) { this->limit = limit; }
+
+/**
+ * @brief 重置PID控制器状态
+ *
+ * @return void
+ */
+void PIDController::reset() {
+	error_prev = 0.0f;
+	output_prev = 0.0f;
+	integral_prev = 0.0f;
+}