/****************************************************************/
//  FILE        Auto-Car
//  DATE        :Tue, Jan 10, 2025
//  DESCRIPTION : 超音波センサーと赤外線センサーを使用して、
//                                自動的に障害物を回避して動く車の製作
//  CPU TYPE    : Arduino UNO
//                                                                    by Chiyabo 
//  This file is generated by Renesas Project Generator. 
/****************************************************************/
//
// インクルードファイル
//
#include < Servo.h >

const int trigPin = 4 ;
//const int echoPin = 5 ; 
const int echoPin = 11 ;  
const int motorL1Pin = 7 ;
const int motorL2Pin = 8 ;
const int motorR1Pin = 9 ;
const int motorR2Pin = 10 ;

const int L_PWM_Pin = 6 ;           //左モーターへのPWM出力
//const int R_PWM_Pin = 11 ;  	//右モーターへのPWM出力   
const int R_PWM_Pin = 5 ;          //右モーターへのPWM出力

long duration ;           // echoPinの読み込み値 
int distance ;              //障害物までの距離、単位cm
int R_distance ;         //右方向の障害物までの距離                
int L_distance ;         //左方向の障害物までの距離
int F_distance ;         //前方向の障害物までの距離

char Act ;                  // 前進処理中　true 
char Count ;             // Time(ms) = 0.1 X Count

Servo myServo ; 

void setup( ){
  pinMode( motorL1Pin, OUTPUT ) ; 
  pinMode( motorL2Pin, OUTPUT ) ; 
  pinMode( motorR1Pin, OUTPUT ) ;
  pinMode( motorR2Pin, OUTPUT ) ;
  pinMode( L_PWM_Pin, OUTPUT ) ;
  pinMode( R_PWM_Pin, OUTPUT ) ;
  
  pinMode( trigPin, OUTPUT ) ; 
  pinMode( echoPin, INPUT ) ;
  myServo.attach( 3 ) ;   //3ピンにサーボモーターを取り付ける 
  Act = false ;
  Count = 0 ;
 
  myServo.write( 0 ) ; //right Distance測定
  delay( 500 ) ; 
  myServo.write( 85 ) ; //forward
  delay( 500 ) ;  
  myServo.write( 170 ) ; //left Distance測定
  delay( 500 ) ; 
  myServo.write( 85 ) ; //forward
  delay( 500 ) ; 
  //UltraRcheck(  ) ;     //超音波センサー右方向の障害物のチェックと動作
  //UltraFcheck(  ) ;     //超音波センサー前方向の障害物のチェックと動作
  //UltraLcheck(  ) ;     //超音波センサー左方向の障害物のチェックと動作
}

void loop( ){ 
  //IRcheck(  ) ;         //赤外線センサーによる障害物のチェックと動作
  //UltraRcheck(  ) ; //超音波センサー右方向の障害物のチェックと動作
  //IRcheck(  ) ;
  //UltraFcheck(  ) ; //超音波センサー前方向の障害物のチェックと動作
  //IRcheck(  ) ;
  //UltraLcheck(  ) ;//超音波センサー左方向の障害物のチェックと動作
  //IRcheck(  ) ;
  //UltraFcheck(  ) ; //超音波センサー前方向の障害物のチェックと動作
  //forward( 135, 135, 130 ) ;   //障害物がない場合は直進、左右のdutyは直進するように調整
  UltraFcheck(  ) ;    //超音波センサー前方向の障害物のチェックと動作
}

void UltraFcheck( ){ 
  //myServo.write( 85 ) ; //forward 
  //delay( 500 ) ;  
  F_distance = calcDistance( ) ; 
  if( F_distance < 13 ){
    stop_back( 500 ) ; 
    myServo.write( 0 ) ; //right Distance測定 
    delay( 500 ) ; 
    R_distance = calcDistance( ) ; 
    myServo.write( 170 ) ; //left Distance測定
    delay( 500 ) ; 
    L_distance = calcDistance( ) ; 
    myServo.write( 85 ) ; //Servo位置をforward
    delay( 300 ) ; 
    if( R_distance > L_distance ){    //左右距離比較し距離の長い方に向きを変える
      right( 130, 130, 500 ) ; 
    }
    else {
      left(130, 130, 500 ) ; 
    }
    Act = false ; 
  } 
  
  if( ++Count >= 5 ){
    Count = 0 ;  
    UltraRcheck(  ) ; 
    UltraLcheck(  ) ; 
    myServo.write( 85 ) ; //forward  
    Act = false ; 
  }
  if( Act == false ){  
    // 9V * X 130/255 = 4.6V 
    forward( 130, 130, 130 ) ;   //障害物がない場合は直進、左右のdutyは直進するように調整
    Act = true ; 
  } 
  delay( 300 ) ;  
} 

