![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
FOC sinewave drive ver2
Diff: main.cpp
- Revision:
- 0:dcfc6150c1d6
- Child:
- 1:e45063ee4be0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Aug 21 01:45:36 2020 +0000 @@ -0,0 +1,626 @@ +#include "mbed.h" +#include "rtos.h" +#define TS1 0.2 +#include <math.h> +int q=0,START=20,i=0,s=0,vector; +float uii=0,ut=0,ut1=0,ut2=0,usi=0; +float vii=0,vt=0,vt1=0,vt2=0,vsi=0; +float wii=0,wt=0,wt1=0,wt2=0,wsi=0; +unsigned int ui=0,vi=0,wi=0,vect=0; +float su=0,sv=0,sw=0,suv=0,svv=0,swv=0; +float uci=0,uc=0; +float vst=0,vstt=0,vsti=0,vstf=0,Edw=0,PI=3.141592; +float Speed; +float w,Vqp; +float dth,phm,W,eth,therr,Em,PLL; +float uvect,vvect,wvect; +float sig=0; +PwmOut mypwmA(PA_8); //PWM_OUT 8 +PwmOut mypwmB(PA_9); //9 +PwmOut mypwmC(PA_10);//10 + +PwmOut Current_Ref(PB_4); + +DigitalOut Vector(PC_4); +DigitalOut EN1(PC_10); +DigitalOut EN2(PC_11); +DigitalOut EN3(PC_12); + +DigitalOut vcl(PC_8); + +InterruptIn HA(PA_15); +InterruptIn HB(PB_3); +InterruptIn HC(PB_10); + +//AnalogIn V_adc(PB_1); //volume +AnalogIn V_adc(PC_2); //Potention + + +AnalogIn Vshuntu(PA_1); +AnalogIn Vshuntv(PA_0); +AnalogIn Vshuntw(PB_0); + +AnalogIn BEMF1(PC_3);//C7_37 +AnalogIn BEMF2(PB_0);//C7_34 +AnalogIn BEMF3(PA_7);//C10_26 + +DigitalIn GPIO_BEMF(PC_9); +DigitalIn CPOUT(PA_12); +AnalogIn Curr_ui(PA_0); +AnalogIn Curr_vi(PC_1); +AnalogIn Curr_wi(PC_0); //PA_1 +Timer uT; +Timer vT; +Timer wT; +AnalogOut SWAVE(PA_4); + +Serial pc(USBTX,USBRX); +DigitalOut myled(LED1); + +float r2=sqrt(2.0f),r3=sqrt(3.0f); +float Cuvw[2][3]={{r2/r3,-1.0f/r2/r3,-1.0f/r2/r3}, + {0,1.0f/r2,-1.0f/r2}}; +float iuvw[3]; +float iab[2],iabi[2],Vab[2]; +float idq[2],idqi[2]; +float thave,th,thu,thv,thw,Ed,Vd,Vq,Vqi,West,Wz,Wo,Icom,Wcom; +float zet=sqrt(2.0f/3.0f),cos23=cos((2.0f/3.0f)*PI); +float cos43=cos((4.0f/3.0f)*PI),sin23=sin((2.0f/3.0f)*PI),sin43=sin((4.0f/3.0f)*PI); +float Curr_u,Curr_v,Curr_w; +float Xin,Xout; +float Idin,Vdout,Iqin,Vqout,Xsi; +int vstctle=0,vstctlz=0; +float Vr_adc=0.0f; +int adc,vtv=0; + +Ticker sinTime; + + void HAH(){ + + ut1=uT.read_us(); + ut=0; + ui=0; + // vect=0; + if(vector==0){ + EN1=1; + EN2=1; + EN3=0; + } + /*if(vector==0){ + mypwmA.write(su*Vr_adc); //0.5 + mypwmB.write(0); + mypwmC.write(0); + }*/ + } + void HAL(){ + + ut2=uT.read_us(); + uT.reset(); + if(Speed<=2500){ + // wait_us(100); + vect=0; //****** + } + + if(vector==0){ + EN1=1; + EN2=0; + EN3=1; + } + } + void HBH(){ + + vt1=vT.read_us(); + vi=0; + if(Speed>2500){ + vect=0; + } + if(vector==0){ + EN1=0; + EN2=1; + EN3=1; + } + /* if(vector==0){ + mypwmA.write(0); + mypwmB.write(sv*Vr_adc); + mypwmC.write(0); + }*/ + } + void HBL(){ + + vt2=vT.read_us(); + // vect=0; + vT.reset(); + if(vector==0){ + EN1=1; + EN2=1; + EN3=0; + } + + } + void HCH(){ + wt1=wT.read_us(); + wi=0; + //vect=0; + if(vector==0){ + EN1=1; + EN2=0; + EN3=1; + } + /* if(vector==0){ + mypwmA.write(0); + mypwmB.write(0); + mypwmC.write(sw*Vr_adc); + }*/ + } + + void HCL(){ + wt2=wT.read_us(); + // vect=0; + wT.reset(); + if(vector==0){ + EN1=0; + EN2=1; + EN3=1; + } + } + + void CPLT(){ + pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc); + } + + void timerTS1(void const*argument){ + CPLT(); + } + +Timer Timer1; +int main() { + + pc.baud(128000); + Timer1.start(); + EN1=1; + EN2=1; + EN3=1; + + mypwmA.period_us(20); //20 + + mypwmB.period_us(20); + + mypwmC.period_us(20); + + Current_Ref.period_us(15); + Current_Ref.write(0.6f); + uT.start(); + vT.start(); + wT.start(); + Timer1.start(); + + RtosTimer RtosTimerTS1(timerTS1); + RtosTimerTS1.start((unsigned int)(TS1*3000)); + Thread::wait(100); + + while(1) { + + /* if(CPOUT==1){ + vect=0; + }*/ + + Vr_adc=V_adc.read(); + +if((Vr_adc>0.08f)&&(q==0)){ + + while(q<10){ + #if 0 + mypwmA.write(0.5f); + mypwmB.write(0); + mypwmC.write(0); + wait_ms(START); + + mypwmA.write(0); + mypwmB.write(0.5f); + mypwmC.write(0.0); + wait_ms(START); + + mypwmA.write(0); + mypwmB.write(0.0); + mypwmC.write(0.5f); + wait_ms(START); +#endif + #if 1 + EN1=1; + EN2=1; + EN3=0; + + mypwmA.write(0.5f); //0.5 + mypwmB.write(0); + mypwmC.write(0); + wait_ms(START); + //wait_ms(START); + + EN1=1; + EN2=0; + EN3=1; + + wait_ms(START); + + EN1=0; + EN2=1; + EN3=1; + + mypwmA.write(0); + mypwmB.write(0.5f); + mypwmC.write(0); + + wait_ms(START); + + EN1=1; + EN2=1; + EN3=0; + + wait_ms(START); + + EN1=1; + EN2=0; + EN3=1; + + mypwmA.write(0); + mypwmB.write(0); + mypwmC.write(0.5f); + + wait_ms(START); + EN1=0; + EN2=1; + EN3=1; + #endif + q++; + + } + } + + /* HA.rise(&HAH); + HC.fall(&HCL); + HB.rise(&HBH); + HA.fall(&HAL); + HC.rise(&HCH); + HB.fall(&HBL);*/ + + HA.rise(&HAH); + HC.fall(&HCL); + HB.rise(&HBH); + HA.fall(&HAL); + HC.rise(&HCH); + HB.fall(&HBL); + + Speed=60*(1/(7.0*2.0*usi*1E-6)); + + if(Vr_adc < 0.05f){ + q=0; + Vr_adc=0; + vector=0; + vst=0; + i=0; + } + + ui=ui+1; + vi=vi+1; + wi=wi+1; + vect++; + /* if(Curr_ui>0.48){ //&&(vtv==0)){ + vect=0; + + vtv=1; + vcl=1; + }*/ + /* if((Curr_ui<=0.4)&&(vtv==0)){ + // vect=0; + vtv=1; + vcl=0; + }*/ + /*if(vect==100000){ + vtv=1; + }*/ + + if(vector==0){ + thu=2*PI*(1/(usi*1E-6))*ui*1.54E-4; //1.24E-4 + thv=2*PI*(1/(vsi*1E-6))*vi*1.54E-4; + thw=2*PI*(1/(wsi*1E-6))*wi*1.54E-4; + + thave=(thu+thv+thw)/3; + th=thu; + s=0; + } + if((vector==1)){ + if((Speed > 500)&&(Speed <=1000)){ + + Vqp=0.6*Vq; + th=W*vect*1.1E-4; + + } + if((Speed > 1000)&&(Speed <=1500)){//1000 + + Vqp=0.6*Vq; //0.5 + th=W*vect*1.1E-4; + + } + if((Speed > 1500)&&(Speed <=2000)){ + + Vqp=0.6*Vq; //0.65 + th=W*vect*1.1E-4; + + } + if((Speed > 2000)&&(Speed <=2500)){ + + Vqp=0.5*Vq; + th=W*vect*1.15E-4; + + } + if((Speed > 2500)&&(Speed <=3000)){ + + Vqp=0.6*Vq; + th=W*vect*1.2E-4; + + } + if((Speed > 3000)&&(Speed <=3500)){ + + Vqp=0.7*Vq; + th=W*vect*1.25E-4; + + } + if((Speed > 3500)&&(Speed <=4000)){ + + Vqp=0.8*Vq; + th=W*vect*1.3E-4; + + } + if((Speed > 4000)&&(Speed <=4500)){ + + Vqp=0.9*Vq; + th=W*vect*1.35E-4; + + } + if((Speed > 4500)&&(Speed <=5000)){ + + Vqp=1.0*Vq; + th=W*vect*1.4E-4; + + } + if((Speed > 5000)&&(Speed <= 6000)){ + + Vqp=1.2*Vq; + th=W*vect*1.45E-4; + + } + if((Speed > 6000)){ + + Vqp=1.3*Vq; + th=W*vect*1.5E-4; + + } + + // th=(W*((vst*1.0E-6))); //1.0E-4 @HAL + th=W*vect*0.75E-4; + } + + if(vector==0){ + su=sin(thu); + sv=sin(thv); + sw=sin(thw); + + mypwmA.write(su*Vr_adc); + mypwmB.write(sv*Vr_adc); + mypwmC.write(sw*Vr_adc); + } + if((vector==1)){ + EN1=1; + EN2=1; + EN3=1; + mypwmA.write(uvect); + mypwmB.write(vvect); + mypwmC.write(wvect); + } + + myled = !myled; + + Curr_u=(Curr_ui); + Curr_v=(Curr_vi); + Curr_w=(Curr_wi); + + float Itau=10.0E-6,Idt=1.0E-6; // Itau=1.0E-6,Idt=1.0E-6 + /****Filter Iu********/ + float Iu1,Iu2,Iu3,Iu4;//0.01 + Iu1=Idt*(Curr_u-iuvw[0])/Itau; + Iu2=Idt*(Curr_u-(iuvw[0]+Iu1/2.0))/Itau; + Iu3=Idt*(Curr_u-(iuvw[0]+Iu2/2.0))/Itau; + Iu4=Idt*(Curr_u-(iuvw[0]+Iu3))/Itau; + iuvw[0]=iuvw[0]+(Iu1+2.0*Iu2+2.0*Iu3+Iu4)/6.0; + /*************************************/ + /****Filter Iv********/ + float Iv1,Iv2,Iv3,Iv4;//0.01 + Iv1=Idt*(Curr_v-iuvw[1])/Itau; + Iv2=Idt*(Curr_v-(iuvw[1]+Iv1/2.0))/Itau; + Iv3=Idt*(Curr_v-(iuvw[1]+Iv2/2.0))/Itau; + Iv4=Idt*(Curr_v-(iuvw[1]+Iv3))/Itau; + iuvw[1]=iuvw[1]+(Iv1+2.0*Iv2+2.0*Iv3+Iv4)/6.0; + /*************************************/ + /****Filter Iw********/ + float Iw1,Iw2,Iw3,Iw4;//0.01 + Iw1=Idt*(Curr_w-iuvw[2])/Itau; + Iw2=Idt*(Curr_w-(iuvw[2]+Iw1/2.0))/Itau; + Iw3=Idt*(Curr_w-(iuvw[2]+Iw2/2.0))/Itau; + Iw4=Idt*(Curr_w-(iuvw[2]+Iw3))/Itau; + iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0; + /*************************************/ + + iabi[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet; + iabi[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet; + /****Filter Ia********/ + float Ia1,Ia2,Ia3,Ia4; + float Iatau= 3.0E-6,Iadt=1.0E-6; //2.0E-4 1.0E-6 + Ia1=Iadt*(iabi[0]-iab[0])/Iatau; + Ia2=Iadt*(iabi[0]-(iab[0]+Ia1/2.0))/Iatau; + Ia3=Iadt*(iabi[0]-(iab[0]+Ia2/2.0))/Iatau; + Ia4=Iadt*(iabi[0]-(iab[0]+Ia3))/Iatau; + iab[0]=iab[0]+(Ia1+2.0*Ia2+2.0*Ia3+Ia4)/6.0; + /*************************************/ + + /****Filter Ib********/ + float Ib1,Ib2,Ib3,Ib4;//0.01 + float Ibtau= 3.0E-6,Ibdt=1.0E-6; //2.0E-2 1.0E-6 + Ib1=Ibdt*(iabi[1]-iab[1])/Ibtau; + Ib2=Ibdt*(iabi[1]-(iab[1]+Ib1/2.0))/Ibtau; + Ib3=Ibdt*(iabi[1]-(iab[1]+Ib2/2.0))/Ibtau; + Ib4=Ibdt*(iabi[1]-(iab[1]+Ib3))/Ibtau; + iab[1]=iab[1]+(Ib1+2.0*Ib2+2.0*Ib3+Ib4)/6.0; + /*************************************/ + + idq[0]=cos(th)*iab[0]+sin(th)*iab[1]; + idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]; + + /****Filter Id********/ + /* float Id1,Id2,Id3,Id4; + float Idtau= 2.0E-4,Iddt=1.0E-6; + Id1=Iddt*(idqi[0]-idq[0])/Idtau; + Id2=Iddt*(idqi[0]-(idq[0]+Id1/2.0))/Idtau; + Id3=Iddt*(idqi[0]-(idq[0]+Id2/2.0))/Idtau; + Id4=Iddt*(idqi[0]-(idq[0]+Id3))/Idtau; + idq[0]=idq[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0;*/ + /*************************************/ + + /****Filter Iq********/ + /* float Iq1,Iq2,Iq3,Iq4;//0.01 + float Iqtau= 2.0E-4,Iqdt=1.0E-6; //2.0E-2 1.0E-6 + Iq1=Iqdt*(idqi[1]-idq[1])/Iqtau; + Iq2=Iqdt*(idqi[1]-(idq[1]+Iq1/2.0))/Iqtau; + Iq3=Iqdt*(idqi[1]-(idq[1]+Iq2/2.0))/Iqtau; + Iq4=Iqdt*(idqi[1]-(idq[1]+Iq3))/Iqtau; + idq[1]=idq[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0;*/ + /*************************************/ + + /*****PID Id *****/ + Idin=0.1-idq[0]; // -0.2 + float adi,bdi,cdi,workdi[2]; + float kpdi=3.0,kidi=1.5,kddi=0.0; //0.3 0.1 0.0 + float dtdi=10.0E-6;//1E-5 + adi=Idin; + bdi=workdi[1]+(Idin+workdi[0])/2.0*dtdi; + cdi=(Idin-workdi[0])/dtdi; + workdi[0]=Idin; + workdi[1]=bdi; + Vd=adi*kpdi+bdi*kidi+cdi*kddi; + /**********************************/ + + /*****PID Iq *****/ + Iqin=(Vr_adc)-idq[1]; //kaisha 600 ie 500 + float aqi,bqi,cqi,workqi[2]; + float kpqi=2.0,kiqi=0.7,kdqi=0.0; // 1.0 0.7 + float dtqi=10.0E-6;//1E-2 + aqi=Iqin; + bqi=workqi[1]+(Iqin+workqi[0])/2.0*dtqi; + cqi=(Iqin-workqi[0])/dtqi; + workqi[0]=Iqin; + workdi[1]=bqi; + Vq=(aqi*kpqi+bqi*kiqi+cqi*kdqi); + /***************************************/ + usi=ut2-ut1; + vsi=vt2-vt1; + wsi=wt2-wt1; + +if(i<10000){ + vst=vsi; + vstf=vst; + } + else { + + + + vst=vstf; + i=10000; + + Wz=(2*PI)/(vst*1E-6); //vst + Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3 0.11 + phm=Ed/(Wz); + dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); // Vd 0.018E-3 + eth=asin(dth); + + /*****PID θ *****/ + //PLL + float as,bs,cs,works[2]; + float kps=5.0,kis=2.0,kds=0.0; + float dts=1.0E-3;//1.0E-6 + Xsi=0.0-eth; + as=Xsi; + bs=works[1]+(Xsi+works[0])/2.0*dts; + cs=(Xsi-works[0])/dts; + works[0]=Xsi; + works[1]=bs; + therr=as*kps+bs*kis+cs*kds; + /*******PLL W ***********/ + if(therr>0.2){ //0.01 + W=(2*PI)/((vst*1E-6)+(therr/Wz)); //+ + } + if(therr<-0.01){ //0.01 + W=(2*PI)/((vst*1E-6)-(therr/Wz)); //- + } + /*****PID ω *****/ + Xin=6500*(1.25-Vr_adc)-vst; // 6500 1.15 + float a,b,c,work[2]; + float kp=1.1,ki=0.7,kd=0.0; // 1.2 0.7 0.0 + float dt=10.0E-6;//100E-6 + a=Xin; + b=work[1]+(Xin+work[0])/2.0*dt; + c=(Xin-work[0])/dt; + work[0]=Xin; + work[1]=b; + Xout=a*kp+b*ki+c*kd; + vstt=Xout; + /********************************/ + if(2000>abs(vst-vstt)){ + vsti=vstt; //vstt + vector=1; + } + else{ + vsti=vst; + vector=0; + } + /****Filter********/ + float dttt=100.0E-6;//100E-6 + float k11,k22,k33,k44,tau1=0.01;//0.01 + k11=dttt*(vsti-vstf)/tau1; + k22=dttt*(vsti-(vstf+k11/2.0))/tau1; + k33=dttt*(vsti-(vstf+k22/2.0))/tau1; + k44=dttt*(vsti-(vstf+k33/2.0))/tau1; + vstf=vstf+(k11+2.0*k22+2.0*k33+k44)/6.0; + /*************************************/ + + }//else kokomade + + + Vab[0]=cos(th)*Vd-sin(th)*Vqp;//Vqp + Vab[1]=sin(th)*Vd+cos(th)*Vqp; + + + uvect=(Vab[0]*zet); + vvect=((Vab[0]*cos23+Vab[1]*sin23)*zet); + wvect=((Vab[0]*cos43+Vab[1]*sin43)*zet); + + // SWAVE=Vd; + // SWAVE=th/4; + // SWAVE=Vq; + // SWAVE=West/5000; + //SWAVE=Wo/5000; + //SWAVE=Wz/5000; + // SWAVE=sin(svv); + // SWAVE=iab[0]+0.5; + //SWAVE=Ed; + // SWAVE=therr+0.5; + //SWAVE=idq[1]; + // SWAVE=Curr_v; + // SWAVE=iuvw[1]; + // SWAVE=vst/5000; + //SWAVE=Vab[1]; + SWAVE=iuvw[0]; + //SWAVE=vvect; + //SWAVE=BEMF1; + Vector=vector; + i++; + } +}