fix(algorithm): 修改FOC控制器中的角度计算和校准参数

修正了FOC控制器中电气角度计算的符号错误（减法改为加法），
移除了不必要的空行，并优化了velocity_open_loop函数中的注释
格式。同时修正了角度控制中PID计算的目标角度差值顺序。

BREAKING CHANGE: 角度校准参数从硬编码值改为配置常量。

Robot_V2/Algorithm/FOC.cpp

@@ -64,7 +64,7 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 	current_c -= current_avg;
 
 	// 计算电气角度
-	float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
+	float angle_el = fmod(shaft_angle * PP + KTH7823.calibration_angle, 360.0f);
 
 	// Clarke变换 (使用三相电流)
 	float I_alpha, I_beta;
@@ -249,7 +249,6 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
 		duty_c = 0.5f;
 		break;
 	}
-
 	// 设置PWM输出
 	Set_3Pwm(duty_a, duty_b, duty_c);
 }
@@ -409,17 +408,14 @@ void FOC_Controller::fast_voltage_check(void) {
  * @return float: q轴电压
  */
 float FOC_Controller::velocity_open_loop() {
-	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
-	shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO/
-										 MOTOR_FREQUENCY,
-					   360.0f);
-	// 获取电机轴角度
-	// float shaft_angle = KTH7823.getcurrent_sensorangle();
-	CAN0_transmiter.testfloat_union.f = shaft_angle;
+	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中
+	shaft_angle =
+		fmod(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY,
+			 360.0f);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
 						   sampler.get_current('W'));
 
-	// 因为在开环控制中，Ud始终为0，只控制Uq
+	// 因为在开环控制中，Ud 始终为 0，只控制 Uq
 	const float Uq = _constrain(target_velocity * REDUCTION_RATIO * 0.005f,
 								2.0f, voltage_power_supply * 0.5f);
 	setSVPWM_PhaseVoltage(Uq, 0.0f);
@@ -434,7 +430,7 @@ float FOC_Controller::velocity_open_loop() {
  */
 float FOC_Controller::angle_control() {
 	// PID 控制计算
-	float Uq = angle_loop(rt_angle - target_angle);
+	float Uq = angle_loop(target_angle - rt_angle);
 
 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();