feat(FOC): 添加带速度限制的角度闭环控制功能

新增angle_control_with_velocity_limit函数，实现带速度限制的角度控制，
包含角度PID和速度PID双环控制，防止电机转速过快导致的不稳定。

- 新增带速度限制的角度控制函数
- 配置新的PID控制器用于速度内环和角度外环

Robot_V2/Algorithm/DengFOC.cpp

@@ -209,6 +209,36 @@ float FOC_Controlroller::angle_control(float target_angle) {
 	return shaft_angle;
 }
 
+/**
+ * @brief 带速度限制的角度闭环控制
+ *
+ * @param target_angle: 目标角度
+ * @param max_velocity: 最大速度限制 (单位: rad/s)
+ * @return float: 当前角度
+ */
+float FOC_Controlroller::angle_control_with_velocity_limit(float target_angle) {
+	// 计算角度误差
+	float angle_error = target_angle - rt_angle;
+	
+	// 使用角度PID控制器计算目标速度
+	float target_velocity = ang_inVelAngloop(angle_error);
+	
+	// 对目标速度进行限幅
+	target_velocity = _constrain(target_velocity, -limit_velocity, limit_velocity);
+	
+	// 使用速度PID控制器计算最终输出电压
+	float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
+	// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
+	
+	// 获取电机轴角度
+	shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
+	
+	// 设置相电压
+	setPhaseVoltage(Uq, 0.0f);
+	
+	return shaft_angle;
+}
+
 /**
  * @brief 角度闭环控制
  *

Robot_V2/Algorithm/DengFOC.h

@@ -25,6 +25,7 @@ class FOC_Controlroller {
 	float Ua = 0, Ub = 0, Uc = 0;
 	float velocity_open_loop();
 	float angle_control(float target_angle);
+	float angle_control_with_velocity_limit(float target_angle);
 	float velocity_control(float target_velocity);
 	float current_control(float target_current);
 	float angle_control_NPID(float target_angle);
@@ -42,13 +43,16 @@ class FOC_Controlroller {
 	float zero_electric_angle = 0;
 	float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
 
-
+	PIDController vel_loop =
-	PIDController vel_loop = PIDController{0.028f, 0.2f, 1e-4, 1000000,
+		PIDController{0.028f, 0.2f, 1e-4, 1000000, voltage_power_supply}; // 12V
-										   voltage_power_supply}; // 12V
 	PIDController angle_loop = PIDController{20.0f, 100.0f, 0.2f, 10000000,
 											 voltage_power_supply}; // 12V
 	PIDController current_loop =
 		PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
+	PIDController vel_inVelAngloop =
+		PIDController{0.35f, 5.0f,1e-3, 1000000, voltage_power_supply};
+	PIDController ang_inVelAngloop =
+		PIDController{30.0f, 0.0f, 1e-3, 1000000, 999};
 
 	void setPhaseVoltage(float Uq, float Ud);
 	float cal_Iq_Id(float current_a, float current_b, float current_c);

Robot_V2/device/Can.cpp

@@ -716,7 +716,7 @@ void CAN_transmiter::communication_check() {
 			if (first_byte >> 3 & 0x01) {
 				KTH7823.angle_set(0);
 				Angle_kalman.init(0); // 角度滤波
-				Vel_kalman.init(0); // 角度滤波
+				Vel_kalman.init(0);	  // 角度滤波
 			}
 			if (eigth_byte & 0x80 >> 7) {
 				prepare_deep_sleep(true);

Robot_V2/main.cpp

@@ -25,8 +25,8 @@ extern FOC_Controlroller FOC;				// FOC控制
 extern FlashEEPROM flashEEP;				// EEPROM类
 
 TaskScheduler scheduler = {0};
-volatile u32 system_runtime_us = 0;
+volatile u32 system_runtime_us = 0; // 电机驱动计算轮次
-volatile u32 cnt_test = 0;
+volatile u32 cnt_test = 0;			// 负载检测
 int main(void) {
 	systick_config();		 // 系统时钟初始化
 	SystemCoreClockUpdate(); // 更新系统时钟变量
@@ -89,9 +89,8 @@ void do_task(u32 currentTasks) {
 			break;
 		}
 		case 1: {
-			FOC.angle_control(FOC.target_angle); // 角度闭环控制
+			// FOC.angle_control(FOC.target_angle); // 角度闭环控制
-			// FOC.angle_control_with_velocity_limit(FOC.target_angle,
+			FOC.angle_control_with_velocity_limit(FOC.target_angle); // 角度闭环控制，带速度限制
-			// FOC.limit_velocity); // 角度闭环控制，带速度限制
 			break;
 		}
 		case 2: {

Robot_V2/sys/flash.cpp

@@ -146,10 +146,10 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const {
 		u32 data_word = *((u32 *)address);
 		// 检查是否是有效数据（非擦除状态）
 		if (data_word != 0xFFFFFFFF) {
-			u8 addr = (u8)(data_word & 0xFF);
+			u8 addr = (u8)(data_word & DATA_ADDR_MASK);
 			// 找到匹配的虚拟地址
 			if (addr == virtualAddr) {
-				*data = (u16)((data_word >> 16) & 0xFFFF);
+				*data = (u16)(data_word & DATA_U16_MASK);
 				return FLASH_EEPROM_OK;
 			}
 		}
@@ -504,10 +504,7 @@ FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
 			fmc_unlock();
 			fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
 			fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM);
-			// printf("write address:0x%X,status:0x%x \r\n", next_address,
-			// 	   fmc_status);
 			fmc_lock();
-
 #if EEPDEBUG
 			printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
 #endif