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();

Robot_V2/device/ang_sensor.h

@@ -14,7 +14,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
 
 class angle_sensor {
   public:
-	float calibration_angle = 195.0f;
+	float calibration_angle = ANGLESENS_CALIBRATION_ANGLE;
 	u8 init(void);
 	float read_sensorAngle(void);
 	float update(void);

Robot_V2/sys/config.h

@@ -89,7 +89,8 @@ constexpr float DIR = 1.0f;  // 默认方向：-1.0f 反转，1.0f 正转
 constexpr u16 REDUCTION_RATIO = 25;
 constexpr float CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移
 constexpr float HOLD_PWM_DUTY = 0.1f;
-constexpr u16 ANGLESENS_OFFSET = +15;
+constexpr u16 ANGLESENS_OFFSET = 0;
+constexpr float ANGLESENS_CALIBRATION_ANGLE = 83.0f;
 constexpr float KE_FACTOR = 0.0f;
 
 // 角度卡尔曼滤波器参数