<h1>窮舉循跡</h1>

class wheel
{
  const int EN;
  const int IN1;
  const int IN2;

 public:
  wheel(int, int, int);
  wheel power(int);
};

wheel wheel::power(int P)
{
  if(P > 255)
    P =255;
   else if(P < -255)
    P =-255;

  if(P >= 0){//go straight Or Stop
    analogWrite(EN, P);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  }
  else{//go back
    analogWrite(EN, -P);
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
    
  return *this;
}

wheel::wheel(int a, int b, int c)
  :EN(a), IN1(b), IN2(c)
{
   pinMode(EN, OUTPUT);
   pinMode(IN1, OUTPUT);
   pinMode(IN2, OUTPUT);
}

void MotorWriting(wheel wheel_L, wheel wheel_R, int vL, int vR){
  wheel_L.power(vL);
  wheel_R.power(vR);
}


#define L_ENA 9
#define L_ENB 3  
#define L_IN1 7
#define L_IN2 6
#define L_IN3 5
#define L_IN4 4

wheel wheel_L(L_ENA, L_IN1, L_IN2);
wheel wheel_R(L_ENB, L_IN3, L_IN4);


void MotorCheck(){
  //forward
  MotorWriting(wheel_L, wheel_R, 100, 100);
  delay(1500);
  //stop
  MotorWriting(wheel_L, wheel_R, 0, 0);
  delay(1500);
  //back
  MotorWriting(wheel_L, wheel_R, -100, -100);
  delay(1500);
  //turn right
  MotorWriting(wheel_L, wheel_R, 100, -60);
  delay(1500);
  //turn left
  MotorWriting(wheel_L, wheel_R, -60, 100);
  delay(1500);
}

void Tracking(){
  int m= digitalRead(A2);
  int l2= digitalRead(A1);
  int l3= digitalRead(A0);
  int r2= digitalRead(A3);
  int r3= digitalRead(A4);

  if(l3==1&&l2==0&&m==0&&r2==0&&r3==0){//10000
  MotorWriting(wheel_L, wheel_R, 10,170);
}
else if(l3==0&&l2==1&&m==0&&r2==0&&r3==0){//01000
  MotorWriting(wheel_L, wheel_R, 70,150);
}
else if(l3==1&&l2==1&&m==0&&r2==0&&r3==0){//11000
  MotorWriting(wheel_L, wheel_R, 50,130);
}
else if(l3==1&&l2==1&&m==1&&r2==0&&r3==0){//11100
  MotorWriting(wheel_L, wheel_R, 90,130);
}
else if(l3==0&&l2==1&&m==1&&r2==0&&r3==0){//01100
  MotorWriting(wheel_L, wheel_R, 120,130);
}
else if(l3==0&&l2==0&&m==1&&r2==0&&r3==0){//00100
  MotorWriting(wheel_L, wheel_R, 130,130);
}
else if(l3==0&&l2==1&&m==1&&r2==1&&r3==0){//01110
  MotorWriting(wheel_L, wheel_R, 130,130);
}
else if(l3==0&&l2==0&&m==1&&r2==1&&r3==0){//00110
  MotorWriting(wheel_L, wheel_R, 130,120);
}
else if(l3==0&&l2==0&&m==1&&r2==1&&r3==1){//00111
  MotorWriting(wheel_L, wheel_R, 130,90);
}
else if(l3==0&&l2==0&&m==0&&r2==1&&r3==1){//00011
  MotorWriting(wheel_L, wheel_R, 130,50);
}
else if(l3==0&&l2==0&&m==0&&r2==1&&r3==0){//00010
  MotorWriting(wheel_L, wheel_R, 150,70);
}
else if(l3==0&&l2==0&&m==0&&r2==0&&r3==1){//00001
  MotorWriting(wheel_L, wheel_R, 170,10);
}
else if(l3==0&&l2==0&&m==0&&r2==0&&r3==0){//00000
  MotorWriting(wheel_L, wheel_R, -70,-70);
}
}


void setup(){
  MotorCheck();
}

void loop(){
  //MotorWriting(wheel_L, wheel_R, 0, 128);
  Tracking();
  
}