#include <Servo.h>

const int Sensor_1 = A0;
const int Sensor_2 = A1;
const int Sensor_3 = A2;
const int Sensor_4 = A3;
const int Sensor_5 = A4;

const int Motor_1 = 9;
const int Motor_2 = 10;
const int Motor_3 = 11;
const int Motor_4 = 12;

Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;

int sensorNumber[5];
int sensorColor = 500;

void setup() 
{
  pinMode(Sensor_1, INPUT);
  pinMode(Sensor_2, INPUT);
  pinMode(Sensor_3, INPUT);
  pinMode(Sensor_4, INPUT);
  pinMode(Sensor_5, INPUT);

  motor1.attach(Motor_1);
  motor2.attach(Motor_2);
  motor3.attach(Motor_3);
  motor4.attach(Motor_4);
}

void loop() 
{
  if (sensorNumber[2] < sensorColor)
    {
      Forward();
    }
  else 
    {
      if (sensorNumber[3] < sensorColor && sensorNumber[4] < sensorColor)
        {
          Right();
        }
      else if (sensorNumber[1] < sensorColor && sensorNumber[0] < sensorColor)
        {
          Left();
        }
      else if (sensorNumber[3] < sensorColor && sensorNumber[2] < sensorColor)
        {
          Right();
        }
      else if (sensorNumber[1] < sensorColor && sensorNumber[2] < sensorColor)
        {
          Left();
        }
      else if (sensorNumber[4] < sensorColor)
        {
          Right();
        }
      else if (sensorNumber[0] < sensorColor)
        {
          Left();
        }
      else
        {
          Back();
        }
    }
}

void Forward() 
{
  motor1.write(90);
  motor2.write(90);
  motor3.write(90);
  motor4.write(90);
}

void Back()
{
  motor1.write(180);
  motor2.write(180);
  motor3.write(180);
  motor4.write(180);
}

void Left()
{
  motor1.write(45);
  motor2.write(45);
  motor3.write(135);
  motor4.write(135);
}

void Right()
{
  motor1.write(135);
  motor2.write(135);
  motor3.write(45);
  motor4.write(45);
}