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++;   
    }
}