cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

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

同时修正了开环速度控制中的电压计算公式，
将固定电压改为基于目标速度的约束电压，
并调整了电机控制频率配置。
This commit is contained in:
cbb
2026-05-16 21:22:14 +08:00
parent c6e00e1a49
commit d6f784db60
2 changed files with 10 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+7 -6
View File
@@ -204,9 +204,9 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
magnitude = voltage_power_supply * limit_percent; 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; 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 T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l);
float T2 = modulation_index * sin_l; float T2 = modulation_index * sin_l;
float T0 = 1.0f - T1 - T2; float T0 = 1.0f - T1 - T2;
@@ -410,17 +410,18 @@ void FOC_Controller::fast_voltage_check(void) {
*/ */
float FOC_Controller::velocity_open_loop() { float FOC_Controller::velocity_open_loop() {
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 / shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO/
MOTOR_FREQUENCY / 2, MOTOR_FREQUENCY,
360.0f); 360.0f);
// 获取电机轴角度 // 获取电机轴角度
float shaft_angle = KTH7823.getcurrent_sensorangle(); // float shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle; CAN0_transmiter.testfloat_union.f = shaft_angle;
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W')); sampler.get_current('W'));
// 因为在开环控制中，Ud始终为0，只控制Uq // 因为在开环控制中，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); setSVPWM_PhaseVoltage(Uq, 0.0f);
return limit_voltage; return limit_voltage;
} }
Robot_V2/sys/config.h
+3 -3
View File
@@ -223,7 +223,7 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
// TIMER2-任务调度 // TIMER2-任务调度
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us 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 = constexpr float TIME_SLICE =
(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期 (TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE; // 频率 constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE; // 频率
@@ -231,10 +231,10 @@ constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
// TIMER1-PWM // TIMER1-PWM
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为 120MHz 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_DEFAULT_PULSE = 0;
constexpr int TIMER1_CH3_COMPARE_VALUE = 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周期）： // 快速检测层（fast_voltage_check，100us周期）：