#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
};
}