/*************************************
 *     TIME:2022.06.25
 *     Development Team: Zhiyi Technology Co., Ltd.
 * 
 *  **************************************/
#define ENA 5
#define ENB 6
#define IN1 3
#define IN2 4
#define IN3 2
#define IN4 7

int Sensor1;
int Sensor2;
int Sensor3;
int Sensor4;

void setup() {

  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  pinMode(8, INPUT);
  pinMode(9, INPUT);
  pinMode(10, INPUT);
  pinMode(11, INPUT);
  Serial.begin(9600);
}

void loop()
{

    Sensor1 = digitalRead(8);//IN1
    Sensor2 = digitalRead(9);//IN2
    Sensor3 = digitalRead(10);//IN3
    Sensor4 = digitalRead(11);//IN4

    Serial.print(Sensor4);
    Serial.print("--");
    Serial.print(Sensor3);
    Serial.print("--");
    Serial.print(Sensor2);
    Serial.print("--");
    Serial.println(Sensor1);

    if(Sensor4 == LOW && Sensor3 == LOW && Sensor2 == LOW && Sensor1 == LOW){
      forward();    
    }else if(Sensor4 == HIGH && Sensor3 == HIGH && Sensor2 == HIGH && Sensor1 == HIGH){
      stop();    
    }else if(Sensor4 == HIGH && Sensor3 == LOW && Sensor2 == LOW && Sensor1 == LOW){
      left_M();
    }else if(Sensor4 == LOW && Sensor3 == HIGH && Sensor2 == LOW && Sensor1 == LOW){
      left();
    }else if(Sensor4 == LOW && Sensor3 == LOW && Sensor2 == HIGH && Sensor1 == LOW){
      right();
    }else if(Sensor4 == LOW && Sensor3 == LOW && Sensor2 == LOW && Sensor1 == HIGH){
      right_M();
    }
}

void forward() {//forward function
  analogWrite(ENA, 150);//Set the speed of ENA
  analogWrite(ENB, 150);//Set the speed of ENB
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  // Serial.println("Forward");
}

void left() {//left function
  analogWrite(ENA, 150);//Set the speed of ENA
  analogWrite(ENB, 150);//Set the speed of ENB
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  // Serial.println("Left");
}

void left_M() {//left function
  analogWrite(ENA, 200);//Set the speed of ENA
  analogWrite(ENB, 200);//Set the speed of ENB
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  // Serial.println("Left");
}

void right() {//right function
  analogWrite(ENA, 150);//Set the speed of ENA
  analogWrite(ENB, 150);//Set the speed of ENB
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  // Serial.println("Right");
}

void right_M() {//right function
  analogWrite(ENA, 200);//Set the speed of ENA
  analogWrite(ENB, 200);//Set the speed of ENB
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  // Serial.println("Right");
}

void stop() {//stop function
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  // Serial.println("Stop!");
}