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

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);

Serial pc(USBTX,USBRX);

Timer uTimer;
Timer vTimer;
Timer wTimer;

Timer Timer1;

#define I_Directive  1.4  /* dwߍől[A] */
/********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   Nerr = 0.0; /* x덷 */
float  kpSpeed = 0.0000001;    /*  QC */
float  kiSpeed = 0.00000005; /*  ϕQC */
float  s_kiSpeed = 0.0; /* ϕ݌v 1/S*/
float   Nrpm = 0;/* x@][r/min] */
float   Nrpm_1 = 0;/* O񑬓x@][r/min]old  */
float   Nrpm_s = 0;/* x ɐt[r/min]  */
float   Nref0=0;/*xw߃[g*/
float   Nref1 = 15000.0;/* xwߍől */
float   Nrpm_ref = 0.0; /* xw1[r/min] */
float   Nrpm_ref0 = 0.0; /* xw0[r/min] */
float   dN_HI_P = 0.5; /* [g*/
float   dN_HI_N = 0.5; /* [g */
float   N_LOW_MAX=5;/*]*/
float   ijou=1500;@/*]ُ*/
unsigned int t_cnt_min= 500; /* Lv`Ԃ̍Œcnt */
float I_Order=0;
float Speed,obj_Speed;
/***************************************************/
  
 /* 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)) )
    { 
           Nrpm_1 = Nrpm;
    }
    else
    {/* xُ */
       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)) )
    { 
           Nrpm_1 = Nrpm;
    }
    else
    {  /* xُ */
       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)) )
    { 
           Nrpm_1 = Nrpm;
    }
    else
    {  /* xُ */
       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\r " ,Speed);
  // 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.05f)
    {   
        I_Order= 0;
        Nerr=0;
        Nrpm=0;
        Nrpm_1 = 0;/* ][r/min]old  */
        Nrpm_s = 0;
        Nrpm_ref0 = 0.0;
        Nrpm_ref = 0.0; 
        Duty=0;  
        t_cnt=0;
        s_kiSpeed = 0.0;
    }else{}
/**************************************/    
    /* ------------ */
    /* 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;
    }
    } 
 
     //Duty=fabs(vr1_ad);
  
/* *********************************************** */    
/* [ 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
/************* 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;  
    }
#if 0
if(R_DR==1){
 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==0){
 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 
 /************************************************ */
 /* [ 2 ] x  */
 /* *********************************************** */     
        
    Nrpm_ref0 = fabs(Nref1*vr1_ad); //aki
        
      if(Nrpm_ref0>0){      
        if(Nref0 > Nrpm_ref0)
        {
                Nref0 -= dN_HI_N; 
                if(Nref0 < Nrpm_ref0)
                {
                Nref0 = Nrpm_ref0;
                }
                else{}
        }
        else{}
        
        if(Nref0 < Nrpm_ref0)
        {
            Nref0 += dN_HI_P;  
                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;
        }  
        
    /* ----------- */
    /* xPI */
    /* ----------- */
    s_kiSpeed += kiSpeed*Nerr;
    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{}
    }   
   Duty= I_Order;  
      
 /******* 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/20000.0;
    //SWAVE=t_cnt/5000;
   //SWAVE=t_cnt/5000.0;
     //SWAVE=Duty;
    //SWAVE= Nerr/1000;
/********s************/
#if 0
scnt_m=scount%2; 
if(scnt_m==0){
   shuki=0;
  }else{
   shuki=1;
  }
   scount++;
#endif
/*************************/
      }//while
     }//main