void setup() {
  // put your setup code here, to run once:
  pinMode(0,INPUT_PULLUP);
  pinMode(1,INPUT_PULLUP);
  pinMode(2,INPUT_PULLUP);
  pinMode(3,INPUT_PULLUP);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  pinMode(8,OUTPUT);
// Serial.begin(9600);

}

int b=0;
  
  int a=0;
  void Input();
  int f=0;

void Right(int dutyR) ;
void Left(int dutyL) ;
void dire();
void Accel();

void Calculation();
void Switch();

void forward();
void backward();
void rightturn();
void leftturn();
void rightcarve();
void leftcarve();
void rightbackcarve();
void leftbackcarve();
void stopA();
double speedR=0.0;
double speedL=0.0;

//最高速度
//int speedAD = 0;   //最高速度の調整，0~255
double maxspeed = 0;     //  最高速度．
double maxspeed1 ;     //  最高速度．
double maxspeed2 ;     //  最高速度．
double maxspeedd=0;

//左右輪同期
int syncro = 0;    //±10だけデューティー値(0~255)をシフト，最高回転数の時は動作しない

double accel ;  //**
int GO;
int accelAD ;
double brake ;


void loop() {
    while (1){
        
            //最高速決定パート

  delayMicroseconds(20); //待機時間，アクイジションタイム，通例20μs
  GO = 1; //A/D変換開始
  //while(GO)   //A/D変換完了待ち
  maxspeed = analogRead(2)/4;   //0-255
  if(maxspeed>=220){
      maxspeed = 220;
  }
  else if(maxspeed<=30){
      maxspeed = 30;
  } 

  maxspeed1 = maxspeed;
  maxspeed2 = maxspeed/2;

//
    //左右調整パート

 delayMicroseconds(20);
 GO = 1;
 //while(GO)
 syncro = analogRead(3)/32-15;    //-15〜+17
 syncro = 2*syncro * speedR /255 ;
 syncro = (int)syncro ;
 


//加速度パート
 
 delayMicroseconds(20);
  GO = 1;
  //while(GO)
  accelAD = analogRead(4)/4;    //A/Dの値（8ビット）を保存
  Accel();


 double brake ;

        Input();

        Switch();
        
         }

}

void Accel(){
  if (accelAD>=250){
  accel = 3;
  }      
  else if(accelAD>=200&&accelAD<250){
  accel = 1 ;
  }
  else if (accelAD>=150&&accelAD<200){
  accel = 0.8 ;
  }  
  else if (accelAD>=100&&accelAD<150){
  accel = 0.5;
  }  
  else if (accelAD>=50&&accelAD<100){
  accel = 0.3 ;
  }
  else if (accelAD<50){
  accel = 0.1 ;
  }  
}

void Input(){
  /*
    int RB0=0;
  int RB1=1;
  int RB2=1;
  int RB3=1;
  */

  int RB0=digitalRead(0);
  int RB1=digitalRead(1);
  int RB2=digitalRead(2);
  int RB3=digitalRead(3);
  
  
        if (RB0 == 0 && RB1 == 1 && RB2 == 1 && RB3 == 1)    //RA0前RA1後RA2左RA3右
        {a = 1;  //前進
        }

        else if (RB0 == 1 && RB1 == 0 && RB2 == 1 && RB3 == 1)
        {
            a = 2;  //後退
        }

        else if (RB0 == 1 && RB1 == 1 && RB2 == 0 && RB3 == 1)
        {
            a = 3;  //左折
        }

        else if (RB0 == 1 && RB1 == 1 && RB2 == 1 && RB3 == 0)
        {
            a = 4;  //右折
        }

        else if (RB0 == 0 && RB1 == 1 && RB2 == 0 && RB3 == 1)
        {
            a = 5;  //左前
        }

        else if (RB0 == 0 && RB1 == 1 && RB2 == 1 && RB3 == 0)
        {
            a = 6;  //右前
        }

        else if (RB0 == 1 && RB1 == 0 && RB2 == 0 && RB3 == 1)
        {
            a = 7;  //左後
        }
        
        else if (RB0 == 1 && RB1 == 0 && RB2 == 1 && RB3 == 0)
        {
            a = 8;  //右後
        }
        
        else {
            a = 9;
        }    
}

void Right(int dutyR){
    dutyR = abs(dutyR);
analogWrite(5,dutyR);

    }


void Left(int dutyL){
    dutyL = abs(dutyL);
analogWrite(6,dutyL);

    }

