%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                        電機(jī)仿真模型
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Te, psai, psaia, psaib, w, cur] = motor(cur,Ua1,Ub1,w,h,Tl)               % cur(4)中1，2為isa，isb，3，4為ira，irb
a = zeros(4,4);
curtemp = zeros(1,4);
k = zeros(4,4);

global Lr;
global Rs1;
global M;
global Rr1;
global Ls;
global Rou;
global np;
global J0;

a(1,1) = -Lr*Rs1;     a(1,2) = w*M*M;  a(1,3) = Rr1*M;     a(1,4) = w*Lr*M;     % 公式1
a(2,1) = -w*M*M;  a(2,2) = -Lr*Rs1;    a(2,3) = -w*Lr*M; a(2,4) = Rr1*M;
a(3,1) = M*Rs1;      a(3,2) = -w*Ls*M; a(3,3) = -Rr1*Ls;    a(3,4) = -w*Lr*Ls;
a(4,1) = w*Ls*M;   a(4,2) = Rs1*M;     a(4,3) = w*Lr*Ls;  a(4,4) = -Ls*Rr1;
u(1) = Lr*Ua1;        u(2) = Lr*Ub1; 	u(3) = -M*Ua1;      u(4) = -M*Ub1;

for ii = 1:4;
    for jj = 1:4;
        a(ii,jj) = a(ii,jj)/Rou;
    end;
    u(ii) = u(ii)/Rou;
    curtemp(ii) = cur(ii);
end;

for q = 1:4;
    for i = 1:4;
        k(q,i) = u(i);
        for j=1:4;
            k(q,i) = k(q,i) + a(i,j)* curtemp(j);
        end;
    end;
    if(q <= 2)
        for i = 1:4;
            curtemp(i) = cur(i)+h*k(q,i)/2.0;
        end;
    else if(q == 3)
            for i = 1:4;
                curtemp(i) = cur(i)+h*k(q,i);
            end;
        end;
    end;
end;

for i =1:4;
    % cur[i]=cur[i]+h*(k[0][i]+4.0*k[1][i]+4.0*k[2][i]+k[3][i])/6.0; 
    cur(i)=cur(i)+h*(k(1,i)+2.0*k(2,i)+2.0*k(3,i)+k(4,i))/6.0; % 牛頓積分法  公式7
    Te = np*M*(cur(2)*cur(3)-cur(1)*cur(4));                     % 轉(zhuǎn)矩給定值
    w = w + h*np*(Te-Tl)/J0;                                    % 轉(zhuǎn)速給定值
    psaia = M*cur(1)+ Lr*cur(3);                                 % 定子磁鏈實(shí)際值（a-b坐標(biāo)系下）
    psaib = M*cur(2)+ Lr*cur(4);                                % 定子磁鏈實(shí)際值
    psai = sqrt(psaia*psaia+psaib*psaib);                       % 定子磁鏈實(shí)際幅值
end;