/********Trapezoid Nucleo-F302R8 IHM07 BR2804@Speed+Current**************************/
#include "mbed.h"
DigitalOut shuki(PC_6);
DigitalOut ad_time(PC_4);

PwmOut PWMu(PA_8); 
PwmOut PWMv(PA_9); 
PwmOut PWMw(PA_10);

AnalogIn V_adc(PC_2); 

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

DigitalIn  HU(PA_15);
DigitalIn  HV(PB_3);
DigitalIn  HW(PB_10);

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

AnalogOut SWAVE(PA_4);

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

Serial pc(USBTX,USBRX);

Timer uTimer;
Timer vTimer;
Timer wTimer;

Timer Timer1;

#define I_Directive 1.4  /* dwߍől[A] */
#define V_Directive  12.0    /* dwߍől[V]*/
/********Softwear Edge***************/
unsigned short edge_UP=1;
unsigned short edge_UN=1;
unsigned short edge_VP=1;
unsigned short edge_VN=1;
unsigned short edge_WP=1;
unsigned short edge_WN=1;
/*******************************************/
/*******************************************/
unsigned int scnt_m=0,scount=0; 
unsigned char  HUVW;
unsigned char  VMODE;
float   Vr_adc_i,vr1_ad,vr_ad,vr1_ad_p;
float   Duty=0;
unsigned char UP,VP,WP;
unsigned char kukei_U,kukei_V,kukei_W;
unsigned char EN_U,EN_V,EN_W;

unsigned char  R_DR = 1; /* R_DR -> 1:CWA0:CCW*/
/*********************************************/
unsigned int t_cnt = 0; /* Lv`Ԃcnt */
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 */
/**********************************************/
/* **************** */
/*     x     */
/* **************** */
float   kaiten=0;
float   Nerr = 0.0,Nerr_fil=0; /* x덷 */
float  kpSpeed = 0.01;//0.01;    /*  QC */
float  kiSpeed = 0.005;//0.005; /*  ϕQC */
float  s_kiSpeed = 0.0; /* ϕ풆g */
float   Nrpm = 0;/* ][r/min] */
float   Nrpm_1 = 0;/* ][r/min]old  */
float   Nrpm_s = 0;/* ] ɐ[r/min]  */
float   Nref0=0;
float   Nref1 = 15000.0;/* xwߍől */
float   Nrpm_ref = 0.0; /* xw[r/min] */
float   Nrpm_ref0 = 0.0; /* xw[r/min] */
float   dN_HI_P = 0.5; /* Cg*/
float   dN_HI_N = 0.5; /* Cg */
float   N_LOW_MAX=5;
float   ijou=1500;
unsigned int t_cnt_min= 500; /* Lv`Ԃ̍Œcnt */
float I_Order=0;
float Speed,obj_Speed;
/*********************************************/
/* **************** */
/*     d     */
/* **************** */
float kpCurrent; /* QC */
float kiCurrent; /* ϕQC */
float s_kiCurrent=0;
float I_err=0;
float I_Shunt=0;
float curr_u,curr_v,curr_w;
float  V_order=0; 
/********************************************/
 /* 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;
    }
    
    /* 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=7 
    if( ((Nrpm-Nrpm_1)<ijou) && ((Nrpm-Nrpm_1)>(-ijou)) )
    { /* xُ */
           Nrpm_1 = Nrpm;
    }
    else
    {
       Nrpm = Nrpm_1;
    }
    Vrise_t_1 = Vrise_t;
    }
     
    /* Hall_WɃLv` */
void Hall_w()
    {           
    Wrise_t = wTimer.read_us();
    t_cnt = Wrise_t - Wrise_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;
    }
    Wrise_t_1 = Wrise_t;
    } 
