/********Trapezoid Nucleo-F302R8 IHM07 BR2804@Sensorless**************************/
#include "mbed.h"
#include <math.h>
#define  ONEPI       (3.1415926535)
#define  TWOPI       (2.0*3.1415926535)
#define V1_LIM  12.0    /* dwߍől[V]68.0 */
#define I1_LIM  1.4  /*3.1   dwߍől[A] 15.1*/

float PI=3.141592;
unsigned int scnt_m=0,scount=0; 
unsigned int tss1=0,tss2=0,tss=0,atss;

DigitalOut GPIO(PC_9);

DigitalOut kukeiU(PC_6);
DigitalOut kukeiV(PC_4);
DigitalOut kukeiW(PC_8);
//PwmOut mypwmA(PA_8); 
//PwmOut mypwmB(PA_9); 
//PwmOut mypwmC(PA_10);


//AnalogIn Curr_ui(PA_0);
//AnalogIn Curr_vi(PC_1);
//AnalogIn Curr_wi(PC_0); //PA_1

//AnalogIn V_adc(PB_1);  //Blue On Board volume
//AnalogIn V_adc(PC_2); // Gaibu Potention IHM07
//AnalogIn V_adc(PC_3); // Gaibu Potention IHM08

DigitalOut EN1(PC_10);
DigitalOut EN2(PC_11);
DigitalOut EN3(PC_12);

InterruptIn  HA(PA_15);
InterruptIn  HB(PB_3);
InterruptIn  HC(PB_10);

DigitalIn  U_in(PA_15);
DigitalIn  V_in(PB_3);
DigitalIn  W_in(PB_10);

Timer uTimer;
Timer vTimer;
Timer wTimer;
Timer Timer1; 

AnalogOut SWAVE(PA_4);

uint16_t ADC_buffer[7];

unsigned char ul=0,vl=0,wl=0;
unsigned char ulf,vlf,wlf;
/*************UTiming****************/
float  Vu_now,Vu_now_p,Vu_max=0.0,Vu_min=0.0,Vu_center;
unsigned char BEmaxU=1,BEminU=1;  
/*************VTiming****************/
float  Vv_now,Vv_now_p,Vv_max=0.0,Vv_min=0.0,Vv_center;
unsigned char BEmaxV=1,BEminV=1; 
/*************WTiming****************/
float  Vw_now,Vw_now_p,Vw_max=0.0,Vw_min=0.0,Vw_center;
unsigned char BEmaxW=1,BEminW=1;      
/*******************************************/

unsigned char  HUVW;/* TvOJnHole IC̏ */
unsigned char  V_MODE = 1;/* PWM 쓮*/

unsigned int t_cnt = 0; /* Lv`Ԃcnt */
unsigned int t_cnt_min=100;
unsigned int  Wrise_t = 0; /* Lv`̒l */
unsigned int Urise_t = 0; /* Lv`̒l */
unsigned int  Vrise_t = 0; /* Lv`̒l */
unsigned int  Wrise_t_1 = 0; /* Lv`̒lold */
unsigned int Urise_t_1 = 0; /* Lv`̒lold */
unsigned int  Vrise_t_1 = 0; /* Lv`̒lold */

unsigned int   cnt = 0;/* ICSɎgp */

float   m = 0;
float   iu_ad, iv_ad, iw_ad;
float   Vu_ad, Vv_ad, Vw_ad;
float   vr1_ad,vr_ad,vr1_ad_p;//vr2_ad;
float   Vr_adc_i;  
float   refu, refv, refw;
float   curr_ub,curr_vb,curr_wb,curr_u,curr_v,curr_w;
float   bemf1,bemf2,bemf3;


float   Nrpm = 0;/* ][r/min] */
float   Nrpm_1 = 0;/* ][r/min]old  */
float   Nrpm_s = 0;/* ][r/min]  */
float   Nrpm_f =0;
float   Nref0=0;
float  Nrpm_max = 8000.0;/* xw߂l 5000*/
float   Nrpm_ref = 0.0; /* xw[r/min] */
float   Nrpm_ref0 = 0.0; /* xw[r/min] */
float   dN_HI_P = 1.1; /* Cg 8000*/
float   dN_HI_N = 1.1; /* Cg 8000 5.0*/
float   N_LOW_MAX=5;
float    ijou=1500;

/* g쓮ɌĒǉ */

