#define in1 18    
#define in2 19    
#define in3 20   
#define in4 21   

#define PIN_TRIG A14
#define PIN_ECHO A15

#define R_S A12 // R s
#define L_S A13 // L s

#define Ultrasonic  0
#define forword 1
#define backword 2
#define turnRight 3
#define turnLeft 4
#define Stop 5
#define R_S 6
#define L_S 7
#define line 8

char Task = 0;

float readDistanceCM();
float distance =0;

unsigned long currentMillis = millis();

unsigned long Ultrasonic_previousMillis = 0;  
const long Ultrasonic_Timer = 1000;  

void Ultrasonic_Task ();
void forword_Task ();
void backword_Task ();
void turnRight_Task ();
void turnLeft_Task ();
void Stop_Task ();
void line_Task();


void setup() {
Serial.begin(115200); 
pinMode(PIN_TRIG, OUTPUT);
pinMode(PIN_ECHO, INPUT);
pinMode(in1, OUTPUT);  
pinMode(in2, OUTPUT);  
pinMode(in3, OUTPUT);    
pinMode(in4, OUTPUT);
pinMode(R_S, INPUT); 
pinMode(L_S, INPUT); 
}

void loop() {

  currentMillis = millis();

switch(Task){

 case Ultrasonic:
    distance = readDistanceCM();
    
    if ((distance > 28) && (((digitalRead(R_S) == 0)&&(digitalRead(L_S) == 0))
    ||((digitalRead(R_S) == 1)&&(digitalRead(L_S) == 1)))){
Task = forword ;
}
else{
 Task = Stop ; 
}
break;

case forword :
forword_Task();
Task = Ultrasonic ; //line
break;

case backword :
backword_Task();
Task = Ultrasonic ; 
break;

case turnRight :
turnRight_Task();
Task = Ultrasonic ; 
break;

case turnLeft :
turnLeft_Task();
Task = Ultrasonic ; 
break;

case Stop :
Stop_Task();
Task = turnRight ; 
break;

case line :
line_Task();
Task = Ultrasonic ; 
break;
 }
}

float readDistanceCM(){
if (currentMillis - Ultrasonic_previousMillis >= Ultrasonic_Timer) {  
  Ultrasonic_previousMillis = currentMillis; 
  digitalWrite(PIN_TRIG, HIGH);
  digitalWrite(PIN_TRIG, LOW);
  int duration = pulseIn(PIN_ECHO, HIGH);
  Serial.print(" CM: ");
  Serial.println(duration / 58);
  return duration * 0.034 / 2;
 }
}

void forword_Task(){ 
digitalWrite(in1, LOW);  
digitalWrite(in2, HIGH);  
digitalWrite(in3, HIGH);  
digitalWrite(in4, LOW);  
}
void backword_Task(){ 
digitalWrite(in1, HIGH);  
digitalWrite(in2, LOW);  
digitalWrite(in3, LOW);  
digitalWrite(in4, HIGH); 
}
void turnRight_Task(){ 
digitalWrite(in1, LOW);  
digitalWrite(in2, HIGH);  
digitalWrite(in3, LOW);  
digitalWrite(in4, HIGH);  
}
void turnLeft_Task(){ 
digitalWrite(in1, HIGH);  
digitalWrite(in2, LOW);  
digitalWrite(in3, HIGH);  
digitalWrite(in4, LOW);  
}
void Stop_Task(){ 
digitalWrite(in1, LOW); 
digitalWrite(in2, LOW);  
digitalWrite(in3, LOW); 
digitalWrite(in4, LOW);  
}

void line_Task(){  
//if((digitalRead(R_S) == 0)&&(digitalRead(L_S) == 0)){forward_Task();}   
if((digitalRead(R_S) == 1)&&(digitalRead(L_S) == 0)){turnRight_Task();} 
if((digitalRead(R_S) == 0)&&(digitalRead(L_S) == 1)){turnLeft_Task();}  
//if((digitalRead(R_S) == 1)&&(digitalRead(L_S) == 1)){forward_Task();}
}