File content as of revision 2:0058dae8606d:

#include "mbed.h"
#include "rtos.h"
#define TS1 0.2
#include <math.h>
int q=0,START=15,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;
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);

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_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],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;

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.5; //3.0f/5.0f;
float aVa;
float a3Vb;

float z=0.7;//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();
    
     }
 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);
 
    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*wsi*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;
    
        thu=2*PI*(1/(2*usi*1E-6))*ui*2.30E-4;  //usi 1.3E-4  vst
          thv=2*PI*(1/(2*vsi*1E-6))*vi*2.30E-4;
           thw=2*PI*(1/(2*wsi*1E-6))*wi*2.30E-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   
        
         mypwmA.write(su*Vr_adc); //kaisha sv ier su
         mypwmB.write(sv*Vr_adc); //kaisha sw ie sv
         mypwmC.write(sw*Vr_adc); //kaisha su ie sw
         }
   if(r==1){
         mypwmA.write(du); // dw ie no white and Black
         mypwmB.write(dv); // du
         mypwmC.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(2000>abs(vst-vstt)){  // ie 1000
        vsti=vstt;
     // 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.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];
        Vector=r;
        i++;   
    }
}