%輸入變量ud,uq為隨時間變化的量
%Lq,Ld,Rs,Lmq,Lmd,Rqr,Rdr,Lqr,Ldr
%faif,E0
%J,T2
clear;
%1,輸入變量
%ud=u*sin(2*pi*f*t+2*pi*p*nr/60*t);
%uq=u*cos(2*pi*f*t+2*pi*p*nr/60*t);
p=3%極對數
Lq=0.010389
Ld=Lq;
Rs=0.114
Lmq=0.009403
Lmd=0.006924
Lqr=0.01091
Ldr=0.008432
Rdr=0.103
Rqr=0.103
E0=211.1
faif=0.0169
J=0.4593
uf=220*sqrt(3);
T2=10;
Q=Lq-(Lmq).^2/Lqr;
D=Ld-(Lmd).^2/Ldr;
Qr1=Lmq-Lq*Lqr/Lmq;
Qr2=Lmq.^2/Lq-Lqr;
Dr1=Lmd-Ldr*Ld/Lmd;
Dr2=Lmd.^2/Ld-Ldr;

%2,計算起動
iq=0;
id=0;
iqr=0;
idr=0;
omig=0;
h=0.001
t0=0;
ts=2;
sita=2*pi*50*0;
nr=0;
k=0;
for t=t0:h:ts
    k=k+1;
    ud=-uf*sin(sita);
    uq=uf*cos(sita);
    k11=1/Q*(uq-Rs*iq+Lmq/Lqr*Rqr*iqr-omig*(Ld*id+Lmd*idr+faif));
    k12=1/D*(ud-Rs*id+Lmd/Ldr*Rdr*idr+omig*(Lq*iq+Lmq*iqr));
    k13=1/Qr1*(uq-Rs*iq-omig*(Ld*id+Lmd*idr+faif))+1/Qr2*Rqr*iqr;
    k14=1/Dr1*(ud-Rs*id+omig*(Lq*iq+Lmq*iqr))+1/Dr2*Rdr*idr;
    k15=2*pi*50-2*pi*p*nr/60;
    Te=p*(faif*iq+Lmd*idr*iq-Lmq*iqr*id+(Ld-Lq)*id*iq);
    T=Te-T2;
    %if T<=0
    %    T=0;
    %end
    k16=p*T/J;
    iq1=iq+k11*h/2;
    id1=id+k12*h/2;
    iqr1=iqr+k13*h/2;
    idr1=idr+k14*h/2;
    sita1=sita+k15*h/2;
    omig1=omig+k16*h/2;
    
    %loge-kuta part2
    ud=-uf*sin(sita1);
    uq=uf*cos(sita1);
    k21=1/Q*(uq-Rs*iq1+Lmq/Lqr*Rqr*iqr1-omig1*(Ld*id1+Lmd*idr1+faif));
    k22=1/D*(ud-Rs*id1+Lmd/Ldr*Rdr*idr1+omig1*(Lq*iq1+Lmq*iqr1));
    k23=1/Qr1*(uq-Rs*iq1-omig1*(Ld*id1+Lmd*idr1+faif))+1/Qr2*Rqr*iqr1;
    k24=1/Dr1*(ud-Rs*id1+omig1*(Lq*iq1+Lmq*iqr1))+1/Dr2*Rdr*idr1;
    k25=2*pi*50-omig1;
    Te=p*(faif*iq1+Lmd*idr1*iq1-Lmq*iqr1*id1+(Ld-Lq)*id1*iq1);
    T=Te-T2;
    %if T<=0;
    %    T=0;
    %end
    k26=p*T/J;
    iq2=iq+k21*h/2;
    id2=id+k22*h/2;
    iqr2=iqr+k23*h/2;
    idr2=idr+k24*h/2;
    sita2=sita+k25*h/2;
    omig2=omig+k26*h/2;
    
    %longe-kuta part3
    ud=-uf*sin(sita2);
    uq=uf*cos(sita2);
    k31=1/Q*(uq-Rs*iq2+Lmq/Lqr*Rqr*iqr2-omig2*(Ld*id2+Lmd*idr2+faif));
    k32=1/D*(ud-Rs*id2+Lmd/Ldr*Rdr*idr2+omig2*(Lq*iq2+Lmq*iqr2));
    k33=1/Qr1*(uq-Rs*iq2-omig2*(Ld*id2+Lmd*idr2+faif))+1/Qr2*Rqr*iqr2;
    k34=1/Dr1*(ud-Rs*id2+omig2*(Lq*iq2+Lmq*iqr2))+1/Dr2*Rdr*idr2;
    k35=2*pi*50-omig2;
    Te=p*(faif*iq2+Lmd*idr2*iq2-Lmq*iqr2*id2+(Ld-Lq)*id2*iq2);
    T=Te-T2;
    %if T<=0;
    %    T=0;
    %end
    k36=p*T/J;
    iq3=iq+k31*h;
    id3=id+k32*h;
    iqr3=iqr+k33*h;
    idr3=idr+k34*h;
    sita3=sita+k35*h;
    omig3=omig+k36*h;
    
    %longe-kuta part4
    ud=-uf*sin(sita3);
    uq=uf*cos(sita3);
    k41=1/Q*(uq-Rs*iq3+Lmq/Lqr*Rqr*iqr3-omig3*(Ld*id3+Lmd*idr3+faif));
    k42=1/D*(ud-Rs*id3+Lmd/Ldr*Rdr*idr3+omig3*(Lq*iq3+Lmq*iqr3));
    k43=1/Qr1*(uq-Rs*iq3-omig3*(Ld*id3+Lmd*idr3+faif))+1/Qr2*Rqr*iqr3;
    k44=1/Dr1*(ud-Rs*id3+omig3*(Lq*iq3+Lmq*iqr3))+1/Dr2*Rdr*idr3;
    k45=2*pi*50-omig3;
    Te=p*(faif*iq3+Lmd*idr3*iq3-Lmq*iqr3*id3+(Ld-Lq)*id3*iq3);
    T=Te-T2;
    %if T<=0;
    %    T=0;
    %end
    k46=p*T/J;
    iq=iq+(k11+2*k21+2*k31+k41)*h/6;
    id=id+(k12+2*k22+2*k32+k42)*h/6;
    iqr=iqr+(k13+2*k23+2*k33+k43)*h/6;
    idr=idr+(k14+2*k24+2*k34+k44)*h/6;
    sita=sita+(k15+2*k25+2*k35+k45)*h/6;
    omig=omig+(k16+2*k26+2*k36+k46)*h/6;
    nr=omig*30/pi/p;
    Te=p*(faif*iq+Lmd*idr*iq-Lmq*iqr*id+(Ld-Lq)*id*iq);
    
    hua_iq(k)=iq;
    hua_id(k)=id;
    hua_iqr(k)=iqr;
    hua_Te(k)=Te;
    hua_T(k)=T;
    hua_dd(k)=sita;
    hua_omig(k)=omig;
    hua_si(k)=sita;
    hua_nr(k)=nr;
    hua_t(k)=t;
    if k==1
        hua_sita(k)=0;
    else
        hua_sita(k)=hua_sita(k-1)+omig*h;
    end
    ud1(k)=ud;
    uq1(k)=uq;
