akiyoshi oguro
Vector SVPWM Drive SPMSM
Diff: main.cpp
- Revision:
- 0:866aafac0128
- Child:
- 1:74d7a989b741
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 02 04:38:25 2019 +0000
@@ -0,0 +1,586 @@
+ #include "mbed.h"
+#include "rtos.h"
+#define TS1 0.2
+#include <math.h>
+int q=0,START=10,i=0,r=0,s=0;
+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,et=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 Wnon,Ednon,W,PLL;
+float uti,vti,wti;
+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); // Gaibu Potention
+
+
+/*AnalogIn Vshuntu(PA_0);
+AnalogIn Vshuntv(PC_1);
+AnalogIn Vshuntw(PC_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],Vab[2];
+float idq[2],idqi[2],idqo[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,iso=0,Vqp;;
+int adc,vtv=1;
+
+float therr,dth,eth,phm;
+float sq32=sqrt(3.0f/2.0f);
+float sq23=2.0f/sqrt(3.0f);
+float sq3=1.0f/sqrt(3.0f);
+float Vdlink=0.6; //3.0f/5.0f; 0.45
+float aVa;
+float a3Vb;
+
+float z=0.5;//0.7
+float Va,Vb;
+float d1,d2,d3,d4,d5,d6,d07;
+float du,dv,dw;
+bool cu,cv,cw;
+
+Ticker sinTime;
+
+ void HAH(){
+ ut1=uT.read_us();
+ ut=0;
+ ui=0;
+
+ }
+ void HAL(){
+ ut2=uT.read_us();
+ uT.reset();
+ ui=0;
+
+ }
+ void HBH(){
+
+ vt1=vT.read_us();
+ vi=0;
+
+ }
+ void HBL(){
+ vt2=vT.read_us();
+ vT.reset();
+
+ }
+ void HCH(){
+ wt1=wT.read_us();
+ wi=0;
+ et=0;
+
+ }
+
+ void HCL(){
+ wt2=wT.read_us();
+ wT.reset();
+
+ }
+
+ 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(50);
+ Current_Ref.write(0.55);
+ uT.start();
+ vT.start();
+ wT.start();
+
+ RtosTimer RtosTimerTS1(timerTS1);
+ RtosTimerTS1.start((unsigned int)(TS1*5000)); //3000
+ Thread::wait(100);
+
+ while(1) {
+
+ Vr_adc=V_adc.read();
+
+if((Vr_adc>0.15f)&&(q==0)){
+
+ while(q<50){
+
+ 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);
+
+
+ q++;
+
+ }
+ }
+ //i++;
+
+ 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.08f){
+ q=0;
+ Vr_adc=0;
+ r=0;
+ vst=0;
+ i=0;
+ }
+
+
+ ui=ui+1;
+ vi=vi+1;
+ wi=wi+1;
+ et=et+1;
+
+ uti=fabs(ut1-ut2)/float(ui)*1E-6;
+ vti=fabs(vt1-vt2)/float(vi)*1E-6;
+ wti=fabs(wt1-wt2)/float(wi)*1E-6;
+ //pc.printf("%.3f %.3f %.3f \r\n", uti, vti, wti);
+ //pc.printf(" %d \r\n",ui);
+ thu=2*PI*(1/(2*usi*1E-6))*ui*1.3E-4; //usi 1.3E-4 vst
+ thv=2*PI*(1/(2*vsi*1E-6))*vi*1.3E-4;
+ thw=2*PI*(1/(2*wsi*1E-6))*wi*1.3E-4;
+
+ if(r==0){
+ thave=(thu+thv+thw)/3;
+ th=thu;
+
+ s=0;
+ }
+ if(r==1){
+
+ if((Speed > 1000)&&(Speed <=1500)){
+ iso=-0.3;
+ Vqp=0.3*Vq;
+ Vdlink=0.3;
+
+ z=0.7;
+ }
+ if((Speed > 1500)&&(Speed <=2000)){
+ iso=0.7;
+ Vqp=0.3*Vq;
+ Vdlink=0.3;
+
+ z=0.7;
+ }
+ if((Speed > 2000)&&(Speed <=3000)){
+ iso=0.8;
+ Vqp=0.3*Vq;
+ Vdlink=0.35;
+
+ z=0.7;
+ }
+ if((Speed > 3000)&&(Speed <=4000)){
+ iso=1.2;
+ Vqp=0.4*Vq;
+ Vdlink=0.5;
+
+ z=0.7;
+ }
+ if((Speed > 4000)&&(Speed <=5000)){
+ iso=1.8;
+ Vqp=0.5*Vq;
+ Vdlink=0.5;
+
+ z=0.7;
+ }
+ if((Speed > 5000)&&(Speed <=5500)){
+ iso=2.2;
+ Vqp=0.5*Vq;
+ Vdlink=0.5;
+
+ z=0.8;
+ }
+ if((Speed > 5500)&&(Speed <=6000)){
+ iso=2.8;
+ Vqp=0.7*Vq;
+ Vdlink=0.6;
+
+ z=0.9;
+ }
+ if((Speed > 6000)){
+ iso=3.2;
+ Vqp=0.7*Vq;
+ Vdlink=0.9;
+
+ z=0.95;
+ }
+
+
+ // th=(W*et*1.9E-4)+iso; //1.9E-4
+ // th=(W*et*7.0E-4)+iso; //5.0E-4 +iso 1.9
+ // th=Wo*wi*1.85E-4;
+ }
+
+ if(r==0){
+ su=(sin(thu)+0.2);//-0.2f; // p51 0.5
+ sv=(sin(thv)+0.2);//-0.05f;
+ sw=(sin(thw)+0.2);//-0.05f;//-0.7
+
+ mypwmA.write(su*Vr_adc);
+ mypwmB.write(sv*Vr_adc);
+ mypwmC.write(sw*Vr_adc);
+ }
+ if(r==1){
+ mypwmA.write(du);
+ mypwmB.write(dv);
+ mypwmC.write(dw);
+ }
+
+
+ myled = !myled;
+ // }
+// }
+
+ Curr_u=(Curr_ui);
+ Curr_v=(Curr_vi);
+ Curr_w=(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]+0.5; //thave
+ idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]+0.5; //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;
+ /*************************************/
+
+ /*****PID Id *****/
+ Idin=(-0.25)-idq[0];
+ float adi,bdi,cdi,workdi[2];
+ float kpdi=1.0,kidi=0.5,kddi=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;
+ /**********************************/
+
+ /*****PID Iq *****/
+ Iqin=Vr_adc-idq[1];
+ 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;
+ /**********************************/
+
+ //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++;
+
+
+ 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=4.0;
+ //float WPLL=150.0;
+ 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=5500*(1.07-Vr_adc)-vstf; // 5500 1.05
+ float a,b,c,work[2];
+ float kp=2.0,ki=0.5,kd=0.0; // 1.2 0.7
+ float dt=1.0E-6;//10.0E-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)){ // ie 1000
+ vsti=vstf;
+ // vsti=vst;
+ r=1;
+ }
+ /*else{
+ vsti=vst;
+ r=0;
+ }*/
+ /****Filter********/
+ float vstfo=vstf;
+ float dttt=10.0E-6;//100E-6
+ float k11,k22,k33,k44,tau1=0.01;//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
+
+
+
+ Va=cos(th)*Vd-sin(th)*Vq; //Vqp
+ Vb=sin(th)*Vd+cos(th)*Vq; //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))*0.5;
+ // d07=0;
+ 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))*0.5;
+ //d07=0;
+ 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))*0.5;
+ // d07=0;
+ du=d07;
+ dv=d3+d4+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))*0.5;
+ //d07=0;
+ 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))*0.5;
+ //d07=0;
+ 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))*0.5;
+ //d07=0;
+ du=d1+d6+d07;
+ dv=d07;
+ dw=d6+d07;
+ }
+
+
+
+ // 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];
+ SWAVE=su;
+ Vector=r;
+ i++;
+ }
+}
\ No newline at end of file