/*
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as
    published by the Free Software Foundation, either version 3 of the
    License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

// use pin 13's LED to indicate intended travel direction.
//  on == forward, off == backward
int led = 13;

int leftMotorEnable = 10;
int leftMotorA = 9;
int leftMotorB = 8;

int rightMotorEnable = 3;
int rightMotorA = 4;
int rightMotorB = 5;

void setupMotor(int motorEnable, int motorA, int motorB){
    pinMode(motorEnable, OUTPUT);
    pinMode(motorA, OUTPUT);
    pinMode(motorB, OUTPUT);

    digitalWrite(motorEnable, LOW);
    digitalWrite(motorA, LOW);
    digitalWrite(motorB, LOW);
}

// the setup routine runs once when you press reset:
void setup() {
  // initialize the digital pin as an output.
  setupMotor(leftMotorEnable, leftMotorA, leftMotorB);
  setupMotor(rightMotorEnable, rightMotorA, rightMotorB);

  pinMode(led, OUTPUT);
}

void motorsRun(int left, int right, int msDelay) {
  // Set left motor direction:
  if (left > 0) {
    // Set left motor to go forward:
    digitalWrite(leftMotorA, HIGH);
    digitalWrite(leftMotorB, LOW);
  } else {
    // Set left motor to go backward:
    digitalWrite(leftMotorA, LOW);
    digitalWrite(leftMotorB, HIGH);
    left = -left;                   // Make left a positive value:
  }

  analogWrite(rightMotorEnable, left); // Start motor in right direction
  // Set left motor direction:
  if (right > 0) {
    // Set right motor to go forward:
    digitalWrite(rightMotorA, HIGH);
    digitalWrite(rightMotorB, LOW);
  } else {
    // Set right motor to go backward:
    digitalWrite(rightMotorA, LOW);
    digitalWrite(rightMotorB, HIGH);
    right = -right;                 // Make right a positive value:
  }
  analogWrite(leftMotorEnable, left); // Start motor in right direction

  delay(msDelay);                  // Wait the specified amount of time

  // Stop both motors:
  analogWrite(leftMotorEnable, 0);
  analogWrite(rightMotorEnable, 0);
}

// the loop routine runs over and over again forever:
void loop() {
  digitalWrite(led, HIGH);   // turn the LED on (HIGH is the voltage level)
  motorsRun(100, 100, 2000);// Run the robot forward
  digitalWrite(led, LOW);    // turn the LED off by making the voltage LOW
  motorsRun(-100, -100, 2000); // Run the robot backward
}