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

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

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

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