SimpleFOC核心代码，解决Id一直为正 无法控为0

        注：（我这个是用simulink仿真做的SimpleFOC，因此代码是m语言的，在stm32可以改成C的，这两种基本差不多，搭建的SimpleFOC仿真下载链接：https://download.csdn.net/download/qq_35239859/88642607?spm=1001.2014.3001.5501 

SimpleFOC核心代码如下：

function [Ta,Tb,Tc] = fcn(Uq,Ud,angle_el)
    Ta=0;Tb=0;Tc=0;
    PI_2 = pi/2;
    PI_3 = pi/3;
    voltage_power_supply=24;
	angle_el = mod(angle_el,2*pi);                    
	
	% U_alpha=Ud*cos(angle_el)-Uq*sin(angle_el);            
	% U_beta=Ud*sin(angle_el)+Uq*cos(angle_el);
	if Ud
	    Uref=sqrt(Uq*Uq + Ud*Ud) / voltage_power_supply;
        angle_el = mod(angle_el+atan2(Uq,Ud),2*pi);
    else
        Uref=abs(Uq) / voltage_power_supply;
    	if Uq>0 
	    angle_el =mod(angle_el+PI_2,2*pi);
	    else
		angle_el =mod(angle_el-PI_2,2*pi);
        end
    end
	if Uref > 0.577  
        Uref = 0.577;
    end

	if Uref<-0.577 
        Uref=-0.577; 
    end
    disp(Uref);


	sector = floor(angle_el / PI_3) + 1;                			                        

	T1 = sqrt(3)*sin(sector*PI_3 - angle_el) * Uref;            
	T2 = sqrt(3)*sin(angle_el - (sector-1.0)*PI_3) * Uref;
	T0 = 1 - T1 - T2;   

	switch(sector) 
		case 1
			Ta = T1 + T2 + T0/2;
			Tb = T2 + T0/2;
			Tc = T0/2;
		case 2
			Ta = T1 +  T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T0/2;
		case 3
			Ta = T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T2 + T0/2;
		case 4
			Ta = T0/2;
			Tb = T1+ T0/2;
			Tc = T1 + T2 + T0/2;
		case 5
			Ta = T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T2 + T0/2;
		case 6
			Ta = T1 + T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T0/2;
    end

         网上很多Stm32移植SimpleFOC的程序，都是做FOC开环的，所以将很多重要的东西都删了，因此要想用SimpleFOC做电流闭环，不能将网上或者视频里的代码直接移植使用，还要注意Ud和Uq电压在核心代码中是怎么处理的，也就是以下几句：

	% U_alpha=Ud*cos(angle_el)-Uq*sin(angle_el);            
	% U_beta=Ud*sin(angle_el)+Uq*cos(angle_el);
	if Ud
	    Uref=sqrt(Uq*Uq + Ud*Ud) / voltage_power_supply;
        angle_el = mod(angle_el+atan2(Uq,Ud),2*pi);
    else
        Uref=abs(Uq) / voltage_power_supply;
    	if Uq>0 
	    angle_el =mod(angle_el+PI_2,2*pi);
	    else
		angle_el =mod(angle_el-PI_2,2*pi);
        end
    end

        这一部分代码，这是根据Ud和Uq算磁场方向的，网上很多教程包括P站，很多都写错了，将这部分代码写成如下方式：

    U_alpha=Ud*cos(angle_el)-Uq*sin(angle_el);            
    U_beta=Ud*sin(angle_el)+Uq*cos(angle_el);
	Uref=sqrt(U_alpha*U_alpha+ U_beta*U_beta) / voltage_power_supply;
    if Uq>0 
	   angle_el =mod(angle_el+PI_2,2*pi);
    else
		angle_el =mod(angle_el-PI_2,2*pi);

        这种方式如果你只是想用开环做FOC控制的话没有问题，如果要做Id和Iq的闭环就不行了，这种写法Ud电压无论正负都只是给Uref电压增加了幅值，做不到磁场矢量根据Uq和Ud的动态调整，因此无法将Id控成0。这也是我在做电流闭环时候，排查很久的坑，如果你也遇到了给定Ud电压正负都一样的效果，看一下程序是不是也是这样写的。