/********Trapezoid Nucleo-F302R8 BR2804 CW CCW **************************/
#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);

Serial pc(USBTX,USBRX);

/*******************************************/
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:]C0: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);
    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);
   vr_ad=V_adc.read(); 
   vr1_ad_p=(vr_ad-Vr_adc_i);
   vr1_ad+=(vr1_ad_p-vr1_ad)*0.1;
    /* ------------ */
    /* 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;
    }
    } 
  //  if(vr1_ad>0.05){
     Duty=fabs(vr1_ad);
   //  }else{}
/************* CW CCW ******/   
      if(vr1_ad>0.05){
          R_DR=1; 
          }else{}
            
     if(vr1_ad<-0.05){
          R_DR=0;
        }else{}
        
 /************* 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;
    }
}
/********s************/
scnt_m=scount%2; 
if(scnt_m==0){
   shuki=0;
  }else{
   shuki=1;
  }
   scount++;
/*************************/
      }//while
     }//main