File content as of revision 0:fa432f8ea1a6:

#include "mbed.h"
#include <math.h>

#include <IO_define.h>
#include <kukei.h>

void time_UP(){
    ut1=uT.read_us(); 
    //ut=0;
    ui=0;
    }
void time_UN(){
     ut2=uT.read_us();
     uT.reset();
     
    }
void time_VP(){
    vt1=vT.read_us();
    vi=0;
    }
void time_VN(){
    vt2=vT.read_us();
    vT.reset();
    }
void time_WP(){
    wt1=wT.read_us();
    wi=0;
    et=0;
    }
void time_WN(){
    wt2=wT.read_us();
    wT.reset();
    }

int main() {
    
    uT.start();
     vT.start();
     wT.start(); 
     //uTc.start();
     
    PWM_IN1_U.period_us(20);//50
    PWM_IN2_V.period_us(20);
    PWM_IN3_W.period_us(20);
     
    // zt.attach_us(&ztr, tau);
     //ztc.attach_us(&ztrc,tau);
     
     Vr_adc_i=V_adc.read();  
     wait_ms(100);
     SH=HALL_U<<2|HALL_V<<1|HALL_W;
      EN1_U=1; 
      EN2_V=1; 
      EN3_W=1;
 
 while(1) {
    
      SH=HALL_U<<2|HALL_V<<1|HALL_W;
    
    Vr_adc=V_adc.read(); 
    y=Vr_adc- Vr_adc_i;
    ay=fabs(y);
    power=ay;
    if(y>0.1){
        frd=0;
        //aout=0.5;
        }
    if(y<-0.1){
        frd=1;
        //aout=0.9;
        }
        
    if(ay<0.1){
        q=0;
        aout=0.0;
        r=0;
        vst=0;
        //Speed=0;
        //SH=HALL_U<<2|HALL_V<<1|HALL_W;
        //power=0.0;
        }
 /************起動********************/   
if(((y<-0.1)||(y>0.1))&&(q==0)){ 
//if(q==0){       
 while(q<20){  
    SH=HALL_U<<2|HALL_V<<1|HALL_W;
    
/*****Forword 起動******/    
 if((frd==0)){
     //SH=HALL_U<<2|HALL_V<<1|HALL_W;
    if(SH==5){ 
     start_F5();
       } 
     if(SH==4){ 
     start_F4();
       } 
     if(SH==6){  
     start_F6();
       } 
     if(SH==2){ 
     start_F2();
       } 
     if(SH==3){ 
     start_F3();
       } 
     if(SH==1){ 
     start_F1();
       } 
    }
    
/*******Reversal　起動************/    
  if((frd==1)){
     //SH=HALL_U<2|HALL_V<<1|HALL_W;
    if(SH==3){ 
     start_R3();
       } 
     if(SH==2){ 
     start_R2();
       } 
     if(SH==6){  
     start_R6();
       } 
     if(SH==4){ 
     start_R4;
       } 
     if(SH==5){ 
     start_R5();
       } 
     if(SH==1){ 
     start_R1();
       } 
      }
          q++;  
      }   
    }
    
 if(Speed<1000){//2000
 /**************矩形波駆動始動***************/
 /*******Forward******************/
 if(frd==0){
        switch(SH){
            case 5: PWM_W();//W
            break;
            case 4:  EN_W();//W
            break;
            case 6: PWM_U();//U
            break;
            case 2:  EN_U();//U
            break;
            case 3: PWM_V();//V
            break;
            case 1:  EN_V();//V
            break;
            default :PWM_W();//W
            break;             
            }
        HALL_Ui.rise(time_WP);
        HALL_Wi.fall(time_VN);
        HALL_Vi.rise(time_UP);
        HALL_Ui.fall(time_WN);
        HALL_Wi.rise(time_VP);
        HALL_Vi.fall(time_UN);
         }
/*******Reversal******************/
  if(frd==1){
        switch(SH){   
            case 5: PWM_W();
            break;
            case 1:  EN_W();
            break;
            case 3: PWM_V();
            break;
            case 2:  EN_V();
            break;
            case 6: PWM_U();
            break;
            case 4:  EN_U();
            break;
            default :PWM_W();
            break;                    
            }   
        HALL_Ui.rise(time_UP);
        HALL_Wi.fall(time_WN);
        HALL_Vi.rise(time_VP);
        HALL_Ui.fall(time_UN);
        HALL_Wi.rise(time_WP);
        HALL_Vi.fall(time_VN);
         }
       
 }//Speed<2000
       
        /*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*fabs(wsi*1E-6)));
         usi=1*(ut2-ut1); 
         vsi=1*(vt2-vt1); 
         wsi=1*(wt2-wt1);     
         usic=1*(ut2c-ut1c); 
     //Speed=60*(1/(6.0*fabs(usi)*1E-6));//CQ 6  
  /*  if(Vr_adc < 0.08f){
          q=0;
         Vr_adc=0; 
         r=0;
         vst=0;  
         i=0;
       }  */  
        
  #if 1
  if(Speed>=1000){      
        ui=ui+1;
        vi=vi+1;
        wi=wi+1;
        et=et+1;
     
        thu=2*PI*(1/(2*usi*1E-6))*ui*3.6E-4;  //usi 2.3E-4  vst
          thv=2*PI*(1/(2*vsi*1E-6))*vi*3.6E-4;
           thw=2*PI*(1/(2*wsi*1E-6))*wi*3.6E-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   
        
         PWM_IN1_U.write((su+0.5)*power); //kaisha sv ier su
         PWM_IN2_V.write((sv+0.5)*power); //kaisha sw ie sv
         PWM_IN3_W.write((sw+0.5)*power); //kaisha su ie sw
         }
   if(r==1){
         PWM_IN1_U.write(du); // dw ie no white and Black
         PWM_IN2_V.write(dv); // du
         PWM_IN3_W.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(frd==1){
   if(300>fabs(vstt-vst)){  // ie 2000
        vsti=vstt;
     // vsti=vst;
     r=1;
     }
    } 
if(frd==0){
      if(300>fabs(vstt-vst)){  // ie 2000
          vsti=vstt;
     // vsti=vst;
         r=1;
     }
         }
    /****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];
        //aout=(su+0.2)*power;
        aout=du;
        
        Vector=r;
        i++; 
        }
    #endif   
    }
    
}