#include "Servo.h"  // 서보모터를 사용하기 위한 헤더파일 호출
#define EA 3 // 모터드라이버 EA 핀, 아두이노 디지털 3번 핀에 연결
#define EB 11 // 모터드라이버 EB 핀, 아두이노 디지털 11번 핀에 연결
#define M_IN1 4 // 모터드라이버 IN1 핀, 아두이노 디지털 4번 핀에 연결
#define M_IN2 5 // 모터드라이버 IN2 핀, 아두이노 디지털 5번 핀에 연결
#define M_IN3 13 // 모터드라이버 IN3 핀, 아두이노 디지털 13번 핀에 연결
#define M_IN4 12 // 모터드라이버 IN4 핀, 아두이노 디지털 12번 핀에 연결
#define echo 6 // 초음파센서 에코(Echo) 핀, 아두이노 우노 보드의 디지털 6번 핀 연결
#define trig 7 // 초음파센서 트리거(Trigger) 핀, 아두이노 우노 보드의 디지털 7번 핀 연결
#define servo_motor 2 // 서보모터 Signal 핀, 아두이노 우노 보드 디지털 3번 핀에 연결
int motorA_vector = 0; 
// 모터의 회전방향이 반대일 시 0을 1로, 1을 0으로 바꿔주면 모터의 회전방향이 바뀜.
int motorB_vector = 1; 
// 모터의 회전방향이 반대일 시 0을 1로, 1을 0으로 바꿔주면 모터의 회전방향이 바뀜.
int motor_speed = 210; // 모터 스피드 0 ~ 255, PWM Width
unsigned int read_ultrasonic(void); // 초음파센서 값 읽어오는 함수
void motorA_con(int M1); // 모터A 컨트롤 함수
void motorB_con(int M2); // 모터B 컨트롤 함수
int timer = 0;

#define Servo_Mt_Speed 10
#define Effective_distance 5
#define Margin_distance 20
void setup() // 초기화
{
  pinMode(EA, OUTPUT); // EA 핀 출력 설정
  pinMode(EB, OUTPUT); // EB 핀 출력 설정
  pinMode(M_IN1, OUTPUT); // IN1 핀 출력 설정
  pinMode(M_IN2, OUTPUT); // IN2 핀 출력 설정
  pinMode(M_IN3, OUTPUT); // IN3 핀 출력 설정
  pinMode(M_IN4, OUTPUT); // IN4 핀 출력 설정
  pinMode(trig, OUTPUT); // trig 출력 설정
  pinMode(echo, INPUT); // echo 입력설정
  delay(3000); // 갑작스러운 움직임을 막기위한 3초간 지연
}

void loop() // 무한루프
{
  static int sequence = 0;
  static int i, j, temp, temp2 = 0;
  if (sequence == 0) // 첫번째 시퀸스
  {
if (read_ultrasonic() < Effective_distance) 
// 만약 측정한 거리 값이 Effective_distance 작으면 자동차 정지
    {
      digitalWrite(M_IN1, LOW); // IN1번에 LOW
      digitalWrite(M_IN2, LOW); // IN2번에 LOW
      digitalWrite(M_IN3, LOW); // IN3번에 LOW
      digitalWrite(M_IN4, LOW); // IN4번에 LOW
      sequence = 1;
    }
    else // 측정한 거리 값이 Effective_distance보다 작지 않으면 자동차 전진
    {
      motorA_con(motor_speed); // 모터A 정방향
      motorB_con(motor_speed); // 모터B 정방향
      sequence = 0;
    }
  }
  else if (sequence == 1) // 두번째 시퀸스
  {
    if (read_ultrasonic() > Margin_distance)
    {
      sequence = 2;
    }
    else //후진
    {
      motorA_con(-(motor_speed)); // 모터A 정방향
      motorB_con(-(motor_speed)); // 모터B 역방향
      sequence = 1;
    }
  }
  else if (sequence == 2) // 세번째 시퀸스수행
  {
    if (read_ultrasonic() < Margin_distance)
    {
      sequence = 1;
      return;
    }

if (read_ultrasonic() > (Margin_distance + 25)) 
// 측정 시 공간이 있는 좌표가 확인되면 해당 지점으로 전진
    {
      digitalWrite(M_IN1, LOW); // IN1번에 LOW
      digitalWrite(M_IN2, LOW); // IN2번에 LOW
      digitalWrite(M_IN3, LOW); // IN3번에 LOW
      digitalWrite(M_IN4, LOW); // IN4번에 LOW
      sequence = 0;
    }
    else
    {
      motorA_con(motor_speed); // 모터A 정방향
      motorB_con(-(motor_speed)); // 모터B 역방향
      sequence = 2;
    }
  }
}

