From d6f784db604ab7114622f4a703fd284a395eec95 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sat, 16 May 2026 21:22:14 +0800 Subject: [PATCH] =?UTF-8?q?fix(FOC):=20=E4=BF=AE=E5=A4=8DSVPWM=E7=9B=B8?= =?UTF-8?q?=E7=94=B5=E5=8E=8B=E8=AE=A1=E7=AE=97=E4=B8=AD=E7=9A=84=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E5=8D=95=E4=BD=8D=E8=BD=AC=E6=8D=A2=E9=94=99=E8=AF=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit SVPWM相电压计算中，arm_sin_cos_f32函数期望接收度数而非弧度， 需要将local_angle乘以RAD2DEG进行单位转换。 同时修正了开环速度控制中的电压计算公式， 将固定电压改为基于目标速度的约束电压， 并调整了电机控制频率配置。 --- Robot_V2/Algorithm/FOC.cpp | 13 +++++++------ Robot_V2/sys/config.h | 6 +++--- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 2d4e6c2..4c8a5e0 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/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; } diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index 7e17ba6..d15eabc 100644 --- a/Robot_V2/sys/config.h +++ b/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周期）：