// Motor steering simulating Arduino Cars with L298N style motor driver boards
// NOT for LAFVIN car (LDR, IR, SONAR, line follow, IRremote)

// LEFT motor
int IN1 = 8; // LEFT FORWARD, DIO pin, only HIGH or LOW
int IN2 = 7; // LEFT REVERSE, DIO pin, only HIGH or LOW
int ENA = 6; // ENABLE (and disable) left motor, PWM pin, speed = 0 - 255

// RIGHT motor
int IN3 = 2; // RIGHT FORWARD, DIO pin, only HIGH or LOW
int IN4 = 4; // RIGHT REVERSE, DIO pin, only HIGH or LOW
int ENB = 3; // ENABLE (and disable) right motor, PWM pin, speed = 0 - 255

// "speed" to run motors, adjustable PWM, range 0 (no speed) - 255 (full speed)
// useful factors of 255 = 1, 3, 5, 15, 17, 51, 85...
int speed =  51; // slow
// int speed = 127; // medium
// int speed = 255; // maximum

int ON  = speed;
int OFF = 0;

int thisDelay = 3000; // delay during motor movement

void setup() {
  Serial.begin(9600); // for Serial Monitor

  pinMode(ENA, OUTPUT); // configure ENABLE pins for output
  pinMode(ENB, OUTPUT);

  pinMode(IN1, OUTPUT); // configure MOTOR DRIVER pins for output
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // lightShow(); // use only with simulation

  // basicSteering(); // forward, reverse, rotate left, rotate right
  eightSteeringModes(); // the above plus forward-skid turns, reverse skid turns
}

void loop() {
}

void lightShow() { // turn on each LED... do not use with motors
  // Serial.println("LED show. For Simulation only.");
  byte led[] = {8, 4, 6, 3, 7, 2};
  for (int i = 0; i < 6; i++) {
    digitalWrite(led[i], HIGH);
    delay(200);
    digitalWrite(led[i], LOW);
  }
  delay(500);
}

void basicSteering() {
  enableMotors(speed);
  allStop(); Serial.println();
  forward();
  reverse();
  rotateLeft();
  rotateRight();
  allStop();
  enableMotors(OFF);
}

void eightSteeringModes () {
  enableMotors(speed);
  allStop();
  // single motor
  forwardSkidLeft();
  reverseSkidRight(); // placed this out of order to keep car stationary
  forwardSkidRight();
  reverseSkidLeft();
  // double motors
  rotateLeft();
  rotateRight();
  forward();
  reverse();
  allStop();
  enableMotors(OFF);
}

//**************************************************
// L298N MOTOR DIRECTIONS
//**************************************************
//  ENA   ENB   IN1   IN2   IN3   IN4
//  HIGH  HIGH  HIGH  LOW   LOW   HIGH  forward - left forward, right forward
//  HIGH  HIGH  LOW   HIGH  HIGH  LOW   reverse - left reverse, right reverse
//  HIGH  HIGH  LOW   HIGH  LOW   HIGH  face left - left reverse, right forward
//  HIGH  HIGH  HIGH  LOW   HIGH  LOW   face right - left forward, right reverse
//  HIGH  HIGH  LOW   LOW   LOW   HIGH  forward skid left - left stop, right forward
//  HIGH  HIGH  HIGH  LOW   LOW   LOW   forward skid right - left forward, right stop
//  HIGH  HIGH  LOW   HIGH  LOW   LOW   reverse skid left - left reverse, right stop
//  HIGH  HIGH  LOW   LOW   HIGH  LOW   reverse skid right - left stop, right reverse
//  HIGH  HIGH  LOW   LOW   LOW   LOW   stop - all LOW
//  HIGH  HIGH  HIGH  HIGH  HIGH  HIGH  brake - all HIGH - do not use, over current
//  LOW   LOW   N/A   N/A   N/A   N/A   stop - both ENABLE LOW
//
// *** LEFT motor and RIGHT motor are facing opposite directions ***
// *** LEFT motor will use HIGH, LOW to go forward ***
// *** RIGHT motor will use LOW, HIGH to go forward ***

//**************************************************
// MOTOR FUNCTIONS
//**************************************************
void driveMotor(bool p1, bool p2, bool p3, bool p4) {
  digitalWrite(IN1, p1);
  digitalWrite(IN2, p2);
  digitalWrite(IN3, p3);
  digitalWrite(IN4, p4);
  delay(thisDelay);
}

void enableMotors(int enab) {
  if (enab) {
    Serial.print("Enable motors... ");
    delay(thisDelay / 2);
  }
  else
    Serial.print("... Disable motors.");
  digitalWrite(ENA, enab);
  digitalWrite(ENB, enab);
}

void allStop() {
  Serial.print("All motors Stop. (L STOP R STOP)");
  driveMotor(LOW, LOW, LOW, LOW);
  delay(thisDelay / 2); // allow motor to be stopped
}

void forwardSkidLeft() {
  Serial.print("\n\tForward Skid-Left.  (L STOP R FWD)");
  digitalWrite(ENA, 0);
  digitalWrite(ENB, speed);
  driveMotor(LOW, LOW, LOW, HIGH); // left motor stop, right motor forward
}

void forwardSkidRight() {
  Serial.print("\tForward Skid-Right. (L FWD R STOP)");
  digitalWrite(ENA, speed);
  digitalWrite(ENB, 0);
  driveMotor(HIGH, LOW, LOW, LOW); // left motor forward, right motor off
}

void reverseSkidLeft()  {
  Serial.print("\tReverse Skid-Left.   (L REV R STOP)\n");
  digitalWrite(ENA, speed);
  digitalWrite(ENB, 0);
  driveMotor(LOW, HIGH, LOW, LOW); // left motor reverse, right motor stop
}

void reverseSkidRight() {
  Serial.print("\tReverse Skid-Right.  (L STOP R REV)\n");
  digitalWrite(ENA, 0);
  digitalWrite(ENB, speed);
  driveMotor(LOW, LOW, HIGH, LOW); // left motor off, right motor reverse
}

void rotateLeft() {
  Serial.print("\tRotate Left.\t    (L REV R FWD)");
  digitalWrite(ENA, speed);
  digitalWrite(ENB, speed);
  driveMotor(LOW, HIGH, LOW, HIGH); // left motor reverse, right motor forward
}

void rotateRight() {
  Serial.print("\tRotate Right.\t     (L FWD R REV)\n");
  digitalWrite(ENA, speed);
  digitalWrite(ENB, speed);
  driveMotor(HIGH, LOW, HIGH, LOW); // left motor forward, right motor reverse
}

void forward() {
  Serial.print("\tForward.\t    (L FWD R FWD)");
  digitalWrite(ENA, speed);
  digitalWrite(ENB, speed);
  driveMotor(HIGH, LOW, LOW, HIGH); // left motor forward, right motor forward
}

void reverse() {
  Serial.print("\tReverse.\t     (L REV R REV)\n");
  digitalWrite(ENA, speed);
  digitalWrite(ENB, speed);
  driveMotor(LOW, HIGH, HIGH, LOW ); // left motor reverse, right motor reverse
}

void pwmDrive() {
  // Move motor with changing speed
  int dutyCycle;
  forward();
  while (dutyCycle <= 255) {
    digitalWrite(ENA, dutyCycle);
    dutyCycle += 51; // useful factors of 255 = 1, 3, 5, 15, 17, 51, 85
    delay(250);
  }
  reverse();
  while (dutyCycle > 0) {
    digitalWrite(ENA, dutyCycle);
    dutyCycle -= 51;
    delay(250);
  }
}