end
figure(1);
plot(hua_t,hua_nr,'r-');
xlabel('時  間  (s)');
ylabel('電機轉速(r/min)');
grid;
figure(2);
plot(hua_t,hua_Te);
xlabel('時  間  (s)');
ylabel('電機轉矩(N/m)');
grid;

for i=1:1001
    ua(i)=sqrt(2)/sqrt(3)*(ud1(i)*cos(hua_sita(i))-uq1(i)*sin(hua_sita(i)));
    ub(i)=sqrt(2)/sqrt(3)*(ud1(i)*cos(hua_sita(i)-2*pi/3)-uq1(i)*sin(hua_sita(i)-2*pi/3));
    uc(i)=sqrt(2)/sqrt(3)*(ud1(i)*cos(hua_sita(i)+2*pi/3)-uq1(i)*sin(hua_sita(i)+2*pi/3));
end

figure(3);
subplot(3,1,1),plot(ua);
ylabel ('相電壓  （Ua）');
subplot(3,1,2),plot(ub);
ylabel ('相電壓  （Ua）');
subplot(3,1,3),plot(uc);
ylabel ('相電壓  （Ua）');
xlabel('轉速');

for i=1:1001
    ia(i)=sqrt(2)/sqrt(3)*(hua_id(i)*cos(hua_sita(i))-hua_iq(i)*sin(hua_sita(i)));
    ib(i)=sqrt(2)/sqrt(3)*(hua_id(i)*cos(hua_sita(i)-2*pi/3)-hua_iq(i)*sin(hua_sita(i)-2*pi/3));
    ic(i)=sqrt(2)/sqrt(3)*(hua_id(i)*cos(hua_sita(i)+2*pi/3)-hua_iq(i)*sin(hua_sita(i)+2*pi/3));
end

figure(4);
subplot(3,1,1),plot(ia);
ylabel('電 流 (ia)');
subplot(3,1,2),plot(ib);
ylabel('電 流 (ib)');
subplot(3,1,3),plot(ic);
xlabel('轉  速 ');
ylabel('電 流 (ic)');
figure(5)
subplot(2,1,1),plot(ud1);
ylabel('ud1 (v)');
subplot(2,1,2),plot(uq1);
ylabel('ud2 (v)');
xlabel('轉  速');