File content as of revision 1:333d2cdd26d0:

//Vector制御　工事中　2月末　公開予定
//矩形波かから正弦波に移行　Speedの範囲を118行、173行目を
//変更して矩形波→正弦波　への移行回転数を制御します。
// ボリュームを中間値設定後、電源投入またはリセットしてください
//ボリューム前後操作で正転/逆転を制御できます。

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

#include <IO_define.h>
#include <kukei.h>
#include <sin.h>
#include <cos.h>
#include <vector.h>


int main(){
     pc.baud(128000); 
    /* RtosTimer RtosTimerTS1(timerTS1);
     RtosTimerTS1.start((unsigned int)(TS1*5000));//2000
     Thread::wait(100);//1000*/
     
     uT.start();
     vT.start();
     wT.start(); 
     
    PWM_IN1_U.period_us(50);
    PWM_IN2_V.period_us(50);
    PWM_IN3_W.period_us(50);
     
     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*2.0;
    if(y>0.05){
        frd=0;
        //aout=0.5;
        }
    if(y<-0.05){
        frd=1;
        //aout=0.9;
        }
        
    if(ay<0.05){
        q=0;
        aout=0.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<500){//2000
 /**************矩形波駆動始動***************/
 /*******Forward******************/
 if(frd==0){
        switch(SH){
            case 5: PWM_W();//W5
            break;
            case 4:  EN_W();//W4
            break;
            case 6: PWM_U();//U6
            break;
            case 2:  EN_U();//U2
            break;
            case 3: PWM_V();//V3
            break;
            case 1:  EN_V();//V1
            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
 
 if((Speed>=500)){//&&(Speed<4000)){//2000
/************sin drive Forward************/
if(frd==1){
        HALL_Ui.rise(&PWM_sinU);
        HALL_Wi.fall(&EN_sinW);
        HALL_Vi.rise(&PWM_sinV);
        HALL_Ui.fall(&EN_sinU);
        HALL_Wi.rise(&PWM_sinW);
        HALL_Vi.fall(&EN_sinV);
        
        }

/*******sin drive Reversal******************/
if(frd==0){
        HALL_Ui.rise(&PWM_sinW);
        HALL_Wi.fall(&EN_sinV);
        HALL_Vi.rise(&PWM_sinU);
        HALL_Ui.fall(&EN_sinW);
        HALL_Wi.rise(&PWM_sinV);
        HALL_Vi.fall(&EN_sinU);
        
     }
 /****************************/
         
         if((uz[2])<=-16383){   //飽和処理
              uz[2]=-16383;
            }
            if(uz[2]>=16383){
                uz[2]=16383;
                }
                
           if(vz[2]<=-16383){   //飽和処理
              vz[2]=-16383;
            }
           if(vz[2]>=16383){
                vz[2]=16383;
                }
           if(wz[2]<=-16383){  //飽和処理
               wz[2]=-16383;
            }   
           if(wz[2]>=16383){
                wz[2]=16383;
                }
        #if 1
         PWM_IN1_U.write(((float(uz[2])/(16383*2))*(power*STOP))+0.5); 
         PWM_IN2_V.write(((float(vz[2])/(16383*2))*(power*STOP))+0.5);
         PWM_IN3_W.write(((float(wz[2])/(16383*2))*(power*STOP))+0.5); 
         
         aout=(float(wz[2])/(16383*2))*(power*STOP)+0.5;//16383
        #endif
         #if 0
        su=(((float(uz[2])/(16383*2))*(power*STOP))+0.5);
        sv=(((float(vz[2])/(16383*2))*(power*STOP))+0.5);
        sw=(((float(wz[2])/(16383*2))*(power*STOP))+0.5);
      
         PWM_IN1_U.write(su); 
         PWM_IN2_V.write(sv);
         PWM_IN3_W.write(sw); 
          aout=su;
          //aout=(float(wz[2])/(16383*2))*(power*STOP)+0.5;//16383
        #endif 
         //filterCurrent();
         //aout=Curr_wf;
        //HALL_Ui.rise(&cosU);
        //HALL_Ui.fall(&cosUN);
       
}//sin

//if(Speed>=4000){     
        
        //HALL_Ui.rise(&PWM_sinU);
        //HALL_Ui.fall(&EN_sinU);
       // HALL_Ui.rise(&cosU);
       // HALL_Ui.fall(&cosUN);
        
        //filterCurrent();
         
         
       // sinth=(float(uz[2])/(16383*2))+0.5;
        //costh=(float(uc[2])/(16383*2))+0.5;
        
        //aout=sinth;
                         
       /* ia=(iu+iv*cos23+iw*cos43)*zet;    
        ib=(iv*sin23+iw*sin43)*zet;
        
        id=costh*ia+sinth*ib; 
        iq=-sinth*ia+costh*ib; 
        
        id_p=0;
        iq_p=Vr_adc;
        id_diff=id_p-id;
        iq_diff=iq_p-iq;
        
        */
        
 /********PI Control id,iq*********************/          
      /*   s_ki_id += ki_id*id_diff;
         Vd = s_ki_id + kp_id*id_diff;
         
         s_ki_iq += ki_iq*(iq_diff+Vr_adc);
         Vq = s_ki_iq + kp_iq*(iq_diff+Vr_adc);*/
 /*******************************************/
/****** detect rotor position*************/      
      /*  Wz=(2*PI)/(2*vst*1E-6); 
        Ed= (Vq)-0.11f*iq-Wz*0.018E-3*id;
        phm=Ed/(Wz);
        dth=(Vd-0.11f*(id)+Wz*0.018E-3*(iq))/(Wz*phm); 
        derrth=asin(dth);*/
     
      /*****PID error θ　*****/
        
      /*  float ki_PLL=0.01; 
        float kp_PLL=0.2;
         s_ki_errth += ki_PLL*derrth;
         therr = s_ki_errth + kp_PLL*errth;*/
        
       /*******PLL W ***********/
     /*  if(therr>0.01){
        W=(2*PI)/((2*vst*1E-6)+(therr/Wz)); 
        }
        if(therr<-0.01){
        W=(2*PI)/((2*vst*1E-6)-(therr/Wz));    
            }   */ 
      /************************************/
      /*  th += W*zint;
        Va=cos(th)*Vd-sin(th)*Vq; 
        Vb=sin(th)*Vd+cos(th)*Vq; 
        
        SVPWM(); 
              
             aout=iq;
            
       
   }//vector*/
/*******************************************************************/  
         usi=2*(ut2-ut1); 
         vsi=2*(vt2-vt1); 
         wsi=2*(wt2-wt1);     
         usic=2*(ut2c-ut1c); 
         //Speed=60*(1/(7.0*(usi*0.5)*1E-6));//BullRun
 /************************************************************/        
         Speed=60*(1/(7.0*usi*1E-6));
       //pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
        // UP=HALL_Ui;VP=HALL_Vi;WP=HALL_Wi;
        // pc.printf("%d , %d , %d \r" , UP,VP,WP);
 
   }  
}