unsigned char  UVW;
unsigned char  U, V, W;
unsigned char  VMODE = 1,VMODE_N;/* PWM[h */
unsigned char  R_DR = 1; /* R_DR -> 1:]C0:t] */

/* **************** */
/*     d     */
/* **************** */

float kpCurr =0;// 0.0133; /* QC 100[rad/s]*/
float kiCurr =0;//0.002165;    /* ϕQC 100[rad/s]*/
float s_kiCurr=0;
float I_ref=0;
float I_err=0;
float I_FB=0;
float Vpi=0;
/* **************** */
/*     x     */
/* **************** */
float   kaiten=0;
float   Nerr = 0.0; /* x덷 */
float kpSpeed = 0.3;    /* 0.333337 QC 40[rad/s]*/
float kiSpeed = 0.05; /* 0.05135 ϕQC 40[rad/s]*/
float s_kiSpeed = 0.0; /* ϕ풆g */

/*******************************************/
 float  mpi=0;
 unsigned int  PWM_frq=200;//700
 float bemf1_f,bemf2_f,bemf3_f;
 float bemf1_fb,bemf2_fb,bemf3_fb;
 float Vu,Vv,Vw;
/**************Hall Sensor******************/
/* ******************* */
/* hCuXe[^X  */
/* ******************* */
/* 0:GATE OFF, 1:ʒu߁C2:쓮C3:ZTX */
unsigned char Drive_state = 0;
unsigned char Drive1_end = 0; /* Drive_state=1ʒȕItO */
unsigned char Drive2_end = 0; /* Drive_state=2̏ItO */
unsigned char  ON;
/***********************/
/* 120xʓdZTX */
/***********************/
float   Doki_accel_rate = 0.2; /*0.05 쓮Cg */
float   doki_MAX = 1000.0;/* 1000  쓮ől */
float   doki_cnt = 0; /* 쓮60degJE^ */
float   doki_cnt_MAX = 0; /* 쓮JE^ől */
float   doki_Speed = 2.0;
unsigned char Drive_ModeUP = 0;

unsigned char Z_CrU = 0,Z_CrU_b=0; /* SL_UM */
unsigned char Z_CrV = 0,Z_CrV_b=0; /* SL_VM */
unsigned char  Z_CrW= 0,Z_CrW_b=0; /* SL_WM */

/*****ʒu***************/
float   I_ichi = 0.0; /* ʒu߁E쓮p */
float   dI_ichi = 0.08; /* 0.05ʒuߓd[g */
float   I_ichi_MAX = 0.7;/* ʒuߍőd */
/************************************************/

/*********************************************/
  
 /* Hall_uɃLv` */
void Hall_u()
    {
              
    Urise_t = uTimer.read_us();
    
    t_cnt = Urise_t - Urise_t_1;
    if(t_cnt<t_cnt_min){t_cnt=t_cnt_min;}
    else{}
    Nrpm = (float)(8571429/ t_cnt);
    if( ((Nrpm-Nrpm_1)< ijou) && ((Nrpm-Nrpm_1)>(- ijou)) )
    { /* xُ */
           Nrpm_1 = Nrpm;
    }
    else
    {
       Nrpm = Nrpm_1;
    }
    Urise_t_1 = Urise_t;
    }
 void Hall_ul() 
    {
        }  
    
    /* Hall_vɃLv` */
void Hall_v()
    {
              
     Vrise_t = vTimer.read_us();

    t_cnt =  Vrise_t -  Vrise_t_1;
    if(t_cnt<t_cnt_min){t_cnt=t_cnt_min;}
    else{}
    Nrpm = (float)(8571429 / t_cnt);//P=8 
    if( ((Nrpm-Nrpm_1)< ijou) && ((Nrpm-Nrpm_1)>(- ijou)) )
    { /* xُ */
           Nrpm_1 = Nrpm;
    }
    else
    {
       Nrpm = Nrpm_1;
    }
     Vrise_t_1 =  Vrise_t;
    }
    
void Hall_vl() 
    {

        }  
    /* Hall_WɃLv` */
void Hall_w()
    {
              
     Wrise_t = wTimer.read_us();
   
    t_cnt =  Wrise_t -  Wrise_t_1;
    //t_cnt =  Wrise_t_1 -  Wrise_t;
    if(t_cnt<t_cnt_min){t_cnt=t_cnt_min;}
    else{}
    Nrpm = (float)(8571429/ t_cnt);
    if( ((Nrpm-Nrpm_1)< ijou) && ((Nrpm-Nrpm_1)>(- ijou)) )
    { /* xُ */
           Nrpm_1 = Nrpm;
    }
    else
    {
       Nrpm = Nrpm_1;
    }
     Wrise_t_1 =  Wrise_t;
    }