void dire()
{
    if(speedR>0){
        digitalWrite(7,0);
        }
    else{
      digitalWrite(7,1);
      }
    
    if(speedL>0){
      digitalWrite(8,0);
      }
    else{
      digitalWrite(8,1);
      }
}


 void Switch(){            
      brake = accel*3 ;
switch (a)
{
case 1:
    if(speedR<0 ||speedL<0){
        stopA();
    }
    else{
    forward();
    }
break;

case 2:
    if(0<speedR || 0<speedL){
    stopA();
    }
    else{
    backward();
    }
break;

case 3:
    if(0<speedR && 0<speedL){
    stopA();
    }
    else{
    rightturn();
    }
break;

case 4:
    if(0<speedR && 0<speedL){
    stopA();
    }
    else{
    leftturn();
    }
break;

case 5:
    if(speedR<0 ||speedL<0){
        stopA();
    }
    else{    
    rightcarve();
    }    
break;

case 6:
    if(speedR<0 ||speedL<0){
        stopA();
    }
    else{
    leftcarve();
    }
break;

case 7:
    if(0<speedR || 0<speedL){
    stopA();
    }
    else{
    rightbackcarve();
    }
break;

case 8:
    if(0<speedR || 0<speedL){
    stopA();
    }
    else{
    leftbackcarve();
    }
break;

case 9:
    stopA();
    break;
}

  }

  void target(double TVR ,double TVL , double DV ){   
    /*あらゆる時に狙った数字(TVR,TVL)に指定した加速度(DV)で持ってきてくれるという便利なテンプレ的な存在*/

    if(speedR >TVR && speedL >  TVL ){     //speedが狙った数字(TVR,TVL)に対して-/-のとき
                
        speedR = speedR - DV;
                speedL = speedL - DV;
        
        if(speedR <= TVR ){
        speedR = TVR;
        }
                if(speedL <=  TVL){
        speedL = TVL;
        }
              
        }

      else if(speedR >TVR && speedL <  TVL ){          //speedが-/+
        
        speedR = speedR - DV;
                speedL = speedL + DV;
        
        if(speedR <= TVR ){
        speedR = TVR;
        }
                if(speedL >=  TVL){
        speedL = TVL;
          }
              
      }

    else if(speedR < TVR && speedL >  TVL ){            //speedが-/+
                
        speedR = speedR + DV;
                speedL = speedL - DV;
        
        if(speedR >= TVR ){
         speedR = TVR;
        }
                if(speedL <= TVL){
         speedL = TVL;
        }
        
        }

      else if(speedR < TVR && speedL <  TVL ){         //speedが+/+
        
        speedR = speedR + DV;
                speedL = speedL + DV;
        
        if(speedR >= TVR ){
        speedR = TVR;
        }
                if(speedL >= TVL){
        speedL = TVL;
          } 
      }

      else if (speedR == TVR && speedL <  TVL){
          speedR = speedR ;
                speedL = speedL + DV;
        
                if(speedL >= TVL){
        speedL = TVL;
          } 
      }

      else if (speedR == TVR && speedL >  TVL){
          speedR = speedR ;
                speedL = speedL - DV;
        
                if(speedL <= TVL){
        speedL = TVL;
          } 
      }

      else if (speedR > TVR && speedL ==  TVL){
          speedR = speedR - DV;
                speedL = speedL ;
        
        if(speedR <= TVR ){
        speedR = TVR;
        }             
      }

      else if (speedR < TVR && speedL ==  TVL){
          speedR = speedR + DV;
                speedL = speedL;
        
        if(speedR >= TVR ){
        speedR = TVR;
        }
      }

      else if (speedR == TVR && speedL ==  TVL){
        speedR = speedR ;
                speedL = speedL ;   
      }


}

  
  void forward(){
      
      target(maxspeed1,maxspeed1,accel);
      
      dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
  }
  
void backward(){

      target(-maxspeed2,-maxspeed2,accel/2);
      
     dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
}

void rightturn(){
    
      target(-maxspeed2,maxspeed2,accel/2);
      
     dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
}

void leftturn(){
    
      target(maxspeed2,-maxspeed2,accel/2);
      
     dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
}

void rightcarve(){
    
      target(maxspeed1*3/5,maxspeed1,accel);
      
     dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
}

void leftcarve(){
      target(maxspeed1,maxspeed1*3/5,accel);
      
     dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
}

void rightbackcarve(){
      target(-maxspeed2,-maxspeed2*2/5,accel/2);
      
     dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
}

void leftbackcarve(){
      target(-maxspeed2*2/5,-maxspeed2,accel/2);
      
     dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
}

void stopA(){
      target(0,0,brake);
      
     dire();
 Right(speedR+syncro);
 Left(speedL-syncro);
}