Vector sin

Dependencies:   mbed-rtos mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #define TS1 0.2
00004 #include <math.h>
00005 int q=0,START=7,i=0,s=0,vector;
00006 float uii=0,ut=0,ut1=0,ut2=0,usi=0;
00007 float vii=0,vt=0,vt1=0,vt2=0,vsi=0;
00008 float wii=0,wt=0,wt1=0,wt2=0,wsi=0;
00009 unsigned int ui=0,vi=0,wi=0,vect=0;
00010 float su=0,sv=0,sw=0,suv=0,svv=0,swv=0;
00011 float uci=0,uc=0;
00012 float vst=0,vstt=0,vsti=0,vstf=0,Edw=0,PI=3.141592;
00013 float Speed;
00014 float w,Vqp;
00015 float dth,phm,W,eth,therr,Em,PLL;
00016 float uvect,vvect,wvect;
00017 float sig=0;
00018 PwmOut mypwmA(PA_8); //PWM_OUT 8
00019 PwmOut mypwmB(PA_9); //9
00020 PwmOut mypwmC(PA_10);//10
00021 
00022 PwmOut  Current_Ref(PB_4);
00023 
00024 DigitalOut Vector(PC_4);
00025 DigitalOut EN1(PC_10);
00026 DigitalOut EN2(PC_11);
00027 DigitalOut EN3(PC_12);
00028 
00029 DigitalOut vcl(PC_8);
00030 
00031 InterruptIn  HA(PA_15);
00032 InterruptIn  HB(PB_3);
00033 InterruptIn  HC(PB_10);
00034 
00035 //AnalogIn V_adc(PB_1);  //volume
00036 AnalogIn V_adc(PC_2); //Potention
00037 
00038 
00039 AnalogIn  Vshuntu(PA_1);
00040 AnalogIn  Vshuntv(PA_0);
00041 AnalogIn  Vshuntw(PB_0);
00042 
00043 AnalogIn BEMF1(PC_3);//C7_37
00044 AnalogIn BEMF2(PB_0);//C7_34
00045 AnalogIn BEMF3(PA_7);//C10_26
00046 
00047 DigitalIn GPIO_BEMF(PC_9);
00048 DigitalIn CPOUT(PA_12);
00049 AnalogIn Curr_ui(PA_0);
00050 AnalogIn Curr_vi(PC_1);
00051 AnalogIn Curr_wi(PC_0); //PA_1
00052 Timer uT;
00053 Timer vT;
00054 Timer wT;
00055 AnalogOut SWAVE(PA_4);
00056 
00057 Serial pc(USBTX,USBRX);
00058 DigitalOut myled(LED1);
00059 
00060 float r2=sqrt(2.0f),r3=sqrt(3.0f);
00061 float Cuvw[2][3]={{r2/r3,-1.0f/r2/r3,-1.0f/r2/r3},
00062                   {0,1.0f/r2,-1.0f/r2}};
00063 float iuvw[3];
00064 float iab[2],iabi[2],Vab[2];
00065 float idq[2],idqi[2];
00066 float thave,th,thu,thv,thw,Ed,Vd,Vq,Vqi,West,Wz,Wo,Icom,Wcom;
00067 float zet=sqrt(2.0f/3.0f),cos23=cos((2.0f/3.0f)*PI);
00068 float cos43=cos((4.0f/3.0f)*PI),sin23=sin((2.0f/3.0f)*PI),sin43=sin((4.0f/3.0f)*PI);
00069 float Curr_u,Curr_v,Curr_w;
00070 float Xin,Xout;
00071 float Idin,Vdout,Iqin,Vqout,Xsi;
00072 int vstctle=0,vstctlz=0;
00073 float Vr_adc=0.0f;
00074 int adc,vtv=0;
00075 
00076 Ticker sinTime;
00077     
00078  void HAH(){
00079    
00080     ut1=uT.read_us();
00081     ut=0;  
00082     ui=0; 
00083    // vect=0;
00084    if(vector==0){
00085       EN1=1; 
00086       EN2=1; 
00087       EN3=0;   
00088       }
00089   /*if(vector==0){  
00090     mypwmA.write(su*Vr_adc);  //0.5
00091     mypwmB.write(0);
00092     mypwmC.write(0);
00093      }*/
00094       }
00095  void HAL(){
00096   
00097     ut2=uT.read_us(); 
00098     uT.reset();
00099      if(Speed<=2500){
00100    // wait_us(100);
00101     vect=0;  //******
00102     }
00103     
00104     if(vector==0){
00105        EN1=1; 
00106        EN2=0; 
00107        EN3=1;   
00108        }
00109      }
00110  void HBH(){
00111   
00112     vt1=vT.read_us();   
00113     vi=0;
00114   if(Speed>2500){
00115     vect=0;
00116     }
00117    if(vector==0){
00118       EN1=0; 
00119       EN2=1; 
00120       EN3=1; 
00121       }
00122   /* if(vector==0){           
00123     mypwmA.write(0);
00124     mypwmB.write(sv*Vr_adc);
00125     mypwmC.write(0);
00126     }*/
00127      }
00128  void HBL(){
00129    
00130     vt2=vT.read_us();
00131     // vect=0;
00132     vT.reset();
00133      if(vector==0){
00134       EN1=1; 
00135       EN2=1; 
00136       EN3=0;
00137       }
00138     
00139      }
00140  void HCH(){
00141     wt1=wT.read_us(); 
00142     wi=0;
00143     //vect=0;
00144      if(vector==0){
00145       EN1=1; 
00146       EN2=0; 
00147       EN3=1;
00148       }
00149   /* if(vector==0){      
00150     mypwmA.write(0);
00151     mypwmB.write(0);
00152     mypwmC.write(sw*Vr_adc);
00153     }*/
00154      }
00155      
00156  void HCL(){
00157     wt2=wT.read_us();
00158    // vect=0;
00159     wT.reset();
00160      if(vector==0){
00161       EN1=0; 
00162       EN2=1; 
00163       EN3=1;
00164       }
00165      }   
00166      
00167    void CPLT(){
00168   pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
00169   }
00170      
00171    void timerTS1(void const*argument){
00172    CPLT();
00173   } 
00174  
00175 Timer Timer1;   
00176 int main() {
00177   
00178   pc.baud(128000); 
00179   Timer1.start();  
00180    EN1=1;
00181    EN2=1;
00182    EN3=1;
00183    
00184     mypwmA.period_us(20); //20
00185    
00186     mypwmB.period_us(20);
00187   
00188     mypwmC.period_us(20);
00189       
00190     Current_Ref.period_us(15);
00191     Current_Ref.write(0.6f);
00192      uT.start();
00193      vT.start();
00194      wT.start();
00195      Timer1.start();
00196      
00197      RtosTimer RtosTimerTS1(timerTS1);
00198      RtosTimerTS1.start((unsigned int)(TS1*3000));
00199      Thread::wait(100); 
00200  
00201  while(1) {
00202     
00203   /*  if(CPOUT==1){
00204         vect=0;
00205         }*/
00206     
00207       Vr_adc=V_adc.read();
00208       
00209 if((Vr_adc>0.08f)&&(q==0)){
00210       
00211  while(q<20){   
00212     
00213   /*  mypwmA.write(0.5f);
00214     mypwmB.write(0);
00215     mypwmC.write(0);
00216     wait_ms(START);
00217            
00218     mypwmA.write(0);
00219     mypwmB.write(0.5f);
00220     mypwmC.write(0.0);
00221     wait_ms(START);
00222          
00223     mypwmA.write(0);
00224     mypwmB.write(0.0);
00225     mypwmC.write(0.5f);
00226     wait_ms(START);*/
00227     
00228       EN1=1; 
00229       EN2=1; 
00230       EN3=0;   
00231     
00232     mypwmA.write(0.5f);  //0.5
00233     mypwmB.write(0);
00234     mypwmC.write(0);
00235     wait_ms(START);
00236     //wait_ms(START);
00237      
00238        EN1=1; 
00239        EN2=0; 
00240        EN3=1; 
00241        
00242    wait_ms(START);
00243 
00244       EN1=0; 
00245       EN2=1; 
00246       EN3=1; 
00247            
00248     mypwmA.write(0);
00249     mypwmB.write(0.5f);
00250     mypwmC.write(0);
00251     
00252     wait_ms(START);
00253 
00254       EN1=1; 
00255       EN2=1; 
00256       EN3=0;
00257       
00258     wait_ms(START);
00259 
00260       EN1=1; 
00261       EN2=0; 
00262       EN3=1;
00263          
00264     mypwmA.write(0);
00265     mypwmB.write(0);
00266     mypwmC.write(0.5f);
00267     
00268     wait_ms(START);
00269       EN1=0; 
00270       EN2=1; 
00271       EN3=1;
00272     q++;
00273     
00274     }
00275     }
00276        
00277        /* HA.rise(&HAH);
00278         HC.fall(&HCL);
00279         HB.rise(&HBH);
00280         HA.fall(&HAL);
00281         HC.rise(&HCH);
00282         HB.fall(&HBL);*/
00283         
00284         HA.rise(&HAH);
00285         HC.fall(&HCL);
00286         HB.rise(&HBH);
00287         HA.fall(&HAL);
00288         HC.rise(&HCH);
00289         HB.fall(&HBL);
00290        
00291     Speed=60*(1/(7.0*2.0*usi*1E-6));
00292       
00293     if(Vr_adc < 0.05f){
00294           q=0;
00295          Vr_adc=0; 
00296          vector=0;
00297          vst=0;  
00298          i=0;
00299        }        
00300         
00301         ui=ui+1;
00302         vi=vi+1;
00303         wi=wi+1;    
00304         vect++;
00305       /* if(Curr_ui>0.48){ //&&(vtv==0)){
00306                vect=0;
00307             
00308                vtv=1;
00309                vcl=1;
00310            }*/
00311      /*  if((Curr_ui<=0.4)&&(vtv==0)){
00312               // vect=0;
00313                vtv=1;
00314                vcl=0;
00315           }*/
00316         /*if(vect==100000){
00317             vtv=1;
00318             }*/
00319         
00320      if(vector==0){
00321         thu=2*PI*(1/(usi*1E-6))*ui*1.54E-4;  //1.24E-4
00322           thv=2*PI*(1/(vsi*1E-6))*vi*1.54E-4;
00323            thw=2*PI*(1/(wsi*1E-6))*wi*1.54E-4;
00324            
00325           thave=(thu+thv+thw)/3;
00326              th=thu;         
00327              s=0;
00328           }
00329     if((vector==1)){
00330                if((Speed > 500)&&(Speed <=1000)){
00331                
00332                  Vqp=0.3*Vq; 
00333                  th=W*vect*1.0E-4;
00334                 
00335                  }
00336               if((Speed > 1000)&&(Speed <=1500)){//1000
00337                 
00338                  Vqp=0.3*Vq; //0.5
00339                  th=W*vect*1.05E-4;
00340         
00341                  }
00342              if((Speed > 1500)&&(Speed <=2000)){
00343               
00344                  Vqp=0.4*Vq;  //0.65
00345                  th=W*vect*1.1E-4;
00346                
00347                  }
00348              if((Speed > 2000)&&(Speed <=2500)){
00349                
00350                  Vqp=0.5*Vq; 
00351                  th=W*vect*1.15E-4;
00352                 
00353                  }
00354               if((Speed > 2500)&&(Speed <=3000)){
00355                
00356                  Vqp=0.6*Vq; 
00357                  th=W*vect*1.2E-4;
00358                 
00359                  }
00360              if((Speed > 3000)&&(Speed <=3500)){
00361               
00362                  Vqp=0.7*Vq; 
00363                  th=W*vect*1.25E-4;
00364                 
00365                  }
00366              if((Speed > 3500)&&(Speed <=4000)){
00367               
00368                  Vqp=0.8*Vq; 
00369                  th=W*vect*1.3E-4;
00370               
00371                  }
00372              if((Speed > 4000)&&(Speed <=4500)){
00373                
00374                  Vqp=0.9*Vq; 
00375                  th=W*vect*1.35E-4;
00376                 
00377                  }
00378              if((Speed > 4500)&&(Speed <=5000)){
00379                
00380                  Vqp=1.0*Vq; 
00381                  th=W*vect*1.4E-4;
00382                 
00383                  }
00384              if((Speed > 5000)&&(Speed <= 6000)){
00385               
00386                  Vqp=1.2*Vq; 
00387                  th=W*vect*1.45E-4;
00388               
00389                  }
00390              if((Speed > 6000)){
00391             
00392                  Vqp=1.3*Vq; 
00393                  th=W*vect*1.5E-4;
00394               
00395                  }   
00396               
00397             //  th=(W*((vst*1.0E-6)));   //1.0E-4 @HAL  
00398             // th=W*vect*1.5E-4;
00399           }
00400               
00401     if(vector==0){  
00402          su=sin(thu);
00403          sv=sin(thv);
00404          sw=sin(thw);  
00405         
00406          mypwmA.write(su*Vr_adc); 
00407          mypwmB.write(sv*Vr_adc); 
00408          mypwmC.write(sw*Vr_adc); 
00409          }
00410    if((vector==1)){
00411          EN1=1;
00412          EN2=1;
00413          EN3=1;
00414          mypwmA.write(uvect); 
00415          mypwmB.write(vvect); 
00416          mypwmC.write(wvect); 
00417        }
00418             
00419         myled = !myled;
00420       
00421        Curr_u=(Curr_ui);
00422        Curr_v=(Curr_vi);
00423        Curr_w=(Curr_wi);
00424        
00425    float Itau=10.0E-6,Idt=1.0E-6;    // Itau=1.0E-6,Idt=1.0E-6
00426      /****Filter Iu********/
00427    float Iu1,Iu2,Iu3,Iu4;//0.01
00428     Iu1=Idt*(Curr_u-iuvw[0])/Itau;
00429     Iu2=Idt*(Curr_u-(iuvw[0]+Iu1/2.0))/Itau;
00430     Iu3=Idt*(Curr_u-(iuvw[0]+Iu2/2.0))/Itau;
00431     Iu4=Idt*(Curr_u-(iuvw[0]+Iu3))/Itau;
00432     iuvw[0]=iuvw[0]+(Iu1+2.0*Iu2+2.0*Iu3+Iu4)/6.0;
00433      /*************************************/
00434        /****Filter Iv********/
00435     float Iv1,Iv2,Iv3,Iv4;//0.01
00436     Iv1=Idt*(Curr_v-iuvw[1])/Itau;
00437     Iv2=Idt*(Curr_v-(iuvw[1]+Iv1/2.0))/Itau;
00438     Iv3=Idt*(Curr_v-(iuvw[1]+Iv2/2.0))/Itau;
00439     Iv4=Idt*(Curr_v-(iuvw[1]+Iv3))/Itau;
00440     iuvw[1]=iuvw[1]+(Iv1+2.0*Iv2+2.0*Iv3+Iv4)/6.0;
00441      /*************************************/
00442        /****Filter Iw********/
00443     float Iw1,Iw2,Iw3,Iw4;//0.01
00444     Iw1=Idt*(Curr_w-iuvw[2])/Itau;
00445     Iw2=Idt*(Curr_w-(iuvw[2]+Iw1/2.0))/Itau;
00446     Iw3=Idt*(Curr_w-(iuvw[2]+Iw2/2.0))/Itau;
00447     Iw4=Idt*(Curr_w-(iuvw[2]+Iw3))/Itau;
00448     iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0;
00449      /*************************************/  
00450        
00451     iabi[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet;    
00452     iabi[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet;
00453          /****Filter Ia********/ 
00454     float Ia1,Ia2,Ia3,Ia4;
00455     float Iatau= 3.0E-6,Iadt=1.0E-6; //2.0E-4  1.0E-6
00456     Ia1=Iadt*(iabi[0]-iab[0])/Iatau;
00457     Ia2=Iadt*(iabi[0]-(iab[0]+Ia1/2.0))/Iatau;
00458     Ia3=Iadt*(iabi[0]-(iab[0]+Ia2/2.0))/Iatau;
00459     Ia4=Iadt*(iabi[0]-(iab[0]+Ia3))/Iatau;
00460     iab[0]=iab[0]+(Ia1+2.0*Ia2+2.0*Ia3+Ia4)/6.0;
00461      /*************************************/ 
00462         
00463      /****Filter Ib********/
00464     float Ib1,Ib2,Ib3,Ib4;//0.01
00465     float Ibtau= 3.0E-6,Ibdt=1.0E-6; //2.0E-2  1.0E-6
00466     Ib1=Ibdt*(iabi[1]-iab[1])/Ibtau;
00467     Ib2=Ibdt*(iabi[1]-(iab[1]+Ib1/2.0))/Ibtau;
00468     Ib3=Ibdt*(iabi[1]-(iab[1]+Ib2/2.0))/Ibtau;
00469     Ib4=Ibdt*(iabi[1]-(iab[1]+Ib3))/Ibtau;
00470     iab[1]=iab[1]+(Ib1+2.0*Ib2+2.0*Ib3+Ib4)/6.0;
00471      /*************************************/   
00472         
00473         idq[0]=cos(th)*iab[0]+sin(th)*iab[1]; 
00474         idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]; 
00475         
00476       /****Filter Id********/ 
00477    /* float Id1,Id2,Id3,Id4;
00478     float Idtau= 2.0E-4,Iddt=1.0E-6; 
00479     Id1=Iddt*(idqi[0]-idq[0])/Idtau;
00480     Id2=Iddt*(idqi[0]-(idq[0]+Id1/2.0))/Idtau;
00481     Id3=Iddt*(idqi[0]-(idq[0]+Id2/2.0))/Idtau;
00482     Id4=Iddt*(idqi[0]-(idq[0]+Id3))/Idtau;
00483     idq[0]=idq[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0;*/
00484      /*************************************/ 
00485         
00486      /****Filter Iq********/
00487    /* float Iq1,Iq2,Iq3,Iq4;//0.01
00488     float Iqtau= 2.0E-4,Iqdt=1.0E-6; //2.0E-2  1.0E-6
00489     Iq1=Iqdt*(idqi[1]-idq[1])/Iqtau;
00490     Iq2=Iqdt*(idqi[1]-(idq[1]+Iq1/2.0))/Iqtau;
00491     Iq3=Iqdt*(idqi[1]-(idq[1]+Iq2/2.0))/Iqtau;
00492     Iq4=Iqdt*(idqi[1]-(idq[1]+Iq3))/Iqtau;
00493     idq[1]=idq[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0;*/
00494      /*************************************/
00495     
00496      /*****PID Id *****/
00497         Idin=0.0-idq[0];  // -0.2
00498         float adi,bdi,cdi,workdi[2];
00499         float kpdi=3.0,kidi=1.5,kddi=0.0;  //0.3 0.1 0.0
00500         float dtdi=10.0E-6;//1E-5
00501         adi=Idin;
00502         bdi=workdi[1]+(Idin+workdi[0])/2.0*dtdi;
00503         cdi=(Idin-workdi[0])/dtdi;
00504         workdi[0]=Idin;
00505         workdi[1]=bdi;
00506         Vd=adi*kpdi+bdi*kidi+cdi*kddi;
00507     /**********************************/
00508     
00509       /*****PID Iq *****/
00510         Iqin=(Vr_adc)-idq[1];  //kaisha 600  ie 500
00511         float aqi,bqi,cqi,workqi[2];
00512         float kpqi=2.0,kiqi=0.7,kdqi=0.0;  // 1.0 0.7
00513         float dtqi=10.0E-6;//1E-2
00514         aqi=Iqin;
00515         bqi=workqi[1]+(Iqin+workqi[0])/2.0*dtqi;
00516         cqi=(Iqin-workqi[0])/dtqi;
00517         workqi[0]=Iqin;
00518         workdi[1]=bqi;
00519         Vq=(aqi*kpqi+bqi*kiqi+cqi*kdqi);
00520     /***************************************/
00521          usi=ut2-ut1;
00522          vsi=vt2-vt1;
00523          wsi=wt2-wt1;
00524          
00525 if(i<10000){
00526           vst=vsi;
00527           vstf=vst;
00528            }
00529  else {  
00530  
00531         
00532       
00533         vst=vstf;  
00534         i=10000; 
00535    
00536         Wz=(2*PI)/(vst*1E-6); //vst
00537         Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3  0.11
00538         phm=Ed/(Wz);
00539         dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); // Vd 0.018E-3
00540         eth=asin(dth);
00541      
00542       /*****PID θ *****/   
00543         //PLL    
00544         float as,bs,cs,works[2];  
00545         float kps=5.0,kis=2.0,kds=0.0;
00546         float dts=1.0E-3;//1.0E-6
00547          Xsi=0.0-eth; 
00548         as=Xsi;
00549         bs=works[1]+(Xsi+works[0])/2.0*dts;
00550         cs=(Xsi-works[0])/dts;
00551         works[0]=Xsi;
00552         works[1]=bs;
00553         therr=as*kps+bs*kis+cs*kds;
00554        /*******PLL W ***********/
00555        if(therr>0.2){        //0.01
00556         W=(2*PI)/((vst*1E-6)+(therr/Wz)); //+ 
00557         }
00558         if(therr<-0.01){     //0.01
00559         W=(2*PI)/((vst*1E-6)-(therr/Wz)); //-   
00560             }   
00561     /*****PID ω *****/
00562         Xin=5000*(1.15-Vr_adc)-vst;  // 6500 1.15
00563         float a,b,c,work[2];
00564         float kp=1.1,ki=0.7,kd=0.0;  // 1.2  0.7  0.0
00565         float dt=10.0E-6;//100E-6
00566         a=Xin;
00567         b=work[1]+(Xin+work[0])/2.0*dt;
00568         c=(Xin-work[0])/dt;
00569         work[0]=Xin;
00570         work[1]=b;
00571         Xout=a*kp+b*ki+c*kd;
00572         vstt=Xout;
00573     /********************************/
00574    if(2000>abs(vst-vstt)){  
00575      vsti=vstt; //vstt
00576      vector=1;
00577      }
00578      else{
00579          vsti=vst;
00580          vector=0;
00581          }
00582     /****Filter********/
00583     float dttt=100.0E-6;//100E-6
00584     float k11,k22,k33,k44,tau1=0.01;//0.01
00585     k11=dttt*(vsti-vstf)/tau1;
00586     k22=dttt*(vsti-(vstf+k11/2.0))/tau1;
00587     k33=dttt*(vsti-(vstf+k22/2.0))/tau1;
00588     k44=dttt*(vsti-(vstf+k33/2.0))/tau1;
00589     vstf=vstf+(k11+2.0*k22+2.0*k33+k44)/6.0;
00590     /*************************************/
00591          
00592     }//else kokomade
00593      
00594               
00595         Vab[0]=cos(th)*Vd-sin(th)*Vqp;//Vqp
00596         Vab[1]=sin(th)*Vd+cos(th)*Vqp;
00597     
00598         
00599         uvect=(Vab[0]*zet);
00600         vvect=((Vab[0]*cos23+Vab[1]*sin23)*zet);
00601         wvect=((Vab[0]*cos43+Vab[1]*sin43)*zet);
00602        
00603         
00604         // SWAVE=Vd; 
00605        // SWAVE=th/4;
00606         // SWAVE=Vq;
00607         // SWAVE=West/5000;
00608         //SWAVE=Wo/5000;
00609        //SWAVE=Wz/5000;
00610        // SWAVE=sin(svv);
00611        // SWAVE=iab[0]+0.5;
00612         //SWAVE=Ed;
00613        // SWAVE=therr+0.5;
00614         //SWAVE=idq[1];
00615        // SWAVE=Curr_v;
00616        // SWAVE=iuvw[1];
00617        // SWAVE=vst/5000;
00618        //SWAVE=Vab[1];
00619          SWAVE=iuvw[0];
00620         //SWAVE=vvect;
00621         //SWAVE=BEMF1;
00622         Vector=vector;
00623         i++;   
00624     }
00625 }