void Hall_wl() 
    { 
        }  
    
/********main***********/
//int main() {
void Function() {
    
   /************* Drive_state ***** */
    //ON = 1;
    if (ON==0)/* Gate OFF */
    {

     }
    else
    {
    if(Drive1_end == 0){Drive_state = 1;
    }
    else
    {
       if(Drive2_end == 0){Drive_state = 2;}
       else{Drive_state = 3;}
    }
    }
       
   vr_ad=ADC_buffer[3]/4095.0;//V_adc.read(); 
   vr1_ad_p=(vr_ad-Vr_adc_i);
   vr1_ad+=(vr1_ad_p-vr1_ad)*0.1;
    
    if(fabs(vr1_ad)<=0.05)
    {
       ON=0;
        I_ref= 0;
        I_err = 0;
        Nerr=0;
        Nrpm_ref = 0.0;
        s_kiSpeed=0;
        kpCurr=0;
        s_kiCurr=0;
        
        mpi=0;   
     
    Drive_state = 0;
    Drive1_end = 0;
    Drive2_end = 0;
    I_ichi = 0;
    doki_cnt = 0; /* 쓮60degJE^  */
   
    doki_Speed = 2.0;
    Drive_ModeUP = 0;
    
   Z_CrU=0;Z_CrV=0;Z_CrW=0;
   Z_CrU_b=0;Z_CrV_b=0;Z_CrW_b=0;   
    kaiten=0; 
    BEmaxU=1;BEminU=1;
    BEmaxV=1;BEminV=1;
    BEmaxW=1;BEminW=1;
    Vu_max=0.0;Vu_min=0.0;Vv_max=0.0;Vv_min=0.0;Vw_max=0.0;Vw_min=0.0;
    Nrpm=0;
    VMODE_N=1;
    }
    else
    {ON=1;}

      Vu_now_p=Vu_now;
      Vv_now_p=Vv_now;
      Vw_now_p=Vw_now;
      
      bemf1_fb=bemf1_f;//ߋl
      bemf2_fb=bemf2_f;//ߋl
      bemf3_fb=bemf3_f;//ߋl
   
      curr_u += ((curr_ub) - curr_u )*0.03;
      curr_v += ((curr_vb) - curr_v )*0.03;
      curr_w += ((curr_wb) - curr_w )*0.03;
      
      
      bemf1_f +=(bemf1-bemf1_f)*0.05;
      bemf2_f +=(bemf2-bemf2_f)*0.05;
      bemf3_f +=(bemf3-bemf3_f)*0.05;
      
      Vu=bemf1_f;
      Vv=bemf2_f; 
      Vw=bemf3_f;
      
    #if 1 
     if((Drive_state==1)||(Drive_state==2)){
       kpCurr=0.00833; //0.000433
       kiCurr =0.0014;//0.00014165;
       }else{
         kpCurr=0.0002; //0.00007
         kiCurr =0.00015;//0.000055;
         }
         
    #endif

/* *********************************************** */    
/* UNd[_o */
/* *********************************************** */
    Vu_now=Vu;
    Vv_now=Vv;
    Vw_now=Vw;
 /************UTiming**************/   
  if(Vu_now>Vu_now_p){
     Vu_max=Vu_now_p;
     }
  if(Vu_now<Vu_now_p){
     Vu_min=Vu_now_p;
     }
  Vu_center=(Vu_max-Vu_min)/2.0+Vu_min;
if((Vu>(Vu_center+0.015))){//0.01
       if(ul==1){
        Z_CrU_b=1;
         kukeiU=Z_CrU_b;
         ul=0;
         Hall_u();
       } 
      }
if(Vu<(Vu_center-0.015)){//-0.01
      if(ul==0){
        Z_CrU_b=0;
         kukeiU=Z_CrU_b;
         ul=1;
       }
   }  
 /************VTiming**************/   
  if(Vv_now>Vv_now_p){
     Vv_max=Vv_now_p;
     }
  if(Vv_now<Vv_now_p){
     Vv_min=Vv_now_p;
    
     }
  Vv_center=(Vv_max-Vv_min)/2.0+Vv_min;

if((Vv>(Vv_center+0.015))){//0.01
       if(vl==1){
        Z_CrV_b=1;
         kukeiV=Z_CrV_b;
         vl=0;
         Hall_v();
       } 
      }
if(Vv<(Vv_center-0.015)){//-0.01
      if(vl==0){
        Z_CrV_b=0;
         kukeiV=Z_CrV_b;
         vl=1;
       }
   }  
 /************WTiming**************/   
  if(Vw_now>Vw_now_p){
     Vw_max=Vw_now_p;
     }
  if(Vw_now<Vw_now_p){
     Vw_min=Vw_now_p;
     }
  Vw_center=(Vw_max-Vw_min)/2.0+Vw_min;
 
if((Vw>(Vw_center+0.015))){//0.01
       if(wl==1){
        Z_CrW_b=1;
         kukeiW=Z_CrW_b;
         wl=0;
         Hall_w();
       } 
      }
if(Vw<(Vw_center-0.015)){//-0.01
      if(wl==0){
        Z_CrW_b=0;
         kukeiW=Z_CrW_b;
         wl=1;
       }
   }
/***************************************/ 
    
    /* ]Ct]̃`FbN */
    if(R_DR==1)/* ]  1 */
    {
        Nrpm_s = (Nrpm_f += (Nrpm-Nrpm_f)*0.02);
    }
    else/* t] */
    {
        Nrpm_s = -(Nrpm_f += (Nrpm-Nrpm_f)*0.02);  //aki -Nrpm  
    }

 #if 1
   if(kaiten<300){
     I_ichi_MAX=1.75;
     }else{
        I_ichi_MAX=0.95;//0.5
        } 
#endif
 
/*ʒu߂܂ 쓮̓dw */
    if((Drive_state == 1)||(Drive_state == 2))
    {
    I_ichi += dI_ichi;
    if(I_ichi >= I_ichi_MAX)
    {
       I_ichi = I_ichi_MAX;
       Drive1_end = 1; /*ʒuߏItO*/
    }
    else{}
    }
    else{}  
/* *********************************************** */
 /* [ 2 ]   */
 /* *********************************************** */ 
 /* 쓮̓dw */
    if(Drive_state == 2)
    {
    doki_Speed += Doki_accel_rate;
    if(doki_Speed >= doki_MAX)
    {
       doki_Speed = doki_MAX;
       Drive2_end = 1;/*Drive_state 3 ڍs*/
       
    }
    else{}
    doki_cnt+=1.0;
    doki_cnt_MAX = 20000.0/doki_Speed;//20000.0

    if(doki_cnt_MAX < doki_cnt)
    {
       Drive_ModeUP = 1;
       doki_cnt = 0;
    }
    else
    {
       Drive_ModeUP = 0;
    }
    }
    else{} 
  /***********************************/ 
  switch(Drive_state)
    {
    case 0: Nrpm_ref = 0.0;
        Nerr = 0.0;
            break;
    case 1: Nrpm_ref = 0.0;
            Nerr = 0.0;
        break;    
    case 2: //Nrpm_ref = doki_Speed;
           Nerr = 0.0;
        break;
    case 3:    
        
    if(Nrpm<8000){  //2150  
    Nrpm_ref0 = fabs(Nrpm_max*vr1_ad); //aki
    }else{
        Nrpm_ref0=8000; //2150
        }
      if(Nrpm_ref0>0){      
        if(Nref0 > Nrpm_ref0)
        {
                Nref0 -= dN_HI_N; //aki -
                if(Nref0 < Nrpm_ref0)
                {
                Nref0 = Nrpm_ref0;
                }
                else{}
        }
        else{}
        
        if(Nref0 < Nrpm_ref0)
        {
            Nref0 += dN_HI_P;  //aki +
                if(Nref0 > Nrpm_ref0)
                {
                Nref0 = Nrpm_ref0;
            }
                else{}
        }
        else{}
     if(Nref0<N_LOW_MAX)
        {
            Nref0 = N_LOW_MAX;/**/
        }
        else{}
       
      } 
      else{}
        
        Nrpm_ref = Nref0; 
      break;
     default: break;
    }   
         if(R_DR==1) {  //1
            Nerr =( Nrpm_ref-Nrpm_s);//- 12 02 2018
        }
        else {
            Nerr = (Nrpm_ref+Nrpm_s);//-
        } 
           
        
    /* ----------- */
    /* ASRPI */
    /* ----------- */
    s_kiSpeed += kiSpeed*Nerr;
    if(s_kiSpeed > I1_LIM)
    {
    s_kiSpeed = I1_LIM;
    }
    else
    {
    if(s_kiSpeed < (-I1_LIM))
    {
       s_kiSpeed = -I1_LIM; 
    }else{}
    }
    
    I_ref = s_kiSpeed + kpSpeed*Nerr;
    
    if(I_ref > I1_LIM)
    {
    I_ref = I1_LIM;
    }
    else
    {
    if(I_ref < (-I1_LIM))
    {
        I_ref = -I1_LIM; 
    }else{}
    }
  
    
/* *********************************************** */
/* [ 3 ] d  */
/* *********************************************** */

    /* ----------- */
    /* ACRPI */
    /* ----------- */
     switch (VMODE)
    {
        case 1: I_FB = (curr_v);// - curr_v )/2.0; /* (U-V)/2 */
        break;
        case 2: I_FB = ( curr_w);// - curr_w )/2.0; /* (U-W)/2 */
        break;
        case 3: I_FB = ( curr_w);// - curr_w)/2.0; /* (V-W)/2 */
        break;
        case 4: I_FB = (curr_u);// - curr_u)/2.0; /* (V-U)/2 */
        break;
        case 5: I_FB = (curr_u);// - curr_u )/2.0; /* (W-U)/2 */
        break;
        case 6: I_FB = (curr_v );//- curr_v )/2.0; /* (W-V)/2 */
        break;
    default:I_FB = 0.0;break;
    }
    
 
        
   switch(Drive_state)
   {
    case 0: 
        I_ref = 0;
        I_err = 0;
        s_kiCurr = 0.0;
        s_kiSpeed = 0.0;
        break;
    case 1: 
        I_ref = I_ichi;
        I_err = I_ref - I_FB;
        break;
    case 2: 
        I_ref = I_ichi;
        I_err = I_ref - I_FB;
        break;
    case 3: 
        //I_ref = I_ref_ASR;
        I_err = I_ref - I_FB;
       // break;
        break;
     default: 
        I_ref = 0;
        I_err = 0;
        s_kiCurr = 0.0;
        s_kiSpeed = 0.0;
        break;
   } 
   
    s_kiCurr += kiCurr*I_err;
    if(s_kiCurr > V1_LIM)
    {
    s_kiCurr = V1_LIM;
    }
    else
    {
    if(s_kiCurr < (-V1_LIM))
    {
       s_kiCurr = -V1_LIM; 
    }else{}
    }
    
    
    Vpi = s_kiCurr + kpCurr*I_err;
    
    if(Vpi > V1_LIM)
    {
    Vpi = V1_LIM;
    }
    else
    {
    if(Vpi < (-V1_LIM))
    {
        Vpi = -V1_LIM; 
    }else{}
    }
 
     kaiten=fabs(Nrpm_s);
   
    /* ------------ */
    /* [h̍XV */
    /* ------------ */
    
   // UVW = UVW_in(); /* ZTtEz[ICM */
if( Drive_state==3){
       Z_CrU=Z_CrU_b;
       Z_CrV=Z_CrV_b;
       Z_CrW=Z_CrW_b;
        }
if(R_DR==0){
   UVW = (Z_CrW << 2) + (Z_CrV<< 1) +Z_CrU;
  }else{ 
   UVW = (Z_CrW << 2) + (Z_CrV << 1) +Z_CrU;
  }
    
  switch(Drive_state)
    {
    case 0: 
        VMODE = 1;
        break;
    case 1: 
        VMODE = 1;
        break;
    case 2:
        if(Drive_ModeUP == 1)
        {
           VMODE_N++;
        if(R_DR==0){//CCW@@I[v[v
            switch(VMODE_N)
           { 
             case 1:VMODE=1;
                break;  
             case 2:VMODE=2;
                break;  
             case 3:VMODE=3;
                break;
             case 4:VMODE=4;
                break;
             case 5:VMODE=5;
                break;
             case 6:VMODE=6;
                break;
             default:VMODE=1;
             } 
            }
        if(R_DR==1){//CW@@I[v[v
            switch(VMODE_N)
           { 
             case 1:VMODE=6;
                break;  
             case 2:VMODE=5;
                break;  
             case 3:VMODE=4;
                break;
             case 4:VMODE=3;
                break;
             case 5:VMODE=2;
                break;
             case 6:VMODE=1;
                break;
             default:VMODE=6;
             } 
            }
           if(VMODE_N == 7){VMODE_N=1;}
           else{}
        
        }
        else{}
        break;
    case 3: /* ****** PWM_MODE *** */  
     
     if(R_DR==0){
        switch (UVW)
            {
                case 1: VMODE = 2;  break;//2
                case 2: VMODE = 4;  break;//4
                case 3: VMODE = 3;  break;//3
                case 4: VMODE = 6;  break;//6
                case 5: VMODE = 1;  break;//1
                case 6: VMODE = 5;  break;//5
                default: VMODE = 1; break;//1
               } 
          break;
        
         }
     #if 1
       if(R_DR==1){
         switch (UVW)
            {
                case 1: VMODE = 1;  break;//5
                case 2: VMODE = 3;  break;//3
                case 3: VMODE = 2;  break;//4
                case 4: VMODE = 5;  break;//1
                case 5: VMODE = 6;  break;//6
                case 6: VMODE = 4;  break;//2
                default: VMODE = 1; break;//1
               } 
          break;
          }
        #endif 
         default: break;
        }//Drive_state
      
    if(vr1_ad>0.05){
          R_DR=1;
          if((Drive_state==1)||(Drive_state==2)){
               mpi = Vpi*0.1;
              }
          else{
               mpi = Vpi*0.07;
              }
           }else{
          //R_DR=0;
            }
            
     if(vr1_ad<-0.05){
          R_DR=0;     
        if((Drive_state==1)||(Drive_state==2)){
               mpi =Vpi*0.1;
              }
          else{
               mpi =Vpi*0.07;
          }
        }else{
          //R_DR=1;
            }
   /* if(Drive_state==1){
         VMODE=1;
        }*/
      #if 1   
 /************* PWM 쓮**** */
    switch (VMODE)
    {
    case 1:   //mypwmA.write(mpi); mypwmB.write(0); mypwmC.write(0); 
              TIM1->CCR1 = PWM_frq*mpi; TIM1->CCR2 = 0; TIM1->CCR3 = 0;
              EN1=1; EN2 = 1; EN3= 0;//0:Active 1:High inpeedance
        break;
    case 2:   //mypwmA.write(mpi); mypwmB.write(0); mypwmC.write(0);
              TIM1->CCR1 = PWM_frq*mpi; TIM1->CCR2 = 0; TIM1->CCR3 = 0;
              EN1=1; EN2 = 0; EN3 = 1;
        break;
    case 3:   //mypwmA.write(0); mypwmB.write(mpi); mypwmC.write(0);
              TIM1->CCR1 = 0; TIM1->CCR2 = PWM_frq*mpi; TIM1->CCR3 = 0;
              EN1=0; EN2 = 1; EN3 = 1;
        break;
    case 4:   //mypwmA.write(0); mypwmB.write(mpi); mypwmC.write(0);
              TIM1->CCR1 = 0; TIM1->CCR2 = PWM_frq*mpi; TIM1->CCR3 = 0;
              EN1=1; EN2 = 1; EN3 = 0;
        break;
    case 5:   //mypwmA.write(0); mypwmB.write(0); mypwmC.write(mpi);
              TIM1->CCR1 = 0; TIM1->CCR2 = 0; TIM1->CCR3 = PWM_frq*mpi;
              EN1=1; EN2 = 0; EN3 = 1;
              
        break;
    case 6:   //mypwmA.write(0); mypwmB.write(0); mypwmC.write(mpi);
              TIM1->CCR1 = 0; TIM1->CCR2 = 0; TIM1->CCR3 = PWM_frq*mpi;
              EN1=0; EN2 = 1; EN3 = 1;
        break;
    default:  //mypwmA.write(0); mypwmB.write(0); mypwmC.write(0);
              TIM1->CCR1 = 0; TIM1->CCR2 = 0; TIM1->CCR3 = 0;
              EN1=0; EN2 = 0; EN3 = 0;
    break;
    }
#endif 
    
  }  //Function
