//모터드라이버핀과 아두이노의 연결번호
#define EA 3
#define EB 11
#define M_IN1 4
#define M_IN2 5
#define M_IN3 13
#define M_IN4 12
//추적모듈 아두이노 핀번호
#define Sensor 8

int motorA_vector = 0;//모터의 회전방향이 반대일시 0을 1로 1을 0으로 바꿔주시면 모터의 회전방향이 바뀝니다.
int motorB_vector = 1;//모터의 회전방향이 반대일시 0을 1로 1을 0으로 바꿔주시면 모터의 회전방향이 바뀝니다.
int motor_speed = 90;//모터 스피드

void setup()  //초기화
{
  Serial.begin(9600);
  //모터관련 핀들을 출력으로 설정한다.
  pinMode(EA, OUTPUT);
  pinMode(EB, OUTPUT);
  pinMode(M_IN1, OUTPUT);
  pinMode(M_IN2, OUTPUT);
  pinMode(M_IN3, OUTPUT);
  pinMode(M_IN4, OUTPUT);
  //센서 입력 설정
  pinMode(Sensor, INPUT);

  delay(3000);
}

void loop() //무한루프
{
  if (digitalRead(Sensor)) { // 센서가 감지되면
    Serial.println(" BLACK ");
    motor_con(motor_speed, 0);  // MotorA 정회전, MotorB 정지
  } else {                  // 센서가 감지되지 않으면
    Serial.println(" WHITE ");
    motor_con(0, motor_speed);  // MotorA 정지, MotorB 정회전
  }
}

void motor_con(int M1, int M2) {

  if (M1 > 0) { //모터A 정회전
    digitalWrite(M_IN1, motorA_vector);
    digitalWrite(M_IN2, !motorA_vector);
  }
  else if (M1 < 0) { //모터A 역회전
    digitalWrite(M_IN1, !motorA_vector);
    digitalWrite(M_IN2, motorA_vector);
  }
  else {       //모터 A 정지
    digitalWrite(M_IN1, LOW);
    digitalWrite(M_IN2, LOW);
  }

  if (M2 > 0) { //모터B 정회전
    digitalWrite(M_IN3, motorB_vector);
    digitalWrite(M_IN4, !motorB_vector);
  }
  else if (M2 < 0) { //모터B 역회전
    digitalWrite(M_IN3, !motorB_vector);
    digitalWrite(M_IN4, motorB_vector);
  }
  else {         //모터 B 정지
    digitalWrite(M_IN3, LOW);
    digitalWrite(M_IN4, LOW);
  }
  analogWrite(EA, abs(M1)); //A모터 속도값
  analogWrite(EB, abs(M2)); //B모터 속도값
}
