![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
FOC sinewave drive ver2
main.cpp
- Committer:
- oguro
- Date:
- 2020-08-21
- Revision:
- 0:dcfc6150c1d6
- Child:
- 1:e45063ee4be0
File content as of revision 0:dcfc6150c1d6:
#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++; } }