void UltraRcheck( ){
  myServo.write( 45 ) ; //right 
  delay( 300 ) ; 
  R_distance = calcDistance( ) ; 
  if( R_distance < 19 ){ 
    motorStop( 10 ) ; 
    left( 130, 130, 50 ) ; 
  } 
} 

void UltraLcheck( ){ 
  //myServo.write( 125 ) ; //left
  myServo.write( 145 ) ; //left 
  delay( 300 ) ; 
  L_distance = calcDistance( ) ;  
  if( L_distance < 19 ){ 
    motorStop( 10 ) ;  
    right( 130, 130, 50 ) ; 
  }
} 
 
void stop_back( int backtime ){ 
  motorStop( 80 ) ; 
  back(160, 160, backtime ) ; 
  motorStop( 80 ) ; 
} 
 
void forward( int Lduty, int Rduty, int keeptime ){                 
  analogWrite( L_PWM_Pin, Lduty ) ; 
  analogWrite( R_PWM_Pin, Rduty ) ; 
  digitalWrite( motorL1Pin, HIGH ) ; 
  digitalWrite( motorL2Pin, LOW ) ;  
  digitalWrite( motorR1Pin, HIGH ) ;  
  digitalWrite( motorR2Pin, LOW ) ;  
  delay( keeptime ) ; 
}  

void back( int Lduty, int Rduty, int keeptime ){ 
  analogWrite( L_PWM_Pin, Lduty ) ; 
  analogWrite( R_PWM_Pin, Rduty ) ; 
  digitalWrite( motorL1Pin, LOW ) ;  
  digitalWrite( motorL2Pin, HIGH ) ; 
  digitalWrite( motorR1Pin, LOW ) ; 
  digitalWrite( motorR2Pin, HIGH ) ;  
  delay( keeptime ) ;  
} 

void right( int Lduty, int Rduty, int keeptime ){
  analogWrite( L_PWM_Pin, Lduty ) ;  
  analogWrite( R_PWM_Pin, Rduty ) ;  
  digitalWrite( motorL1Pin, HIGH ) ;  
  digitalWrite( motorL2Pin, LOW ) ; 
  digitalWrite( motorR1Pin, LOW ) ; 
  digitalWrite( motorR2Pin, HIGH ) ; 
  delay( keeptime ) ;  
} 
 
void left( int Lduty, int Rduty, int keeptime ){ 
  analogWrite( L_PWM_Pin, Lduty ) ; 
  analogWrite( R_PWM_Pin, Rduty ) ; 
  digitalWrite( motorL1Pin, LOW ) ; 
  digitalWrite( motorL2Pin, HIGH ) ; 
  digitalWrite( motorR1Pin, HIGH ) ; 
  digitalWrite( motorR2Pin, LOW ) ; 
  delay( keeptime ) ; 
} 

void motorStop( int keeptime ){  
  digitalWrite( motorL1Pin, LOW ) ;  
  digitalWrite( motorL2Pin, LOW ) ; 
  digitalWrite( motorR1Pin, LOW ) ; 
  digitalWrite( motorR2Pin, LOW ) ; 
  delay( keeptime ) ; 
}  

//超音波センサーによって測定された距離を計算するための関数
int calcDistance( ){ 
  digitalWrite( trigPin, LOW ) ; 
  delayMicroseconds( 2 ) ;  
  // trigPinを10マイクロ秒間HIGH状態に設定します  
  digitalWrite( trigPin, HIGH ) ;  
  delayMicroseconds( 10 ) ; 
  digitalWrite( trigPin, LOW ) ;  
  duration = pulseIn( echoPin, HIGH ) ; // echoPinを読み込み、音波の伝搬時間をマイクロ秒で返します
  distance = duration*0.034/2 ;  
  if( distance < 0 ){ 
    distance = 20 ; 
  }
  return distance ;  
} 

void  IRcheck( ){ 
  int R_IR ;
  int L_IR ; 
  R_IR = analogRead( A1 ) ;   // 右IRセンサーから読込む
  L_IR = analogRead( A0 ) ;   // 左IRセンサーから読込む 
  // ( R_IR < 100 & L_IR > 100 ){  
  if( ( R_IR < 100 ) && ( L_IR > 100 ) ){ 
    stop_back( 500 ) ; 
    left( 130, 130, 200 ) ;  //左回転 
  }
  //else if ( R_IR > 100 & L_IR < 100 ){ 
  else if( ( R_IR > 100 ) && ( L_IR < 100 ) ){ 
    stop_back( 500 ) ; 
    right( 130, 130,200 ) ; //右回転 
  } 
  //else if( R_IR < 100 & L_IR < 100 ){ 
  else if( ( R_IR < 100 ) && ( L_IR < 100 ) ){ 
    stop_back( 500 ) ; 
    left( 130, 130, 300 ) ;  //左回転 
  }
  else{
    ; 
  } 
}