以下是一个简化版的FOC（磁场定向控制，Field-Oriented Control）算法代码示例，用于控制永磁同步电机（PMSM）。代码包含核心逻辑，实际工程中需结合硬件平台优化：

// 1. 读取电机三相电流（Ia, Ib, Ic）和编码器角度
float Ia = read_current_A();  // 实际需通过ADC采样
float Ib = read_current_B();
float Ic = read_current_C();
float theta = read_encoder_angle(); // 电角度

// 2. Clarke变换（3相→2相静止坐标系）
float I_alpha = Ia;
float I_beta = (Ia + 2*Ib) / sqrt(3);

// 3. Park变换（静止坐标系→旋转坐标系）
float sin_theta = sin(theta);
float cos_theta = cos(theta);
float I_d = I_alpha * cos_theta + I_beta * sin_theta;  // 直轴分量
float I_q = -I_alpha * sin_theta + I_beta * cos_theta; // 交轴分量

// 4. PI控制器调节（目标I_d=0，I_q=期望转矩）
float target_I_q = get_target_speed(); // 根据速度环输出
float error_d = 0 - I_d;
float error_q = target_I_q - I_q;

// 更新PI控制器积分项（需做积分限幅）
integral_d += error_d * dt;
integral_q += error_q * dt;

// 计算输出电压
float V_d = Kp_d * error_d + Ki_d * integral_d;
float V_q = Kp_q * error_q + Ki_q * integral_q;

// 5. 逆Park变换
float V_alpha = V_d * cos_theta - V_q * sin_theta;
float V_beta = V_d * sin_theta + V_q * cos_theta;

// 6. SVPWM生成（空间矢量调制）
float U_alpha = V_alpha;
float U_beta = V_beta * (2/sqrt(3)); // 幅值修正

// 计算三相占空比（简化版）
float T_a = U_alpha;
float T_b = (-U_alpha + sqrt(3)*U_beta)/2;
float T_c = (-U_alpha - sqrt(3)*U_beta)/2;

// 7. 更新PWM输出（需根据硬件PWM模块实现）
set_pwm_duty(T_a, T_b, T_c);

关键说明：

1. 硬件依赖：需根据具体MCU实现ADC采样、PWM生成和编码器接口
2. 参数整定：Kp_d, Ki_d 和 Kp_q, Ki_q 需通过调试确定
3. 安全机制：需添加过流保护、死区时间补偿等
4. 扩展功能：
   • 弱磁控制（当电压饱和时）
   • 无传感器控制（需加入观测器）
   • MTPA（最大转矩电流比）控制

典型执行流程：

while(1) {
    读取传感器数据 → 
    Clarke/Park变换 → 
    PI调节 → 
    逆变换 → 
    SVPWM生成 → 
    更新PWM输出
}

如果需要更具体的某部分实现（如SVPWM细节、无传感器算法或代码移植到STM32等平台），可以进一步说明需求。