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