unsigned int read_ultrasonic(void) // 초음파센서 값 읽어오는 함수
{
  float return_time, howlong; // 시간 값과 거리 값을 저장하는 변수를 만든다.
  // 초음파센서는 초음파를 발사 후 돌아오는 시간을 역산하여 거리 값으로 계산한다.
  digitalWrite(trig, LOW); // Trig 핀 Low
  delayMicroseconds(2); // 2uS 지연
  digitalWrite(trig, HIGH); // Trig 핀 High, 초음파 송출
  delayMicroseconds(10); // 10uS 지연
  digitalWrite(trig, LOW); // 초음파 송출 정지
  return_time = pulseIn(echo, HIGH); // 피드백된 펄스폭 읽어들여 return_time 변수에 저장
  howlong = ((float)(340 *return_time) / 10000) / 2; // 시간을 거리 값으로 환산
  return howlong; // 함수 종료 시 거리 값 반환
}

void motorA_con(int M1) // 모터A 컨트롤 함수
{
  if (M1 > 0) // M1이 양수일 때, 모터A 정회전
  {
    digitalWrite(M_IN1, motorA_vector); // IN1번에 HIGH(motorA_vector가 0이면 LOW)
    digitalWrite(M_IN2, !motorA_vector); // IN2번에 LOW(motorA_vector가 0이면 HIGH)
  }
  else if (M1 < 0) // M1이 음수일 대, 모터A 역회전
  {
    digitalWrite(M_IN1, !motorA_vector); // IN1번에 LOW(motorA_vector가 0이면 HIGH)
    digitalWrite(M_IN2, motorA_vector); // IN2번에 HIGH(motorA_vector가 0이면 LOW)
  }
  else // 모터A 정지
  {
    digitalWrite(M_IN1, LOW); // IN1번에 LOW
    digitalWrite(M_IN2, LOW); // IN2번에 LOW
  }

  analogWrite(EA, abs(M1)); // M1의 절대값으로 모터A 속도 제어
}

void motorB_con(int M2) // 모터B 컨트롤 함수
{
  if (M2 > 0) // M2가 양수일 때, 모터B 정회전
  {
    digitalWrite(M_IN3, motorB_vector); // IN3번에 HIGH(motorB_vector가 0이면 LOW)
    digitalWrite(M_IN4, !motorB_vector); // IN4번에 LOW(motorB_vector가 0이면 HIGH)
  }
  else if (M2 < 0) // M2가 음수일 때, 모터B 역회전
  {
    digitalWrite(M_IN3, !motorB_vector); // IN3번에 LOW(motorB_vector가 0이면 HIGH)
    digitalWrite(M_IN4, motorB_vector); // IN4번에 HIGH(motorB_vector가 0이면 LOW)
  }
  else // 모터B 정지
  {
    digitalWrite(M_IN3, LOW); // IN3번에 LOW
    digitalWrite(M_IN4, LOW); // IN4번에 LOW
  }

  analogWrite(EB, abs(M2)); // M2의 절대값으로 모터B 속도 제어
}
