Revision 0:fa432f8ea1a6, committed 2019-06-28

Comitter:

oguro

Date:

Fri Jun 28 11:52:27 2019 +0000

Commit message:

Trapezoid sin Vector Control

Changed in this revision

diff -r 000000000000 -r fa432f8ea1a6 IO_define.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IO_define.h	Fri Jun 28 11:52:27 2019 +0000
@@ -0,0 +1,113 @@
+Serial pc(USBTX,USBRX);
+
+PwmOut PWM_IN1_U(PA_8); 
+PwmOut PWM_IN2_V(PA_9); 
+PwmOut PWM_IN3_W(PA_10);
+
+DigitalOut EN1_U(PC_10);
+DigitalOut EN2_V(PC_11);
+DigitalOut EN3_W(PC_12);
+
+DigitalOut Vector(PC_4);
+
+DigitalIn  HALL_U(PA_15); 
+DigitalIn  HALL_V(PB_3);
+DigitalIn  HALL_W(PB_10);
+
+/*DigitalIn  HALL_U(PC_8); 
+DigitalIn  HALL_V(PC_6);
+DigitalIn  HALL_W(PC_10);*/
+
+
+/*InterruptIn  HALL_Ui(PC_8);
+InterruptIn  HALL_Vi(PC_6);
+InterruptIn  HALL_Wi(PC_5);*/
+
+InterruptIn  HALL_Ui(PA_15);
+InterruptIn  HALL_Vi(PB_3);
+InterruptIn  HALL_Wi(PB_10);
+
+AnalogIn V_adc(PC_2); // gaibu Volume
+//AnalogIn V_adc(PB_1); //On Board Volume 
+AnalogIn Curr_ui(PA_0);
+AnalogIn Curr_vi(PC_1);
+AnalogIn Curr_wi(PC_0); 
+
+AnalogOut aout(PA_4);
+
+float Vr_adc,Vr_adc_i;
+
+unsigned short SH,frd=1,q;
+short f1=0,a1=0,f1c=0,a1c=0;
+float kido=0.5,START=10.0,STOP=1.0,power;
+float y,ay,Speed;
+ 
+float ut1=0,ut2=0,usi=0;
+float vt1=0,vt2=0,vsi=0;
+float wt1=0,wt2=0,wsi=0; 
+float ut1c=0,ut2c=0,usic=0;
+unsigned int ui=0,vi=0,wi=0,et=0;
+int r=0,i=0,s=0;
+float Xin,Xout;
+
+float thave,th,thu,thv,thw,Ed,Vd,Vq,Vqi,West,Wz,Wo,Icom,Wcom;
+float vst=0,vstt=0,vsti=0,vstf=0,Edw=0;
+float iso=0,Vqp;
+//float tau=50; //drone 50
+//float zint=tau*1E-6;
+ 
+Timer uT;
+Timer vT;
+Timer wT;
+Timer uTc;
+  
+Ticker zt;
+Ticker ztc;
+Ticker cnt;
+
+float Curr_u=0,Curr_uf=0,iu;
+float Curr_v=0,Curr_vf=0,iv;
+float Curr_w=0,Curr_wf=0,iw;
+float iuvw[3];
+float iab[2],Vab[2];
+float idq[2],idqi[2],idqo[2];
+
+float Idin,Vdout,Iqin,Vqout,Xsi;
+
+short uz[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2 
+short vz[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2 
+short wz[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2
+
+short uc[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2
+short vc[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2
+short wc[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2
+
+float PI=3.141592;
+float ia=0,iaf=0,ib=0,ibf=0,iq=0,iqf=0,id=0,idf=0;
+//float th;
+
+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 sinth,costh;
+float id_diff,id_p,iq_diff,iq_p;
+float s_ki_id, ki_id,kp_id, s_ki_iq ,ki_iq,kp_iq;
+float Wnon,Ednon,W,PLL;
+//float Vd,Vq;
+float s_ki_errth,s_kp_errth,derrth;
+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; //0.6
+float aVa;
+float a3Vb;
+
+float errth;
+float su=0,sv=0,sw=0,suv=0,svv=0,swv=0;
+
+float z=0.3;//0.5
+float Va,Vb;
+float d1,d2,d3,d4,d5,d6,d07;
+float du,dv,dw;
+

diff -r 000000000000 -r fa432f8ea1a6 kukei.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kukei.h	Fri Jun 28 11:52:27 2019 +0000
@@ -0,0 +1,111 @@
+
+
+/********* 矩形反　Drive *******************/
+void PWM_U(){       
+   PWM_IN1_U.write(STOP*power);  
+   PWM_IN2_V.write(0);
+   PWM_IN3_W.write(0); 
+   ut1=uT.read_us();
+}
+void EN_U(){
+   PWM_IN1_U.write(STOP*power);  
+   PWM_IN2_V.write(0);
+   PWM_IN3_W.write(0);       
+}
+void PWM_V(){      
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(STOP*power);
+    PWM_IN3_W.write(0);  
+}
+void EN_V(){
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(STOP*power);
+    PWM_IN3_W.write(0);          
+}
+void PWM_W(){     
+     PWM_IN1_U.write(0);  
+     PWM_IN2_V.write(0);
+     PWM_IN3_W.write(STOP*power);  
+}
+void EN_W(){        
+     PWM_IN1_U.write(0);  
+     PWM_IN2_V.write(0);
+     PWM_IN3_W.write(STOP*power);
+}
+/*******************************************/
+/*********Forward 起動 FNC*******************/
+void start_F5(){  
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(0);
+    PWM_IN3_W.write(kido);  //kido
+     wait_ms(START);
+    }
+ void start_F4(){  
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(0);
+    PWM_IN3_W.write(kido);
+       wait_ms(START);
+    }
+void start_F6(){      
+    PWM_IN1_U.write(kido);  
+    PWM_IN2_V.write(0);
+    PWM_IN3_W.write(0);   
+     wait_ms(START);
+ }
+void start_F2(){    
+    PWM_IN1_U.write(kido);  
+    PWM_IN2_V.write(0);
+    PWM_IN3_W.write(0); 
+      wait_ms(START); 
+}
+void start_F3(){ 
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(kido);
+    PWM_IN3_W.write(0);    
+     wait_ms(START);
+}
+void start_F1(){ 
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(kido);
+    PWM_IN3_W.write(0); 
+       wait_ms(START);
+}    
+/*********Reversal 起動 FNC*******************/
+void start_R5(){ 
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(0);
+    PWM_IN3_W.write(kido);  
+    wait_ms(START);
+}
+void start_R1(){  
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(0);
+    PWM_IN3_W.write(kido); 
+      wait_ms(START);  
+}
+void start_R3(){ 
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(kido);
+    PWM_IN3_W.write(0); 
+    wait_ms(START);
+}
+void start_R2(){     
+    PWM_IN1_U.write(0);  
+    PWM_IN2_V.write(kido);
+    PWM_IN3_W.write(0);       
+      wait_ms(START);
+}
+void start_R6(){
+    PWM_IN1_U.write(kido);  
+    PWM_IN2_V.write(0);
+    PWM_IN3_W.write(0); 
+    wait_ms(START);
+}
+void start_R4(){  
+    PWM_IN1_U.write(kido);  
+    PWM_IN2_V.write(0);
+    PWM_IN3_W.write(0); 
+      wait_ms(START);
+}
+
+/**********************************************/
\ No newline at end of file

diff -r 000000000000 -r fa432f8ea1a6 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 28 11:52:27 2019 +0000
@@ -0,0 +1,638 @@
+#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   
+    }
+    
+} 
\ No newline at end of file

diff -r 000000000000 -r fa432f8ea1a6 mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Fri Jun 28 11:52:27 2019 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#58563e6cba1e

diff -r 000000000000 -r fa432f8ea1a6 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jun 28 11:52:27 2019 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10
\ No newline at end of file