// MCU BASIC: https://www.basic4mcu.com
// DateTime : 2019-11-30 오후 3:43:20
// by Ok-Hyun Park
//
// 라인트레이서 //
// 라인을 따라 움직이도록 하며 중간에 장애물 발견시 정지한다.
// 장애물을 치우면 다시 전진하는 코드를 작성할 것
const int Left_motor_back=9;   //(IN1)
const int Left_motor_go=5;     //(IN2)
const int Right_motor_go=6;    //(IN3)
const int Right_motor_back=10; //(IN4)
const int key=A0,beep=13;
const int Echo=12,trig=11;
const int SensorRight=A2,SensorLeft=A3;
//
int   SL,SR;
float duration,distance;
//
void setup(){
  Serial.begin(9600);
  pinMode(trig,OUTPUT);
  pinMode(beep,OUTPUT);
  keysacn(); //키를 누르면 동작 시작
}
//
void run(){
  analogWrite(Right_motor_go,255); analogWrite(Right_motor_back,0);
  analogWrite(Left_motor_go ,255); analogWrite(Left_motor_back ,0);
}
//
void brake(){
  analogWrite(Right_motor_go,0); analogWrite(Right_motor_back,0);
  analogWrite(Left_motor_go ,0); analogWrite(Left_motor_back ,0);
}
//
void left(){
  analogWrite(Right_motor_go,255); analogWrite(Right_motor_back,0);
  analogWrite(Left_motor_go ,  0); analogWrite(Left_motor_back ,0);
}
//
void right(){
  analogWrite(Right_motor_go,  0); analogWrite(Right_motor_back,0);
  analogWrite(Left_motor_go ,255); analogWrite(Left_motor_back ,0);
}
//
void back(int time){
  analogWrite(Right_motor_go,0); analogWrite(Right_motor_back,255);
  analogWrite(Left_motor_go ,0); analogWrite(Left_motor_back ,255);
  delay(time*100);
}
//==========================================================
void keysacn(){
  while(!digitalRead(key)); digitalWrite(beep,HIGH); // 부저 소리 남
  while( digitalRead(key)); digitalWrite(beep,LOW);  // 부저 소리 없음
}
//
void loop(){
  digitalWrite(trig,HIGH); delayMicroseconds(10); digitalWrite(trig,LOW);
  duration=pulseIn(Echo,HIGH);
  distance=((float)(340*duration)/10000)/2;
  Serial.print("\nDIstance: "); Serial.println(distance);
  //
  SR=digitalRead(SensorRight);
  SL=digitalRead(SensorLeft);
  if     (SL==LOW  && SR==LOW )run();   // LOW=센서on // 둘 다 켜졌을 때,전진
  else if(SL==HIGH && SR==LOW )left();  // 오른쪽만 켜졌을 때,왼쪽 회전
  else if(SL==LOW  && SR==HIGH)right(); // 왼쪽만 켜졌을 때,오른쪽 회전
  else{ //SL==HIGH && SR==HIGH
    if(distance<20)brake(); // 장애물 20cm 이내발견되면 정지
    else           run();   // 장애물 치웠을 때,다시 전진
  }
}