Revision 0:ce0449e386ce, committed 2018-02-24

Comitter:

oguro

Date:

Sat Feb 24 08:25:42 2018 +0000

Child:

1:0768eefd7892

Commit message:

Vector sin

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 24 08:25:42 2018 +0000
@@ -0,0 +1,630 @@
+#include "mbed.h"
+#include "rtos.h"
+#define TS1 0.2
+#include <math.h>
+int q=0,START=7,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<20){   
+    
+  /*  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);*/
+    
+      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;
+    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.3*Vq; 
+                 th=W*vect*1.0E-4;
+                
+                 }
+              if((Speed > 1000)&&(Speed <=1500)){//1000
+                
+                 Vqp=0.3*Vq; //0.5
+                 th=W*vect*1.05E-4;
+        
+                 }
+             if((Speed > 1500)&&(Speed <=2000)){
+              
+                 Vqp=0.4*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*1.5E-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.0-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=(Speed/100); //7 21 28
+        PLL=1.0; //Speed*2*PI/(60*7); //7  //2
+        //  PLL=W/Wz; 
+        //PLL=30; //30
+        //float WPLL=150.0; // 170
+        float as,bs,cs,works[2];
+       // float kps=PLL,kis=PLL*PLL/5.0,kds=0.0;  
+        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=5000*(1.15-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++;   
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Sat Feb 24 08:25:42 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#5713cbbdb706

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Feb 24 08:25:42 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b484a57bc302
\ No newline at end of file