#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, stoptime = 200;

void setup() {
  Serial.begin(115200);

  pinMode(IN1, OUTPUT); // RIGHT +
  servoR.attach(ENA); // attaches to SERVO to show PWM deflection
  pinMode(IN2, OUTPUT); // RIGHT -

  pinMode(IN3, OUTPUT); // LEFT +
  servoL.attach(ENB); // attaches to SERVO to show PWM deflection
  pinMode(IN4, OUTPUT); // LEFT -

  stop(); // set stop
}

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

void stop() {
  runmotor(LOW, LOW, 0, LOW, LOW, 0, stoptime, "STOP"); // fwdL, revL, pwmL, fwdR, revR, pwmR, pause, text
}

void rev(int DURATION) {
  int speedL = random (50, 255); // random PWM speed
  int speedR = random (50, 255);
  runmotor(HIGH, LOW, speedL, HIGH, LOW, speedR, DURATION, "REVS");
}

void fwd(int DURATION) {
  int speedL = random (50, 255);
  int speedR = random (50, 255);
  runmotor(LOW, HIGH, speedL, LOW, HIGH, speedR, DURATION, "FWRD");
}

void left(int DURATION) {
  int speedL = random (50, 255);
  int speedR = random (50, 255);
  runmotor(LOW, LOW, 0, LOW, HIGH, speedR, DURATION, "LEFT");
}

void right(int DURATION) {
  int speedL = random (50, 255);
  int speedR = random (50, 255);
  runmotor(LOW, HIGH, speedL, LOW, LOW, 0, DURATION, "RGHT");
}

void runmotor(int FWDL, int REVL, int PWML, int FWDR, int REVR, int PWMR, int RUN, char text[]) {
  Serial.print(text);
  Serial.print(F(" = "));

  int VALL = map(PWML, 0, 255, 0, 180); // translate PWM (0 - 255) to servo (0 to 180)
  format(PWML, "(PWML");
  digitalWrite(IN3, REVL);
  digitalWrite(IN4, FWDL);
  servoL.write(VALL);

  int VALR = map(PWMR, 0, 255, 0, 180);  // translate PWM (0 - 255) to servo (0 to 180)
  format(PWMR, ") (PWMR");
  digitalWrite(IN1, REVR);
  digitalWrite(IN2, FWDR);
  servoR.write(VALR);


  Serial.println(F(")"));
  delay(RUN);
}

void format(int value, char text[]) {
  Serial.print(text);
  if (value < 100) Serial.print(F(" "));
  if (value < 10) Serial.print(F(" "));
  Serial.print(F(" "));
  Serial.print(value);
  Serial.print(F("/255"));
}