File content as of revision 0:866aafac0128:

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