#include <Servo.h>
Servo PointMotor1;
Servo PointMotor2;
Servo PointMotor3;
Servo PointMotor4;

void setup() {
  PointMotor1.attach (6);
  PointMotor2.attach (9);
  PointMotor3.attach (10);
  PointMotor4.attach (11);
  PointMotor1.write(2400);
  PointMotor2.write(544);
  PointMotor3.write(2400);
  PointMotor4.write(544);
  pinMode(4, INPUT_PULLUP);
  pinMode(5, INPUT_PULLUP);
  pinMode(7, INPUT_PULLUP);
  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);
  pinMode(A3, OUTPUT);
  pinMode(A4, OUTPUT);
  pinMode(A5, OUTPUT);
  digitalWrite(A0, HIGH);  // ALL RED WHILE MOVING
  digitalWrite(A0, LOW);  //MAIN RED
  digitalWrite(A1, HIGH); //MAIN GREEN
  digitalWrite(A2, HIGH);  //RIGH RED
  digitalWrite(A3, LOW); //RIGH GREEN
  digitalWrite(A4, HIGH);  //LEFT RED
  digitalWrite(A5, LOW); //LEFT GREEN
  delay (1000);
  PointMotor1.detach ();
  PointMotor2.detach ();
  PointMotor3.detach ();
  PointMotor4.detach ();
  digitalWrite(A0, LOW);  //MAIN RED
  digitalWrite(A1, HIGH); //MAIN GREEN
  digitalWrite(A2, HIGH);  //RIGH RED
  digitalWrite(A3, LOW); //RIGH GREEN
  digitalWrite(A4, HIGH);  //LEFT RED
  digitalWrite(A5, LOW); //LEFT GREEN
}

void loop() {
  if (digitalRead(5)==LOW){  // STRAIGHT
    digitalWrite(A0, HIGH);  //MAIN RED ALL RED WHILE MOVING
    digitalWrite(A1, LOW); //MAIN GREEN
    digitalWrite(A2, HIGH);  //RIGH RED
    digitalWrite(A3, LOW); //RIGH GREEN
    digitalWrite(A4, HIGH);  //LEFT RED
    digitalWrite(A5, LOW); //LEFT GREEN
    PointMotor1.attach (6);
    PointMotor2.attach (9);
    PointMotor3.attach (10);
    PointMotor4.attach (11);
    PointMotor1.write(2400); // STRAIGHT
    PointMotor2.write(544);  // STRAIGHT
    PointMotor3.write(2400); // STRAIGHT
    PointMotor4.write(544);  // STRAIGHT
    delay (1000);
    PointMotor1.detach ();
    PointMotor2.detach ();
    PointMotor3.detach ();
    PointMotor4.detach ();
    digitalWrite(A0, LOW);  //MAIN RED
    digitalWrite(A1, HIGH); //MAIN GREEN
    digitalWrite(A2, HIGH);  //RIGH RED
    digitalWrite(A3, LOW); //RIGH GREEN
    digitalWrite(A4, HIGH);  //LEFT RED
    digitalWrite(A5, LOW); //LEFT GREEN
  };
  if (digitalRead(4)==LOW){  // RIGHT
    digitalWrite(A0, HIGH);  //MAIN RED ALL RED WHILE MOVING
    digitalWrite(A1, LOW); //MAIN GREEN
    digitalWrite(A2, HIGH);  //RIGH RED
    digitalWrite(A3, LOW); //RIGH GREEN
    digitalWrite(A4, HIGH);  //LEFT RED
    digitalWrite(A5, LOW); //LEFT GREEN
    PointMotor1.attach (6);
    PointMotor2.attach (9);
    PointMotor3.attach (10);
    PointMotor4.attach (11);
    PointMotor1.write(2400); // STRAIGHT
    PointMotor2.write(2400); // TURN
    PointMotor3.write(544);  // TURN
    PointMotor4.write(544);  // STRAIGHT
    delay (1000);
    PointMotor1.detach ();
    PointMotor2.detach ();
    PointMotor3.detach ();
    PointMotor4.detach ();
    digitalWrite(A0, HIGH);  //MAIN RED 
    digitalWrite(A1, LOW); //MAIN GREEN
    digitalWrite(A2, LOW);  //RIGH RED
    digitalWrite(A3, HIGH); //RIGH GREEN
    digitalWrite(A4, HIGH);  //LEFT RED
    digitalWrite(A5, LOW); //LEFT GREEN
  };
  if (digitalRead(7)==LOW){  //LEFT
    digitalWrite(A0, HIGH);  //MAIN RED ALL RED WHILE MOVING
    digitalWrite(A1, LOW); //MAIN GREEN
    digitalWrite(A2, HIGH);  //RIGH RED
    digitalWrite(A3, LOW); //RIGH GREEN
    digitalWrite(A4, HIGH);  //LEFT RED
    digitalWrite(A5, LOW); //LEFT GREEN
    PointMotor1.attach (6);
    PointMotor2.attach (9);
    PointMotor3.attach (10);
    PointMotor4.attach (11);
    PointMotor1.write(544);  // TURN
    PointMotor2.write(544);  // STRAIGHT
    PointMotor3.write(2400); // STRAIGHT
    PointMotor4.write(2400); // TURN
    delay (1000);
    PointMotor1.detach ();
    PointMotor2.detach ();
    PointMotor3.detach ();
    PointMotor4.detach ();
    digitalWrite(A0, HIGH);  //MAIN RED 
    digitalWrite(A1, LOW); //MAIN GREEN
    digitalWrite(A2, HIGH);  //RIGH RED
    digitalWrite(A3, LOW); //RIGH GREEN
    digitalWrite(A4, LOW);  //LEFT RED
    digitalWrite(A5, HIGH); //LEFT GREEN
  };
}