// Motor steering for Arduino motor kits using DC motors (L298N motor driver).
// Fourteen steering demonstrations: Four basic steering. Four skid steering. Six PWM steering.

#include <Servo.h> // to "show" changing speed
Servo servo1;
Servo servo2;
byte rightServoPin = 9;
byte leftServoPin = 10;

// 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

// useful factors of 255 = 1, 3, 5, 15, 17, 51, 85...
int stop  = 0; // stop
int speed =  51; // 1/6 speed. 51, 102, 127, 153, 204, 255
int accel; // PWM acceleration

int thisDelay = 2000; // between motor movement

void setup() {
  Serial.begin(115200); // start Serial Monitor

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

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

  servo1.attach(leftServoPin);
  servo1.write(0);
  servo2.attach(rightServoPin);
  servo2.write(0);

  basicSteering(); // forward, reverse, rotate left, rotate right, stop
  skidSteering(); // basicSteering() plus "skid steering"
  pwmSteering(); // variable motor speed steering

  Serial.println("Fin.");
}

void loop() {
  // empty
}

void basicSteering() { // double motor
  Serial.println("BASIC STEERING (FOUR plus STOP)");
  enableMotors(speed);
  allStop();
  forward();
  reverse();
  rotateLeft();
  rotateRight();
  allStop();
  enableMotors(stop);
}

void skidSteering () { // single motor
  Serial.println("SKID STEERING (FOUR plus STOP)");
  enableMotors(speed);
  allStop();
  forwardSkidLeft();
  reverseSkidRight(); // placed out of sequence to rotate car around start point
  forwardSkidRight();
  reverseSkidLeft();
  allStop();
  enableMotors(stop);
}

void pwmSteering() { // single and double motor
  Serial.println("PWM STEERING (SIX plus STOP)");
  thisDelay = 500;
  accel = 15;
  forwardPWM();
  reversePWM();
  rightRotatePWM();
  leftRotatePWM();
  rightSpinPWM();
  leftSpinPWM();
}

//**************************************************
// 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); // pause between changes for motors to run
}

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

void allStop() {
  Serial.print("All motors Stop. (L STOP R STOP)");
  driveMotor(LOW, LOW, LOW, LOW);
}

//**************************************************
// MOTOR STEERING
//**************************************************

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("\n\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
}

//**************************************************
// PWM FUNCTIONS
//**************************************************

void forwardPWM() {
  Serial.print("Forward (L FWD R FWD) ");
  driveMotor(HIGH, LOW, LOW, HIGH); // left motor forward, right motor forward
  increasePWM(accel, 1, 1);  // reverse acceleration, both motors
  decreasePWM(accel, 1, 1);  // reverse reverse-acceleration, both motors
  stopPWM();
}

void reversePWM() {
  Serial.print("Reverse (L REV R REV) ");
  driveMotor(LOW, HIGH, HIGH, LOW); // left motor reverse, right motor reverse
  increasePWM(accel, 1, 1);  // reverse acceleration, both motors
  decreasePWM(accel, 1, 1);  // reverse reverse-acceleration, both motors
  stopPWM();
}

void rightRotatePWM() {
  Serial.print("Right rotate (L FWD R OFF) ");
  driveMotor(HIGH, LOW, LOW, LOW); // left motor forward, right motor stop
  increasePWM(accel, 1, 0);  // reverse acceleration, right motor OFF
  decreasePWM(accel, 1, 0);  // reverse reverse-acceleration, right motor OFF
  stopPWM();
}

void leftRotatePWM() {
  Serial.print("Left rotate (L OFF R FWD) ");
  driveMotor(LOW, LOW, LOW, HIGH); // left motor stop, right motor forward
  increasePWM(accel, 0, 1);  // reverse acceleration, left motor OFF
  decreasePWM(accel, 0, 1);  // reverse reverse-acceleration, left motor OFF
  stopPWM();

}

void rightSpinPWM() {
  Serial.print("Right spin (L FWD R REV) ");
  driveMotor(HIGH, LOW, HIGH, LOW); // left motor forward, right motor reverse
  increasePWM(accel, 1, 1);  // reverse acceleration, both motors
  decreasePWM(accel, 1, 1);  // reverse reverse-acceleration, both motors
  stopPWM();
}

void leftSpinPWM() {
  Serial.print("Left spin (L REV R FWD) ");
  driveMotor(LOW, HIGH, LOW, HIGH); // left motor reverse, right motor forward
  increasePWM(accel, 1, 1);  // reverse acceleration, both motors
  decreasePWM(accel, 1, 1);  // reverse reverse-acceleration, both motors
  stopPWM();
}

void increasePWM(int accel, bool lmot, bool rmot) {
  Serial.print("\nIncrease PWM speed: ");
  int dutyCycle = 0;
  while (dutyCycle <= 256) { // do not exceed 255
    Serial.print(dutyCycle);
    Serial.print(" ");
    if (lmot) {
      digitalWrite(ENA, dutyCycle);
      servo1.write(map(dutyCycle, 0, 255, 0, 180));
    }
    if (rmot) {
      digitalWrite(ENB, dutyCycle);
      servo2.write(map(dutyCycle, 0, 255, 0, 180));
    }
    dutyCycle += accel; // increase
    delay(50);
  }
}

void decreasePWM(int accel, bool lmot, bool rmot) {
  Serial.print("\nDecrease PWM speed: ");
  int dutyCycle = 255 + accel;
  while (dutyCycle > 0) { // do not exceed 0
    dutyCycle -= accel; // decrease
    Serial.print(dutyCycle);
    Serial.print(" ");
    if (lmot) {
      digitalWrite(ENA, dutyCycle);
      servo1.write(map(dutyCycle, 0, 255, 0, 180));
    }
    if (rmot) {
      digitalWrite(ENB, dutyCycle);
      servo2.write(map(dutyCycle, 0, 255, 0, 180));
    }
    delay(50);
  }
}

void stopPWM() {
  Serial.println("\nStop PWM\n");
  int dutyCycle = 0;
  digitalWrite(ENA, dutyCycle); // disable motor
  digitalWrite(ENB, dutyCycle); // disable motor
  servo1.write(dutyCycle); // reset PWM meter
  servo2.write(dutyCycle); // reset PWM meter
  driveMotor(LOW, LOW, LOW, LOW); // left motor stop, right motor stop
}

//**************************************************
// L298N MOTOR STEERING
//**************************************************
//  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 ***