fix(FOC): 修复SVPWM相电压计算中的角度单位转换错误

SVPWM相电压计算中，arm_sin_cos_f32函数期望接收度数而非弧度，
需要将local_angle乘以RAD2DEG进行单位转换。

同时修正了开环速度控制中的电压计算公式，
将固定电压改为基于目标速度的约束电压，
并调整了电机控制频率配置。

Robot_V2/Algorithm/FOC.cpp

@@ -204,9 +204,9 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
 		magnitude = voltage_power_supply * limit_percent;
 	}
 
-	// 使用arm_sin_cos_f32计算local_angle的sin和cos
+	// local_angle 是弧度，arm_sin_cos_f32 期望度数，需转换
 	float sin_l, cos_l;
-	arm_sin_cos_f32(local_angle, &sin_l, &cos_l);
+	arm_sin_cos_f32(local_angle * RAD2DEG, &sin_l, &cos_l);
 	float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
 	float T2 = modulation_index * sin_l;
 	float T0 = 1.0f - T1 - T2;
@@ -410,17 +410,18 @@ void FOC_Controller::fast_voltage_check(void) {
  */
 float FOC_Controller::velocity_open_loop() {
 	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
-	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
-										 MOTOR_FREQUENCY / 2,
+	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO/
+										 MOTOR_FREQUENCY,
 					   360.0f);
 	// 获取电机轴角度
-	float shaft_angle = KTH7823.getcurrent_sensorangle();
+	// 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;
+	const float Uq = _constrain(target_velocity * REDUCTION_RATIO * 0.005f,
+								2.0f, voltage_power_supply * 0.5f);
 	setSVPWM_PhaseVoltage(Uq, 0.0f);
 	return limit_voltage;
 }

Robot_V2/sys/config.h

@@ -223,7 +223,7 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
 // TIMER2-任务调度
 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
 constexpr int TIMER2_PERIOD = 49;	  // 1 / 1MHz * 50 = 50us
-constexpr int MOTOR_PERIOD = 2;		  // 100us/50us = 2
+constexpr int MOTOR_PERIOD = 1;		  // 控制每50us执行一次，无分频
 constexpr float TIME_SLICE =
 	(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
 constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE;				 // 频率
@@ -231,10 +231,10 @@ constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
 
 // TIMER1-PWM
 constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为 120MHz
-constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
+constexpr int TIMER1_PERIOD = 2999; // 120MHz / (0+1) / (2999+1) = 40kHz计数, 中心对齐实际PWM 20kHz
 constexpr int TIMER1_DEFAULT_PULSE = 0;
 constexpr int TIMER1_CH3_COMPARE_VALUE =
-	(TIMER1_PERIOD + 1) * 0.41 - 1; // PWM 周期 50% 位置触发中断
+	(TIMER1_PERIOD + 1) * 0.41 - 1; // PWM 周期 41% 位置触发ADC采样
 
 // 电压检测参数（基于跌幅检测，不依赖固定额定电压）
 // 快速检测层（fast_voltage_check，100us周期）：