/*********************************/
unsigned char UVW_In(void)
{
    unsigned char temp8;
    temp8 = HW;  //W 
    temp8 = (temp8+temp8) + HV; //V
    temp8 = (temp8+temp8) + HU; //U
    
    return temp8;
}   
/********main***********/
int main(){
    
    
    pc.baud(128000); 
    
      
    PWMu.period_us(20);
    PWMv.period_us(20);
    PWMw.period_us(20);
    
    Timer1.start();  
    uTimer.start();
    vTimer.start();
    wTimer.start();
    
    wait_ms(100);
     Vr_adc_i=V_adc.read();  
     
       
 while(1) {
   //pc.printf( "%d\r", SystemCoreClock );
   UP=HU; VP=HV; WP=HW;
   EN_U=EN1;EN_V=EN2;EN_W=EN3;
   //pc.printf("%d,%d,%d,%d,%d,%d \r" ,kukei_W,kukei_V,kukei_U,WP,VP,UP);
   //pc.printf("%d,%d,%d,%d,%d,%d \r" ,kukei_W,kukei_V,kukei_U,EN_W,EN_V,EN_U);
   //pc.printf("%d \r" ,scnt_m);
    // pc.printf("%.1f,%.1f\r" ,Speed,obj_Speed);
/*******{[dǍ************/
   vr_ad=V_adc.read(); 
   vr1_ad_p=(vr_ad-Vr_adc_i);
   vr1_ad+=(vr1_ad_p-vr1_ad)*0.1;
   
/**********[_t߂̐ݒ***********/
    if(fabs(vr1_ad)<0.05)
    {   
        I_Order= 0;
        Nerr=0;
        Nrpm=0;
        Nrpm_ref = 0.0;
        Nrpm_1 = 0;/* ][r/min]old  */
        Nrpm_s = 0;
        s_kiSpeed = 0.0;
        s_kiCurrent = 0.0;
        Duty=0;
        Nrpm_ref0 = 0.0;
        Duty=0;  
        t_cnt=0; 
        V_order=0;  
       I_err=0;
       I_Shunt=0;  
       Nref0=0;
    }else{}
/*********VgdǍ*******************/
     ad_time=1;
      curr_u += ((Curr_ui) - curr_u )*0.1;
      curr_v += ((Curr_vi) - curr_v )*0.1;
      curr_w += ((Curr_wi) - curr_w )*0.1;
    ad_time=0;
   
/********dPIϐݒ******************/  
 #if 1
    if( Nrpm>100){
                kpCurrent= 0.0003; /* QC */
                kiCurrent =0.0001; /* ϕQC */
               
        }else{
             kpCurrent=0.001; 
             kiCurrent =0.005;
            
            }
#endif  
/*************************************/
    /* ------------ */
    /* z[ZTǍ */
    /* ------------ */
    HUVW = UVW_In(); /* ZTtEz[ICM */
     if(R_DR==1){
          switch (HUVW)
            {
                case 1: VMODE = 2;  break;
                case 2: VMODE = 4;  break;
                case 3: VMODE = 3;  break;
                case 4: VMODE = 6;  break;
                case 5: VMODE = 1;  break;
                case 6: VMODE = 5;  break;
                default: VMODE = 1; break;
               } 
               
               }
   if(R_DR==0){
   switch (HUVW)
    {
        case 1: VMODE = 5;      break;
        case 2: VMODE = 1;      break;
        case 3: VMODE = 6;      break;
        case 4: VMODE = 3;      break;
        case 5: VMODE = 4;      break;
        case 6: VMODE = 2;      break;
        default: VMODE = 1;           
        break;
    }
    } 
  
/* *********************************************** */    
/* [ 1 ] xo */
/* *********************************************** */ 
 #if 1 
  HA.rise(&Hall_u);
  //HA.fall(&Hall_u);
  HB.rise(&Hall_v);
  //HB.fall(&Hall_v);
  HC.rise(&Hall_w);
  //HC.fall(&Hall_w);
  #endif 
  #if 0
if(R_DR==0){
 if((HU==1)&&(edge_UP==1)){
     Hall_u();
     edge_UP=0;
    edge_VP=1;
    edge_WP=0;
   
 }   

if((HV==1)&&(edge_VP==1)){
     Hall_v();
     edge_UP=0;
    edge_VP=0;
    edge_WP=1;
   
 }
if((HW==1)&&(edge_WP==1)){
     Hall_w();
    edge_UP=1;
    edge_VP=0;
    edge_WP=0;
   
 } 
}

if(R_DR==1){
 if((HU==1)&&(edge_UP==1)){
     Hall_u();
     edge_UP=0;
    edge_VP=0;
    edge_WP=1;
    
 }    
if((HV==1)&&(edge_VP==1)){
     Hall_v();
     edge_UP=1;
    edge_VP=0;
    edge_WP=0;
    
 }
if((HW==1)&&(edge_WP==1)){
     Hall_w();
    edge_UP=0;
    edge_VP=1;
    edge_WP=0;
    
 } 
} 
#endif 
  
/************* CW CCW ******/   
    if(vr1_ad>0.05){//cw
          R_DR=1;   
          }else{}
            
     if(vr1_ad<-0.05){//ccw
          R_DR=0;
        }else{}
        
    /* ] ɐ@ */
    if(R_DR==1)/* CW */
    {
        Nrpm_s = Nrpm;
    }
    else/* CCW */
    {
        Nrpm_s = -Nrpm;  
    }
  
 /************************************************ */
 /* [ 2 ] x  */
 /* *********************************************** */     
        
    Nrpm_ref0 = fabs(Nref1*vr1_ad); //aki
        
      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; 
        
        if(R_DR==1) { 
            Nerr =( Nrpm_ref-Nrpm_s);
        }
        else {
            Nerr = Nrpm_ref+Nrpm_s;
        }  
      Nerr_fil=(Nerr-Nerr_fil)*0.01;  
    /* ----------- */
    /* xPI */
    /* ----------- */
    s_kiSpeed += kiSpeed*Nerr_fil;
    if(s_kiSpeed > I_Directive)
    {
    s_kiSpeed = I_Directive;
    }
    else
    {
    if(s_kiSpeed < (-I_Directive))
    {
       s_kiSpeed = -I_Directive; 
    }else{}
    }
    
    I_Order = s_kiSpeed + kpSpeed*Nerr;
    
    if(I_Order > I_Directive)
    {
    I_Order = I_Directive;
    }
    else
    {
    if(I_Order < (-I_Directive))
    {
        I_Order = -I_Directive; 
    }else{}
    }   
/* *********************************************** */
/* [ 3 ] d  */
/* *********************************************** */

    /* ----------- */
    /* ACRPI */
    /* ----------- */
/*********z[ZT񂩂̃VgdI*******************/
if(R_DR==1){
     switch (VMODE)
    {
        case 1: I_Shunt = (curr_v);
        break;
        case 2: I_Shunt = (curr_w );
        break;
        case 3: I_Shunt = (curr_w);
        break;
        case 4: I_Shunt = (curr_u );
        break;
        case 5: I_Shunt = (curr_u );
        break;
        case 6: I_Shunt = (curr_v);
        break;
        default:I_Shunt = 0.0;
      }
    }
if(R_DR==0){
     switch (VMODE)
    {
        case 1: I_Shunt = (curr_w);
        break;
        case 2: I_Shunt = (curr_w );
        break;
        case 3: I_Shunt = (curr_u);
        break;
        case 4: I_Shunt = (curr_u );
        break;
        case 5: I_Shunt = (curr_v );
        break;
        case 6: I_Shunt = (curr_v);
        break;
        default:I_Shunt = 0.0;
      }
    }
/*********dPI*******************/
    I_err = I_Order - I_Shunt;
    s_kiCurrent += kiCurrent*I_err;
    if(s_kiCurrent > V_Directive)
    {
    s_kiCurrent = V_Directive;
    }
    else
    {
    if(s_kiCurrent < (-V_Directive))
    {
       s_kiCurrent = -V_Directive; 
    }else{}
    }
    
    
    V_order = s_kiCurrent + kpCurrent*I_err;
    
    if(V_order > V_Directive)
    {
    V_order = V_Directive;
    }
    else
    {
    if(V_order < (-V_Directive))
    {
        V_order = -V_Directive; 
    }else{}
    } 
    Duty= V_order*0.3;    
 /************* PWM 쓮********/
  if(R_DR==1){
   switch (VMODE)
    {
    case 1:   
              PWMu.write(Duty); PWMv.write(0); PWMw.write(0); 
              EN1=1; EN2 = 1; EN3= 0;//0:Active 1:High inpeedance
              kukei_U=shuki;kukei_V=0;kukei_W=0;
        break;
    case 2:  
             PWMu.write(Duty); PWMv.write(0); PWMw.write(0);
              EN1=1; EN2 = 0; EN3 = 1;
              kukei_U=shuki;kukei_V=0;kukei_W=0;
        break;
    case 3:      
              PWMu.write(0); PWMv.write(Duty); PWMw.write(0);
              EN1=0; EN2 = 1; EN3 = 1;
              kukei_U=0;kukei_V=shuki;kukei_W=0;
        break;
    case 4:       
              PWMu.write(0); PWMv.write(Duty); PWMw.write(0);
              EN1=1; EN2 = 1; EN3 = 0;
              kukei_U=0;kukei_V=shuki;kukei_W=0;
        break;
    case 5:                 
              PWMu.write(0); PWMv.write(0); PWMw.write(Duty);
              EN1=1; EN2 = 0; EN3 = 1;
              kukei_U=0;kukei_V=0;kukei_W=shuki;
        break;
    case 6:      
              PWMu.write(0); PWMv.write(0); PWMw.write(Duty);
              EN1=0; EN2 = 1; EN3 = 1;
              kukei_U=0;kukei_V=0;kukei_W=shuki;
        break;
    default:  PWMu.write(0); PWMv.write(0); PWMw.write(0);
              EN1=0; EN2 = 0; EN3 = 0;
              kukei_U=0;kukei_V=0;kukei_W=0;
    break;
    }
}

 if(R_DR==0){
    switch (VMODE)
    {
    case 1:   PWMu.write(Duty); PWMv.write(0); PWMw.write(0); 
              EN1=1; EN2 = 0; EN3= 1;//0:Active 1:High inpeedance
              kukei_U=shuki;kukei_V=0;kukei_W=0;
        break;
    case 2:   PWMu.write(0); PWMv.write(Duty); PWMw.write(0);
              EN1=0; EN2 = 1; EN3 = 1;
              kukei_U=0;kukei_V=shuki;kukei_W=0;
        break;
    case 3:   PWMu.write(0); PWMv.write(Duty); PWMw.write(0);
              EN1=1; EN2 = 1; EN3 = 0;
              kukei_U=0;kukei_V=shuki;kukei_W=0;
        break;
    case 4:   PWMu.write(0); PWMv.write(0); PWMw.write(Duty);
              EN1=1; EN2 = 0; EN3 = 1;
              kukei_U=0;kukei_V=0;kukei_W=shuki;
        break;
    case 5:   PWMu.write(0); PWMv.write(0); PWMw.write(Duty);
              EN1=0; EN2 = 1; EN3 = 1;
              kukei_U=0;kukei_V=0;kukei_W=shuki;
        break;
    case 6:   PWMu.write(Duty); PWMv.write(0); PWMw.write(0);
              EN1=1; EN2 = 1; EN3 = 0;
              kukei_U=shuki;kukei_V=0;kukei_W=0;
        break;
    default:  PWMu.write(0); PWMv.write(0); PWMw.write(0);
              EN1=0; EN2 = 0; EN3 = 0;
              kukei_U=0;kukei_V=0;kukei_W=0;
    break;
    }
}
Speed=(Nrpm);
obj_Speed=(Nrpm_ref0);
/*******DA************/
     SWAVE=Nrpm/3000;
   //SWAVE=Nerr_fil/10;
   //SWAVE=t_cnt/10000.0;
      //SWAVE=I_Shunt;
     //SWAVE=I_Order/1.4;
     //SWAVE=I_err/2.0;
      //SWAVE=curr_w;
     // SWAVE=V_order/3.3;
/********s************/
scnt_m=scount%2; 
if(scnt_m==0){
   shuki=0;
  }else{
   shuki=1;
  }
   scount++;
/*************************/
      }//while
     }//main