#include <Servo.h>
Servo servoR;
Servo servoL;
int IN1 = 4, IN2 = 2, ENA = 3; // RIGHT
int IN3 = 7, IN4 = 5, ENB = 6; // LEFT

int runtime = 2000;

void setup() {
  Serial.begin(115200);
  pinMode(IN1, OUTPUT);
  servoR.attach(ENA);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  servoL.attach(ENB);
  pinMode(IN4, OUTPUT);
  stop();
}

void loop() {
  fwd(runtime);
  stop();
  rev(runtime);
  stop();
  left(runtime);
  stop();
  right(runtime);
  stop();
}

void stop() {
  Serial.println("STP ");
  runmotor(LOW, LOW, 0, LOW, LOW, 0, 200);
}

void rev(int DURATION) {
  Serial.print("REV ");
  runmotor(HIGH, LOW, 180, HIGH, LOW, 180, DURATION);
}

void fwd(int DURATION) {
  Serial.print("FWD ");
  runmotor(LOW, HIGH, 180, LOW, HIGH, 180, DURATION);
}

void left(int DURATION) {
  Serial.print("LFT ");
  runmotor(LOW, LOW, 0, HIGH, LOW, 180, DURATION);
}

void right(int DURATION) {
  Serial.print("RIT ");
  runmotor(HIGH, LOW, 180, LOW, LOW, 0, DURATION);
}

void runmotor(int FWDL, int REVL, int PWML, int FWDR, int REVR, int PWMR, int RUN) {
  int VALR = map(PWMR, 0, 255, 0, 179);
  digitalWrite(IN1, REVR);
  digitalWrite(IN2, FWDR);
  servoR.write(VALR);

  int VALL = map(PWML, 0, 255, 0, 179);
  digitalWrite(IN3, REVL);
  digitalWrite(IN4, FWDL);
  servoL.write(VALL);

  delay(RUN);
}