/********main***********/

int main() {
    
    Timer1.start();  
    uTimer.start();
    vTimer.start();
    wTimer.start(); 
 
RCC->AHBENR |= (1 << 17); // GPIOA
RCC->AHBENR |= (1 << 18); // GPIOB
RCC->AHBENR |= (1 << 19); // GPIOC
RCC->APB2ENR |= (1 << 11); // TIM1
GPIOA-> MODER      = 0;
GPIOB-> MODER      = 0;
GPIOC-> MODER      = 0;
GPIOC-> MODER      |= 0b01 << 8;
GPIOC-> MODER      |= 0b01 << 12;
GPIOC-> MODER      |= 0b01 << 16;
GPIOC-> MODER      |= 0b01 << 18;//PC_9
GPIOC-> MODER      |= 0b01 << 20;
GPIOC-> MODER      |= 0b01 << 22;
GPIOC-> MODER      |= 0b01 << 24;
/************PWM Timer Setting************/
#if 1
    TIM1->CR1 = 0; // make sure Counter is disabled before changing configuration
    TIM1->CR2 = 0;
    TIM1->PSC = 1;
    TIM1->ARR =  PWM_frq;  // 500 33KHz 0x010f 166KHz 0x018f 125KHz   0x01ff 70KHz 90
    TIM1->CCMR1=0x6060;
    TIM1->CCMR2=0x0060;
    TIM1->CCER |= 0x0111;//555
    TIM1->BDTR |= 0x800a; // 0x80ca Dead Time
    TIM1->SR = 0;      // Clear flags.    
    TIM1->CR1 |= 0x0021; // 0x0021enable counter
     TIM1 ->DIER |= TIM_DIER_UIE;
     TIM1->DIER |= TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC3IE;
#endif    

/************PWM Port Setting************/
#if 1

GPIOA->MODER |= (0b10 << 16); // PA8 Alternate TIM1_CH1
GPIOA->MODER |= (0b10 << 18); // PA9 Alternate TIM1_CH2
GPIOA->MODER |= (0b10 << 20); // PA10 Alternate TIM1_CH3

GPIOB->MODER |= (0b10 << 26); // PB13 Alternate TM1_CH1N
GPIOB->MODER |= (0b10 << 28); // PB14 Alternate TM1_CH2N
GPIOB->MODER |= (0b10 << 2);  // PB1 Alternate  TM1_CH3N

GPIOA-> AFR[1] |= 0b0110 << 0;
GPIOA-> AFR[1] |= 0b0110 << 4;
GPIOA-> AFR[1] |= 0b0110 << 8;

GPIOB-> AFR[1] |= 0b0110 << 20;
GPIOB-> AFR[1] |= 0b0110 << 24;
GPIOB-> AFR[0] |= 0b0110 << 4;
#endif 

/************ADC1 Analog Port Setting************/
#if 1
GPIOA->MODER |= (0b11 << 0); // PA0 for ADC1

GPIOC->MODER |= (0b11 << 0); // PC0 for ADC2
GPIOC->MODER |= (0b11 << 2); // PC1 for ADC3
GPIOC->MODER |= (0b11 << 4); // PC2 for ADC4

GPIOC->MODER |= (0b11 << 6); // PC3 for ADC5 BEMF1
GPIOB->MODER |= (0b11 << 0); // PB0 for ADC6 BEMF2
GPIOA->MODER |= (0b11 << 14); // PA7 for ADC7 BEMF3

//GPIOC->MODER |= (0b11 << 6); // PB1 for ADC4
#endif 

/************ADC1 Setting************/

    RCC->AHBENR |= (1 << 28); // turn on ADC12 clock
    ADC1_COMMON->CCR |= (0b10 << 16); // 0b01
 #if 1   
    ADC1->CR =0;
    ADC1->CFGR=0;
     ADC1->CFGR |= ADC_CFGR_DMAEN | ADC_CFGR_DMACFG ; // enable DMA as Circular Mode.
     ADC1->CFGR |=  ADC_CFGR_CONT | ADC_CFGR_OVRMOD  ; // continuous single convert mode
   
#endif
#if 1
    ADC1->SQR1 |= 6;
    ADC1->SQR1 |= (0b00001 << 6);//PA0 CurentU
    ADC1->SQR1 |= (0b00111 << 12);//PC1 CurentV
    ADC1->SQR1 |= (0b00110 << 18);//PC0 CurentW
    
    ADC1->SQR1 |= (0b01000 << 24);//PC2 Volume
    
    ADC1->SQR2 |= (0b01001 << 0);//PC3 BEMF1
    ADC1->SQR2 |= (0b01011 << 6);//PB0 BEMF2
    ADC1->SQR2 |= (0b01111 << 12);//PA7 BEMF3
    
    //ADC1->SMPR1 =0x00000010;
    ADC1->SMPR1 |= (0b001 << 3); // 0b000 -> 7.5 clock cycles, shortest available sampling time
    ADC1->SMPR1 |= (0b001 << 6); 
    ADC1->SMPR1 |= (0b001 << 9); 
    ADC1->SMPR1 |= (0b001 << 12); 
    ADC1->SMPR1 |= (0b001 << 15);
    ADC1->SMPR1 |= (0b001 << 18);
    ADC1->SMPR1 |= (0b001 << 21);
    ADC1->SMPR1 |= (0b001 << 24); 
    ADC1->SMPR1 |= (0b001 << 27);    
    
#endif   
    //ADC1->IER =0x00000000;
    ADC1->CR |=0x00000001;
    //ADC1->CR |= ADC_CR_ADSTART; // start a/d convert
    while (!(ADC1->ISR & (1 << 0)));
 
/************DMA Setting************/
#if 1
    RCC->AHBENR |= RCC_AHBENR_DMA1EN; // supply clock to DMA1
    DMA1_Channel1->CPAR = (uint32_t)&ADC1->DR; // peripheral addresss
    DMA1_Channel1->CMAR = (uint32_t)ADC_buffer; // memory address
    DMA1_Channel1->CNDTR = 7; // 2 data *ύX*
    DMA1_Channel1->CCR |= DMA_CCR_PSIZE_0 | DMA_CCR_MSIZE_0 | DMA_CCR_CIRC;
    DMA1_Channel1->CCR |=(0b11<<12);
    // Transfer complete interrupt enable
     DMA1_Channel1->CCR |= (1 << 1);
    // Peripheral: 32bit, Memory: 16bit, Read from Peripheral, circular mode.
    DMA1_Channel1->CCR |= DMA_CCR_EN | DMA_CCR_MINC ;
     
#endif
/*****************************************************************************/
  
   //GPIOC-> ODR |= 1<<10;//EN1=1;
   //GPIOC-> ODR |= 1<<11;//EN2=1;
   //GPIOC-> ODR |= 1<<12;//EN3=1;
   wait_ms(750);
    Timer1.start();  
    uTimer.start();
    vTimer.start();
    wTimer.start();
   GPIO=1;
   if((TIM1->SR & TIM_SR_UIF)){//&&(TIM1->CNT>=900)) {  
           ADC1->CR |= 4; // start a/d convert
           while(!(ADC1->ISR & 8 )){}
            TIM1->SR = 0; /* clear UIF */ 
            Vr_adc_i=ADC_buffer[3]/4095.0;//V_adc.read(); 
   
         } 
 while(1){  
   
    #if 1
     if((TIM1->SR & TIM_SR_UIF)){ 
           ADC1->CR |= 4; // start a/d convert
           while(!(ADC1->ISR & 8 )){}
            TIM1->SR = 0; /* clear UIF */ 
            Function();
            curr_ub=ADC_buffer[0]/(4095.0);//Curr_ui.read();
            curr_vb=ADC_buffer[1]/(4095.0);//Curr_vi.read();
            curr_wb=ADC_buffer[2]/(4095.0);//Curr_wi.read();
            bemf1=ADC_buffer[4]/(4095.0);
            bemf2=ADC_buffer[5]/(4095.0);
            bemf3=ADC_buffer[6]/(4095.0);
            
         } 
    #endif
   
#if 0
/********s************/
scnt_m=scount%2; 
if(scnt_m==0){
  shuki=0;
   tss1=Timer1.read_us();
  
  }else{
  shuki=1;
   tss2=Timer1.read_us();
  }
   tss= (tss2-tss1);
  // atss=abs(tss);
scount++;
/*************************/
#endif
          //SWAVE=Nrpm_s/10000.0+0.5;
          //SWAVE=Nrpm_ref/5000.0;
         // SWAVE=vr1_ad+0.5;
         //SWAVE=Vr_adc_i;
          // SWAVE=Vpi+0.5;
          //SWAVE=I_ref/20+0.5;
         // SWAVE=Nerr/50+0.5;
         //SWAVE=curr_wb;
         // SWAVE=curr_u;
         //SWAVE=I_FB;
       // SWAVE=I_FB+0.5;
          //SWAVE=bemf3_f;
          //SWAVE=Vu;
          //SWAVE=Vv;
         // SWAVE=Vu;
          //SWAVE=bemf3;
           //SWAVE=Vu;
          //SWAVE=bemf1;
           //SWAVE=Vu_min;
            //SWAVE=BEmax;
            SWAVE=bemf1_f;
            //SWAVE=Nrpm/10000.0;
            //SWAVE=doki_cnt/500;
            // SWAVE=Drive_ModeUP;
     }//while
     }//main