![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Trapezoid sin Vector
Diff: main.cpp
- Revision:
- 0:fa432f8ea1a6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jun 28 11:52:27 2019 +0000 @@ -0,0 +1,638 @@ +#include "mbed.h" +#include <math.h> + +#include <IO_define.h> +#include <kukei.h> + +void time_UP(){ + ut1=uT.read_us(); + //ut=0; + ui=0; + } +void time_UN(){ + ut2=uT.read_us(); + uT.reset(); + + } +void time_VP(){ + vt1=vT.read_us(); + vi=0; + } +void time_VN(){ + vt2=vT.read_us(); + vT.reset(); + } +void time_WP(){ + wt1=wT.read_us(); + wi=0; + et=0; + } +void time_WN(){ + wt2=wT.read_us(); + wT.reset(); + } + +int main() { + + uT.start(); + vT.start(); + wT.start(); + //uTc.start(); + + PWM_IN1_U.period_us(20);//50 + PWM_IN2_V.period_us(20); + PWM_IN3_W.period_us(20); + + // zt.attach_us(&ztr, tau); + //ztc.attach_us(&ztrc,tau); + + Vr_adc_i=V_adc.read(); + wait_ms(100); + SH=HALL_U<<2|HALL_V<<1|HALL_W; + EN1_U=1; + EN2_V=1; + EN3_W=1; + + while(1) { + + SH=HALL_U<<2|HALL_V<<1|HALL_W; + + Vr_adc=V_adc.read(); + y=Vr_adc- Vr_adc_i; + ay=fabs(y); + power=ay; + if(y>0.1){ + frd=0; + //aout=0.5; + } + if(y<-0.1){ + frd=1; + //aout=0.9; + } + + if(ay<0.1){ + q=0; + aout=0.0; + r=0; + vst=0; + //Speed=0; + //SH=HALL_U<<2|HALL_V<<1|HALL_W; + //power=0.0; + } + /************起動********************/ +if(((y<-0.1)||(y>0.1))&&(q==0)){ +//if(q==0){ + while(q<20){ + SH=HALL_U<<2|HALL_V<<1|HALL_W; + +/*****Forword 起動******/ + if((frd==0)){ + //SH=HALL_U<<2|HALL_V<<1|HALL_W; + if(SH==5){ + start_F5(); + } + if(SH==4){ + start_F4(); + } + if(SH==6){ + start_F6(); + } + if(SH==2){ + start_F2(); + } + if(SH==3){ + start_F3(); + } + if(SH==1){ + start_F1(); + } + } + +/*******Reversal 起動************/ + if((frd==1)){ + //SH=HALL_U<2|HALL_V<<1|HALL_W; + if(SH==3){ + start_R3(); + } + if(SH==2){ + start_R2(); + } + if(SH==6){ + start_R6(); + } + if(SH==4){ + start_R4; + } + if(SH==5){ + start_R5(); + } + if(SH==1){ + start_R1(); + } + } + q++; + } + } + + if(Speed<1000){//2000 + /**************矩形波駆動始動***************/ + /*******Forward******************/ + if(frd==0){ + switch(SH){ + case 5: PWM_W();//W + break; + case 4: EN_W();//W + break; + case 6: PWM_U();//U + break; + case 2: EN_U();//U + break; + case 3: PWM_V();//V + break; + case 1: EN_V();//V + break; + default :PWM_W();//W + break; + } + HALL_Ui.rise(time_WP); + HALL_Wi.fall(time_VN); + HALL_Vi.rise(time_UP); + HALL_Ui.fall(time_WN); + HALL_Wi.rise(time_VP); + HALL_Vi.fall(time_UN); + } +/*******Reversal******************/ + if(frd==1){ + switch(SH){ + case 5: PWM_W(); + break; + case 1: EN_W(); + break; + case 3: PWM_V(); + break; + case 2: EN_V(); + break; + case 6: PWM_U(); + break; + case 4: EN_U(); + break; + default :PWM_W(); + break; + } + HALL_Ui.rise(time_UP); + HALL_Wi.fall(time_WN); + HALL_Vi.rise(time_VP); + HALL_Ui.fall(time_UN); + HALL_Wi.rise(time_WP); + HALL_Vi.fall(time_VN); + } + + }//Speed<2000 + + /*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*fabs(wsi*1E-6))); + usi=1*(ut2-ut1); + vsi=1*(vt2-vt1); + wsi=1*(wt2-wt1); + usic=1*(ut2c-ut1c); + //Speed=60*(1/(6.0*fabs(usi)*1E-6));//CQ 6 + /* if(Vr_adc < 0.08f){ + q=0; + Vr_adc=0; + r=0; + vst=0; + i=0; + } */ + + #if 1 + if(Speed>=1000){ + ui=ui+1; + vi=vi+1; + wi=wi+1; + et=et+1; + + thu=2*PI*(1/(2*usi*1E-6))*ui*3.6E-4; //usi 2.3E-4 vst + thv=2*PI*(1/(2*vsi*1E-6))*vi*3.6E-4; + thw=2*PI*(1/(2*wsi*1E-6))*wi*3.6E-4; + //th=(thu+thv+thw)/3; + if(r==0){ + thave=(thu+thv+thw)/3; + th=thu; + //th=thave; + s=0; + } + if(r==1){ + if((Speed > 1000)&&(Speed <=1500)){ + iso=-0.3; + Vqp=0.3*Vq; //0.2 + Vdlink=0.4; + Edw=3500; + z=0.7; + } + if((Speed > 1500)&&(Speed <=2000)){ + iso=0.7; + Vqp=0.3*Vq; //0.2 + Vdlink=0.45; + Edw=3600; + z=0.7; + } + if((Speed > 2000)&&(Speed <=3000)){ + iso=0.8; + Vqp=0.3*Vq; //0.2 + Vdlink=0.5; + Edw=3700; + z=0.7; + } + if((Speed > 3000)&&(Speed <=4000)){ + iso=1.2; //1.2 + Vqp=0.3*Vq; //0.3 + Vdlink=0.5; //0.5 + Edw=3700; //3700 + z=0.7; + } + if((Speed > 4000)&&(Speed <=5000)){ + iso=1.8; //1.8 + Vqp=0.3*Vq; //0.3 + Vdlink=0.6; //0.6 + Edw=3800; //3800 + z=0.7; + } + if((Speed > 5000)&&(Speed <=5500)){ + iso=2.2; //2.2 + Vqp=0.3*Vq; //0.3 + Vdlink=0.5; //0.7 + Edw=3900; //3900 + z=0.7; //0.66 + } + if((Speed > 5500)&&(Speed <=6000)){ + iso=2.8; //2.2 + Vqp=1.0*Vq; //0.35 + Vdlink=0.5; //0.8 + Edw=6000; //6000 + z=0.8; //0.7 + } + if((Speed > 6000)){ + iso=2.2; //2.2 + Vqp=1.0*Vq; // 0.4 + Vdlink=0.5; //0.9 + Edw=7000;//7000 + z=0.8; //0.8 + } + // th=(Wz*et*1.9E-4)+iso; //1.99E-4 + th=(W*et*1.9E-4)+iso; //1.9E-4 + // th=Wo*wi*1.85E-4; + } + + if(r==0){ + su=sin(thu);//-0.2f; // p51 0.5 + sv=sin(thv);//-0.05f; + sw=sin(thw);//-0.05f;//-0.7 + + PWM_IN1_U.write((su+0.5)*power); //kaisha sv ier su + PWM_IN2_V.write((sv+0.5)*power); //kaisha sw ie sv + PWM_IN3_W.write((sw+0.5)*power); //kaisha su ie sw + } + if(r==1){ + PWM_IN1_U.write(du); // dw ie no white and Black + PWM_IN2_V.write(dv); // du + PWM_IN3_W.write(dw); // dv + } + + + // myled = !myled; + // } +// } + + Curr_u=(Curr_ui);//0.33f; kaisha v w u narabikae + Curr_v=(Curr_vi);///0.33f; + Curr_w=(Curr_wi);///0.33f; + /* iuvw[0]=(Curr_ui); + iuvw[1]=(Curr_vi); + iuvw[2]=(Curr_wi);*/ + float Itau=1.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/2.0))/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/2.0))/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/2.0))/Itau; + iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0; + /*************************************/ + + iab[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet; + iab[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet; + + + idq[0]=cos(th)*iab[0]+sin(th)*iab[1]; //thave + idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]; //th + + + /****Filter Id********/ + idqo[0]=idq[0]; + float Id1,Id2,Id3,Id4;//0.01 + float Idtau= 1.0E-6,Iddt=1.0E-6; //1E-1 + Id1=Iddt*(idqi[0]-idqo[0])/Idtau; + Id2=Iddt*(idqi[0]-(idqo[0]+Id1/2.0))/Idtau; + Id3=Iddt*(idqi[0]-(idqo[0]+Id2/2.0))/Idtau; + Id4=Iddt*(idqi[0]-(idqo[0]+Id3))/Idtau; + idq[0]=idqo[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0; + /*************************************/ + + /****Filter Iq********/ + idqo[1]=idq[1]; + float Iq1,Iq2,Iq3,Iq4;//0.01 + float Iqtau= 1.0E-6,Iqdt=1.0E-6; + Iq1=Iqdt*(idqi[1]-idqo[1])/Iqtau; + Iq2=Iqdt*(idqi[1]-(idqo[1]+Iq1/2.0))/Iqtau; + Iq3=Iqdt*(idqi[1]-(idqo[1]+Iq2/2.0))/Iqtau; + Iq4=Iqdt*(idqi[1]-(idqo[1]+Iq3/2.0))/Iqtau; + idq[1]=idqo[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0; + /*************************************/ + + // Wcom=(2*PI)/(vstf*1E-6); + + + // Vd=(idq[0]);//tmp vdq + // Vq=(idq[1]);//tmp vdq + + /*****PID Id *****/ + Idin=(-0.25)-idq[0]; // -0.25 + float adi,bdi,cdi,workdi[2]; + float kpdi=1.0,kidi=0.5,kddi=0.0; //p51 0.5 1.2 0.0 + float dtdi=1.0E-6;//1E-6 + 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; + /**********************************/ + //Icom=0.5; + /*****PID Iq *****/ + Iqin=Vr_adc-idq[1]; //kaisha 600 ie 500 + float aqi,bqi,cqi,workqi[2]; + float kpqi=2.0,kiqi=1.2,kdqi=0.0; // 1.5 0.8 + float dtqi=1.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; + /**********************************/ + /****Filter Vq********/ + /*float Vq1,Vq2,Vq3,Vq4;//0.01 + float Vqtau= 1.0E-6,Vqdt=1.0E-6; + Vq1=Vqdt*(Vqi-Vq)/Vqtau; + Vq2=Vqdt*(Vqi-(Vq+Vq1/2.0))/Vqtau; + Vq3=Vqdt*(Vqi-(Vq+Vq2/2.0))/Vqtau; + Vq4=Vqdt*(Vqi-(Vq+Vq3/2.0))/Vqtau; + Vq=Vq+(Vq1+2.0*Vq2+2.0*Vq3+Vq4)/6.0;*/ + /*************************************/ + //Vq=Vr_adc; + //if(i<10000){ +//if(r==0){ + /* usi=ut2-ut1; + vsi=vt2-vt1; + wsi=wt2-wt1;*/ + +if(i<10000){ + vst=vsi; + vstf=vst; + } + //} + else { //kokokara else + //if(r==1){ + // usi=vstf;vsi=vstf;wsi=vstf; + vst=vstf; + + i=10000; + // } + // r=i%100; + // i++; + + // Vd=(idq[0]);//tmp vdq + // Vq=(idq[1]);//tmp vdq + + + /* Ed= (Vd)-0.11f*idq[0]+Wz*0.018E-3*idq[1];//0.018 + + if(Ed>=0){ + Wz=(2*PI)/((vst+Edw*Ed*0.2)*1E-6);// 0.2 + } + else{ + Wz=(2*PI)/((vst-Edw*Ed*0.2)*1E-6); //0.45 + } */ + + Wz=(2*PI)/(vst*1E-6); //vst + Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3 + phm=Ed/(Wz); + dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); //0.018E-3 + eth=asin(dth); + + /*****PID θ *****/ + // PLL=(Speed/100); //7 21 28 + //PLL=Speed*2*PI/(60*28); + // PLL=W/Wz; + PLL=1.0; //30 + //float WPLL=150.0; // 170 + float as,bs,cs,works[2]; + float kps=PLL,kis=PLL*PLL/5.0,kds=0.0; + float dts=1.0E-6; + Xsi=5.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.01){ + W=(2*PI)/((vst*1E-6)+(therr/Wz)); + } + if(therr<-0.01){ + W=(2*PI)/((vst*1E-6)-(therr/Wz)); + } + + /*****PID ω *****/ + Xin=5000*(1.05-Vr_adc)-vst; // 5500 + float a,b,c,work[2]; + float kp=2.0,ki=0.5,kd=0.0; // 2.0 0.5 + float dt=10.0E-6;//10E-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; + /**********************************/ + // float dtt=1000.0E-6; + + /*float k1,k2,k3,k4,tau=0.001;//0.01 + k1=dtt*(Xout-vstt)/tau; + k2=dtt*(Xout-(vstt+k1/2.0))/tau; + k3=dtt*(Xout-(vstt+k2/2.0))/tau; + k4=dtt*(Xout-(vstt+k3/2.0))/tau; + vstt=vstt+(k1+2.0*k2+2.0*k3+k4)/6.0;*/ + vstt=Xout; + /********************************/ +if(frd==1){ + if(300>fabs(vstt-vst)){ // ie 2000 + vsti=vstt; + // vsti=vst; + r=1; + } + } +if(frd==0){ + if(300>fabs(vstt-vst)){ // ie 2000 + vsti=vstt; + // vsti=vst; + r=1; + } + } + /****Filter********/ + float vstfo=vstf; + float dttt=10.0E-6;//100E-6 + float k11,k22,k33,k44,tau1=0.1;//0.01 + k11=dttt*(vsti-vstfo)/tau1; + k22=dttt*(vsti-(vstfo+k11/2.0))/tau1; + k33=dttt*(vsti-(vstfo+k22/2.0))/tau1; + k44=dttt*(vsti-(vstfo+k33))/tau1; + vstf=vstfo+(k11+2.0*k22+2.0*k33+k44)/6.0; + //vstf=vsti; + /*************************************/ + + + + }//else kokomade + /* if((Vr_adc > 0.9f)&&(Vr_adc <= 0.95f)){ + vstf =800.0f; + } + if((Vr_adc > 0.95f)&&(Vr_adc < 0.975f)){ + vstf=700.0f; + } + if(Vr_adc >= 0.975f){ + vstf=600.0f; + } */ + + + + Va=cos(th)*Vd-sin(th)*Vqp; //Vqp 負荷小 Vq*0.8~1.0負荷大 + Vb=sin(th)*Vd+cos(th)*Vqp; //Vqp + + aVa=abs(Va); + a3Vb=abs(sq3*Vb); + + if((Va>=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 0 + + d1=sq32*(Va-sq3*Vb)*Vdlink; + d2=sq32*(sq23*Vb)*Vdlink; + d07=z-(d1+d2); + du=d1+d2+d07; + dv=d2+d07; + dw=d07; + } + if((aVa<=sq3*Vb)){ //sect 1 + + d3=sq32*(-Va+sq3*Vb)*Vdlink; + d2=sq32*(Va+sq3*Vb)*Vdlink; + d07=z-(d2+d3); + du=d2+d07; + dv=d2+d3+d07; + dw=d07; + } + + if((Va<=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 2 + + d3=sq32*sq23*Vb*Vdlink; + d4=sq32*(-Va-sq3*Vb)*Vdlink; + d07=z-(d3+d4); + du=d07; + dv=d3+d2+d07; + dw=d4+d07; + } + + if((Va<=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 3 + + d5=-sq32*sq23*Vb*Vdlink; + d4=sq32*(-Va+sq3*Vb)*Vdlink; + d07=z-(d4+d5); + du=d07; + dv=d4+d07; + dw=d4+d5+d07; + } + + if((aVa<=-sq3*Vb)){ //sect 4 + + d5=sq32*(-Va-sq3*Vb)*Vdlink;; + d6=sq32*(Va-sq3*Vb)*Vdlink; + d07=z-(d5+d6); + du=d6+d07; + dv=d07; + dw=d5+d6+d07; + } + + if((Va>=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 5 + + d1=sq32*(Va+sq3*Vb)*Vdlink;; + d6=-sq32*sq23*Vb*Vdlink;; + d07=z-(d1+d6); + du=d1+d6+d07; + dv=d07; + dw=d6+d07; + } + + /* mypwmA.write(du); + mypwmB.write(dv); + mypwmC.write(dw);*/ + + + /* suv=Vab[0]*zet; + svv=(Vab[0]*cos23+Vab[1]*sin23)*zet; + swv=(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(suv); + // SWAVE=iab[0]+0.5; + // SWAVE=Ed/10; + // SWAVE=idq[1]; + // SWAVE=sin(th); + // SWAVE=vst/5000; + //SWAVE=Vb; + // SWAVE=iuvw[0]; + //aout=(su+0.2)*power; + aout=du; + + Vector=r; + i++; + } + #endif + } + +} \ No newline at end of file